Feature/analytical ik

src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml

@@ -6,6 +6,6 @@
 
 /bitbots_extrinsic_imu_calibration:
   ros__parameters:
-    offset_x: 0.295
-    offset_y: 0.06
+    offset_x: 0.3
+    offset_y: 0.047
     offset_z: 0.0

src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp

@@ -81,6 +81,7 @@ class DynupEngine : public bitbots_splines::AbstractEngine<DynupRequest, DynupRe
   bitbots_splines::PoseSpline r_hand_spline_;
 
   DynupResponse goals_;
+  std::shared_ptr<rclcpp::Executor> walk_param_executor_;
   std::shared_ptr<rclcpp::Node> walking_param_node_;
   std::shared_ptr<rclcpp::SyncParametersClient> walking_param_client_;
 

src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp

@@ -3,11 +3,13 @@
 
 #include <bio_ik/bio_ik.hpp>
 #include <bitbots_dynup/dynup_parameters.hpp>
+#include <bitbots_dynup/ik.hpp>
 #include <bitbots_splines/abstract_ik.hpp>
 #include <moveit/robot_model_loader/robot_model_loader.hpp>
 #include <moveit/robot_state/robot_state.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <tf2/convert.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "dynup_utils.hpp"
@@ -23,13 +25,13 @@ class DynupIK : public bitbots_splines::AbstractIK<DynupResponse> {
   moveit::core::RobotStatePtr get_goal_state();
   void set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
 
+  const std::vector<std::string> getLeftLegJointNames();
+  const std::vector<std::string> getRightLegJointNames();
+
  private:
   rclcpp::Node::SharedPtr node_;
-  moveit::core::JointModelGroup* all_joints_group_;
   moveit::core::JointModelGroup* l_arm_joints_group_;
-  moveit::core::JointModelGroup* l_leg_joints_group_;
   moveit::core::JointModelGroup* r_arm_joints_group_;
-  moveit::core::JointModelGroup* r_leg_joints_group_;
   moveit::core::RobotStatePtr goal_state_;
   sensor_msgs::msg::JointState::ConstSharedPtr joint_state_;
   DynupDirection direction_;