Merge branch 'main' into feature/stabilize_x_offset

Author: Flova

bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(controller_manager REQUIRED)
 find_package(diagnostic_msgs REQUIRED)
 find_package(dynamixel_workbench_toolbox REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(realtime_tools REQUIRED)
@@ -60,6 +62,8 @@ ament_target_dependencies(
   diagnostic_msgs
   dynamixel_workbench_toolbox
   hardware_interface
+  moveit_core
+  moveit_ros_planning
   pluginlib
   rclcpp
   realtime_tools

bitbots_lowlevel/bitbots_ros_control/include/bitbots_ros_control/dynamixel_servo_hardware_interface.hpp

@@ -70,6 +70,8 @@ class DynamixelServoHardwareInterface : public bitbots_ros_control::HardwareInte
   unsigned int joint_count_;
 
   std::vector<std::string> joint_names_;
+  std::vector<double> lower_joint_limits_;
+  std::vector<double> upper_joint_limits_;
 
   std::vector<double> goal_position_;
   std::vector<double> goal_effort_;

bitbots_lowlevel/bitbots_ros_control/src/dynamixel_servo_hardware_interface.cpp

@@ -1,3 +1,6 @@
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+
 #include <bitbots_ros_control/dynamixel_servo_hardware_interface.hpp>
 #include <bitbots_ros_control/utils.hpp>
 #include <utility>
@@ -63,6 +66,18 @@ bool DynamixelServoHardwareInterface::init() {
     joint_map_[joint_names_[i]] = i;
   }
 
+  // read lower and upper limits
+  robot_model_loader::RobotModelLoader rml(nh_, "robot_description", false);
+  moveit::core::RobotModelPtr model = rml.getModel();
+  lower_joint_limits_.resize(joint_count_);
+  upper_joint_limits_.resize(joint_count_);
+  for (const std::string &joint_name : joint_names_) {
+    moveit::core::JointModel *jm = model->getJointModel(joint_name);
+    // we use getVariableBounds()[0] because there is only a single variable for all of our joints
+    lower_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].min_position_;
+    upper_joint_limits_[joint_map_[joint_name]] = jm->getVariableBounds()[0].max_position_;
+  }
+
   std::string control_mode;
   control_mode = nh_->get_parameter("servos.control_mode").as_string();
   RCLCPP_INFO(nh_->get_logger(), "Control mode: %s", control_mode.c_str());
@@ -88,10 +103,19 @@ void DynamixelServoHardwareInterface::commandCb(const bitbots_msgs::msg::JointCo
     RCLCPP_ERROR(nh_->get_logger(), "Dynamixel Controller got command with inconsistent array lengths.");
     return;
   }
-  // todo improve performance
   for (unsigned int i = 0; i < command_msg.joint_names.size(); i++) {
     int joint_id = joint_map_[command_msg.joint_names[i]];
-    goal_position_[joint_id] = command_msg.positions[i];
+    if (command_msg.positions[i] > upper_joint_limits_[joint_id] ||
+        command_msg.positions[i] < lower_joint_limits_[joint_id]) {
+      RCLCPP_WARN_STREAM(nh_->get_logger(), "Invalid position for " << command_msg.joint_names[i] << ": "
+                                                                    << command_msg.positions[i] << " not in ("
+                                                                    << lower_joint_limits_[joint_id] << ", "
+                                                                    << upper_joint_limits_[joint_id] << ")");
+      goal_position_[joint_id] =
+          std::clamp(command_msg.positions[i], lower_joint_limits_[joint_id], upper_joint_limits_[joint_id]);
+    } else {
+      goal_position_[joint_id] = command_msg.positions[i];
+    }
     goal_velocity_[joint_id] = command_msg.velocities[i];
     goal_acceleration_[joint_id] = command_msg.accelerations[i];
     goal_effort_[joint_id] = command_msg.max_currents[i];

bitbots_wolfgang/wolfgang_description/urdf/robot.urdf

@@ -724,15 +724,15 @@
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="r_upper_arm"/>
     <child link="r_lower_arm"/>
-    <limit effort="7.3" lower="-1.0472" upper="1.5708" velocity="8.17"/>
+    <limit effort="7.3" lower="-0.78" upper="1.5708" velocity="8.17"/>
     <dynamics damping="0.65" friction="1.73"/>
   </joint>
   <joint name="RShoulderRoll" type="revolute">
     <origin rpy="1.5708 8.32338e-16 -1.5708" xyz="-0.01695 0.042 0.0"/>
     <axis xyz="-0.0 -0.0 -1.0"/>
     <parent link="r_shoulder"/>
     <child link="r_upper_arm"/>
-    <limit effort="7.3" lower="0.0" upper="3.14159" velocity="8.17"/>
+    <limit effort="7.3" lower="-0.12" upper="3.1416" velocity="8.17"/>
     <dynamics damping="0.65" friction="1.73"/>
   </joint>
   <joint name="RShoulderPitch" type="revolute">
@@ -1079,15 +1079,15 @@
     <axis xyz="0.0 0.0 1.0"/>
     <parent link="l_upper_arm"/>
     <child link="l_lower_arm"/>
-    <limit effort="7.3" lower="-1.5708" upper="1.0472" velocity="8.17"/>
+    <limit effort="7.3" lower="-1.5708" upper="0.78" velocity="8.17"/>
     <dynamics damping="0.65" friction="1.73"/>
   </joint>
   <joint name="LShoulderRoll" type="revolute">
     <origin rpy="-1.5708 -5.19331e-15 1.5708" xyz="-0.01695 0.042 -2.77556e-17"/>
     <axis xyz="-0.0 -0.0 -1.0"/>
     <parent link="l_shoulder"/>
     <child link="l_upper_arm"/>
-    <limit effort="7.3" lower="-3.14159" upper="0.0" velocity="8.17"/>
+    <limit effort="7.3" lower="-3.1416" upper="0.12" velocity="8.17"/>
     <dynamics damping="0.65" friction="1.73"/>
   </joint>
   <joint name="LShoulderPitch" type="revolute">
@@ -2204,7 +2204,7 @@
     <axis xyz="0.0 -1.0 0.0"/>
     <parent link="r_ankle"/>
     <child link="r_foot"/>
-    <limit effort="10" lower="-0.63" upper="1.58" velocity="5.76"/>
+    <limit effort="10" lower="-1.4" upper="1.58" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="RAnklePitch" type="revolute">
@@ -2220,23 +2220,23 @@
     <axis xyz="-0.0 -1.0 -0.0"/>
     <parent link="r_upper_leg"/>
     <child link="r_lower_leg"/>
-    <limit effort="12.9" lower="-2.88" upper="0.0" velocity="3.87"/>
+    <limit effort="12.9" lower="-2.8" upper="0.2" velocity="3.87"/>
     <dynamics damping="2.92" friction="1.49"/>
   </joint>
   <joint name="RHipPitch" type="revolute">
     <origin rpy="-1.5708 1.5708 0.0" xyz="-0.0265 -9.71445e-17 -0.0691"/>
     <axis xyz="-0.0 1.0 -0.0"/>
     <parent link="r_hip_2"/>
     <child link="r_upper_leg"/>
-    <limit effort="10" lower="-2.246" upper="0.9" velocity="5.76"/>
+    <limit effort="10" lower="-2.77" upper="0.79" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="RHipRoll" type="revolute">
     <origin rpy="-3.14159 1.5708 0.0" xyz="-0.046 0.0414 6.93889e-18"/>
     <axis xyz="-0.0 -0.0 -1.0"/>
     <parent link="r_hip_1"/>
     <child link="r_hip_2"/>
-    <limit effort="10" lower="-1.769" upper="0.871" velocity="5.76"/>
+    <limit effort="10" lower="-1.9" upper="1.6" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="RHipYaw" type="revolute">
@@ -3353,7 +3353,7 @@
     <axis xyz="-0.0 -1.0 -0.0"/>
     <parent link="l_ankle"/>
     <child link="l_foot"/>
-    <limit effort="10" lower="-1.58" upper="0.63" velocity="5.76"/>
+    <limit effort="10" lower="-1.58" upper="1.4" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="LAnklePitch" type="revolute">
@@ -3369,23 +3369,23 @@
     <axis xyz="-0.0 1.0 -0.0"/>
     <parent link="l_upper_leg"/>
     <child link="l_lower_leg"/>
-    <limit effort="12.9" lower="0" upper="2.88" velocity="3.87"/>
+    <limit effort="12.9" lower="-0.2" upper="2.8" velocity="3.87"/>
     <dynamics damping="2.92" friction="1.49"/>
   </joint>
   <joint name="LHipPitch" type="revolute">
     <origin rpy="2.67116e-16 -1.5708 0.0" xyz="0.026 6.93889e-18 -0.0691"/>
     <axis xyz="7.95491e-17 -3.00167e-16 -1.0"/>
     <parent link="l_hip_2"/>
     <child link="l_upper_leg"/>
-    <limit effort="10" lower="-0.9" upper="2.246" velocity="5.76"/>
+    <limit effort="10" lower="-0.79" upper="2.77" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="LHipRoll" type="revolute">
     <origin rpy="3.14159 1.5708 0.0" xyz="-0.046 0.0414 6.93889e-18"/>
     <axis xyz="-0.0 -0.0 -1.0"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
-    <limit effort="10" lower="-0.871" upper="1.769" velocity="5.76"/>
+    <limit effort="10" lower="-1.6" upper="1.9" velocity="5.76"/>
     <dynamics damping="1.23" friction="2.55"/>
   </joint>
   <joint name="LHipYaw" type="revolute">
@@ -3622,7 +3622,7 @@
     <axis xyz="-0.0 -0.0 1.0"/>
     <parent link="neck"/>
     <child link="head"/>
-    <limit effort="7.3" lower="-1.5708" upper="1.0472" velocity="8.17"/>
+    <limit effort="7.3" lower="-1.923" upper="1.24" velocity="8.17"/>
     <dynamics damping="0.65" friction="1.73"/>
   </joint>
   <joint name="HeadPan" type="revolute">