Integration weekend tunes (#521)

Author: Flova

bitbots_motion/bitbots_dynup/CMakeLists.txt

@@ -37,8 +37,13 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
 generate_parameter_library(dynup_parameters config/dynup_config.yaml)
 
 rosidl_generate_interfaces(
-  ${PROJECT_NAME} "msg/DynupEngineDebug.msg" "msg/DynupPoses.msg" DEPENDENCIES
-  std_msgs geometry_msgs)
+  ${PROJECT_NAME}
+  "msg/DynupEngineDebug.msg"
+  "msg/DynupPoses.msg"
+  "msg/DynupIkOffset.msg"
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs)
 
 include_directories(include ${PYTHON_INCLUDE_DIRS})
 

bitbots_motion/bitbots_dynup/bitbots_dynup_py/py_dynup_wrapper.py

@@ -49,10 +49,10 @@ def special_reset(self, time: float):
     def set_engine_goal(self, direction):
         self.py_dynup_wrapper.set_engine_goal(direction)
 
-    def step(self, dt: float, imu_msg, jointstate_msg=None):
+    def step(self, dt: float, imu_msg, joint_state_msg):
         if dt == 0.0:
             dt = 0.001
-        step = self.py_dynup_wrapper.step(dt, self._to_cpp(imu_msg))
+        step = self.py_dynup_wrapper.step(dt, self._to_cpp(imu_msg), self._to_cpp(joint_state_msg))
         result = self._from_cpp(step, JointCommand)
         return result
 

bitbots_motion/bitbots_dynup/config/dynup_config.yaml

@@ -21,13 +21,13 @@ bitbots_dynup:
       hand_walkready_pitch:
         type: double
         description: "End pose hand pitch. Depends on walkready of walking."
-        default_value: 40.0
+        default_value: 8.0
         validation:
           bounds<>: [0.0, 90.0]
       hand_walkready_height:
         type: double
         description: "End pose hand height. Depends on walkready of walking."
-        default_value: -0.17
+        default_value: -0.29
         validation:
           bounds<>: [-1.0, 1.0]
       trunk_height:
@@ -231,7 +231,7 @@ bitbots_dynup:
       rise_time:
         type: double
         description: "Time to rise in seconds"
-        default_value: 1.0
+        default_value: 2.0
         validation:
           bounds<>: [0.0, 10.0]
 

bitbots_motion/bitbots_dynup/config/dynup_sim.yaml

@@ -3,8 +3,6 @@ dynup:
     engine:
       end_pose:
         arm_extended_length: 0.3
-        hand_walkready_pitch: 34.2
-        hand_walkready_height: -0.18
         trunk_height: 0.4
         trunk_pitch: 0.13
         trunk_x_final: 0.0

bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp

@@ -6,6 +6,7 @@
 #include <tf2/convert.h>
 
 #include <bitbots_splines/abstract_ik.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include "dynup_parameters.hpp"
@@ -20,6 +21,7 @@ class DynupIK : public bitbots_splines::AbstractIK<DynupResponse> {
   void reset() override;
   void setDirection(DynupDirection direction);
   moveit::core::RobotStatePtr get_goal_state();
+  void set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
 
  private:
   rclcpp::Node::SharedPtr node_;
@@ -29,6 +31,7 @@ class DynupIK : public bitbots_splines::AbstractIK<DynupResponse> {
   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_;
 };
 

bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp

@@ -67,13 +67,14 @@ class DynupNode {
   /**
    * Retrieve current positions of left foot and trunk relative to right foot
    *
-   * @return The pair of (right foot, left foot) poses if transformation was successfull
+   * @return The pair of (right foot, left foot) poses if transformation was successful
    */
   bitbots_dynup::msg::DynupPoses getCurrentPoses();
 
   bitbots_msgs::msg::JointCommand step(double dt);
 
-  bitbots_msgs::msg::JointCommand step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg);
+  bitbots_msgs::msg::JointCommand step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg,
+                                       const sensor_msgs::msg::JointState::SharedPtr joint_state_msg);
 
   geometry_msgs::msg::PoseArray step_open_loop(double dt);
 
