Merge branch 'frankaemika:develop' into fix/external_torque

.ci/Dockerfile.melodic

@@ -1,8 +1,8 @@
 FROM osrf/ros:melodic-desktop-bionic
 RUN apt-get update -y && apt-get install -y \
-    clang-6.0 \
-    clang-format-6.0 \
-    clang-tidy-6.0 \
+    clang-7 \
+    clang-format-7 \
+    clang-tidy-7 \
     pycodestyle \
     python3-catkin-tools \
     ros-melodic-libfranka \

.ci/Dockerfile.noetic

@@ -1,8 +1,8 @@
 FROM osrf/ros:noetic-desktop-focal
 RUN apt-get update -y && apt-get install -y \
-    clang-6.0 \
-    clang-format-6.0 \
-    clang-tidy-6.0 \
+    clang-7 \
+    clang-format-7 \
+    clang-tidy-7 \
     pycodestyle \
     liborocos-kdl-dev \
     python3-catkin-tools \

CHANGELOG.md

@@ -8,6 +8,10 @@
 * 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
+* Make finger collisions primitive in `franka_gazebo`
+* add 'gravity_vector' gravity ROS parameter to FrankaHWSim
 
 ## 0.8.1 - 2021-09-08
 

cmake/ClangTools.cmake

@@ -1,11 +1,11 @@
 include(CMakeParseArguments)
 
-find_program(CLANG_FORMAT_PROG clang-format-6.0 DOC "'clang-format' executable")
+find_program(CLANG_FORMAT_PROG clang-format-7 DOC "'clang-format' executable")
 if(CLANG_FORMAT_PROG AND NOT TARGET format)
   add_custom_target(format)
   add_custom_target(check-format)
 endif()
-find_program(CLANG_TIDY_PROG clang-tidy-6.0 DOC "'clang-tidy' executable")
+find_program(CLANG_TIDY_PROG clang-tidy-7 DOC "'clang-tidy' executable")
 if(CLANG_TIDY_PROG AND NOT TARGET tidy)
   if(NOT CMAKE_EXPORT_COMPILE_COMMANDS)
     message(WARNING "Invoke Catkin/CMake with '-DCMAKE_EXPORT_COMPILE_COMMANDS=ON'

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

franka_control/package.xml

@@ -30,6 +30,7 @@
   <exec_depend>franka_description</exec_depend>
   <exec_depend>franka_gripper</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
 
   <export>

franka_description/robots/hand.xacro

@@ -52,6 +52,13 @@
         </geometry>
       </collision>
     </link>
+    <!-- Define the hand_tcp frame -->
+    <link name="${ns}_hand_tcp" />
+    <joint name="${ns}_hand_tcp_joint" type="fixed">
+      <origin xyz="0 0 0.1034" rpy="0 0 0" />
+      <parent link="${ns}_hand" />
+      <child link="${ns}_hand_tcp" />
+    </joint>
     <link name="${ns}_leftfinger">
       <visual>
         <geometry>

franka_description/robots/panda_arm.urdf.xacro

@@ -61,7 +61,6 @@
 
     <gazebo>
       <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-        <robotNamespace>$(arg arm_id)</robotNamespace>
         <controlPeriod>0.001</controlPeriod>
         <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
       </plugin>

franka_description/robots/panda_gazebo.xacro

@@ -301,18 +301,45 @@
       </inertial>
     </link>
 
+    <xacro:macro name="finger-collision" params="sign">
+      <!-- screw mount -->
+      <collision>
+        <origin xyz="0 ${sign}18.5e-3 11e-3" rpy="0 0 0"/>
+        <geometry>
+          <box size="22e-3 15e-3 20e-3" />
+        </geometry>
+      </collision>
+      <!-- cartriage sledge -->
+      <collision>
+        <origin xyz="0 ${sign}6.8e-3 2.2e-3" rpy="0 0 0"/>
+        <geometry>
+          <box size="22e-3 8.8e-3 3.8e-3" />
+        </geometry>
+      </collision>
+      <!-- diagonal finger -->
+      <collision>
+	      <origin xyz="0 ${sign}15.9e-3 28.35e-3" rpy="${sign}${pi/6} 0 0"/>
+        <geometry>
+          <box size="17.5e-3 7e-3 23.5e-3" />
+        </geometry>
+      </collision>
+      <!-- rubber tip with which to grasp -->
+      <collision>
+        <origin xyz="0 ${sign}7.58e-3 45.25e-3" rpy="0 0 0"/>
+        <geometry>
+          <box size="17.5e-3 15.2e-3 18.5e-3" />
+        </geometry>
+      </collision>
+    </xacro:macro>
+
     <link name="${ns}_leftfinger">
       <xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
       <visual>
         <geometry>
           <mesh filename="package://franka_description/meshes/visual/finger.dae"/>
         </geometry>
       </visual>
-      <collision>
-        <geometry>
-          <mesh filename="package://franka_description/meshes/collision/finger.stl" />
-        </geometry>
-      </collision>
+      <xacro:finger-collision sign="+" />
     </link>
 
     <link name="${ns}_rightfinger">
@@ -323,12 +350,7 @@
           <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>
+      <xacro:finger-collision sign="-" />
     </link>
 
     <!-- Friction specific material for Rubber/Rubber contact -->
@@ -342,7 +364,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"/>
-      <dynamics damping="0.3"/>
+      <dynamics friction="0.0" damping="0.03"/>
     </joint>
 
     <joint name="${ns}_finger_joint2" type="prismatic">
@@ -351,7 +373,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"/>
-      <dynamics damping="0.3"/>
+      <dynamics friction="0.0" damping="0.03"/>
     </joint>
   </xacro:macro>
 </robot>

franka_example_controllers/src/force_example_controller.cpp

@@ -106,15 +106,16 @@ void ForceExampleController::update(const ros::Time& /*time*/, const ros::Durati
       robot_state.tau_J_d.data());
   Eigen::Map<Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
 
-  Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
+  Eigen::Matrix<double, 7, 1> tau_d, tau_cmd, tau_ext;
+  Eigen::Matrix<double, 6, 1> desired_force_torque;
   desired_force_torque.setZero();
   desired_force_torque(2) = desired_mass_ * -9.81;
   tau_ext = tau_measured - gravity - tau_ext_initial_;
-  tau_d << jacobian.transpose() * desired_force_torque;
+  tau_d = jacobian.transpose() * desired_force_torque;
   tau_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
   // FF + PI control (PI gains are initially all 0)
   tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
-  tau_cmd << saturateTorqueRate(tau_cmd, tau_J_d);
+  tau_cmd = saturateTorqueRate(tau_cmd, tau_J_d);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_cmd(i));

franka_gazebo/config/franka_hw_sim.yaml

@@ -6,7 +6,7 @@ franka_gripper:
   arm_id:  $(arg arm_id)
 
   finger1:
-    gains: { p: 100, i: 25, d: 20 }
+    gains: { p: 100, i: 0, d: 1.0 }
 
   finger2:
-    gains: { p: 100, i: 25, d: 20 }
+    gains: { p: 100, i: 0, d: 1.0 }