diff --git a/CHANGELOG.md b/CHANGELOG.md
index 60226b8fdbe74903013fd85262df381ca3dd7692..b2c4a7f4609f47bf1813dadb61b6c0e1b2e1b587 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -2,10 +2,11 @@
## 0.2.0 - UNRELEASED
- * Specified run-time dependencies on `franka_description`
+ * Added missing run-time dependencies to `franka_description` and `franka_control`
* Added epsilon to grasp action
* Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
to the robot state
+ * Use O_T_EE_d in examples
## 0.1.2 - 2017-10-10
diff --git a/franka_control/package.xml b/franka_control/package.xml
index a49e49e7036eef78e83738316f18a4f09b734012..02dfbe8f8dff6b4f0e0e316c707cce6b665dc25c 100644
--- a/franka_control/package.xml
+++ b/franka_control/package.xml
@@ -31,7 +31,9 @@
<depend>tf</depend>
<exec_depend>franka_description</exec_depend>
+ <exec_depend>joint_state_publisher</exec_depend>
<exec_depend>message_runtime</exec_depend>
+ <exec_depend>robot_state_publisher</exec_depend>
<export>
<controller_interface plugin="${prefix}/franka_controller_plugins.xml"/>
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
index 2bb94b410447d66e66e444322461e074050d33a7..a09f668201808ebb883174639db2f8c38ea8ddce 100644
--- a/franka_example_controllers/CMakeLists.txt
+++ b/franka_example_controllers/CMakeLists.txt
@@ -1,6 +1,8 @@
cmake_minimum_required(VERSION 2.8.3)
project(franka_example_controllers)
+set(CMAKE_BUILD_TYPE Release)
+
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -11,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
realtime_tools
roscpp
message_generation
+ dynamic_reconfigure
)
add_message_files(FILES
@@ -19,6 +22,11 @@ add_message_files(FILES
generate_messages()
+generate_dynamic_reconfigure_options(
+ cfg/compliance_param.cfg
+ cfg/desired_mass_param.cfg
+)
+
catkin_package(
INCLUDE_DIRS include
LIBRARIES franka_example_controllers
@@ -34,12 +42,16 @@ add_library(franka_example_controllers
src/joint_velocity_example_controller.cpp
src/model_example_controller.cpp
src/joint_impedance_example_controller.cpp
+ src/cartesian_impedance_example_controller.cpp
+ src/force_example_controller.cpp
)
add_dependencies(franka_example_controllers
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
${PROJECT_NAME}_generate_messages_cpp
+ ${PROJECT_NAME}_gencpp
+ ${PROJECT_NAME}_gencfg
)
target_link_libraries(franka_example_controllers ${catkin_LIBRARIES})
diff --git a/franka_example_controllers/cfg/compliance_param.cfg b/franka_example_controllers/cfg/compliance_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..c5a448923cb8dd3bbd19eb5d53c8bf9166a3f567
--- /dev/null
+++ b/franka_example_controllers/cfg/compliance_param.cfg
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 20, 0, 100)
+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)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param"))
diff --git a/franka_example_controllers/cfg/desired_mass_param.cfg b/franka_example_controllers/cfg/desired_mass_param.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..15fc30ea764e7838cfac4fe54da9ed7697b5abed
--- /dev/null
+++ b/franka_example_controllers/cfg/desired_mass_param.cfg
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("desired_mass", double_t, 0, "desired mass for rendered force due to gravity applied in the z axis", 0.0, 0.0, 5.0)
+gen.add("k_p", double_t, 0, "force P gain", 0.0, 0.0, 20.0)
+gen.add("k_i", double_t, 0, "force I gain", 0.0, 0.0, 20.0)
+
+exit(gen.generate(PACKAGE, "dynamic_mass", "desired_mass_param"))
diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml
index 699fea415dc3e22d21c6cb908866a1cf75b2b603..43f97246de922f0adcdafbca2e4a27768dad9247 100644
--- a/franka_example_controllers/config/franka_example_controllers.yaml
+++ b/franka_example_controllers/config/franka_example_controllers.yaml
@@ -14,6 +14,18 @@ model_example_controller:
type: franka_example_controllers/ModelExampleController
arm_id: panda
+force_example_controller:
+ type: franka_example_controllers/ForceExampleController
+ arm_id: panda
+ joint_names:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
+
joint_impedance_example_controller:
type: franka_example_controllers/JointImpedanceExampleController
arm_id: panda
@@ -46,3 +58,15 @@ joint_impedance_example_controller:
vel_max: 0.15
publish_rate: 10.0
coriolis_factor: 1.0
+
+cartesian_impedance_example_controller:
+ type: franka_example_controllers/CartesianImpedanceExampleController
+ arm_id: panda
+ joint_names:
+ - panda_joint1
+ - panda_joint2
+ - panda_joint3
+ - panda_joint4
+ - panda_joint5
+ - panda_joint6
+ - panda_joint7
diff --git a/franka_example_controllers/franka_example_controllers_plugin.xml b/franka_example_controllers/franka_example_controllers_plugin.xml
index 99277291cf9da9d36564d14a4b573fdd3b2c62cf..25382fe23b836af4546cd279e9441fe49e48aaf8 100644
--- a/franka_example_controllers/franka_example_controllers_plugin.xml
+++ b/franka_example_controllers/franka_example_controllers_plugin.xml
@@ -24,9 +24,20 @@
A controller that evaluates and prints the dynamic model of Franka
</description>
</class>
- <class name="franka_example_controllers/JointImpedanceExampleController" type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+ <class name="franka_example_controllers/JointImpedanceExampleController" type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
<description>
A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs.
</description>
</class>
+ <class name="franka_example_controllers/CartesianImpedanceExampleController" type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively.
+ </description>
+ </class>
+ <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure.
+ </description>
+ </class>
+
</library>
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..06ae8f69eea3364bd7d08bbcc9977c56449dc1f2
--- /dev/null
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
@@ -0,0 +1,74 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <boost/scoped_ptr.hpp>
+#include <eigen3/Eigen/Dense>
+
+#include <franka_example_controllers/compliance_paramConfig.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+namespace franka_example_controllers {
+
+class CartesianImpedanceExampleController : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ CartesianImpedanceExampleController();
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
+ void starting(const ros::Time&);
+ void update(const ros::Time&, const ros::Duration& period);
+
+ private:
+ std::string arm_id_;
+ ros::NodeHandle node_handle_;
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+ std::vector<std::string> joint_names_;
+ double filter_params_{0.005};
+ double nullspace_stiffness_{20.0};
+ double nullspace_stiffness_target_{20.0};
+ const double delta_tau_max_{1.0};
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
+ Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
+ Eigen::Matrix<double, 6, 6> cartesian_damping_;
+ Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
+ Eigen::Matrix<double, 7, 1> q_d_nullspace_;
+ Eigen::Vector3d position_d_;
+ Eigen::Quaterniond orientation_d_;
+ Eigen::Vector3d position_d_target_;
+ Eigen::Quaterniond orientation_d_target_;
+
+ // Saturation
+ Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming)
+ const Eigen::Matrix<double, 7, 1>& gravity);
+
+ // Dynamic reconfigure
+ boost::scoped_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
+ dynamic_server_compliance_param_;
+ ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+ void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config,
+ uint32_t level);
+
+ // Equilibrium pose subscriber
+ ros::Subscriber sub_equilibrium_pose_;
+ void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
+};
+
+} // namespace franka_example_controllers
diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9f36a688150df18b3001b8eb6dadfd80cc21e9e
--- /dev/null
+++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
@@ -0,0 +1,59 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+#include <eigen3/Eigen/Core>
+
+#include <dynamic_reconfigure/server.h>
+#include <franka_example_controllers/desired_mass_paramConfig.h>
+#include <boost/scoped_ptr.hpp>
+
+namespace franka_example_controllers {
+
+class ForceExampleController : public controller_interface::MultiInterfaceController<
+ franka_hw::FrankaModelInterface,
+ hardware_interface::EffortJointInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ ForceExampleController();
+ bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle);
+ void starting(const ros::Time&);
+ void update(const ros::Time&, const ros::Duration& period);
+
+ private:
+ std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+ std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+ std::vector<hardware_interface::JointHandle> joint_handles_;
+ double desired_mass_{0.0};
+ double target_mass_{0.0};
+ double k_p_{0.0};
+ double k_i_{0.0};
+ double target_k_p_{0.0};
+ double target_k_i_{0.0};
+ double filter_gain_{0.001};
+ Eigen::Matrix<double, 7, 1> tau_ext_initial_;
+ Eigen::Matrix<double, 7, 1> tau_error_;
+
+ // Dynamic reconfigure
+ boost::scoped_ptr<
+ dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
+ dynamic_server_desired_mass_param_;
+ ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_;
+ void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig& config,
+ uint32_t level);
+};
+
+} // namespace franka_example_controllers
diff --git a/franka_example_controllers/launch/cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d4de9d4773252380b80d243d05726800a6d0fbfd
--- /dev/null
+++ b/franka_example_controllers/launch/cartesian_impedance_example_controller.launch
@@ -0,0 +1,18 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot_ip" default="robot.franka.de" />
+ <arg name="arm_id" value="panda" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find franka_control)/launch/franka_control.launch" >
+ <arg name="robot_ip" value="$(arg robot_ip)" />
+ <arg name="arm_id" value="$(arg arm_id)" />
+ <arg name="load_gripper" value="$(arg load_gripper)" />
+ </include>
+ <group ns="$(arg arm_id)">
+ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller --shutdown-timeout 2"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
+ <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen" />
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+ </group>
+</launch>
diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..76f16691ae1c00ab6607217fd67c804e6b5ee467
--- /dev/null
+++ b/franka_example_controllers/launch/force_example_controller.launch
@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot_ip" default="robot.franka.de" />
+ <arg name="arm_id" value="panda" />
+ <arg name="load_gripper" default="true" />
+ <include file="$(find franka_control)/launch/franka_control.launch" >
+ <arg name="robot_ip" value="$(arg robot_ip)" />
+ <arg name="arm_id" value="$(arg arm_id)" />
+ <arg name="load_gripper" value="$(arg load_gripper)" />
+ </include>
+ <group ns="$(arg arm_id)">
+ <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="force_example_controller --shutdown-timeout 2"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_description)/rviz/franka_description.rviz"/>
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+ </group>
+</launch>
diff --git a/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..d48a6db40c95fd1f65ee86e4d899bcb51bbe987c
--- /dev/null
+++ b/franka_example_controllers/launch/rviz/franka_description_with_marker.rviz
@@ -0,0 +1,181 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5
+ Tree Height: 573
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/InteractiveMarkers
+ Enable Transparency: true
+ Enabled: true
+ Name: InteractiveMarkers
+ Show Axes: false
+ Show Descriptions: true
+ Show Visual Aids: false
+ Update Topic: /panda/equilibrium_pose_marker/update
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: panda_link0
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 4.644041061401367
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.785398006439209
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 0.785398006439209
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000027000002c50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000042fc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 295
+ Y: 39
diff --git a/franka_example_controllers/launch/scripts/interactive_marker.py b/franka_example_controllers/launch/scripts/interactive_marker.py
new file mode 100755
index 0000000000000000000000000000000000000000..d691044687fe32c0de31f5818d6521a97a3ffc80
--- /dev/null
+++ b/franka_example_controllers/launch/scripts/interactive_marker.py
@@ -0,0 +1,140 @@
+#!/usr/bin/python
+
+import rospy
+import tf.transformations
+import numpy as np
+
+from interactive_markers.interactive_marker_server import \
+ InteractiveMarkerServer, InteractiveMarkerFeedback
+from visualization_msgs.msg import InteractiveMarker, \
+ InteractiveMarkerControl
+from geometry_msgs.msg import PoseStamped
+from franka_msgs.msg import FrankaState
+
+marker_pose = PoseStamped()
+initial_pose_found = False
+pose_pub = None
+# [[min_x, max_x], [min_y, max_y], [min_z, max_z]]
+position_limits = [[-0.6, 0.6], [-0.6, 0.6], [0.05, 0.9]]
+
+
+def publisherCallback(msg, arm_id):
+ marker_pose.header.frame_id = arm_id + "_link0"
+ marker_pose.header.stamp = rospy.Time(0)
+ pose_pub.publish(marker_pose)
+
+
+def franka_state_callback(msg):
+ initial_quaternion = \
+ tf.transformations.quaternion_from_matrix(
+ np.transpose(np.reshape(msg.O_T_EE,
+ (4, 4))))
+ marker_pose.pose.orientation.x = initial_quaternion[0]
+ marker_pose.pose.orientation.y = initial_quaternion[1]
+ marker_pose.pose.orientation.z = initial_quaternion[2]
+ marker_pose.pose.orientation.w = initial_quaternion[3]
+ marker_pose.pose.position.x = msg.O_T_EE[12]
+ marker_pose.pose.position.y = msg.O_T_EE[13]
+ marker_pose.pose.position.z = msg.O_T_EE[14]
+ global initial_pose_found
+ initial_pose_found = True
+
+
+def processFeedback(feedback):
+ if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
+ marker_pose.pose.position.x = max([min([feedback.pose.position.x,
+ position_limits[0][1]]),
+ position_limits[0][0]])
+ marker_pose.pose.position.y = max([min([feedback.pose.position.y,
+ position_limits[1][1]]),
+ position_limits[1][0]])
+ marker_pose.pose.position.z = max([min([feedback.pose.position.z,
+ position_limits[2][1]]),
+ position_limits[2][0]])
+ marker_pose.pose.orientation = feedback.pose.orientation
+ server.applyChanges()
+
+
+if __name__ == "__main__":
+ rospy.init_node("equilibrium_pose_node")
+ state_sub = rospy.Subscriber("franka_state_controller/franka_states",
+ FrankaState, franka_state_callback)
+ listener = tf.TransformListener()
+ arm_id = rospy.get_param("arm_id")
+
+ # Get initial pose for the interactive marker
+ while not initial_pose_found:
+ rospy.sleep(1)
+ state_sub.unregister()
+
+ pose_pub = rospy.Publisher(
+ "/equilibrium_pose", PoseStamped, queue_size=10)
+ server = InteractiveMarkerServer("equilibrium_pose_marker")
+ int_marker = InteractiveMarker()
+ int_marker.header.frame_id = arm_id + "_link0"
+ int_marker.scale = 0.3
+ int_marker.name = "equilibrium_pose"
+ int_marker.description = ("Equilibrium Pose\nBE CAREFUL! "
+ "If you move the \nequilibrium "
+ "pose the robot will follow it\n"
+ "so be aware of potential collisions")
+ int_marker.pose = marker_pose.pose
+ # run pose publisher
+ rospy.Timer(rospy.Duration(0.005),
+ lambda msg: publisherCallback(msg, arm_id))
+
+ # insert a box
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "rotate_x"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 1
+ control.orientation.y = 0
+ control.orientation.z = 0
+ control.name = "move_x"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "rotate_y"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 1
+ control.orientation.z = 0
+ control.name = "move_y"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "rotate_z"
+ control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
+ int_marker.controls.append(control)
+ control = InteractiveMarkerControl()
+ control.orientation.w = 1
+ control.orientation.x = 0
+ control.orientation.y = 0
+ control.orientation.z = 1
+ control.name = "move_z"
+ control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+ int_marker.controls.append(control)
+ server.insert(int_marker, processFeedback)
+
+ server.applyChanges()
+
+ rospy.spin()
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
index 22a0a4059fe199ad92b04411fe373a80ab106dac..71f68269626adf75d634958e795da20a33be2e8c 100644
--- a/franka_example_controllers/package.xml
+++ b/franka_example_controllers/package.xml
@@ -15,6 +15,7 @@
<build_depend>roscpp</build_depend>
<build_depend>message_generation</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
<depend>controller_interface</depend>
<depend>franka_hw</depend>
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..81e78c27444cb172033b73c725973771806d31e2
--- /dev/null
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -0,0 +1,259 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/cartesian_impedance_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+#include "pseudo_inversion.h"
+
+namespace franka_example_controllers {
+
+CartesianImpedanceExampleController::CartesianImpedanceExampleController() = default;
+
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::vector<double> cartesian_stiffness_vector;
+ std::vector<double> cartesian_damping_vector;
+ node_handle_ = node_handle;
+
+ sub_equilibrium_pose_ = node_handle_.subscribe(
+ "/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
+ ros::TransportHints().reliable().tcpNoDelay());
+
+ if (!node_handle_.getParam("arm_id", arm_id_)) {
+ ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ if (!node_handle_.getParam("joint_names", joint_names_) || joint_names_.size() != 7) {
+ ROS_ERROR(
+ "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
+ "aborting "
+ "controller init!");
+ return false;
+ }
+
+ franka_hw::FrankaModelInterface* model_interface =
+ robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting model interface from hardware");
+ return false;
+ }
+ try {
+ model_handle_.reset(
+ new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id_ + "_model")));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting model handle from interface: "
+ << ex.what());
+ return false;
+ }
+
+ franka_hw::FrankaStateInterface* state_interface =
+ robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting state interface from hardware");
+ return false;
+ }
+ try {
+ state_handle_.reset(
+ new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id_ + "_robot")));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting state handle from interface: "
+ << ex.what());
+ return false;
+ }
+
+ hardware_interface::EffortJointInterface* effort_joint_interface =
+ robot_hw->get<hardware_interface::EffortJointInterface>();
+ if (effort_joint_interface == nullptr) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
+ return false;
+ }
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ joint_handles_.push_back(effort_joint_interface->getHandle(joint_names_[i]));
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+
+ dynamic_reconfigure_compliance_param_node_ =
+ ros::NodeHandle("dynamic_reconfigure_compliance_param_node");
+
+ dynamic_server_compliance_param_.reset(
+ new dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>(
+ dynamic_reconfigure_compliance_param_node_));
+ dynamic_server_compliance_param_->setCallback(
+ boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+ position_d_.setZero();
+ orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+ position_d_target_.setZero();
+ orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+ cartesian_stiffness_.setZero();
+ cartesian_damping_.setZero();
+
+ return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) {
+ // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+ // to initial configuration
+ franka::RobotState initial_state = state_handle_->getRobotState();
+ // get jacobian
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector, initial_state);
+ // convert to eigen
+ Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > dq_initial(initial_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > q_initial(initial_state.q.data());
+ Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+ // set equilibrium point to current state
+ position_d_ = initial_transform.translation();
+ orientation_d_ = Eigen::Quaterniond(initial_transform.linear());
+ position_d_target_ = initial_transform.translation();
+ orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear());
+
+ // set nullspace equilibrium configuration to initial q
+ q_d_nullspace_ = q_initial;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
+ const ros::Duration& /*period*/) {
+ // get state variables
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 7> gravity_array =
+ model_handle_->getGravity(robot_state.m_load, robot_state.F_x_Cload);
+ std::array<double, 7> coriolis_array =
+ model_handle_->getCoriolis(robot_state.I_load, robot_state.m_load, robot_state.F_x_Cload);
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
+
+ // convert to Eigen
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > coriolis(coriolis_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+ Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > q(robot_state.q.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > dq(robot_state.dq.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_ext(robot_state.tau_ext_hat_filtered.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_J_d( // NOLINT (readability-identifier-naming)
+ robot_state.tau_J_d.data());
+ Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+ Eigen::Vector3d position(transform.translation());
+ Eigen::Quaterniond orientation(transform.linear());
+
+ // compute error to desired pose
+ // position error
+ Eigen::Matrix<double, 6, 1> error;
+ error.head(3) << position - position_d_;
+
+ // orientation error
+ // "difference" quaternion
+ Eigen::Quaterniond error_quaternion(orientation * orientation_d_.inverse());
+ // convert to axis angle
+ Eigen::AngleAxisd error_quaternion_angle_axis(error_quaternion);
+ // compute "orientation error"
+ error.tail(3) << error_quaternion_angle_axis.axis() * error_quaternion_angle_axis.angle();
+
+ // compute control
+ // allocate variables
+ Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+
+ // pseudoinverse for nullspace handling
+ // kinematic pseuoinverse
+ Eigen::MatrixXd jacobian_transpose_pinv;
+ pseudo_inverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+ // Cartesian PD control with damping ratio = 1
+ tau_task << jacobian.transpose() *
+ (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq));
+ // nullspace PD control with damping ratio = 1
+ tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+ jacobian.transpose() * jacobian_transpose_pinv) *
+ (nullspace_stiffness_ * (q_d_nullspace_ - q) -
+ (2.0 * sqrt(nullspace_stiffness_)) * dq);
+ // Desired torque
+ tau_d << tau_task + tau_nullspace + coriolis - tau_ext;
+ // Saturate torque rate to avoid discontinuities
+ tau_d << saturateTorqueRate(tau_d, tau_J_d, gravity);
+ for (size_t i = 0; i < 7; ++i) {
+ joint_handles_[i].setCommand(tau_d(i));
+ }
+
+ // update parameters changed online either through dynamic reconfigure or through the interactive
+ // target by filtering
+ cartesian_stiffness_ =
+ filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_;
+ cartesian_damping_ =
+ filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_;
+ nullspace_stiffness_ =
+ filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
+ position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
+ Eigen::AngleAxisd aa_orientation_d(orientation_d_);
+ Eigen::AngleAxisd aa_orientation_d_target(orientation_d_target_);
+ aa_orientation_d.axis() = filter_params_ * aa_orientation_d_target.axis() +
+ (1.0 - filter_params_) * aa_orientation_d.axis();
+ aa_orientation_d.angle() = filter_params_ * aa_orientation_d_target.angle() +
+ (1.0 - filter_params_) * aa_orientation_d.angle();
+ orientation_d_ = Eigen::Quaterniond(aa_orientation_d);
+}
+
+Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
+ const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+ const Eigen::Matrix<double, 7, 1>& tau_J_d, // NOLINT (readability-identifier-naming)
+ const Eigen::Matrix<double, 7, 1>& gravity) {
+ Eigen::Matrix<double, 7, 1> tau_d_saturated{};
+ for (size_t i = 0; i < 7; i++) {
+ // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
+ double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
+ tau_d_saturated[i] =
+ (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+ }
+ return tau_d_saturated;
+}
+
+void CartesianImpedanceExampleController::complianceParamCallback(
+ franka_example_controllers::compliance_paramConfig& config,
+ uint32_t /*level*/) {
+ cartesian_stiffness_target_.setIdentity();
+ cartesian_stiffness_target_.topLeftCorner(3, 3)
+ << config.translational_stiffness * Eigen::Matrix3d::Identity();
+ cartesian_stiffness_target_.bottomRightCorner(3, 3)
+ << config.rotational_stiffness * Eigen::Matrix3d::Identity();
+ cartesian_damping_target_.setIdentity();
+ // Damping ratio = 1
+ cartesian_damping_target_.topLeftCorner(3, 3)
+ << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity();
+ cartesian_damping_target_.bottomRightCorner(3, 3)
+ << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity();
+ nullspace_stiffness_target_ = config.nullspace_stiffness;
+}
+
+void CartesianImpedanceExampleController::equilibriumPoseCallback(
+ const geometry_msgs::PoseStampedConstPtr& msg) {
+ position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+ Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
+ orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w;
+ if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) {
+ orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
+ }
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+ controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index bd00e6ae7b09d7bd2953611b77bc0d0be8b4ae38..441119d4703d41786c9217ae220b0f7d348917db 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -47,7 +47,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
}
void CartesianPoseExampleController::starting(const ros::Time& /*time*/) {
- initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE;
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
}
void CartesianPoseExampleController::update(const ros::Time& /*time*/,
diff --git a/franka_example_controllers/src/force_example_controller.cpp b/franka_example_controllers/src/force_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a44c55f2d3bf18c662b20e2a0cb5ee8b3ba14a3a
--- /dev/null
+++ b/franka_example_controllers/src/force_example_controller.cpp
@@ -0,0 +1,141 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/force_example_controller.h>
+
+#include <cmath>
+
+#include <controller_interface/controller_base.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka/robot_state.h>
+
+namespace franka_example_controllers {
+
+ForceExampleController::ForceExampleController() = default;
+
+bool ForceExampleController::init(hardware_interface::RobotHW* robot_hw,
+ ros::NodeHandle& node_handle) {
+ std::vector<std::string> joint_names;
+ std::string arm_id;
+ ROS_WARN(
+ "ForceExampleController: Make sure your robot's endeffector is in contact "
+ "with a horizontal surface before starting the controller!");
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("ForceExampleController: Could not read parameter arm_id");
+ return false;
+ }
+ if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
+ ROS_ERROR(
+ "ForceExampleController: Invalid or no joint_names parameters provided, aborting "
+ "controller init!");
+ return false;
+ }
+
+ franka_hw::FrankaModelInterface* model_interface =
+ robot_hw->get<franka_hw::FrankaModelInterface>();
+ if (model_interface == nullptr) {
+ ROS_ERROR_STREAM("ForceExampleController: Error getting model interface from hardware");
+ return false;
+ }
+ try {
+ model_handle_.reset(
+ new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "ForceExampleController: Exception getting model handle from interface: " << ex.what());
+ return false;
+ }
+
+ franka_hw::FrankaStateInterface* state_interface =
+ robot_hw->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR_STREAM("ForceExampleController: Error getting state interface from hardware");
+ return false;
+ }
+ try {
+ state_handle_.reset(
+ new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
+ } catch (hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM(
+ "ForceExampleController: Exception getting state handle from interface: " << ex.what());
+ return false;
+ }
+
+ hardware_interface::EffortJointInterface* effort_joint_interface =
+ robot_hw->get<hardware_interface::EffortJointInterface>();
+ if (effort_joint_interface == nullptr) {
+ ROS_ERROR_STREAM("ForceExampleController: Error getting effort joint interface from hardware");
+ return false;
+ }
+ for (size_t i = 0; i < 7; ++i) {
+ try {
+ joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+ } catch (const hardware_interface::HardwareInterfaceException& ex) {
+ ROS_ERROR_STREAM("ForceExampleController: Exception getting joint handles: " << ex.what());
+ return false;
+ }
+ }
+
+ dynamic_reconfigure_desired_mass_param_node_ =
+ ros::NodeHandle("dynamic_reconfigure_desired_mass_param_node");
+ dynamic_server_desired_mass_param_.reset(
+ new dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>(
+ dynamic_reconfigure_desired_mass_param_node_));
+ dynamic_server_desired_mass_param_->setCallback(
+ boost::bind(&ForceExampleController::desiredMassParamCallback, this, _1, _2));
+
+ return true;
+}
+
+void ForceExampleController::starting(const ros::Time& /*time*/) {
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 7> gravity_array =
+ model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > gravity(gravity_array.data());
+ // Bias correction for the current external torque
+ tau_ext_initial_ = tau_measured - gravity;
+ tau_error_.setZero();
+}
+
+void ForceExampleController::update(const ros::Time& /*time*/, const ros::Duration& period) {
+ franka::RobotState robot_state = state_handle_->getRobotState();
+ std::array<double, 42> jacobian_array =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
+ std::array<double, 7> gravity_array =
+ model_handle_->getGravity(robot_state, 0.0, {{0.0, 0.0, 0.0}});
+ Eigen::Map<Eigen::Matrix<double, 6, 7> > jacobian(jacobian_array.data());
+ Eigen::Map<Eigen::Matrix<double, 7, 1> > tau_measured(robot_state.tau_J.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);
+ 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_error_ = tau_error_ + period.toSec() * (tau_d - tau_ext);
+ // FF + PI control (PI gains are intially all 0)
+ tau_cmd = tau_d + k_p_ * (tau_d - tau_ext) + k_i_ * tau_error_;
+ for (size_t i = 0; i < 7; ++i) {
+ joint_handles_[i].setCommand(tau_cmd(i));
+ }
+
+ // Update signals changed online through dynamic reconfigure
+ desired_mass_ = filter_gain_ * target_mass_ + (1 - filter_gain_) * desired_mass_;
+ k_p_ = filter_gain_ * target_k_p_ + (1 - filter_gain_) * k_p_;
+ k_i_ = filter_gain_ * target_k_i_ + (1 - filter_gain_) * k_i_;
+}
+
+void ForceExampleController::desiredMassParamCallback(
+ franka_example_controllers::desired_mass_paramConfig& config,
+ uint32_t /*level*/) {
+ target_mass_ = config.desired_mass;
+ target_k_p_ = config.k_p;
+ target_k_i_ = config.k_i;
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ForceExampleController,
+ controller_interface::ControllerBase)
diff --git a/franka_example_controllers/src/joint_impedance_example_controller.cpp b/franka_example_controllers/src/joint_impedance_example_controller.cpp
index a5cea2bde74fb3f5c0a24ab350143aac1af95af4..bf6aad553a56315ace6ae2ebfbd5f022b5ea3c5c 100644
--- a/franka_example_controllers/src/joint_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_example_controller.cpp
@@ -140,7 +140,7 @@ bool JointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw
}
void JointImpedanceExampleController::starting(const ros::Time& /*time*/) {
- initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE;
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
}
void JointImpedanceExampleController::update(const ros::Time& /*time*/,
diff --git a/franka_example_controllers/src/pseudo_inversion.h b/franka_example_controllers/src/pseudo_inversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..6170154323cabdd02642b5054dc4c239c2cf31f1
--- /dev/null
+++ b/franka_example_controllers/src/pseudo_inversion.h
@@ -0,0 +1,32 @@
+// Author: Enrico Corvaglia
+// https://github.com/CentroEPiaggio/kuka-lwr/blob/master/lwr_controllers/include/utils/pseudo_inversion.h
+// File provided under public domain
+// pseudo_inverse() computes the pseudo inverse of matrix M_ using SVD decomposition (can choose
+// between damped and not)
+// returns the pseudo inverted matrix M_pinv_
+
+#ifndef PSEUDO_INVERSION_H
+#define PSEUDO_INVERSION_H
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/LU>
+#include <eigen3/Eigen/SVD>
+using namespace Eigen;
+
+inline void pseudo_inverse(const Eigen::MatrixXd& M_,
+ Eigen::MatrixXd& M_pinv_,
+ bool damped = true) {
+ double lambda_ = damped ? 0.2 : 0.0;
+
+ JacobiSVD<MatrixXd> svd(M_, ComputeFullU | ComputeFullV);
+ JacobiSVD<MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
+ MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed.
+ S_.setZero();
+
+ for (int i = 0; i < sing_vals_.size(); i++)
+ S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
+
+ M_pinv_ = MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
+}
+
+#endif // PSEUDO_INVERSION_H