Merge PRs #194 (tool-frame), #190 (mimic-joint), #191 (drop-joint-sta…

…tes-desired) and #189 (rework-hand-collision) into develop
frankaemika · Nov 11, 2021 · cc5bb7d · cc5bb7d
4 parents 83670b7 + 4ead405 + 6942e27 + 6bc0977
commit cc5bb7d
Show file tree

Hide file tree

Showing 14 changed files with 197 additions and 403 deletions.
diff --git a/.travis.yml b/.travis.yml
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -2,11 +2,14 @@
 
 ## 0.x - UNRELEASED
 
+* Fix: Allow interactive marker server to shut down if not initialized
 * Add realistic hand/finger collision geometries to the Gazebo robot description
 * Add `joint_state_desired` publisher to `franka_gazebo`
 * No further ROS Kinetic support, since [End-of-Life was in April 2021](http://wiki.ros.org/Distributions)
 * Make position + orientation targets threadsafe in cartesian example controller
 * Add singularity warning to `franka_gazebo` if Jacobian becomes singular
+* **BREAKING** Make `/panda` namespace of `franka_gazebo` optional
+* Add effort joint trajectory controller to be used by MoveIT
 
 ## 0.8.1 - 2021-09-08
 

diff --git a/franka_control/config/default_controllers.yaml b/franka_control/config/default_controllers.yaml
@@ -10,20 +10,41 @@ position_joint_trajectory_controller:
     - panda_joint7
   constraints:
     goal_time: 0.5
-    panda_joint1:
-      goal: 0.05
-    panda_joint2:
-      goal: 0.05
-    panda_joint3:
-      goal: 0.05
-    panda_joint4:
-      goal: 0.05
-    panda_joint5:
-      goal: 0.05
-    panda_joint6:
-      goal: 0.05
-    panda_joint7:
-      goal: 0.05
+    panda_joint1: { goal: 0.05}
+    panda_joint2: { goal: 0.05}
+    panda_joint3: { goal: 0.05}
+    panda_joint4: { goal: 0.05}
+    panda_joint5: { goal: 0.05}
+    panda_joint6: { goal: 0.05}
+    panda_joint7: { goal: 0.05}
+
+effort_joint_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+  gains:
+    panda_joint1: { p: 600, d: 30, i: 0 }
+    panda_joint2: { p: 600, d: 30, i: 0 }
+    panda_joint3: { p: 600, d: 30, i: 0 }
+    panda_joint4: { p: 600, d: 30, i: 0 }
+    panda_joint5: { p: 250, d: 10, i: 0 }
+    panda_joint6: { p: 150, d: 10, i: 0 }
+    panda_joint7: { p: 50, d: 5, i: 0 }
+  constraints:
+    goal_time: 0.5
+    panda_joint1: { goal: 0.05}
+    panda_joint2: { goal: 0.05}
+    panda_joint3: { goal: 0.05}
+    panda_joint4: { goal: 0.05}
+    panda_joint5: { goal: 0.05}
+    panda_joint6: { goal: 0.05}
+    panda_joint7: { goal: 0.05}
 
 franka_state_controller:
   type: franka_control/FrankaStateController

diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch
@@ -22,10 +22,4 @@
     <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
     <param name="rate" value="30"/>
   </node>
-  <node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
-    <rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
-    <rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired] </rosparam>
-    <param name="rate" value="30"/>
-    <remap from="/joint_states" to="/joint_states_desired" />
-  </node>
 </launch>
diff --git a/franka_description/robots/hand.xacro b/franka_description/robots/hand.xacro
@@ -12,9 +12,16 @@
     <link name="${ns}_hand">
       <visual>
         <geometry>
-          <mesh filename="package://franka_description/meshes/visual/hand.dae"/>
+          <mesh filename="package://franka_description/meshes/visual/hand.dae" />
         </geometry>
       </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/hand.stl" />
+        </geometry>
+      </collision>
+    </link>
+    <link name="${ns}_hand_coarse">
       <collision>
         <origin xyz="0 0 0.04" rpy="0 ${pi/2} ${pi/2}"/>
         <geometry>
@@ -52,6 +59,10 @@
         </geometry>
       </collision>
     </link>
+    <joint name="${ns}_hand_coarse_joint" type="fixed">
+      <parent link="${ns}_hand" />
+      <child link="${ns}_hand_coarse" />
+    </joint>
     <!-- Define the hand_tcp frame -->
     <link name="${ns}_hand_tcp" />
     <joint name="${ns}_hand_tcp_joint" type="fixed">
@@ -65,6 +76,11 @@
           <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/finger.stl" />
+        </geometry>
+      </collision>
     </link>
     <link name="${ns}_rightfinger">
       <visual>
@@ -73,6 +89,12 @@
           <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
+        <geometry>
+          <mesh filename="package://franka_description/meshes/collision/finger.stl" />
+        </geometry>
+      </collision>
    </link>
     <joint name="${ns}_finger_joint1" type="prismatic">
       <parent link="${ns}_hand"/>

diff --git a/franka_description/robots/panda_arm.urdf.xacro b/franka_description/robots/panda_arm.urdf.xacro
@@ -1,20 +1,21 @@
 <?xml version='1.0' encoding='utf-8'?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
 
-  <xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
-  <xacro:arg name="hand"   default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
-  <xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->
-
-  <xacro:property name="arm_id" value="$(arg arm_id)" />
+  <!-- Name of this panda -->
+  <xacro:arg name="arm_id" default="panda" />
+  <!-- Should a franka_gripper be mounted at the flange?" -->
+  <xacro:arg name="hand" default="false" />
+  <!-- Is the robot being simulated in gazebo?" -->
+  <xacro:arg name="gazebo" default="false" />
 
   <xacro:unless value="$(arg gazebo)">
     <!-- Create a URDF for a real hardware -->
     <xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
