Merge pull request #9 from franzesegiovanni/ft-external-fk

Ft external fk

Author: franzesegiovanni

CMakeLists.txt

@@ -20,10 +20,12 @@ find_package(catkin REQUIRED COMPONENTS
   realtime_tools
   roscpp
   rospy
+  urdf
 )
 
 find_package(Eigen3 REQUIRED)
 find_package(Franka 0.7.0 REQUIRED)
+find_package(pinocchio REQUIRED)
 
 add_message_files(FILES
   JointTorqueComparison.msg
@@ -60,6 +62,7 @@ catkin_package(
 add_library(franka_human_friendly_controllers
   src/cartesian_variable_impedance_controller.cpp
   src/joint_variable_impedance_controller.cpp
+  src/cartesian_variable_impedance_external_model_controller.cpp
 )
 
 add_dependencies(franka_human_friendly_controllers
@@ -73,12 +76,14 @@ add_dependencies(franka_human_friendly_controllers
 target_link_libraries(franka_human_friendly_controllers PUBLIC
   ${Franka_LIBRARIES}
   ${catkin_LIBRARIES}
+  pinocchio::pinocchio
 )
 
 target_include_directories(franka_human_friendly_controllers SYSTEM PUBLIC
   ${Franka_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
+  ${PINOCCHIO_INCLUDE_DIRS}
 )
 target_include_directories(franka_human_friendly_controllers PUBLIC
   include

cfg/compliance_param.cfg

@@ -8,9 +8,9 @@ gen = ParameterGenerator()
 gen.add("translational_stiffness_X", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000)
 gen.add("translational_stiffness_Y", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000)
 gen.add("translational_stiffness_Z", double_t, 0, "Cartesian translational stiffness", 400, 0, 4000)
-gen.add("rotational_stiffness_X", double_t, 0, "Cartesian rotational stiffness", 30, 0, 50)
-gen.add("rotational_stiffness_Y", double_t, 0, "Cartesian rotational stiffness", 30, 0, 50)
-gen.add("rotational_stiffness_Z", double_t, 0, "Cartesian rotational stiffness", 30, 0, 50)
+gen.add("rotational_stiffness_X", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40)
+gen.add("rotational_stiffness_Y", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40)
+gen.add("rotational_stiffness_Z", double_t, 0, "Cartesian rotational stiffness", 30, 0, 40)
 
 gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0, 0, 100)
 

config/franka_human_friendly_controllers.yaml

@@ -19,4 +19,15 @@ joint_variable_impedance_controller:
         - panda_joint4
         - panda_joint5
         - panda_joint6
-        - panda_joint7
+        - panda_joint7
+cartesian_variable_impedance_external_model_controller:
+    type: franka_human_friendly_controllers/CartesianVariableImpedanceExternalModelController
+    arm_id: panda
+    joint_names:
+        - panda_joint1
+        - panda_joint2
+        - panda_joint3
+        - panda_joint4
+        - panda_joint5
+        - panda_joint6
+        - panda_joint7

config/latest_config.json

@@ -0,0 +1 @@
+{"mass":0.9,"centerOfMass":[-0.002,0,0.034],"transformation":[0.7071,-0.7071,0,0,0.7071,0.7071,0,0,0,0,1,0,0,0,0.1034,1],"inertia":[0.001,0,0,0,0.0025,0,0,0,0.0017],"collisionModel":{"pointA":[0.03535533905932738,0.03535533905932738,0.04,0.03535533905932738,0.03535533905932738,0.1,0,0,0],"pointB":[-0.03535533905932738,-0.03535533905932738,0.04,-0.03535533905932738,-0.03535533905932738,0.1,0,0,0],"radius":[0.04,0.02,0]}}

franka_human_friendly_controllers_plugin.xml

@@ -9,4 +9,9 @@
     A controller that renders a spring damper system in joint space.
     </description>
   </class>
+  <class name="franka_human_friendly_controllers/CartesianVariableImpedanceExternalModelController" type="franka_human_friendly_controllers::CartesianVariableImpedanceExternalModelController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that renders a spring damper system in cartesian space using an external urdf file as a model for forward kinematics and jacobian computation.
+    </description>
+  </class>
 </library>

include/franka_human_friendly_controllers/cartesian_variable_impedance_controller.h

@@ -37,6 +37,7 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override;
   void starting(const ros::Time&) override;
   void update(const ros::Time&, const ros::Duration& period) override;
+  virtual void loadModel();
 
  private:
   // Saturation
@@ -45,7 +46,8 @@ class CartesianVariableImpedanceController : public controller_interface::MultiI
   const Eigen::Matrix<double, 7, 1>& tau_J_d);  // NOLINT (readability-identifier-naming)
 
   double calculateTauJointLimit(double q_value, double threshold, double magnitude, double upper_bound, double lower_bound);
-
+  virtual std::array<double, 42> get_jacobian(franka::RobotState robot_state);
+  virtual double* get_fk(franka::RobotState robot_state);
 
 
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;

include/franka_human_friendly_controllers/cartesian_variable_impedance_external_model_controller.h

@@ -0,0 +1,23 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <franka_human_friendly_controllers/cartesian_variable_impedance_controller.h>
+
+namespace franka_human_friendly_controllers {
+
+class CartesianVariableImpedanceExternalModelController : public CartesianVariableImpedanceController {
+  private:
+    std::string urdf_path_;
+    pinocchio::Model model_pin_;
+    pinocchio::Data* data_pin_;
+    std::string frame_name_;
+    int frame_id_;
+
+  public:
+    double* get_fk(franka::RobotState robot_state) override;
+    std::array<double, 42> get_jacobian(franka::RobotState robot_state) override;
+    void loadModel() override;
+};
+
+}  // namespace franka_human_friendly_controllers