@@ -93,10 +94,6 @@ class DynupNode {
   // Store reference to the "real" node
   rclcpp::Node::SharedPtr node_;
 
-  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr debug_publisher_;
-  rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr joint_goal_publisher_;
-  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
-
   rclcpp_action::Server<DynupGoal>::SharedPtr action_server_;
 
   std::vector<std::string> param_names_;
@@ -106,21 +103,33 @@ class DynupNode {
   // Data structure to hold all parameters, which is build from the schema in the 'parameters.yaml'
   bitbots_dynup::Params params_;
 
+  // Subcomponents of the dynup
   DynupEngine engine_;
   Stabilizer stabilizer_;
   Visualizer visualizer_;
   DynupIK ik_;
+
+  // Variables for node
   int stable_duration_ = 0;
   int failed_tick_counter_ = 0;
   double last_ros_update_time_ = 0;
   double start_time_ = 0;
   bool server_free_ = true;
   bool debug_ = false;
+
+  // TF2 related things
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+
+  // MoveIt related things
   std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_;
   moveit::core::RobotModelPtr kinematic_model_;
 
+  // Publishers and subscribers
+  rclcpp::Publisher<bitbots_msgs::msg::JointCommand>::SharedPtr joint_goal_publisher_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_;
+
   void execute(const std::shared_ptr<DynupGoalHandle> goal);
 
   /**

bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_pywrapper.hpp

@@ -18,7 +18,7 @@ using namespace ros2_python_extension;
 class PyDynupWrapper {
  public:
   explicit PyDynupWrapper(std::string ns);
-  py::bytes step(double dt, py::bytes &imu_msg);
+  py::bytes step(double dt, py::bytes &imu_msg, py::bytes &joint_state_msg);
   py::bytes step_open_loop(double dt);
   py::bytes get_poses();
   void reset();

bitbots_motion/bitbots_dynup/include/bitbots_dynup/visualizer.hpp

@@ -1,14 +1,20 @@
 #ifndef BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_
 #define BITBOTS_DYNUP_INCLUDE_BITBOTS_DYNUP_VISUALIZER_H_
 
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
 #include <tf2/LinearMath/Vector3.h>
 
+#include <bitbots_dynup/dynup_utils.hpp>
+#include <bitbots_dynup/msg/dynup_ik_offset.hpp>
+#include <bitbots_splines/abstract_ik.hpp>
 #include <bitbots_splines/abstract_visualizer.hpp>
 #include <bitbots_splines/smooth_spline.hpp>
 #include <bitbots_splines/spline_container.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
+#include <tf2_eigen/tf2_eigen.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include "dynup_parameters.hpp"
@@ -22,10 +28,13 @@ class Visualizer : bitbots_splines::AbstractVisualizer {
   void setParams(bitbots_dynup::Params::Visualizer params);
 
   void displaySplines(bitbots_splines::PoseSpline splines, const std::string &frame);
+  void publishIKOffsets(const moveit::core::RobotModelPtr &model, const DynupResponse &response,
+                        const bitbots_splines::JointGoals &ik_joint_goals);
 
  private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr spline_publisher_;
+  rclcpp::Publisher<bitbots_dynup::msg::DynupIkOffset>::SharedPtr ik_debug_publisher_;
   std::string base_topic_;
   const std::string marker_ns_ = "bitbots_dynup";
   bitbots_dynup::Params::Visualizer params_;

bitbots_motion/bitbots_dynup/msg/DynupIkOffset.msg

@@ -0,0 +1,7 @@
+std_msgs/Header header
+
+# offset between goal position and position computed by running FK on the IK results
+geometry_msgs/Pose left_foot_ik_offset
+geometry_msgs/Pose right_foot_ik_offset
+geometry_msgs/Pose left_hand_ik_offset
+geometry_msgs/Pose right_hand_ik_offset

bitbots_motion/bitbots_dynup/src/dynup_ik.cpp

@@ -19,17 +19,27 @@ void DynupIK::init(moveit::core::RobotModelPtr kinematic_model) {
 void DynupIK::reset() {
   // we have to set some good initial position in the goal state, since we are using a gradient
   // based method. Otherwise, the first step will be not correct
-  std::vector<std::string> names_vec = {"LHipPitch", "LKnee", "LAnklePitch", "RHipPitch", "RKnee", "RAnklePitch"};
-  std::vector<double> pos_vec = {0.7, 1.0, -0.4, -0.7, -1.0, 0.4};
-  for (size_t i = 0; i < names_vec.size(); i++) {
-    // besides its name, this method only changes a single joint position...
-    goal_state_->setJointPositions(names_vec[i], &pos_vec[i]);
+
+  // Use the current joint state as a starting point for the IK
+  if (joint_state_) {
+    goal_state_->setVariablePositions(joint_state_->name, joint_state_->position);
+  } else {
+    // if we don't have a joint state, we set some default values that are a reasonable starting point
+    RCLCPP_WARN(node_->get_logger(),
+                "No joint state received, using hardcoded initial positions for IK "
+                "initialization");
+    std::vector<std::string> names_vec = {"LHipPitch", "LKnee", "LAnklePitch", "RHipPitch", "RKnee", "RAnklePitch"};
+    std::vector<double> pos_vec = {0.7, 1.0, -0.4, -0.7, -1.0, 0.4};
+    for (size_t i = 0; i < names_vec.size(); i++) {
+      // besides its name, this method only changes a single joint position...
+      goal_state_->setJointPositions(names_vec[i], &pos_vec[i]);
+    }
   }
 }
 
 void DynupIK::setDirection(DynupDirection direction) { direction_ = direction; }
 
-bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse &ik_goals) {
+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();
   ik_options.return_approximate_solution = true;
@@ -104,4 +114,7 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse &ik_goals) {
 
 moveit::core::RobotStatePtr DynupIK::get_goal_state() { return goal_state_; }
 
+void DynupIK::set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state) {
+  joint_state_ = joint_state;
+}
 }  // namespace bitbots_dynup

bitbots_motion/bitbots_dynup/src/dynup_node.cpp

@@ -5,18 +5,19 @@ using namespace std::chrono_literals;
 
 DynupNode::DynupNode(rclcpp::Node::SharedPtr node, const std::string &ns, std::vector<rclcpp::Parameter> parameters)
     : node_(node),
-      debug_publisher_(node_->create_publisher<visualization_msgs::msg::Marker>("debug_markers", 1)),
-      joint_goal_publisher_(node_->create_publisher<bitbots_msgs::msg::JointCommand>("dynup_motor_goals", 1)),
-      imu_subscriber_(node_->create_subscription<sensor_msgs::msg::Imu>("imu/data", 1,
-                                                                        std::bind(&DynupNode::imuCallback, this, _1))),
       param_listener_(node_),
       params_(param_listener_.get_params()),
       engine_(node_, params_.engine),
       stabilizer_(node_, params_.stabilizer),
       visualizer_(node_, params_.visualizer, "debug/dynup"),
       ik_(node_),
       tf_buffer_(node_->get_clock()),
-      tf_listener_(tf_buffer_, node_) {
+      tf_listener_(tf_buffer_, node_),
+      joint_goal_publisher_(node_->create_publisher<bitbots_msgs::msg::JointCommand>("dynup_motor_goals", 1)),
+      imu_subscriber_(node_->create_subscription<sensor_msgs::msg::Imu>("imu/data", 1,
+                                                                        std::bind(&DynupNode::imuCallback, this, _1))),
+      joint_state_subscriber_(node_->create_subscription<sensor_msgs::msg::JointState>(
+          "joint_states", 1, std::bind(&DynupNode::jointStateCallback, this, _1))) {
   // We need to create a new node for moveit, otherwise dynamic reconfigure will be broken...
   auto moveit_node = std::make_shared<rclcpp::Node>(ns + "dynup_moveit_node");
 
@@ -84,16 +85,21 @@ DynupNode::DynupNode(rclcpp::Node::SharedPtr node, const std::string &ns, std::v
   RCLCPP_INFO(node_->get_logger(), "Initialized DynUp and waiting for actions");
 }
 
-bitbots_msgs::msg::JointCommand DynupNode::step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg) {
+bitbots_msgs::msg::JointCommand DynupNode::step(double dt, const sensor_msgs::msg::Imu::SharedPtr imu_msg,
+                                                const sensor_msgs::msg::JointState::SharedPtr joint_state) {
   // method for python interface. take all messages as parameters instead of using ROS
   imuCallback(imu_msg);
+  jointStateCallback(joint_state);
   // update dynup engine response
   bitbots_msgs::msg::JointCommand joint_goals = step(dt);
   return joint_goals;
 }
 
 bitbots_msgs::msg::JointCommand DynupNode::step(double dt) {
   if (dt <= 0) {
+    RCLCPP_WARN(node_->get_logger(),
+                "dt was 0. this can happen in simulation if your update rate is higher than the "
+                "simulators.");
     dt = 0.001;
   }
 
@@ -108,6 +114,8 @@ bitbots_msgs::msg::JointCommand DynupNode::step(double dt) {
   // Calculate the joint goals (IK)
   bitbots_splines::JointGoals goals = ik_.calculate(stabilized_response);
 
+  visualizer_.publishIKOffsets(kinematic_model_, stabilized_response, goals);
+
   // Check if we found a solution
   if (goals.first.empty()) {
     failed_tick_counter_++;
@@ -135,6 +143,10 @@ geometry_msgs::msg::PoseArray DynupNode::step_open_loop(double dt) {
 
 void DynupNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { stabilizer_.setImu(msg); }
 
+void DynupNode::jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr joint_states) {
+  ik_.set_joint_positions(joint_states);
+}
+
 void DynupNode::onSetParameters() {
   engine_.setParams(params_.engine);
   stabilizer_.setParams(params_.stabilizer);
@@ -276,7 +288,7 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr<DynupGoalHandle> goal_
 }
 
 bitbots_dynup::msg::DynupPoses DynupNode::getCurrentPoses() {
-  rclcpp::Time time = node_->get_clock()->now();
+  rclcpp::Time time;
   /* Transform the left foot into the right foot frame and all other splines into the base link frame*/
   bitbots_dynup::msg::DynupPoses msg;
   try {
@@ -288,13 +300,13 @@ bitbots_dynup::msg::DynupPoses DynupNode::getCurrentPoses() {
 
     // Get the transforms of the end effectors
     geometry_msgs::msg::Transform l_foot_transformed =
-        tf_buffer_.lookupTransform(tf_names.r_sole_frame, tf_names.l_sole_frame, time, timeout).transform;
+        tf_buffer_.lookupTransform(tf_names.r_sole_frame, tf_names.l_sole_frame, time).transform;
     geometry_msgs::msg::Transform r_foot_transformed =
-        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.r_sole_frame, time, timeout).transform;
+        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.r_sole_frame, time).transform;
     geometry_msgs::msg::Transform l_hand_transformed =
-        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.l_wrist_frame, time, timeout).transform;
+        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.l_wrist_frame, time).transform;
     geometry_msgs::msg::Transform r_hand_transformed =
