Support franka panda robot.

Author: florent-lamiraux

CMakeLists.txt

@@ -79,6 +79,7 @@ INSTALL(FILES
 INSTALL(FILES
   config/sot_params_common.yaml
   config/sot_params_ur.yaml
+  config/sot_params_franka_panda.yaml
   DESTINATION share/${PROJECT_NAME}/config)
 
 # Install default URDF file

config/sot_params_franka_panda.yaml

@@ -0,0 +1,37 @@
+sot_controller:
+  control_mode:
+    panda_joint1:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint2:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint3:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint4:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint5:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint6:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+    panda_joint7:
+      ros_control_mode: POSITION
+      sot_control_mode: VELOCITY
+
+  joint_names:
+    [$(arg arm_id)_joint1, $(arg arm_id)_joint2, $(arg arm_id)_joint3,
+    $(arg arm_id)_joint4, $(arg arm_id)_joint5, $(arg arm_id)_joint6,
+    $(arg arm_id)_joint7]
+  joints:
+    [$(arg arm_id)_joint1, $(arg arm_id)_joint2, $(arg arm_id)_joint3,
+    $(arg arm_id)_joint4, $(arg arm_id)_joint5, $(arg arm_id)_joint6,
+    $(arg arm_id)_joint7]
+sot:
+  state_vector_map:
+    [$(arg arm_id)_joint1, $(arg arm_id)_joint2, $(arg arm_id)_joint3,
+    $(arg arm_id)_joint4, $(arg arm_id)_joint5, $(arg arm_id)_joint6,
+    $(arg arm_id)_joint7]

launch/sot_params.launch

@@ -11,9 +11,20 @@
         0.01
       </rosparam>
     </group>
+    <group ns="sot_controller" if="$(eval robot == 'franka')">
+      <rosparam param="dt" if="$(arg simulation)">
+        0.001
+      </rosparam>
+      <rosparam param="dt" unless="$(arg simulation)">
+        0.01
+      </rosparam>
+    </group>
     <!-- Sot Controller configuration -->
     <rosparam command="load"
               file="$(find sot_universal_robot)/config/sot_params_common.yaml"/>
+    <rosparam command="load" if="$(eval robot == 'franka')"
+              file="$(find sot_universal_robot)/config/sot_params_franka_$(arg arm_id).yaml"
+              subst_value="true"/>
     <rosparam command="load" if="$(eval robot == 'ur')"
               file="$(find sot_universal_robot)/config/sot_params_ur.yaml"/>
 </launch>

src/CMakeLists.txt

@@ -42,6 +42,7 @@ DYNAMIC_GRAPH_PYTHON_MODULE("sot/robot_arm/${PYTHON_LIBRARY_NAME}"
 # Install Python files.
 SET(PYTHON_MODULE dynamic_graph/sot/robot_arm)
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py")
+PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "franka.py")
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot_arm.py")
 PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "universal_robot.py")

src/device-python.cc

@@ -9,4 +9,6 @@ BOOST_PYTHON_MODULE(wrap)
 
   dynamicgraph::python::exposeEntity<SoTRobotArmDevice,
     bp::bases<dg::sot::Device> >();
+  dynamicgraph::python::exposeEntity<DeviceToDynamic,
+    bp::bases<dg::Entity> >();
 }

src/device.cc

@@ -25,6 +25,7 @@
 
 #define ENABLE_RT_LOG
 #include "device.hh"
+#include <dynamic-graph/entity.h>
 #include <dynamic-graph/factory.h>
 #include <dynamic-graph/all-commands.h>
 #include <dynamic-graph/real-time-logger.h>
@@ -268,3 +269,5 @@ void SoTRobotArmDevice::integrate(const double &dt)
     CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
   }
 }
+
+DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DeviceToDynamic, "DeviceToDynamic");

src/device.hh

@@ -96,4 +96,31 @@ protected:
   dynamicgraph::Vector d_gains_;
 }; // class SoTRobotArmDevice
 
+/// Entity that converts a vector of dimension 7 to a vector of dimension 8
+///
+/// Useful for Franka robot where device.state is of dimension 7 while
+/// dynamic.position expects a vector of dimension 8.
+/// The last component is set to 0
+class DeviceToDynamic : public dynamicgraph::Entity
+{
+public:
+  static const std::string CLASS_NAME;
+  DeviceToDynamic(const std::string& name) :
+    dynamicgraph::Entity(name), sinSIN(0x0, "DeviceToDynamic("+name+")::input(vector)::sin"),
+    soutSOUT(boost::bind(&DeviceToDynamic::compute, this, _1, _2),
+             sinSIN, "DeviceToDynamic("+name+")::ouput(vector)::sout")
+  {
+    signalRegistration(sinSIN << soutSOUT);
+  }
+private:
+  dynamicgraph::Vector& compute(dynamicgraph::Vector& res, int time)
+  {
+    res.head<7>() = sinSIN(time);
+    res[7] = 0;
+    return res;
+  }
+  dynamicgraph::SignalPtr<dynamicgraph::Vector, int> sinSIN;
+  dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> soutSOUT;
+};
+
 #endif // SOT_UNIVERSAL_ROBOT_DEVICE_HH

src/dynamic_graph/sot/robot_arm/franka.py

@@ -0,0 +1,59 @@
+# Copyright, 2022 CNRS
+# Author: Florent Lamiraux
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are
+# met:
+
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+from .robot_arm import RobotArm
+from .sot_robot_arm_device import DeviceToDynamic
+
+class Franka(RobotArm):
+    """
+    This class defines a robot of type Franka Panda with gripper
+    """
+    defaultFilename = "package://sot_universal_robot/urdf/franka.urdf"
+
+    def __init__(self, name, device=None, tracer=None, loadFromFile=False):
+        RobotArm.__init__(self, name, device, tracer, loadFromFile)
+
+    # Resize the signal from dimension 7 to 8
+    #
+    # self.dynamic is initialized using the URDF model containing
+    # 9 joints: 7 for the arm, 2 for the fingers. The second finger
+    # joint mimics the first finger joint. The dimension is therefore 8.
+    #
+    #  self.device is initialized via ros params and contains only 7 joints.
+    def setClosedLoop(self, closedLoop):
+        if closedLoop:
+            raise RuntimeError('Closed loop not possible for Franka robot.')
+        else:
+            d = DeviceToDynamic('d2d')
+            plug(self.device.state, d.signal('sin'))
+            plug(d.signal('sout'), self.dynamic.position)
+            self.device.setClosedLoop(False)
+
+    ## Only the 7 arm joints are actuated
+    #
+    #  The fingers are not controlled via roscontrol.
+    def getActuatedJoints(self):
+        return range(0,7)