fix initial walk ready transition in simulation

Author: Flova

.vscode/settings.json

@@ -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

bitbots_motion/bitbots_dynup/CMakeLists.txt

@@ -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)
@@ -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)
@@ -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
@@ -94,6 +96,7 @@ ament_target_dependencies(
   libpy_dynup
   PUBLIC
   ament_cmake
+  bio_ik
   bitbots_msgs
   bitbots_splines
   bitbots_utils

bitbots_motion/bitbots_dynup/src/dynup_ik.cpp

@@ -1,4 +1,7 @@
+#include <bio_ik/bio_ik.h>  // TODO remove this include
+
 #include <bitbots_dynup/dynup_ik.hpp>
+
 namespace bitbots_dynup {
 
 DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {}
@@ -54,13 +57,19 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) {
   bool success;
   goal_state_->updateLinkTransforms();
 
+  // Add auxiliary goal for the knees to prevent bending in the wrong direction
+  bio_ik::BioIKKinematicsQueryOptions leg_ik_options;
+  leg_ik_options.return_approximate_solution = true;
+
+  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();