Skip to content

fix initial walk ready transition in simulation #618

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 5 commits into from
Nov 8, 2024
Merged

fix initial walk ready transition in simulation #618

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
3582054
fix initial walk ready transition in simulation
Flova Nov 7, 2024
aeba0ab
Also play startup animation in simulator
Flova Nov 7, 2024
416a38a
Merge branch 'main' into fix/initial-walk-ready-transition-in-simulation
cleWu03 Nov 7, 2024
f75724e
Apply review and fix comment
Flova Nov 7, 2024
f8835af
Merge branch 'fix/initial-walk-ready-transition-in-simulation' of git…
Flova Nov 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -213,7 +213,8 @@
"variant": "cpp",
"regex": "cpp",
"future": "cpp",
"*.ipp": "cpp"
"*.ipp": "cpp",
"span": "cpp"
},
// Tell the ROS extension where to find the setup.bash
// This also utilizes the COLCON_WS environment variable, which needs to be set
Expand Down
9 changes: 6 additions & 3 deletions bitbots_motion/bitbots_dynup/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,14 @@ set(PYBIND11_FINDPYTHON ON)

find_package(ament_cmake REQUIRED)
find_package(backward_ros REQUIRED)
find_package(bio_ik REQUIRED)
find_package(bitbots_msgs REQUIRED)
find_package(bitbots_splines REQUIRED)
find_package(bitbots_utils REQUIRED)
find_package(control_msgs REQUIRED)
find_package(control_toolbox REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(generate_parameter_library REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(rclcpp REQUIRED)
Expand All @@ -27,8 +30,6 @@ find_package(tf2 REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(generate_parameter_library REQUIRED)

find_package(ros2_python_extension REQUIRED)
find_package(pybind11 REQUIRED)
Expand Down Expand Up @@ -64,13 +65,14 @@ add_executable(DynupNode ${SOURCES})
ament_target_dependencies(
DynupNode
ament_cmake
bio_ik
bitbots_msgs
bitbots_splines
bitbots_utils
control_msgs
control_toolbox
geometry_msgs
generate_parameter_library
geometry_msgs
moveit_ros_planning_interface
rclcpp
ros2_python_extension
Expand All @@ -94,6 +96,7 @@ ament_target_dependencies(
libpy_dynup
PUBLIC
ament_cmake
bio_ik
bitbots_msgs
bitbots_splines
bitbots_utils
Expand Down
1 change: 1 addition & 0 deletions bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_
#define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_DYNUP_IK_H_

#include <bio_ik/bio_ik.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include <tf2/convert.h>
Expand Down
13 changes: 10 additions & 3 deletions bitbots_motion/bitbots_dynup/src/dynup_ik.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <bitbots_dynup/dynup_ik.hpp>

namespace bitbots_dynup {

DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {}
Expand Down Expand Up @@ -41,7 +42,7 @@ void DynupIK::setDirection(DynupDirection direction) { direction_ = direction; }

bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) {
/* ik options is basically the command which we send to bio_ik and which describes what we want to do */
auto ik_options = kinematics::KinematicsQueryOptions();
kinematics::KinematicsQueryOptions ik_options;
ik_options.return_approximate_solution = true;

geometry_msgs::msg::Pose right_foot_goal_msg, left_foot_goal_msg, right_hand_goal_msg, left_hand_goal_msg;
Expand All @@ -54,13 +55,19 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) {
bool success;
goal_state_->updateLinkTransforms();

bio_ik::BioIKKinematicsQueryOptions leg_ik_options;
leg_ik_options.return_approximate_solution = true;

// Add auxiliary goal to prevent bending the knees in the wrong direction when we go from init to walkready
leg_ik_options.goals.push_back(std::make_unique<bio_ik::AvoidJointLimitsGoal>());

success = goal_state_->setFromIK(l_leg_joints_group_, left_foot_goal_msg, 0.005,
moveit::core::GroupStateValidityCallbackFn(), ik_options);
moveit::core::GroupStateValidityCallbackFn(), leg_ik_options);

goal_state_->updateLinkTransforms();

success &= goal_state_->setFromIK(r_leg_joints_group_, right_foot_goal_msg, 0.005,
moveit::core::GroupStateValidityCallbackFn(), ik_options);
moveit::core::GroupStateValidityCallbackFn(), leg_ik_options);

goal_state_->updateLinkTransforms();

Expand Down
2 changes: 1 addition & 1 deletion bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
-->HCM
$StartHCM
START_UP --> $Simulation
YES --> @RobotStateStartup, @PlayAnimationDynup + direction:walkready + r:false
YES --> @RobotStateStartup, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
NO --> @RobotStateStartup, @Wait + time:1 + r:false, @PlayAnimationStartup, @PlayAnimationDynup + direction:walkready + r:false
RUNNING --> $CheckMotors
MOTORS_NOT_STARTED --> @RobotStateStartup, @Wait
Expand Down
Loading