Add support for child platforms connected to UR arm.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

Author: Jelmerdw

alliander_core/src/alliander_description/ur/urdf/adapted/ur_gz.ros2_control.xacro

@@ -14,7 +14,7 @@ https://code.visualstudio.com/docs/editing/codebasics#_compare-files
     name
     tf_prefix
     transmission_hw_interface:=hardware_interface/PositionJointInterface
-    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=1.57,wrist_1_joint=-1.57,wrist_2_joint=-1.57,wrist_3_joint=0.0)}
+    initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=1.57,wrist_1_joint=-1.57,wrist_2_joint=-1.57,wrist_3_joint=-1.57)}
     ">
 
 

alliander_core/src/alliander_description/ur/urdf/ur.urdf.xacro

@@ -9,6 +9,7 @@ SPDX-License-Identifier: Apache-2.0
   <xacro:arg name="simulation" default="" />
   <xacro:arg name="simulation_controllers" default="" />
   <xacro:arg name="ros_namespace" default="" />
+  <xacro:arg name="childs" default="${[]}" />
 
   <xacro:arg name="name" default="ur" />
   <xacro:arg name="ur_type" default="ur7e" />
@@ -20,4 +21,8 @@ SPDX-License-Identifier: Apache-2.0
     <xacro:include filename="$(find ur_robot_driver)/urdf/ur.urdf.xacro" />
   </xacro:unless>
 
+  <!-- Create detachable joints with given childs: -->
+  <xacro:include filename="$(find alliander_utilities)/urdf/detachable_joints.urdf.xacro" />
+  <xacro:detachable_joints childs="${$(arg childs)}" />
+
 </robot>

alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py

@@ -191,6 +191,8 @@ def default_link_to_child(self) -> str:
                 return "base_link"
             case "franka":
                 return "fr3_hand"
+            case "ur":
+                return "wrist_3_link"
             case _:
                 raise ValueError(
                     f"No link_to_child for unknown platform name: {self.name}"

alliander_moveit/src/alliander_ur_moveit_config/config/arm.srdf

@@ -52,15 +52,15 @@ are defined
         <joint name="elbow_joint" value="1.57" />
         <joint name="wrist_1_joint" value="-1.57" />
         <joint name="wrist_2_joint" value="-1.57" />
-        <joint name="wrist_3_joint" value="0" />
+        <joint name="wrist_3_joint" value="-1.57" />
     </group_state>
     <group_state name="drop" group="arm">
         <joint name="shoulder_pan_joint" value="-1.57" />
         <joint name="shoulder_lift_joint" value="-1.57" />
         <joint name="elbow_joint" value="1.57" />
         <joint name="wrist_1_joint" value="-1.57" />
         <joint name="wrist_2_joint" value="-1.57" />
-        <joint name="wrist_3_joint" value="0" />
+        <joint name="wrist_3_joint" value="-1.57" />
     </group_state>
 
     <!--END_EFFECTOR:

alliander_ur/src/alliander_ur/launch/ur.launch.py

@@ -34,6 +34,12 @@ def launch_setup(context: LaunchContext) -> list:
                 "alliander_description", ["ur", "config"], "controllers.yaml"
             ),
             "ros_namespace": arm_config.namespace,
+            "childs": str(
+                [
+                    [child.connects_to, child.namespace, child.link]
+                    for child in arm_config.childs
+                ]
+            ),
         },
     )
 

predefined_configurations.py

@@ -148,6 +148,14 @@ def config_ur_rviz_motion_planning(self) -> None:  # noqa: D102
         self.plat_conf.platforms = [arm]
         self.viz_conf.gui = True
 
+    @register_configuration("ur_realsense")
+    def config_ur_realsense(self) -> None:  # noqa: D102
+        arm = Arm("ur", moveit=True)
+        camera = Camera("realsense", (0.05, 0, 0), (0, -90, 180))
+
+        link(arm, camera)
+        self.plat_conf.platforms = [arm, camera]
+
     # Panther:
     @register_configuration("panther")
     def config_panther(self) -> None:  # noqa: D102