Allow elbow commanding with Cartesian pose and velocity.

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

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

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

franka_example_controllers/include/franka_example_controllers/elbow_example_controller.h

+// 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

franka_example_controllers/launch/elbow_example_controller.launch

+<?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>

franka_example_controllers/src/elbow_example_controller.cpp

+// 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)

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_;
 };
 
 /**

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_);