Merge branch 'feature/better_ball_search' of github.com:bit-bots/bitbots_main into feature/better_ball_search

Author: cl03

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py

@@ -76,7 +76,7 @@ def perform(self, reevaluate=False):
 
 class GoToAbsolutePositionFieldFraction(GoToAbsolutePosition):
     def __init__(self, blackboard, dsd, parameters):
-        """Go to the own goal"""
+        """Go to an absolute position of the field, specified by the fraction of the field size"""
         super().__init__(blackboard, dsd, parameters)
         point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
         self.point = (

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -158,7 +158,7 @@ $IsPenalized
         READY --> $AnyGoalScoreRecently + time:50
             YES --> #PositioningReady
             NO --> $DoOnce
-                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:1, @GetWalkready + r:false
+                NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
                 DONE --> #PositioningReady
         SET --> $SecondaryStateDecider
             PENALTYSHOOT --> $SecondaryStateTeamDecider

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_misc/bitbots_docs/docs/manual/tutorials/extrinsic_calibration.rst

@@ -14,7 +14,8 @@ Setup
 1. Start the visualization launch-file including the necessary motion and vision nodes.
 
 .. code-block:: bash
-ros2 launch bitbots_extrinsic_calibration viz_extrinsic_calibration.launch
+
+   ros2 launch bitbots_extrinsic_calibration viz_extrinsic_calibration.launch
 
 2. In Dynamic Reconfigure open the parameters (left panel) for the nodes: :code:`bitbots_extrinsic_imu_calibration` and :code:`bitbots_extrinsic_camera_calibration`.
 
@@ -26,6 +27,10 @@ Do the calibration
 
 2. Open rqt and navigate to **Plugins** > **Configurations** > **Dynamic Reconfigure** where you can configure the parameters.
 
+3. Place the robot outside the field exactly in front of the middle line.
+
+4. Use the :code:`2D Pose Estimate` button in RViz to place the virtual robot in the corresponding pose.
+
 .. note::
   If you change the calibration first change all parameters to :code:`0.0`.
   Then start with the adjustment of the IMU parameters.

bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml

@@ -1,6 +1,6 @@
 /bitbots_extrinsic_camera_calibration:
   ros__parameters:
-    offset_x: -0.025
+    offset_x: -0.02
     offset_y: 0.0
     offset_z: 0.0
 

bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml

@@ -1,11 +1,11 @@
 /bitbots_extrinsic_camera_calibration:
   ros__parameters:
-    offset_x: -0.07
-    offset_y: 0.0
+    offset_x: -0.06
+    offset_y: 0.01
     offset_z: 0.0
 
 /bitbots_extrinsic_imu_calibration:
   ros__parameters:
-    offset_x: -0.0
-    offset_y: 0.01
+    offset_x: 0.0
+    offset_y: 0.017
     offset_z: 0.0

bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml

@@ -4,6 +4,8 @@ walking:
   ros__parameters:
     engine:
       trunk_x_offset: 0.0
+      trunk_y_offset: -0.01
+
     node:
       x_bias: -0.005
       y_bias: -0.008

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml

@@ -7,8 +7,8 @@ walking:
       foot_apex_phase: 0.5
       foot_apex_pitch: 0.0
       foot_distance: 0.2
-      foot_overshoot_phase: 0.67
-      foot_overshoot_ratio: 0.1
+      foot_overshoot_phase: 0.92
+      foot_overshoot_ratio: 0.0
       foot_put_down_phase: 1.0
       foot_put_down_z_offset: 0.0
       foot_rise: 0.04
@@ -26,7 +26,7 @@ walking:
       trunk_pitch_p_coef_forward: 0.0
       trunk_pitch_p_coef_turn: 0.0
       trunk_swing: 0.25
-      trunk_x_offset: -0.02
+      trunk_x_offset: -0.01
       trunk_x_offset_p_coef_forward: 0.1
       trunk_x_offset_p_coef_turn: 0.0
       trunk_y_offset: 0.0
@@ -74,11 +74,11 @@ walking:
       trunk_pid:
         pitch:
           antiwindup: false
-          d: 0.0
+          d: 0.004
           i: 0.0
           i_clamp_max: 0.0
           i_clamp_min: 0.0
-          p: 0.0
+          p: 0.0035
         roll:
           antiwindup: false
           d: 0.0

bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp

@@ -15,7 +15,7 @@ void WalkStabilizer::reset() {
   pid_trunk_fused_roll_.reset();
 }
 
-WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcpp::Duration &dt) {
+WalkResponse WalkStabilizer::stabilize(const WalkResponse& response, const rclcpp::Duration& dt) {
   // compute orientation with PID control
   double goal_pitch, goal_roll, goal_yaw;
   tf2::Matrix3x3(response.support_foot_to_trunk.getRotation()).getRPY(goal_roll, goal_pitch, goal_yaw);
@@ -33,14 +33,15 @@ WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcp
   double fused_pitch_correction =
       pid_trunk_fused_pitch_.computeCommand(goal_fused.fusedPitch - response.current_fused_pitch, dt);
 
-  tf2::Quaternion corrected_orientation;
-  goal_fused.fusedRoll += fused_roll_correction;
-  goal_fused.fusedPitch += fused_pitch_correction;
-  Eigen::Quaterniond goal_orientation_eigen_corrected = rot_conv::QuatFromFused(goal_fused);
-  tf2::convert(goal_orientation_eigen_corrected, corrected_orientation);
+  // Change trunk x offset (in the trunks frame of reference) based on the PID controllers
+  WalkResponse stabilized_response{response};
 
-  WalkResponse stabilized_response = response;
-  stabilized_response.support_foot_to_trunk.setRotation(corrected_orientation);
+  tf2::Transform trunk_offset;
+  trunk_offset.setOrigin(tf2::Vector3(fused_pitch_correction, fused_roll_correction, 0.0));
+  trunk_offset.setRotation(tf2::Quaternion::getIdentity());
+
+  // apply the trunk offset to the trunk pose
+  stabilized_response.support_foot_to_trunk = trunk_offset * response.support_foot_to_trunk;
 
   return stabilized_response;
 }

bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml

@@ -85,19 +85,19 @@ bitbots_path_planning:
         bounds<>: [0.0, 1.0]
     max_vel_x:
       type: double
-      default_value: 0.07
+      default_value: 0.12
       description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [0.0, 1.0]
     min_vel_x:
       type: double
-      default_value: -0.05
+      default_value: -0.06
       description: 'Minimum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [-1.0, 0.0]
     max_vel_y:
       type: double
-      default_value: 0.06
+      default_value: 0.07
       description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [0.0, 1.0]

bitbots_navigation/bitbots_path_planning/test/__snapshots__/test_controller.ambr

@@ -2,9 +2,9 @@
 # name: test_default_setup
   dict({
     'carrot_distance': 4,
-    'max_rotation_vel': 0.4,
-    'max_vel_x': 0.12,
-    'max_vel_y': 0.05,
+    'max_rotation_vel': 0.3,
+    'max_vel_x': 0.07,
+    'max_vel_y': 0.06,
     'min_vel_x': -0.05,
     'orient_to_goal_distance': 1.0,
     'rotation_i_factor': 0.0,
@@ -14,5 +14,5 @@
   })
 # ---
 # name: test_step_cmd_vel_smoothing
-  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.056147836300549855, y=0.03337183236993486, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.20666164469042259))'
+  'geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.04351964304912127, y=0.030817565809235677, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.19312811636676133))'
 # ---

bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml

@@ -2,7 +2,7 @@ team_comm:
   ros__parameters:
     # UDP broadcast address is the highest IP in the subnet e.g. 172.20.255.255
     # Sets local mode if set to loopback (127.0.0.1)
-    target_ip: 10.87.255.255
+    target_ip: 192.168.255.255
 
     # Only used in non local mode with specific target_ip
     target_port: 3737

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

requirements/dev.txt

@@ -4,9 +4,10 @@ colorama  # Colorful terminal formatting
 exhale  # Necessary for rst rendering
 fabric  # Manages SSH sessions for the deploy tool
 GitPython  # Used by the deploy tool to check current state of the git repository
+invoke  # Necessary for paramiko
 paramiko  # Necessary for fabric
 pre-commit  # Installs and runs pre-commit hooks for git
+pytest-mock   # Mocking for pytest
 rich  # Rich terminal output
 ruff  # Python linting
 syrupy  # Python snapshot testing
-pytest-mock   # Mocking for pytest