Dynamics controllers for Computed Torque Control / Gravity Compensation

dynamics_controllers/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dynamics_controllers)
+
+# Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+# For now, latest build from source is needed
+find_package(orocos_kdl REQUIRED)
+
+# Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  controller_interface
+  kdl_parser
+)
+
+# Declare catkin package
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  DEPENDS
+    orocos_kdl
+  CATKIN_DEPENDS
+    controller_interface
+    kdl_parser
+)
+
+include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+  src/dynamics_controller_base.cpp
+  src/kdl_chain_controller.cpp
+  src/kdl_tree_controller.cpp
+  src/kdl/treeidsolver_recursive_newton_euler.cpp # NOTE: this will be removed
+)
+
+# the options are used to force the linker to report undefined symbols as errors
+target_link_libraries(${PROJECT_NAME} ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES} -Wl,--no-undefined)
+
+# Install the library
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(
+  FILES dynamics_controllers.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

dynamics_controllers/README.md

@@ -0,0 +1,158 @@
+## Dynamics Controllers
+
+Package that implements controllers that add a "dynamic control layer" to
+existing effort controllers.
+
+The controllers contained in this package allow to take into account the
+dynamics of a kinematic chain/tree and to perform Computed Torque Control.
+Controllers are meant to operate in conjunction with existing controllers that
+operate with `hardware_interface::EffortJointInterface`, which act as a
+"*sub-controller*".
+The command from the sub-controller - which would normally be an effort - is
+instead interpreted as an acceleration and injected in the Inverse Dynamic Model
+(IDM) of the mechanism, allowing to better compensate for the nonlinearity of
+robots' Dynamic Models.
+
+Currently, there exist two controllers: `KdlChainController` and
+`KdlTreeController`.
+They are both based on KDL's implementation of the Recursive Newton-Euler
+algorithm for the inverse dynamics.
+
+**Note**: as of today, you need to install KDL from source (master branch)
+in order to have the `KdlTreeController`.
+
+
+
+### Gravity Compensation vs Full Inverse Dynamics
+
+By default, the controllers perform full Inverse Dynamics computations.
+However, you can ask them to perform gravity compensation only.
+In this case, the command evaluated by the sub-controller is added to the
+gravitational efforts - this corresponds to assuming the Generalized Inertia
+Matrix to be the Identity and Coriolis/Centrifugal efforts to be null.
+
+To enable gravity compensation only, you can specify the parameter
+`gravity_compensation_only: true` in the controller configuration.
+
+
+
+### Example of Controller Configuration
+
+Assuming that you have a simple kinematic chain with two joints `joint1` and
+`joint2`, you might have some controllers configured as:
+
+```yaml
+position_controller:
+  type: effort_controllers/JointGroupPositionController
+  joints:
+    - joint1
+    - joint2
+  joint1/pid: {p: 10.0, i: 0.0, d: 5.0}
+  joint2/pid: {p: 10.0, i: 0.0, d: 5.0}
+
+velocity_controller:
+  type: effort_controllers/JointGroupVelocityController
+  joints:
+    - joint1
+    - joint2
+  joint1/pid: {p: 10.0, i: 0.0, d: 5.0}
+  joint2/pid: {p: 10.0, i: 0.0, d: 5.0}
+```
+
+To use the new controllers, you can change it, *e.g.*, as follows:
+```yaml
+gravity: [0, 0, -9.81] # this should be projected on the base frame
+
+position_controller:
+  type: dynamics_controllers/KdlChainController
+  sub_controller:
+    # the subcontroller is basically the controller you had before!
+    type: effort_controllers/JointGroupPositionController
+    joints:
+      - joint1
+      - joint2
+    joint1/pid: {p: 10.0, i: 0.0, d: 5.0}
+    joint2/pid: {p: 10.0, i: 0.0, d: 5.0}
+
+velocity_controller:
+  type: dynamics_controllers/KdlTreeController
+  sub_controller:
+    # the subcontroller is basically the controller you had before!
+    type: effort_controllers/JointGroupVelocityController
+    joints:
+      - joint1
+      - joint2
+    joint1/pid: {p: 10.0, i: 0.0, d: 5.0}
+    joint2/pid: {p: 10.0, i: 0.0, d: 5.0}
+```
+
+Note that you will likely have to tune again the gains of the sub-controllers to
+achieve good performances.
+
+#### Detailed Controllers Description
+
+Below, parameters relative to each controller are listed.
+Those marked as "searched for" can live in any parent namespace since the
+`NodeHandle::searchParam` method is used to locate them.
+This facilitates "sharing" common parameters such as the gravity vector, which
+should not depend on a specific controller (at least in principle!).
+
+##### KdlChainController
+
+- `sub_controller`: should contain the configuration of a controller of type
+  compatible with `EffortJointInterface`.
+- `robot_description`: should contain the URDF of the robot. Searched for.
+- `gravity`: list with three elements, representing the gravity vector in the
+  base frame of the chain. Searched for. Default: `[0,0,0]`.
+- `chain_base`: base of the chain. Searched for. Default: name of the root link
+  from the robot description.
+- `chain_tip`: tip of the chain. Searched for. Default: if a single "branch"
+  is rooted at `base_link`, `chain_tip` will correspond to the last link of the
+  chain. If at any point multiple children are found, loading will fail.
+- `gravity_compensation_only`: as discussed above. Default: `false`.
+
+##### KdlTreeController
+
+- `sub_controller`: should contain the configuration of a controller of type
+  compatible with `EffortJointInterface`.
+- `robot_description`: should contain the URDF of the robot. Searched for.
+- `gravity`: list with three elements, representing the gravity vector in the
+  base frame of the chain. Searched for. Default: `[0,0,0]`.
+- `tree_root`: base of the tree. Searched for. Default: name of the root link
+  from the robot description.
+- `gravity_compensation_only`: as discussed above. Default: `false`.
+
+
+
+### Limitations
+
+- They all assume a static base. The main implication is that you cannot
+  "serially join" controllers for different parts of the robot.
+- They require proper identification of the Dynamic Parameters.
+- The parameters must be provided in URDF format. This is a limitation since
+  the identification will often return a smaller set of regrouped parameters.
+- The sub-controller must control all joints of the internal `Chain`/`Tree`.
+
+
+
+### Possible Improvements
+
+- [ ] Compensation of external payloads, mainly in two ways:
+    - By adding a subscriber that can update the `wrenches_` members
+    - Allowing to dynamically modify the `Chain`/`Tree` instances
+- [ ] Support for moving bases. This likely requires to re-implement the
+      controllers using another library, such as pinocchio, or to expand KDL.
+      A possible workaround is to add a "floating joint" (in the form of a
+      3T3R chain) as the parent of the base.
+- [ ] Additional parameters such as joint inertia and viscous friction.
+- [ ] Allow multiple sub-controllers to manage different joints.
+
+
+### Personal todo list:
+
+- [ ] How should the license be pasted? Some headers contain only WillowGarage,
+  some many institutions. Should I put myself as well?
+- [x] Check CMakeLists.txt (install targets etc).
+- [x] Tell about the KDL version that is needed!
+- [x] Complete `KdlTreeController` so that proper sub-tree extraction is possible.
+- [ ] Enforce effort limits

dynamics_controllers/dynamics_controllers.xml

@@ -0,0 +1,23 @@
+<library path="lib/libdynamics_controllers">
+
+  <class
+    name="dynamics_controllers/KdlChainController"
+    type="dynamics_controllers::KdlChainController"
+    base_class_type="controller_interface::ControllerBase">
+    <description>
+      Computes the efforts based on the Inverse Dynamics of a kinematic chain.
+      Wraps around controllers compatible with an EffortJointInterface.
+    </description>
+  </class>
+
+  <class
+    name="dynamics_controllers/KdlTreeController"
+    type="dynamics_controllers::KdlTreeController"
+    base_class_type="controller_interface::ControllerBase">
+    <description>
+      Computes the efforts based on the Inverse Dynamics of a kinematic tree.
+      Wraps around controllers compatible with an EffortJointInterface.
+    </description>
+  </class>
+
+</library>

dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h

@@ -0,0 +1,158 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  Copyright (c) 2014, Fraunhofer IPA
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * 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.
+ *   * Neither the name of the PAL Robotics nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ *********************************************************************/
+
+/*
+ * Author: Franco Fusco - [email protected]
+ */
+
+#ifndef DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H
+#define DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H
+
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <pluginlib/class_loader.h>
+#include <stdexcept>
+
+
+namespace dynamics_controllers {
+
+/// Base class for dynamics controllers.
+/** This class defines the common interface for dynamics controllers. Such
+  * controllers are supposed to compute the efforts from the acceleration
+  * provided by another "sub-controller".
+  * 
+  * To allow wrapping around the sub-controller, this class creates a "fake
+  * hardware".
+  * It is used to provide the current robot state to the sub-controller and to
+  * retrieve its command.
+  *
+  * To create a specific dynamics controller, simply inherit from this class
+  * and implement the pure virtual methods `initDynamics()` and `computeEfforts()`.
+  */
+class DynamicsControllerBase : public controller_interface::Controller<hardware_interface::EffortJointInterface> {
+public:
+  DynamicsControllerBase();
+
+  /// Initializes the controller.
+  /** The method performs the follwing steps:
+    * - call `initDynamics()`
+    * - initialize the fake hardware
+    * - initialize the sub-controller
+    */
+  bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle& nh) override;
+
+  /// Start the sub-controller.
+  void starting(const ros::Time& time) override;
+
+  /// Compute the efforts.
+  /** The method performs the following steps:
+    * - copy the current state of the robot in the fake hardware
+    * - update the sub-controller
+    * - copy the command of the sub-controller in `sub_command_`
+    * - call `computeEfforts()`
+    */
+  void update(const ros::Time& time, const ros::Duration& period) override;
+
+  /// Stop the sub-controller.
+  void stopping(const ros::Time& time) override;
+
+protected:
+
+  /// Called when initializing the controller.
+  /** The method allows to perform sub-class specific initialization, eg,
+    * loading dynamic solvers.
+    * \note The method should also initialize the member `joint_names_`, which
+    * will be used to generate the `FakeHW` instance for the low-level
+    * controller.
+    */
+  virtual bool initDynamics(ros::NodeHandle& nh) = 0;
+
+  /// Sub-classes should override this method so that they do specific dynamic control.
+  /** The subclass is required to either set the efforts directly (using
+    * `joint_handles_`) or to call the method `setEfforts()`.
+    */
+  virtual void computeEfforts() = 0;
+
+  /// Utility to set the commanded efforts.
+  /** It assumes that the input vector is ordered as in `joint_names_`.
+    */
+  inline void setEfforts(const std::vector<double>& effort_command) {
+    for(unsigned int i=0; i<joint_names_.size(); i++)
+      joint_handles_[i].setCommand(effort_command[i]);
+  }
+
+  /// Joint names controlled by this object.
+  std::vector<std::string> joint_names_;
+
+  /// Stores the command evaluated by the sub-controller.
+  std::vector<double> sub_command_;
+
+  /// Handles associated to the controlled joints.
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+
+private:
+  class FakeHW; // forward declaration
+  /// Used to create runtime instances of the controller.
+  pluginlib::ClassLoader<controller_interface::ControllerBase> controller_loader_;
+  /// Sub-controller instance.
+  std::unique_ptr<controller_interface::ControllerBase> controller_;
+  /// Simplified replica of the available hardware, to be given to the sub-controller.
+  std::unique_ptr<FakeHW> fake_hw_;
+
+  /// A fake hardware used to excange information with a "sub-controller".
+  class FakeHW : public hardware_interface::RobotHW {
+    public:
+      /// Generates a hardware that contains state and effort handles for a set of joints.
+      FakeHW(const std::vector<std::string>& joint_names);
+      /// Update the internal state of the joints from the information given in `joint_handles`
+      void copyState(const std::vector<hardware_interface::JointHandle> &joint_handles);
+      /// Retrieve the command stored in `cmd_`.
+      void getEfforts(std::vector<double> &joint_effort_command);
+    private:
+      std::vector<double> pos_; ///< Joint positions.
+      std::vector<double> vel_; ///< Joint velocities.
+      std::vector<double> eff_; ///< Joint efforts.
+      std::vector<double> cmd_; ///< Commanded efforts.
+      hardware_interface::JointStateInterface state_interface_; ///< Provides joint state handles.
+      hardware_interface::EffortJointInterface effort_interface_; ///< Provides command handles.
+  };
+
+};
+
+}
+
+#endif