Adapt joint names

Signed-off-by: Florian Vahl <florian@flova.de>

Author: Flova

src/bitbots_misc/bitbots_teleop/bitbots_teleop/joy_node.py

@@ -224,7 +224,7 @@ def joy_cb(self, msg: Joy):
                 self.denormalize_joy(self.config["head"]["gain_tilt"], self.config["head"]["stick_tilt"], msg, -1)
             )
 
-            self.head_msg.joint_names = ["HeadPan", "HeadTilt"]
+            self.head_msg.joint_names = ["head_yaw_joint", "head_pitch_joint"]
             self.head_msg.positions = [pan_goal, tilt_goal]
             self.head_pub.publish(self.head_msg)
 

src/bitbots_misc/bitbots_teleop/scripts/teleop_keyboard.py

@@ -144,7 +144,7 @@ def __init__(self):
         self.head_msg.max_torques = [-1.0] * 2
         self.head_msg.velocities = [5.0] * 2
         self.head_msg.accelerations = [40.0] * 2
-        self.head_msg.joint_names = ["HeadPan", "HeadTilt"]
+        self.head_msg.joint_names = ["head_yaw_joint", "head_pitch_joint"]
         self.head_msg.positions = [0.0] * 2
 
         self.head_mode_msg = HeadMode(head_mode=HeadMode.DONT_MOVE)
@@ -194,9 +194,9 @@ def generate_kick_goal(self, x, y, direction):
         return kick_goal
 
     def joint_state_cb(self, msg):
-        if "HeadPan" in msg.name and "HeadTilt" in msg.name:
-            self.head_pan_pos = msg.position[msg.name.index("HeadPan")]
-            self.head_tilt_pos = msg.position[msg.name.index("HeadTilt")]
+        if "head_yaw_joint" in msg.name and "head_pitch_joint" in msg.name:
+            self.head_pan_pos = msg.position[msg.name.index("head_yaw_joint")]
+            self.head_tilt_pos = msg.position[msg.name.index("head_pitch_joint")]
 
     def loop(self):
         try:

src/bitbots_misc/bitbots_utils/scripts/motor_goals_viz_helper.py

@@ -18,7 +18,7 @@ def __init__(self):
         # get rid of additional ROS args when used in launch file
 
         parser = argparse.ArgumentParser()
-        parser.add_argument("--robot-type", "-t", help="What type of robot to use", default="wolfgang")
+        parser.add_argument("--robot-type", "-t", help="What type of robot to use", default="piplus")
         parser.add_argument("--walking", "-w", help="Directly get walking motor goals", action="store_true")
         parser.add_argument("--animation", "-a", help="Directly get animation motor goals", action="store_true")
         parser.add_argument("--head", help="Directly get head motor goals", action="store_true")
@@ -100,28 +100,31 @@ def __init__(self):
                 "neckYaw",
                 "neckPitch",
             ]