-    <xacro:panda_arm arm_id="${arm_id}" safety_distance="0.03"/>
+    <xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>
 
     <xacro:if value="$(arg hand)">
       <xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
-      <xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" safety_distance="0.03"/>
+      <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
     </xacro:if>
   </xacro:unless>
 
@@ -27,40 +28,39 @@
     <xacro:include filename="$(find franka_description)/robots/utils.xacro" />
     <xacro:include filename="$(find franka_description)/robots/panda_gazebo.xacro" />
 
-    <xacro:panda_arm arm_id="${arm_id}" />
+    <xacro:panda_arm arm_id="$(arg arm_id)" />
 
     <xacro:if value="$(arg hand)">
-      <xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" />
-      <xacro:gazebo-joint joint="${arm_id}_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
-      <xacro:gazebo-joint joint="${arm_id}_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
+      <xacro:gazebo-joint joint="$(arg arm_id)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
     </xacro:if>
 
     <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
     <link name="world" />
     <joint name="world_joint" type="fixed">
       <origin xyz="$(arg xyz)" rpy="$(arg rpy)" />
       <parent link="world" />
-      <child  link="${arm_id}_link0" />
+      <child  link="$(arg arm_id)_link0" />
     </joint>
 
 
-    <xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/EffortJointInterface" />
-    <xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/EffortJointInterface" />
+    <xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="hardware_interface/EffortJointInterface" />
 
-    <xacro:transmission-franka-state arm_id="${arm_id}" />
-    <xacro:transmission-franka-model arm_id="${arm_id}"
-       root="${arm_id}_joint1"
-       tip="${arm_id}_joint8"
+    <xacro:transmission-franka-state arm_id="$(arg arm_id)" />
+    <xacro:transmission-franka-model arm_id="$(arg arm_id)"
+       root="$(arg arm_id)_joint1"
+       tip="$(arg arm_id)_joint8"
      />
 
     <gazebo>
       <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-        <robotNamespace>${arm_id}</robotNamespace>
         <controlPeriod>0.001</controlPeriod>
         <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
       </plugin>

diff --git a/franka_description/robots/panda_gazebo.xacro b/franka_description/robots/panda_gazebo.xacro
@@ -8,7 +8,7 @@
   <!-- || PandaRobot With Retrieval of Feasible Parameters Using    || -->
   <!-- || Penalty-Based Optimization                                || -->
   <!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva,         || -->
-  <!-- || Paolo Robuffo Giordano, Alessandro de Lucaa               || -->
+  <!-- || Paolo Robuffo Giordano, Alessandro de Luca                || -->
   <!-- =============================================================== -->
 
   <xacro:macro name="panda_arm" params="arm_id:='panda'">
@@ -300,6 +300,11 @@
                                           izz="0.0017" />
       </inertial>
     </link>
+    <link name="${ns}_hand_coarse" />
+    <joint name="${ns}_hand_coarse_joint" type="fixed">
+      <parent link="${ns}_hand" />
+      <child link="${ns}_hand_coarse" />
+    </joint>
 
     <link name="${ns}_leftfinger">
       <xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
@@ -351,6 +356,7 @@
       <origin xyz="0 0 0.0584" rpy="0 0 0"/>
       <axis xyz="0 -1 0"/>
       <limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
+      <mimic joint="${ns}_finger_joint1" />
       <dynamics damping="0.3"/>
     </joint>
   </xacro:macro>

diff --git a/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/dual_arm_cartesian_impedance_example_controller.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0" ?>
 <launch>
-  <!--Be sure to pass the IPs of your pandas like robot_ips:={panda_1/robot_ip: <my_ip_1>, panda_2/robot_ip: <my_ip_2>}   -->
+  <!--Be sure to pass the IPs of your pandas like robot_ips:="{panda_1/robot_ip: <my_ip_1>, panda_2/robot_ip: <my_ip_2>}"   -->
   <arg name="robot_ips" />
 
   <arg name="robot_id" default="panda_dual" />

diff --git a/franka_example_controllers/scripts/dual_arm_interactive_marker.py b/franka_example_controllers/scripts/dual_arm_interactive_marker.py
@@ -16,8 +16,6 @@
 
 marker_pose = PoseStamped()
 
-centering_frame_ready = False
-
 has_error = False
 left_has_error = False
 right_has_error = False
@@ -80,39 +78,15 @@ def right_franka_state_callback(msg):
     has_error = left_has_error or right_has_error
 
 
-def centering_pose_callback(msg):
-    """
-    This callback function sets the marker pose to the current centering pose from a subscribed
-    topic.
-    :return: None
-    """
-    global centering_frame_ready
-    global marker_pose
-
-    marker_pose = msg
-    centering_frame_ready = True
-
-
 def reset_marker_pose_blocking():
     """
     This function resets the marker pose to current "middle pose" of left and right arm EEs.
     :return: None
     """
 
-    global centering_frame_ready
     global marker_pose
-
-    centering_frame_ready = False
-
-    centering_frame_pose_sub = rospy.Subscriber(
-        "dual_arm_cartesian_impedance_example_controller/centering_frame",
-        PoseStamped, centering_pose_callback)
-
-    # Get initial pose for the interactive marker
-    while not centering_frame_ready:
-        rospy.sleep(0.1)
-
-    centering_frame_pose_sub.unregister()
+    marker_pose = rospy.wait_for_message(
+        "dual_arm_cartesian_impedance_example_controller/centering_frame", PoseStamped)
 
 
 def process_feedback(feedback):