diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
index bce35bb1db1c6008d5598078f3ff2e41d2147e5c..453f2508fe276b13a9225d4615a951cf9f24ca06 100644
--- a/franka_example_controllers/CMakeLists.txt
+++ b/franka_example_controllers/CMakeLists.txt
@@ -49,6 +49,7 @@ catkin_package(
)
add_library(franka_example_controllers
+ src/elbow_example_controller.cpp
src/cartesian_pose_example_controller.cpp
src/cartesian_velocity_example_controller.cpp
src/joint_position_example_controller.cpp
diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml
index 47fbc9d762a478b9c37b81bd0fdcb2566e05753a..4b7707efc53ad63104834189ee45cdd48d510c33 100644
--- a/franka_example_controllers/config/franka_example_controllers.yaml
+++ b/franka_example_controllers/config/franka_example_controllers.yaml
@@ -24,6 +24,10 @@ cartesian_pose_example_controller:
type: franka_example_controllers/CartesianPoseExampleController
arm_id: panda
+elbow_example_controller:
+ type: franka_example_controllers/ElbowExampleController
+ arm_id: panda
+
cartesian_velocity_example_controller:
type: franka_example_controllers/CartesianVelocityExampleController
arm_id: panda
diff --git a/franka_example_controllers/franka_example_controllers_plugin.xml b/franka_example_controllers/franka_example_controllers_plugin.xml
index 25382fe23b836af4546cd279e9441fe49e48aaf8..559a9f63eccaa1f924094702ffe1d38ec5dd9789 100644
--- a/franka_example_controllers/franka_example_controllers_plugin.xml
+++ b/franka_example_controllers/franka_example_controllers_plugin.xml
@@ -19,6 +19,11 @@
A controller that executes a short motion based on cartesian velocities to demonstrate correct usage
</description>
</class>
+ <class name="franka_example_controllers/ElbowExampleController" type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerBase">
+ <description>
+ A controller that executes a short motion based on cartesian poses and elbow to demonstrate correct usage
+ </description>
+ </class>
<class name="franka_example_controllers/ModelExampleController" type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerBase">
<description>
A controller that evaluates and prints the dynamic model of Franka
diff --git a/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h b/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..70a8cfb16cfa85ba695fc1393c9daaad6156ec04
--- /dev/null
+++ b/franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h
@@ -0,0 +1,35 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <memory>
+#include <string>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <franka_hw/franka_state_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+
+#include <franka_hw/franka_cartesian_command_interface.h>
+
+namespace franka_example_controllers {
+
+class ElbowExampleController
+ : public controller_interface::MultiInterfaceController<franka_hw::FrankaPoseCartesianInterface,
+ franka_hw::FrankaStateInterface> {
+ public:
+ bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
+ void starting(const ros::Time&) override;
+ void update(const ros::Time&, const ros::Duration& period) override;
+
+ private:
+ franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
+ std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
+ ros::Duration elapsed_time_;
+ std::array<double, 16> initial_pose_{};
+ std::array<double, 2> initial_elbow_{};
+};
+
+} // namespace franka_example_controllers
diff --git a/franka_example_controllers/launch/elbow_example_controller.launch b/franka_example_controllers/launch/elbow_example_controller.launch
new file mode 100644
index 0000000000000000000000000000000000000000..1b2322f26bd7a8ae7a21f3b61aa35f29997b7235
--- /dev/null
+++ b/franka_example_controllers/launch/elbow_example_controller.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<launch>
+ <arg name="robot_ip" />
+ <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="load_gripper" value="$(arg load_gripper)" />
+ </include>
+
+ <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="elbowf_example_controller"/>
+ <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
+</launch>
diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f12137ebd6f75927d16de71ad6a13bb2a47d2290
--- /dev/null
+++ b/franka_example_controllers/src/elbow_example_controller.cpp
@@ -0,0 +1,85 @@
+// 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/elbow_example_controller.h>
+
+#include <cmath>
+#include <stdexcept>
+#include <string>
+
+#include <controller_interface/controller_base.h>
+#include <franka_hw/franka_cartesian_command_interface.h>
+#include <hardware_interface/hardware_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+namespace franka_example_controllers {
+
+bool ElbowExampleController::init(hardware_interface::RobotHW* robot_hardware,
+ ros::NodeHandle& node_handle) {
+ cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
+ if (cartesian_pose_interface_ == nullptr) {
+ ROS_ERROR("ElbowExampleController: Could not get Cartesian Pose interface from hardware");
+ return false;
+ }
+
+ std::string arm_id;
+ if (!node_handle.getParam("arm_id", arm_id)) {
+ ROS_ERROR("ElbowExampleController: Could not get parameter arm_id");
+ return false;
+ }
+
+ try {
+ cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
+ cartesian_pose_interface_->getHandle(arm_id + "_robot")));
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM("ElbowExampleController: Exception getting Cartesian handle: " << e.what());
+ return false;
+ }
+
+ auto state_interface = robot_hardware->get<franka_hw::FrankaStateInterface>();
+ if (state_interface == nullptr) {
+ ROS_ERROR("ElbowExampleController: Could not get state interface from hardware");
+ return false;
+ }
+
+ try {
+ auto state_handle = state_interface->getHandle(arm_id + "_robot");
+
+ std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
+ for (size_t i = 0; i < q_start.size(); i++) {
+ if (std::abs(state_handle.getRobotState().q_d[i] - q_start[i]) > 0.1) {
+ ROS_ERROR_STREAM(
+ "ElbowExampleController: Robot is not in the expected starting position for "
+ "running this example. Run `roslaunch franka_example_controllers move_to_start.launch "
+ "robot_ip:=<robot-ip> load_gripper:=<has-attached-gripper>` first.");
+ return false;
+ }
+ }
+ } catch (const hardware_interface::HardwareInterfaceException& e) {
+ ROS_ERROR_STREAM("ElbowExampleController: Exception getting state handle: " << e.what());
+ return false;
+ }
+
+ return true;
+}
+
+void ElbowExampleController::starting(const ros::Time& /* time */) {
+ initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d;
+ initial_elbow_ = cartesian_pose_handle_->getRobotState().elbow_d;
+ elapsed_time_ = ros::Duration(0.0);
+}
+
+void ElbowExampleController::update(const ros::Time& /* time */, const ros::Duration& period) {
+ elapsed_time_ += period;
+
+ double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_.toSec()));
+ auto elbow = initial_elbow_;
+ elbow[0] += angle;
+
+ cartesian_pose_handle_->setCommand(initial_pose_, elbow);
+}
+
+} // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController,
+ controller_interface::ControllerBase)
diff --git a/franka_hw/include/franka_hw/franka_cartesian_command_interface.h b/franka_hw/include/franka_hw/franka_cartesian_command_interface.h
index d5b82e9f1ba2c68f5d036550e3b8a57873997bd1..1e04ce7f528eec24d56048b1cc14eab66848e66a 100644
--- a/franka_hw/include/franka_hw/franka_cartesian_command_interface.h
+++ b/franka_hw/include/franka_hw/franka_cartesian_command_interface.h
@@ -21,17 +21,35 @@ class FrankaCartesianPoseHandle : public FrankaStateHandle {
*
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian pose command wrapped by this handle.
+ * @param[in] elbow A reference to the elbow wrapped by this handle.
*/
FrankaCartesianPoseHandle(const FrankaStateHandle& franka_state_handle,
- std::array<double, 16>& command)
- : FrankaStateHandle(franka_state_handle), command_(&command) {}
+ std::array<double, 16>& command,
+ std::array<double, 2>& elbow)
+ : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
/**
* Sets the given command.
*
* @param[in] command Command to set.
+ * @param[in] elbow Elbow to set. Default none.
*/
- void setCommand(const std::array<double, 16>& command) noexcept { *command_ = command; }
+ void setCommand(const std::array<double, 16>& command) noexcept {
+ *command_ = command;
+ *elbow_ = {};
+ }
+
+ /**
+ * Sets the given command.
+ *
+ * @param[in] command Command to set.
+ * @param[in] elbow Elbow to set.
+ */
+ void setCommand(const std::array<double, 16>& command,
+ const std::array<double, 2>& elbow) noexcept {
+ *command_ = command;
+ *elbow_ = elbow;
+ }
/**
* Gets the current command.
@@ -42,6 +60,7 @@ class FrankaCartesianPoseHandle : public FrankaStateHandle {
private:
std::array<double, 16>* command_;
+ std::array<double, 2>* elbow_;
};
/**
@@ -61,10 +80,12 @@ class FrankaCartesianVelocityHandle : public FrankaStateHandle {
/**
* @param[in] franka_state_handle Robot state handle.
* @param[in] command A reference to the Cartesian velocity command wrapped by this handle.
+ * @param[in] elbow A reference to the elbow wrapped by this handle.
*/
FrankaCartesianVelocityHandle(const FrankaStateHandle& franka_state_handle,
- std::array<double, 6>& command)
- : FrankaStateHandle(franka_state_handle), command_(&command) {}
+ std::array<double, 6>& command,
+ std::array<double, 2>& elbow)
+ : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}
/**
* Sets the given command.
@@ -73,6 +94,18 @@ class FrankaCartesianVelocityHandle : public FrankaStateHandle {
*/
void setCommand(std::array<double, 6>& command) noexcept { *command_ = command; }
+ /**
+ * Sets the given command.
+ *
+ * @param[in] command Command to set.
+ * @param[in] elbow Elbow to set.
+ */
+ void setCommand(const std::array<double, 6>& command,
+ const std::array<double, 2>& elbow) noexcept {
+ *command_ = command;
+ *elbow_ = elbow;
+ }
+
/**
* Gets the current command.
*
@@ -82,6 +115,7 @@ class FrankaCartesianVelocityHandle : public FrankaStateHandle {
private:
std::array<double, 6>* command_;
+ std::array<double, 2>* elbow_;
};
/**
diff --git a/franka_hw/src/franka_hw.cpp b/franka_hw/src/franka_hw.cpp
index f753cdd10d7ada09fe5ef5a3635466af0057c3e5..181d82779451a607d50f7d87df8f5cc08fcc3c9d 100644
--- a/franka_hw/src/franka_hw.cpp
+++ b/franka_hw/src/franka_hw.cpp
@@ -100,11 +100,11 @@ FrankaHW::FrankaHW(const std::array<std::string, 7>& joint_names,
FrankaStateHandle franka_state_handle(arm_id_ + "_robot", robot_state_);
franka_state_interface_.registerHandle(franka_state_handle);
- FrankaCartesianPoseHandle franka_cartesian_pose_handle(franka_state_handle,
- pose_cartesian_command_.O_T_EE);
+ FrankaCartesianPoseHandle franka_cartesian_pose_handle(
+ franka_state_handle, pose_cartesian_command_.O_T_EE, pose_cartesian_command_.elbow);
franka_pose_cartesian_interface_.registerHandle(franka_cartesian_pose_handle);
FrankaCartesianVelocityHandle franka_cartesian_velocity_handle(
- franka_state_handle, velocity_cartesian_command_.O_dP_EE);
+ franka_state_handle, velocity_cartesian_command_.O_dP_EE, velocity_cartesian_command_.elbow);
franka_velocity_cartesian_interface_.registerHandle(franka_cartesian_velocity_handle);
registerInterface(&franka_state_interface_);