-        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.r_wrist_frame, time, timeout).transform;
+        tf_buffer_.lookupTransform(tf_names.base_link_frame, tf_names.r_wrist_frame, time).transform;
 
     std::function transform2pose = [](geometry_msgs::msg::Transform transform) {
       geometry_msgs::msg::Pose pose;

bitbots_motion/bitbots_dynup/src/dynup_pywrapper.cpp

@@ -12,9 +12,10 @@ PyDynupWrapper::PyDynupWrapper(const std::string ns) {
 
 void PyDynupWrapper::spin_some() { rclcpp::spin_some(node_); }
 
-py::bytes PyDynupWrapper::step(double dt, py::bytes &imu_msg) {
-  bitbots_msgs::msg::JointCommand result =
-      dynup_->step(dt, std::make_shared<sensor_msgs::msg::Imu>(fromPython<sensor_msgs::msg::Imu>(imu_msg)));
+py::bytes PyDynupWrapper::step(double dt, py::bytes &imu_msg, py::bytes &joint_state_msg) {
+  bitbots_msgs::msg::JointCommand result = dynup_->step(
+      dt, std::make_shared<sensor_msgs::msg::Imu>(fromPython<sensor_msgs::msg::Imu>(imu_msg)),
+      std::make_shared<sensor_msgs::msg::JointState>(fromPython<sensor_msgs::msg::JointState>(joint_state_msg)));
   return toPython<bitbots_msgs::msg::JointCommand>(result);
 }
 

bitbots_motion/bitbots_dynup/src/dynup_stabilizer.cpp

@@ -21,7 +21,7 @@ DynupResponse Stabilizer::stabilize(const DynupResponse &ik_goals, const rclcpp:
   tf2::Transform right_foot_goal;
 
   // Check if we have all the necessary data to stabilize
-  if (!ik_goals.is_stabilizing_needed and r_sole_to_trunk_ and imu_) {
+  if (ik_goals.is_stabilizing_needed and r_sole_to_trunk_ and imu_) {
     // Convert to eigen quaternion
     tf2::Quaternion quat;
     tf2::convert(imu_->orientation, quat);