Merge branch 'main' into objectathon2

Author: davetcoleman

src/moveit_pro_franka_configs/franka_dual_arm_sim/config/config.yaml

@@ -33,6 +33,7 @@ hardware:
     # [Optional]
     urdf_params:
       - mujoco_model: "description/mujoco/scene.xml"
+      - mujoco_viewer: false
     simulated_hardware_launch_file:
       package: "moveit_studio_agent"
       path: "launch/blank.launch.py"
@@ -129,6 +130,8 @@ ros2_control:
   # for running the application.
   # [Optional, default=[]]
   controllers_active_at_startup:
+    - "left_gripper_controller"
+    - "right_gripper_controller"
     - "joint_state_broadcaster"
     - "joint_trajectory_controller"
   # Load but do not start these controllers so they can be activated later if needed.

src/moveit_pro_franka_configs/franka_dual_arm_sim/config/control/franka_ros2_control.yaml

@@ -15,6 +15,10 @@ controller_manager:
       type: velocity_force_controller/VelocityForceController
     right_velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    left_gripper_controller:
+      type: position_controllers/GripperActionController
+    right_gripper_controller:
+      type: position_controllers/GripperActionController
 
 joint_trajectory_controller:
   ros__parameters:
@@ -38,6 +42,7 @@ joint_trajectory_controller:
       - right_fr3_joint5
       - right_fr3_joint6
       - right_fr3_joint7
+    allow_partial_joints_goal: true
     gains:
       left_fr3_joint1: { p: 600., d: 30., i: 0., i_clamp: 1. }
       left_fr3_joint2: { p: 600., d: 30., i: 0., i_clamp: 1. }
@@ -54,6 +59,22 @@ joint_trajectory_controller:
       right_fr3_joint6: { p: 150., d: 10., i: 0., i_clamp: 1. }
       right_fr3_joint7: { p: 50., d: 5., i: 0., i_clamp: 1. }
 
+left_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: left_fr3_finger_joint1
+    allow_stalling: true
+    stall_timeout: 0.1
+    goal_tolerance: 0.005
+
+right_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: right_fr3_finger_joint1
+    allow_stalling: true
+    stall_timeout: 0.1
+    goal_tolerance: 0.005
+
 left_joint_trajectory_controller:
   ros__parameters:
     command_interfaces:
@@ -154,7 +175,7 @@ left_velocity_force_controller:
     # Base frame of the kinematic chain, for computation of arm kinematics.
     base_frame: left_fr3_link0
     # The tip link of the kinematic chain, i.e. the frame that will be controlled.
-    ee_frame: left_fr3_link8
+    ee_frame: left_grasp_link
     # The frame where the force / torque sensor reading is given.
     # (Needs to exist in the kinematic chain).
     sensor_frame: left_fr3_link8
@@ -229,7 +250,7 @@ right_velocity_force_controller:
     # Base frame of the kinematic chain, for computation of arm kinematics.
     base_frame: right_fr3_link0
     # The tip link of the kinematic chain, i.e. the frame that will be controlled.
-    ee_frame: right_fr3_link8
+    ee_frame: right_grasp_link
     # The frame where the force / torque sensor reading is given.
     # (Needs to exist in the kinematic chain).
     sensor_frame: right_fr3_link8

src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/franka.srdf

@@ -1,48 +1,49 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="franka">
   <!-- Planning groups -->
-  <!-- Right arm -->
-  <group name="right_arm">
-    <chain base_link="right_fr3_link0" tip_link="right_fr3_link8"/>
-  </group>
-  <group name="right_hand">
-    <link name="right_fr3_hand"/>
+
+  <!-- All right gripper links, excluding arm. -->
+  <group name="right_gripper">
+    <link name="right_fr3_hand_tcp"/>  <!-- root of gripper -->
     <link name="right_fr3_leftfinger"/>
     <link name="right_fr3_rightfinger"/>
-    <link name="right_fr3_hand_tcp"/>
-    <!--<link name="right_grasp_link"/>-->
+    <link name="right_grasp_link"/>
     <joint name="right_fr3_finger_joint1"/>
     <passive_joint name="right_fr3_finger_joint2"/>
   </group>
+
+  <!-- All right robot links, including gripper. -->
   <group name="right_manipulator">
-    <chain base_link="right_fr3_link0" tip_link="right_fr3_link8"/>
+    <chain base_link="right_fr3_link0" tip_link="right_grasp_link"/>
   </group>
-  <!-- Left arm -->
-  <group name="left_arm">
-    <chain base_link="left_fr3_link0" tip_link="left_fr3_link8"/>
-  </group>
-  <group name="left_hand">
-    <link name="left_fr3_hand"/>
+
+  <!-- Right end effectors -->
+  <end_effector name="right_hand_tcp" parent_link="right_grasp_link" group="right_gripper" parent_group="right_manipulator"/>
+
+  <!-- All left gripper links, excluding arm. -->
+  <group name="left_gripper">
+    <link name="left_fr3_hand_tcp"/>
     <link name="left_fr3_leftfinger"/>
     <link name="left_fr3_rightfinger"/>