-            self.joint_goals = [
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
-                float(0),
+            self.joint_goals = [0.0] * 20
+        elif self.args.robot_type == "piplus":
+            self.joint_names = [
+                "head_yaw_joint",
+                "head_pitch_joint",
+                "l_shoulder_pitch_joint",
+                "l_shoulder_roll_joint",
+                "l_elbow_joint",
+                "r_shoulder_pitch_joint",
+                "r_shoulder_roll_joint",
+                "r_elbow_joint",
+                "l_hip_roll_joint",
+                "l_hip_roll_joint",
+                "l_hip_pitch_joint",
+                "l_calf_joint",
+                "l_ankle_pitch_joint",
+                "l_ankle_roll_joint",
+                "r_hip_roll_joint",
+                "r_hip_roll_joint",
+                "r_hip_pitch_joint",
+                "r_calf_joint",
+                "r_ankle_pitch_joint",
+                "r_ankle_roll_joint",
             ]
+            self.joint_goals = [0.0] * 22
         else:
             print(f"Unknown robot type {self.args.robot_type}")
             exit()

src/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py

@@ -95,35 +95,33 @@ def __init__(self, context):
         # Motor hierarchy
         self._motor_hierarchy = {  # TODO this should be a parameter / loaded from the urdf
             "Body": {
-                "Head": ["HeadPan", "HeadTilt"],
+                "Head": ["head_yaw_joint", "head_pitch_joint"],
                 "Arms": {
                     "Left": [
-                        "LShoulderPitch",
-                        "LShoulderRoll",
-                        "LElbow",
+                        "l_shoulder_pitch_joint",
+                        "l_shoulder_roll_joint",
+                        "l_elbow_joint",
                     ],
                     "Right": [
-                        "RShoulderPitch",
-                        "RShoulderRoll",
-                        "RElbow",
+                        "r_shoulder_pitch_joint",
+                        "r_shoulder_roll_joint",
+                        "r_elbow_joint",
                     ],
                 },
                 "Legs": {
                     "Left": [
-                        "LHipYaw",
-                        "LHipRoll",
-                        "LHipPitch",
-                        "LKnee",
-                        "LAnklePitch",
-                        "LAnkleRoll",
+                        "l_hip_roll_joint",
+                        "l_hip_pitch_joint",
+                        "l_calf_joint",
+                        "l_ankle_pitch_joint",
+                        "l_ankle_roll_joint",
                     ],
                     "Right": [
-                        "RHipYaw",
-                        "RHipRoll",
-                        "RHipPitch",
-                        "RKnee",
-                        "RAnklePitch",
-                        "RAnkleRoll",
+                        "r_hip_roll_joint",
+                        "r_hip_pitch_joint",
+                        "r_calf_joint",
+                        "r_ankle_pitch_joint",
+                        "r_ankle_roll_joint",
                     ],
                 },
             }
@@ -301,8 +299,8 @@ def connect_gui_callbacks(self) -> None:
         self._widget.actionAnimation_until_Frame.triggered.connect(self.play_until)
         self._widget.actionDuplicate_Frame.triggered.connect(self.duplicate)
         self._widget.actionDelete_Frame.triggered.connect(self.delete)
-        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("R"))
-        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("L"))
+        self._widget.actionLeft.triggered.connect(lambda: self.mirror_frame("r"))
+        self._widget.actionRight.triggered.connect(lambda: self.mirror_frame("l"))
         self._widget.actionInvert.triggered.connect(self.invert_frame)
         self._widget.actionUndo.triggered.connect(self.undo)
         self._widget.actionRedo.triggered.connect(self.redo)
@@ -588,12 +586,12 @@ def redo(self):
         self._widget.statusBar.showMessage(status)
         self.update_frames()
 
-    def mirror_frame(self, source: Literal["L", "R"]) -> None:
+    def mirror_frame(self, source: Literal["l", "r"]) -> None:
         """
         Copies all motor values from one side of the robot to the other. Inverts values, if necessary
         """
         # Get direction to mirror to
-        mirrored_source = {"R": "L", "L": "R"}[source]
+        mirrored_source = {"r": "l", "l": "r"}[source]
 
         # Go through all active motors
         for motor_name, angle in self._working_angles.items():
@@ -626,10 +624,10 @@ def invert_frame(self):
         # Go through all active motors
         for motor_name, angle in self._working_angles.items():
             # Check if the motor is on the right or left side and get the mirrored motor name
-            if motor_name.startswith("R"):
-                mirrored_motor_name = "L" + motor_name[1:]
-            elif motor_name.startswith("L"):
-                mirrored_motor_name = "R" + motor_name[1:]
+            if motor_name.startswith("r"):
+                mirrored_motor_name = "l" + motor_name[1:]
+            elif motor_name.startswith("l"):
+                mirrored_motor_name = "r" + motor_name[1:]
             else:
                 # Just copy over if the motor is not on the left or right side
                 mirrored_motors[motor_name] = angle

src/bitbots_motion/bitbots_head_mover/src/move_head.cpp

@@ -377,7 +377,7 @@ class HeadMover {
     // Send the motor goals including the position, speed and acceleration
     bitbots_msgs::msg::JointCommand pos_msg;
     pos_msg.header.stamp = rclcpp::Clock().now();
-    pos_msg.joint_names = {"HeadPan", "HeadTilt"};
+    pos_msg.joint_names = {"head_yaw_joint", "head_pitch_joint"};
     pos_msg.positions = {goal_pan, goal_tilt};
     pos_msg.velocities = {pan_speed, tilt_speed};
     pos_msg.accelerations = {params_.max_acceleration_pan, params_.max_acceleration_pan};
@@ -395,9 +395,9 @@ class HeadMover {
 
     // Iterate over all joints and find the head pan and tilt joints
     for (size_t i = 0; i < current_joint_state_->name.size(); i++) {
-      if (current_joint_state_->name[i] == "HeadPan") {
+      if (current_joint_state_->name[i] == "head_yaw_joint") {
         head_pan = current_joint_state_->position[i];
-      } else if (current_joint_state_->name[i] == "HeadTilt") {
+      } else if (current_joint_state_->name[i] == "head_pitch_joint") {
         head_tilt = current_joint_state_->position[i];
       }
     }