-    <link name="left_fr3_hand_tcp"/>
-    <!--<link name="left_grasp_link"/>-->
+    <link name="left_grasp_link"/>
     <joint name="left_fr3_finger_joint1"/>
     <passive_joint name="left_fr3_finger_joint2"/>
   </group>
+  
+  <!-- All left robot links, including gripper. -->
   <group name="left_manipulator">
-    <chain base_link="left_fr3_link0" tip_link="left_fr3_link8"/>
+    <chain base_link="left_fr3_link0" tip_link="left_grasp_link"/>
   </group>
 
+  <!-- Left end effectors -->
+  <end_effector name="left_hand_tcp" parent_link="left_grasp_link" group="left_gripper" parent_group="left_manipulator"/>
+  
   <!-- Group containing both arms -->
   <group name="manipulator">
     <group name="left_manipulator"/>
     <group name="right_manipulator"/>
   </group>
 
-  <end_effector name="left_hand_tcp" parent_link="left_fr3_link8" group="left_hand" parent_group="left_arm"/>
-  <end_effector name="right_hand_tcp" parent_link="right_fr3_link8" group="right_hand" parent_group="right_arm"/>
-
   <!-- Arm collisions -->
   <virtual_joint name="right_virtual_joint" type="fixed" parent_frame="right_mount" child_link="right_fr3_link0"/>
   <virtual_joint name="left_virtual_joint" type="fixed" parent_frame="left_mount" child_link="left_fr3_link0"/>

src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_distance.yaml

@@ -1,18 +1,9 @@
-left_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "optimize_distance"
-  optimization_timeout: 0.005
 left_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "optimize_distance"
   optimization_timeout: 0.005
-right_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "optimize_distance"
-  optimization_timeout: 0.005
+
 right_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001

src/moveit_pro_franka_configs/franka_dual_arm_sim/config/moveit/pose_ik_speed.yaml

@@ -1,16 +1,9 @@
-right_arm:
+left_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "first_found"
+
 right_manipulator:
   kinematics_solver: pose_ik_plugin/PoseIKPlugin
   target_tolerance: 0.001
   solve_mode: "first_found"
-left_arm:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "first_found"
-left_manipulator:
-  kinematics_solver: pose_ik_plugin/PoseIKPlugin
-  target_tolerance: 0.001
-  solve_mode: "first_found"

src/moveit_pro_franka_configs/franka_dual_arm_sim/description/franka.urdf.xacro

@@ -30,9 +30,11 @@
                       arm_prefix="right"
                       connected_to= "right_mount">
   </xacro:franka_robot>
+
+  <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
   <link name="right_grasp_link" />
   <joint name="right_grasp_link_joint" type="fixed">
-     <parent link="right_fr3_link8"/>
+     <parent link="right_fr3_hand_tcp"/>
      <child link = "right_grasp_link"/>
      <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
   </joint>
@@ -42,6 +44,7 @@
      <child link = "left_mount"/>
     <origin xyz="0.035 0.050681 0.356" rpy="-0.8936 -0.1746 -0.4636"/>
   </joint>
+
   <xacro:franka_robot arm_id="fr3"
                       joint_limits="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}"
                       inertials="${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')}"
@@ -61,10 +64,10 @@
                       connected_to= "left_mount">
   </xacro:franka_robot>
 
-    <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
+   <!-- This link aligns the axes of the grasp frame to our required convention for motion planning tasks. -->
    <link name="left_grasp_link" />
    <joint name="left_grasp_link_joint" type="fixed">
-      <parent link="world"/>
+      <parent link="left_fr3_hand_tcp"/>
       <child link = "left_grasp_link"/>
       <origin xyz="0 0 0" rpy="0 0 -1.5708"/>
     </joint>
@@ -147,6 +150,17 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="right_fr3_finger_joint1">
+        <command_interface name="position">
+          <param name="min">-0.0</param>
+          <param name="max">0.04</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.035</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
       <joint name="left_fr3_joint1">
         <command_interface name="position">
           <param name="min">-2.8973</param>
@@ -224,14 +238,25 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <joint name="left_fr3_finger_joint1">
+        <command_interface name="position">
+          <param name="min">-0.0</param>
+          <param name="max">0.04</param>
+        </command_interface>
+        <state_interface name="position">
+          <param name="initial_value">0.035</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+      </joint>
       <hardware>
         <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
         <param name="mujoco_model">$(arg mujoco_model)</param>
         <param name="mujoco_model_package">franka_dual_arm_sim</param>
         <param name="render_publish_rate">10</param>
         <param name="tf_publish_rate">60</param>
-        <param name="mujoco_viewer">false</param>
         <param name="mujoco_keyframe">home</param>
+        <param name="mujoco_viewer">$(arg mujoco_viewer)</param>
       </hardware>
     </ros2_control>
 </robot>