diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt
new file mode 100644
index 000000000..32183988a
--- /dev/null
+++ b/dynamics_controllers/CMakeLists.txt
@@ -0,0 +1,52 @@
+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
+ controller_manager
+ kdl_parser
+ pluginlib
+)
+
+# Declare catkin package
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ DEPENDS
+ orocos_kdl
+ CATKIN_DEPENDS
+ controller_interface
+ kdl_parser
+ pluginlib
+)
+
+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}
+)
diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md
new file mode 100644
index 000000000..a263ce6d9
--- /dev/null
+++ b/dynamics_controllers/README.md
@@ -0,0 +1,180 @@
+## 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
+"*sub-controllers*".
+The command from the sub-controllers - 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.
+
+
+
+### 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-controllers 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.
+
+
+
+### Resource Handling
+
+Since the dynamic model of a generic robot operates on multiple joints at the
+same time, sub-controllers must claim all joints exactly once.
+As an example, if a dynamic controller is loaded for a chain with three joints,
+*e.g.*, `joint1`, `joint2` and `joint3`, the sub-controllers must be loaded so
+that they handle all of them without conflicts.
+For instance, a valid option is to have a single sub-controller that claims all
+the joints.
+However, a single controller claiming only `joint1` and `joint2` would cause
+failure upon initialization (since `joint3` would remain unclaimed).
+On the other hand, having one sub-controller for `joint1` and `joint3` plus
+another controller handling `joint2` would be legit.
+Note that if the second controller in this last example was also managing
+`joint1`, then initialization would fail since both the first and second
+controllers try to claim the same resource.
+Finally, failure will also happen if any sub-controller tries to claim any
+joint not being part of the chain/tree (such as `joint4` in the example).
+
+
+
+### 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
+joint1_position_controller:
+ type: effort_controllers/JointPositionController
+ joint: joint1
+ pid: {p: 10.0, i: 0.0, d: 5.0}
+
+joint2_position_controller:
+ type: effort_controllers/JointPositionController
+ joint: 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 sub-controller is basically the set of controllers you had before!
+ joint1:
+ type: effort_controllers/JointPositionController
+ joint: joint1
+ pid: {p: 10.0, i: 0.0, d: 5.0}
+ joint2:
+ type: effort_controllers/JointPositionController
+ joint: 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 Configuration
+
+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 set of controllers
+ of type compatible with `EffortJointInterface`. It cen either be a single
+ "unnamed" controller directly living in the `sub_controller` namespace or a
+ set of "named" controllers.
+- `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 set of controllers
+ of type compatible with `EffortJointInterface`. It cen either be a single
+ "unnamed" controller directly living in the `sub_controller` namespace or a
+ set of "named" controllers.
+- `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
+
+The following is a list of "limitations" in the sense that if you need one or
+more of the following features, you will likely have to write new controllers
+by yourself or to find proper workarounds.
+
+- 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-controllers 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.
+ Note that some of these parameters are already inside KDL,
+ even though they are still not used in many solvers (eg, the damping).
+- [x] Allow multiple sub-controllers to manage different joints.
diff --git a/dynamics_controllers/dynamics_controllers.xml b/dynamics_controllers/dynamics_controllers.xml
new file mode 100644
index 000000000..c011d261c
--- /dev/null
+++ b/dynamics_controllers/dynamics_controllers.xml
@@ -0,0 +1,23 @@
+
+
+
+
+ Computes the efforts based on the Inverse Dynamics of a kinematic chain.
+ Wraps around controllers compatible with an EffortJointInterface.
+
+
+
+
+
+ Computes the efforts based on the Inverse Dynamics of a kinematic tree.
+ Wraps around controllers compatible with an EffortJointInterface.
+
+
+
+
diff --git a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h
new file mode 100644
index 000000000..cefd38cfd
--- /dev/null
+++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h
@@ -0,0 +1,162 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Franco Fusco
+ * 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 - franco.fusco@ls2n.fr
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+
+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 {
+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& effort_command) {
+ for(unsigned int i=0; i joint_names_;
+
+ /// Stores the command evaluated by the sub-controller.
+ std::vector sub_command_;
+
+ /// Handles associated to the controlled joints.
+ std::vector joint_handles_;
+
+private:
+ class FakeHW; // forward declaration
+ /// Used to create runtime instances of the controller.
+ controller_manager::ControllerLoaderInterfaceSharedPtr controller_loader_;
+ /// Sub-controllers instances.
+ std::map sub_controllers_;
+ /// Simplified replica of the available hardware, to be given to the sub-controllers.
+ std::unique_ptr fake_hw_;
+
+ // Handling control limits
+ std::map has_effort_limits_; ///< Tells whether a given joint has effort limits or not.
+ std::map effort_limits_; ///< Effort limit of each joint.
+
+ /// Allows to get a map of controllers names (key) with corresponding types (values).
+ static bool getSubControllersMap(ros::NodeHandle&, std::map&);
+
+ /// Enforce joint limits (currently, simply saturate the efforts if needed).
+ void enforceLimits();
+
+ /// A fake hardware used to exchange 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& joint_names);
+ /// Update the internal state of the joints from the information given in `joint_handles`
+ void copyState(const std::vector &joint_handles);
+ /// Retrieve the command stored in `cmd_`.
+ void getEfforts(std::vector &joint_effort_command);
+ private:
+ std::vector pos_; ///< Joint positions.
+ std::vector vel_; ///< Joint velocities.
+ std::vector eff_; ///< Joint efforts.
+ std::vector cmd_; ///< Commanded efforts.
+ hardware_interface::JointStateInterface state_interface_; ///< Provides joint state handles.
+ hardware_interface::EffortJointInterface effort_interface_; ///< Provides command handles.
+ };
+
+};
+
+}
diff --git a/dynamics_controllers/include/dynamics_controllers/kdl_chain_controller.h b/dynamics_controllers/include/dynamics_controllers/kdl_chain_controller.h
new file mode 100644
index 000000000..b58f9863a
--- /dev/null
+++ b/dynamics_controllers/include/dynamics_controllers/kdl_chain_controller.h
@@ -0,0 +1,75 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Franco Fusco
+ * 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 - franco.fusco@ls2n.fr
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+
+namespace dynamics_controllers {
+
+class KdlChainController : public DynamicsControllerBase {
+public:
+ KdlChainController();
+
+protected:
+ /// Loads the chain and the dynamics solver.
+ bool initDynamics(ros::NodeHandle& nh) override;
+ /// IDM of a serial chain.
+ void computeEfforts() override;
+
+ /// If true, just add gravity efforts; if false, do a "full CTC" control.
+ bool gravity_compensation_only_;
+ /// Serial chain, storing the dynamics parameters.
+ KDL::Chain chain_;
+ /// Dynamics solver to compute efforts from the position, velocity and acceleration.
+ std::unique_ptr rne_;
+ /// Configuration of the manipulator (position, velocity and acceleration).
+ KDL::JntArrayAcc cfg_;
+ /// Vector of external efforts, passed to the solver.
+ KDL::Wrenches wrenches_;
+ /// Computed efforts.
+ KDL::JntArray efforts_;
+
+
+};
+
+}
diff --git a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h
new file mode 100644
index 000000000..878f3b2ab
--- /dev/null
+++ b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h
@@ -0,0 +1,93 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Franco Fusco
+ * 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 - franco.fusco@ls2n.fr
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+// #include
+/* NOTE: the include above was removed and a forward declaration has been added
+ below. This is because currently the solver is available only in the
+ online repo and not in Melodic's version of KDL.
+*/
+
+// forward declaration, will stay here until the solver is officially
+// released in Melodic. See the note above.
+namespace KDL {
+class TreeIdSolver_RNE;
+typedef std::map WrenchMap;
+}
+
+namespace dynamics_controllers {
+
+namespace internal {
+/* The "sub-tree extraction" functionality was proposed in:
+ * https://github.com/orocos/orocos_kinematics_dynamics/pull/174
+ * However, the PR is not available yet. As a workaround, a similar function
+ * is given here. If the PR is merged, the following will be removed.
+ */
+bool extractSubTree(const KDL::Tree& tree, const std::string& root_name, KDL::Tree& subtree);
+}
+
+class KdlTreeController : public DynamicsControllerBase {
+public:
+ KdlTreeController();
+
+protected:
+ /// Loads the tree and the dynamics solver.
+ bool initDynamics(ros::NodeHandle& nh) override;
+ /// IDM of a tree.
+ void computeEfforts() override;
+
+ /// If true, just add gravity efforts; if false, do a "full CTC" control.
+ bool gravity_compensation_only_;
+ /// Tree structure, storing the dynamics parameters.
+ KDL::Tree tree_;
+ /// Dynamics solver to compute efforts from the position, velocity and acceleration.
+ std::unique_ptr rne_;
+ /// Configuration of the manipulator (position, velocity and acceleration).
+ KDL::JntArrayAcc cfg_;
+ /// Vector of external efforts, passed to the solver.
+ KDL::WrenchMap wrenches_;
+ /// Computed efforts.
+ KDL::JntArray efforts_;
+};
+
+}
diff --git a/dynamics_controllers/package.xml b/dynamics_controllers/package.xml
new file mode 100644
index 000000000..c7d0c4bd6
--- /dev/null
+++ b/dynamics_controllers/package.xml
@@ -0,0 +1,23 @@
+
+
+ dynamics_controllers
+ 0.0.0
+ Adds a "dynamics control layer" to standard effort controllers.
+
+ Franco FUSCO
+ Franco FUSCO
+ Bence Magyar
+ BSD
+
+ catkin
+
+ controller_interface
+ controller_manager
+ pluginlib
+ orocos_kdl
+ kdl_parser
+
+
+
+
+
diff --git a/dynamics_controllers/src/dynamics_controller_base.cpp b/dynamics_controllers/src/dynamics_controller_base.cpp
new file mode 100644
index 000000000..de0871e7a
--- /dev/null
+++ b/dynamics_controllers/src/dynamics_controller_base.cpp
@@ -0,0 +1,426 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Franco Fusco
+ * 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 - franco.fusco@ls2n.fr
+ */
+
+#include
+#include
+#include
+
+
+namespace dynamics_controllers {
+
+
+DynamicsControllerBase::FakeHW::FakeHW(const std::vector& joint_names)
+: pos_(joint_names.size()),
+ vel_(joint_names.size()),
+ eff_(joint_names.size()),
+ cmd_(joint_names.size())
+{
+ // create a fake HW to be passed to a "sub-controller"
+ for(unsigned int j=0; j& joint_handles) {
+ for(unsigned int j=0; j& joint_effort_command) {
+ for(unsigned int j=0; j& controllers_map
+)
+{
+ // this is used to get the list of available controllers
+ XmlRpc::XmlRpcValue controllers;
+ if(!nh.getParam(nh.getNamespace(), controllers)) {
+ ROS_ERROR("Failed to get list of sub-controllers from '%s'",
+ nh.getNamespace().c_str()
+ );
+ return false;
+ }
+
+ if(controllers.size() == 0) {
+ ROS_ERROR("Sub-controllers namespace '%s' does not contain any parameter",
+ nh.getNamespace().c_str()
+ );
+ return false;
+ }
+
+ try {
+ std::string type;
+ for(const auto& c : controllers) {
+ if(!nh.getParam(c.first + "/type", type))
+ ROS_WARN_STREAM("Failed to get type of sub-controller " << c.first);
+ else
+ controllers_map[c.first] = type;
+ }
+ }
+ catch(const XmlRpc::XmlRpcException& ex) {
+ ROS_ERROR("Caught XmlRpc exception while loading sub-controllers map: %s",
+ ex.getMessage().c_str()
+ );
+ return false;
+ }
+
+ return true;
+}
+
+
+
+
+
+DynamicsControllerBase::DynamicsControllerBase()
+ : controller_loader_(new controller_manager::ControllerLoader("controller_interface", "controller_interface::ControllerBase"))
+{
+}
+
+
+bool DynamicsControllerBase::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle& nh) {
+ ROS_DEBUG("Initializing DynamicsControllerBase.");
+
+ // reset sub-controllers
+ sub_controllers_.clear();
+
+ // sub-class specific initialization
+ if(!initDynamics(nh)) {
+ ROS_ERROR("Failed to initialize dynamics (initDynamics() returned false)");
+ return false;
+ }
+
+ // check if the sub-class properly set the joint names
+ if(joint_names_.size() == 0) {
+ ROS_ERROR("After dynamics initialization, no joint names are available to "
+ "build the FakeHW instance"
+ );
+ return false;
+ }
+
+ // Parse the URDF (to get limits)
+ urdf::Model urdf;
+ if (!urdf.initParamWithNodeHandle("robot_description", nh)) {
+ ROS_ERROR("Failed to parse urdf file to obtain effort limits");
+ return false;
+ }
+
+ urdf::JointConstSharedPtr joint_urdf;
+ for(const auto& joint : joint_names_) {
+ // give a warning if the joint is not in the URDF (although this should
+ // never happen) and proceed to the next joint
+ joint_urdf = urdf.getJoint(joint);
+ if (!joint_urdf) {
+ ROS_WARN("Could not find joint '%s' in urdf", joint.c_str());
+ continue;
+ }
+
+ // check if the joint has effort limits
+ has_effort_limits_[joint] = (joint_urdf->limits != nullptr);
+ if(has_effort_limits_[joint]) {
+ auto lim = joint_urdf->limits->effort;
+ if(lim > 0)
+ effort_limits_[joint] = lim;
+ else {
+ ROS_WARN("Joint '%s' has non-positive effort limit %f. It will be "
+ "ignored", joint.c_str(), lim
+ );
+ has_effort_limits_[joint] = false;
+ }
+ }
+ }
+
+ // retrieve joint handles; will throw an exception on failure (gracefully handled by ControllerManager)
+ sub_command_.resize(joint_names_.size());
+ joint_handles_.clear();
+ joint_handles_.reserve(joint_names_.size());
+ for(const auto& j : joint_names_)
+ joint_handles_.push_back(hw->getHandle(j));
+
+ ROS_DEBUG("Generating FakeHW");
+ fake_hw_.reset( new FakeHW(joint_names_) );
+
+ std::string controller_ns("sub_controller");
+ if(!nh.hasParam(controller_ns)) {
+ ROS_ERROR("Cannot find sub-namespace '%s' (required to instanciate the "
+ "acceleration sub-controller)", controller_ns.c_str());
+ return false;
+ }
+
+ ros::NodeHandle cnh(nh, controller_ns);
+ std::string ctype;
+ std::map controllers_map; // name --> type
+ if(cnh.getParam("type", ctype)) {
+ // single (unnamed) sub-controller
+ controllers_map[""] = ctype;
+ }
+ else {
+ // try load multiple controllers from the given namespace
+ if(!getSubControllersMap(cnh, controllers_map))
+ return false;
+ else if(controllers_map.empty()){
+ // if no controller has been found, just exit
+ ROS_ERROR("No controller has been succesfully located in '%s'",
+ cnh.getNamespace().c_str()
+ );
+ }
+ }
+
+ // load all controllers
+ try {
+ for(const auto& c : controllers_map) {
+ ROS_DEBUG("Creating sub-controller instance of type %s", c.second.c_str());
+ auto cptr = controller_loader_->createInstance(c.second);
+ if(cptr == nullptr) {
+ std::stringstream ss;
+ ss << "Failed to create sub-controller";
+ if(c.first != "")
+ ss << " '" << c.first << "'";
+ ss << " of type '" << c.second << "'; available controller types are:";
+ auto classes = controller_loader_->getDeclaredClasses();
+ std::for_each(classes.begin(), classes.end(), [&](const std::string& n) {ss << " " << n << ";";});
+ throw std::runtime_error(ss.str());
+ }
+ sub_controllers_[c.first] = cptr;
+ }
+ }
+ catch(std::exception& ex) {
+ ROS_ERROR("Exception while creating sub-controllers. What: %s", ex.what());
+ return false;
+ }
+
+ // map whose keys are joints (to be claimed) and values are controllers
+ // (claiming the joints) - this is used to check resource acquisition
+ std::map claimed_resources;
+
+ // initialize all sub-controllers
+ try {
+ for(auto& c : sub_controllers_) {
+ ROS_DEBUG("Initializing sub-controller %s", c.first.c_str());
+ ClaimedResources resources;
+ bool init_ok = false;
+ // init the controller - the namespace definition changes if this is the
+ // "unnamed" controller (which directly lives in 'sub_controller')
+ if(c.first == "")
+ init_ok = c.second->initRequest(fake_hw_.get(), nh, cnh, resources);
+ else {
+ ros::NodeHandle _cnh(cnh, c.first);
+ init_ok = c.second->initRequest(fake_hw_.get(), nh, _cnh, resources);
+ }
+
+ // if controller init request fails, give an error and exit
+ if(!init_ok) {
+ std::stringstream ss;
+ ss << "Init request failed for sub-controller";
+ if(c.first != "")
+ ss << " " << c.first;
+ throw std::runtime_error(ss.str());
+ }
+
+ // check that resources were claimed from a single interface (this should
+ // always be true since the controller has only access to the fake hw)
+ if(resources.size() != 1) {
+ std::stringstream ss;
+ ss << "Sub-controller ";
+ if(c.first != "")
+ ss << c.first << " ";
+ ss << "claimed resources from multiple hardware interfaces, which"
+ " might lead to problems. Resources were claimed from the"
+ " following interfaces:";
+ for(const auto& r : resources)
+ ss << " " << r.hardware_interface;
+ ROS_WARN_STREAM(ss.str());
+ }
+
+ // The hardware interface should be the EffortJointInterface (again, this
+ // should be always true as this is the only interface exposed by FakeHW)
+ if(resources[0].hardware_interface != "hardware_interface::EffortJointInterface") {
+ std::stringstream ss;
+ ss << "First hardware interface claimed by the sub-controller ";
+ if(c.first != "")
+ ss << c.first << " ";
+ ss << "is " << resources[0].hardware_interface << ", but "
+ "hardware_interface::EffortJointInterface was expected. This might "
+ "lead to undefined behavior...";
+ ROS_WARN_STREAM(ss.str());
+ }
+
+ // check that no resource claimed by this controller is used by any other controller
+ for(const auto& joint : resources[0].resources) {
+ auto it = claimed_resources.find(joint);
+ if(it != claimed_resources.end()) {
+ throw std::runtime_error("Joint " + joint + " cannot be handled both"
+ " by " + it->second + " and " + c.first);
+ }
+ claimed_resources[joint] = c.first;
+ }
+ }
+ }
+ catch(std::exception& ex) {
+ ROS_ERROR("Exception while initializing sub-controllers. What: %s", ex.what());
+ return false;
+ }
+
+ // check that all joints used in the dynamic model have been claimed by
+ // subcontrollers as well
+ for(const auto& joint : joint_names_) {
+ auto it = claimed_resources.find(joint);
+ if(it == claimed_resources.end()) {
+ ROS_ERROR("No sub-controller claimed joint %s. Note that this controller"
+ " expects the sub-controller(s) to claim the all the joints used by"
+ " the dyanmic model", joint.c_str()
+ );
+ return false;
+ }
+ }
+
+ return true;
+}
+
+
+void DynamicsControllerBase::starting(const ros::Time& time) {
+ // forward the start-request to the low-level controllers
+ bool ok = true;
+ std::stringstream ss;
+ for(auto& c : sub_controllers_) {
+ if(!c.second->startRequest(time)) {
+ ROS_ERROR("Failed start low-level sub-controller %s", c.first.c_str());
+ ok = false;
+ ss << " " << c.first;
+ }
+ }
+
+ // if the request failed for any controller, stop all of them
+ if(!ok) {
+ for(auto& c : sub_controllers_) {
+ c.second->stopRequest(time);
+ }
+ throw std::runtime_error("Could not start sub-controllers" + ss.str());
+ }
+}
+
+
+void DynamicsControllerBase::update(const ros::Time& time, const ros::Duration& period) {
+ // update the sub-controllers
+ fake_hw_->copyState(joint_handles_);
+ for(auto& c : sub_controllers_)
+ c.second->update(time, period);
+ fake_hw_->getEfforts(sub_command_);
+
+ // perform sub-class specific dynamics
+ try {
+ computeEfforts();
+ enforceLimits();
+ }
+ catch(std::exception& e) {
+ ROS_ERROR("Caught exception while computing efforts: %s", e.what());
+ throw e;
+ }
+ catch(...) {
+ ROS_ERROR("Caught exception while computing efforts");
+ throw;
+ }
+}
+
+
+void DynamicsControllerBase::stopping(const ros::Time& time) {
+ // forward the stop-request to the low-level controllers
+ bool ok = true;
+ std::stringstream ss;
+ for(auto& c : sub_controllers_) {
+ if(!c.second->stopRequest(time)) {
+ ROS_ERROR("Failed stop sub-controller %s", c.first.c_str());
+ ok = false;
+ ss << " " << c.first;
+ }
+ }
+
+ // if the request failed for any controller, stop all of them
+ if(!ok) {
+ throw std::runtime_error("Could not stop sub-controllers" + ss.str());
+ }
+}
+
+
+void DynamicsControllerBase::enforceLimits() {
+ for(unsigned int i=0; i lim)
+ joint_handles_[i].setCommand(lim);
+ else if(cmd < -lim)
+ joint_handles_[i].setCommand(-lim);
+ }
+}
+
+}
diff --git a/dynamics_controllers/src/kdl/treeidsolver.hpp b/dynamics_controllers/src/kdl/treeidsolver.hpp
new file mode 100644
index 000000000..2c2c6bf4b
--- /dev/null
+++ b/dynamics_controllers/src/kdl/treeidsolver.hpp
@@ -0,0 +1,62 @@
+// Copyright (C) 2009 Ruben Smits
+
+// Version: 1.0
+// Author: Franco Fusco
+// Maintainer: Ruben Smits
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+#ifndef KDL_TREE_IDSOLVER_HPP
+#define KDL_TREE_IDSOLVER_HPP
+
+#include
+#include
+#include
+#include
+
+namespace KDL
+{
+
+ typedef std::map WrenchMap;
+
+ /**
+ * \brief This abstract class encapsulates the inverse
+ * dynamics solver for a KDL::Tree.
+ *
+ */
+ class TreeIdSolver : public KDL::SolverI
+ {
+ public:
+ /**
+ * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces
+ * to joint torques/forces.
+ *
+ * @param q input joint positions
+ * @param q_dot input joint velocities
+ * @param q_dotdot input joint accelerations
+ * @param f_ext the external forces (no gravity) on the segments
+ * @param torque output joint torques
+ *
+ * @return if < 0 something went wrong
+ */
+ virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext,JntArray &torques)=0;
+
+ // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation.
+ };
+
+}
+
+#endif
diff --git a/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.cpp b/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.cpp
new file mode 100644
index 000000000..a035a07ae
--- /dev/null
+++ b/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.cpp
@@ -0,0 +1,140 @@
+// Copyright (C) 2009 Ruben Smits
+
+// Version: 1.0
+// Author: Franco Fusco
+// Maintainer: Ruben Smits
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+#include "treeidsolver_recursive_newton_euler.hpp"
+#include
+#include
+
+namespace KDL {
+
+ TreeIdSolver_RNE::TreeIdSolver_RNE(const Tree& tree_, Vector grav):
+ tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments())
+ {
+ ag=-Twist(grav,Vector::Zero());
+ initAuxVariables();
+ }
+
+ void TreeIdSolver_RNE::updateInternalDataStructures() {
+ nj = tree.getNrOfJoints();
+ ns = tree.getNrOfSegments();
+ initAuxVariables();
+ }
+
+ void TreeIdSolver_RNE::initAuxVariables() {
+ const SegmentMap& segments = tree.getSegments();
+ for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) {
+ X[seg->first] = Frame();
+ S[seg->first] = Twist();
+ v[seg->first] = Twist();
+ a[seg->first] = Twist();
+ f[seg->first] = Wrench();
+ }
+ }
+
+ int TreeIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques)
+ {
+ //Check that the tree was not modified externally
+ if(nj != tree.getNrOfJoints() || ns != tree.getNrOfSegments())
+ return (error = E_NOT_UP_TO_DATE);
+
+ //Check sizes of joint vectors
+ if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj)
+ return (error = E_SIZE_MISMATCH);
+
+ try {
+ //Do the recursion here
+ rne_step(tree.getRootSegment(), q, q_dot, q_dotdot, f_ext, torques);
+ }
+ catch(const std::out_of_range&) {
+ //If in rne_step we get an out_of_range exception it means that some call
+ //to map::at failed. This can happen only if updateInternalDataStructures
+ //was not called after changing something in the tree. Note that it
+ //should be impossible to reach this point as consistency of the tree is
+ //checked above.
+ return (error = E_NOT_UP_TO_DATE);
+ }
+ return (error = E_NOERROR);
+ }
+
+
+ void TreeIdSolver_RNE::rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques) {
+ const Segment& seg = GetTreeElementSegment(segment->second);
+ const std::string& segname = segment->first;
+ const std::string& parname = GetTreeElementParent(segment->second)->first;
+
+ //Do forward calculations involving velocity & acceleration of this segment
+ double q_, qdot_, qdotdot_;
+ unsigned int j = GetTreeElementQNr(segment->second);
+ if(seg.getJoint().getType()!=Joint::None){
+ q_ = q(j);
+ qdot_ = q_dot(j);
+ qdotdot_ = q_dotdot(j);
+ }
+ else
+ q_ = qdot_ = qdotdot_ = 0.0;
+
+ //Calculate segment properties: X,S,vj,cj
+
+ //Remark this is the inverse of the frame for transformations from the parent to the current coord frame
+ X.at(segname) = seg.pose(q_);
+
+ //Transform velocity and unit velocity to segment frame
+ Twist vj = X.at(segname).M.Inverse( seg.twist(q_,qdot_) );
+ S.at(segname) = X.at(segname).M.Inverse( seg.twist(q_,1.0) );
+
+ //calculate velocity and acceleration of the segment (in segment coordinates)
+ if(segment == tree.getRootSegment()) {
+ v.at(segname) = vj;
+ a.at(segname) = X.at(segname).Inverse(ag) + S.at(segname)*qdotdot_+ v.at(segname)*vj;
+ }
+ else {
+ v.at(segname) = X.at(segname).Inverse(v.at(parname)) + vj;
+ a.at(segname) = X.at(segname).Inverse(a.at(parname)) + S.at(segname)*qdotdot_ + v.at(segname)*vj;
+ }
+
+ //Calculate the force for the joint
+ //Collect RigidBodyInertia and external forces
+ const RigidBodyInertia& I = seg.getInertia();
+ f.at(segname) = I*a.at(segname) + v.at(segname)*(I*v.at(segname));
+ if(f_ext.find(segname) != f_ext.end())
+ f.at(segname) = f.at(segname) - f_ext.at(segname);
+
+ //propagate calculations over each child segment
+ SegmentMap::const_iterator child;
+ for (unsigned int i = 0; i < GetTreeElementChildren(segment->second).size(); i++) {
+ child = GetTreeElementChildren(segment->second)[i];
+ rne_step(child, q, q_dot, q_dotdot, f_ext, torques);
+ }
+
+ //do backward calculations involving wrenches and joint efforts
+
+ //If there is a moving joint, evaluate its effort
+ if(seg.getJoint().getType()!=Joint::None){
+ torques(j) = dot(S.at(segname), f.at(segname));
+ // NOTE: in the current Melodic version of KDL, getInertia() is not available
+ // torques(j) += seg.getJoint().getInertia()*q_dotdot(j); // add torque from joint inertia
+ }
+
+ //add reaction forces to parent segment
+ if(segment != tree.getRootSegment())
+ f.at(parname) = f.at(parname) + X.at(segname)*f.at(segname);
+ }
+}//namespace
diff --git a/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.hpp b/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.hpp
new file mode 100644
index 000000000..95af77a01
--- /dev/null
+++ b/dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.hpp
@@ -0,0 +1,84 @@
+// Copyright (C) 2009 Ruben Smits
+
+// Version: 1.0
+// Author: Franco Fusco
+// Maintainer: Ruben Smits
+// URL: http://www.orocos.org/kdl
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 2.1 of the License, or (at your option) any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser General Public
+// License along with this library; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
+
+#include "treeidsolver.hpp"
+
+namespace KDL{
+ /**
+ * \brief Recursive newton euler inverse dynamics solver for kinematic trees.
+ *
+ * It calculates the torques for the joints, given the motion of
+ * the joints (q,qdot,qdotdot), external forces on the segments
+ * (expressed in the segments reference frame) and the dynamical
+ * parameters of the segments.
+ *
+ * This is an extension of the inverse dynamic solver for kinematic chains,
+ * \see ChainIdSolver_RNE. The main difference is the use of STL maps
+ * instead of vectors to represent external wrenches (as well as internal
+ * variables exploited during the recursion).
+ */
+ class TreeIdSolver_RNE : public TreeIdSolver {
+ public:
+ /**
+ * Constructor for the solver, it will allocate all the necessary memory
+ * \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored.
+ * \param grav The gravity vector to use during the calculation.
+ */
+ TreeIdSolver_RNE(const Tree& tree, Vector grav);
+
+ /**
+ * Function to calculate from Cartesian forces to joint torques.
+ * Input parameters;
+ * \param q The current joint positions
+ * \param q_dot The current joint velocities
+ * \param q_dotdot The current joint accelerations
+ * \param f_ext The external forces (no gravity) on the segments
+ * Output parameters:
+ * \param torques the resulting torques for the joints
+ */
+ int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques);
+
+ /// @copydoc KDL::SolverI::updateInternalDataStructures
+ virtual void updateInternalDataStructures();
+
+ private:
+ ///Helper function to initialize private members X, S, v, a, f
+ void initAuxVariables();
+
+ ///One recursion step
+ void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques);
+
+ const Tree& tree;
+ unsigned int nj;
+ unsigned int ns;
+ std::map X;
+ std::map S;
+ std::map v;
+ std::map a;
+ std::map f;
+ Twist ag;
+ };
+}
+
+#endif
diff --git a/dynamics_controllers/src/kdl_chain_controller.cpp b/dynamics_controllers/src/kdl_chain_controller.cpp
new file mode 100644
index 000000000..6f651dbd0
--- /dev/null
+++ b/dynamics_controllers/src/kdl_chain_controller.cpp
@@ -0,0 +1,273 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Franco Fusco
+ * 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 - franco.fusco@ls2n.fr
+ */
+
+#include
+#include
+#include
+#include
+
+
+namespace dynamics_controllers {
+
+KdlChainController::KdlChainController()
+: DynamicsControllerBase()
+{
+}
+
+
+bool KdlChainController::initDynamics(ros::NodeHandle& nh) {
+ // Look for the parameter robot_description
+ std::string robot_description_param;
+ if(!nh.searchParam("robot_description", robot_description_param)) {
+ ROS_ERROR("Could not locate 'robot_description' in the parameter server ("
+ "search started from namespace '%s')", nh.getNamespace().c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("Parameter 'robot_description' found in %s", robot_description_param.c_str());
+
+ // Get the URDF
+ std::string robot_description_xml;
+ if(!nh.getParam(robot_description_param, robot_description_xml)) {
+ ROS_ERROR("Failed to retrieve 'robot_description' from the parameter '%s'",
+ robot_description_param.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("URDF retrieved succesfully");
+
+ KDL::Tree tree;
+ if(!kdl_parser::treeFromString(robot_description_xml, tree))
+ {
+ ROS_ERROR("Failed to build KDL::Tree object from URDF");
+ return false;
+ }
+ ROS_DEBUG("KDL parser: succesfully converted URDF into Tree");
+
+ // Look for the base and tip of the chain. By using "searchParam", it is
+ // possible to write these two parameters just once in the "common"
+ // controllers namespace.
+ std::string base, tip, base_param, tip_param;
+
+ if(!nh.searchParam("chain_base", base_param)) {
+ base = tree.getRootSegment()->first;
+ ROS_INFO("Could not locate 'chain_base' in the parameter server (search "
+ "started from namespace '%s'). Assuming you want to start from the root "
+ "'%s' of the kinematic tree.", nh.getNamespace().c_str(), base.c_str()
+ );
+ }
+ else if(!nh.getParam(base_param, base)) {
+ ROS_ERROR("Failed to retrieve 'chain_base' from parameter '%s'",
+ base_param.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("'chain_base' is: '%s'", base.c_str());
+
+ bool look_for_tip = false;
+ if(!nh.searchParam("chain_tip", tip_param)) {
+ ROS_INFO("Could not locate 'chain_tip' in the parameter server (search "
+ "started from namespace '%s'). Assuming that there is a single branch "
+ "rooted at '%s' and that you want to reach the unique leaf.",
+ nh.getNamespace().c_str(), base.c_str()
+ );
+ look_for_tip = true;
+ }
+ else if(!nh.getParam(tip_param, tip)) {
+ ROS_ERROR("Failed to retrieve 'chain_tip' from parameter '%s'",
+ tip_param.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("'chain_tip' is: '%s'", tip.c_str());
+
+ if(look_for_tip) {
+ // check if there is a single branch roted at the given base and get the
+ // corresponding tip
+ ROS_INFO("Looking for the leaf segment of the chain rooted at '%s'", base.c_str());
+
+ // check if the base is a valid starting point
+ auto segment = tree.getSegment(base);
+ if(segment == tree.getSegments().end()) {
+ ROS_ERROR("Base segment '%s' not found in the tree.", base.c_str());
+ return false;
+ }
+
+ // traverse the branch looking for unique children until the leaf is reached
+ while(GetTreeElementChildren(segment->second).size() > 0) {
+ const auto& children = GetTreeElementChildren(segment->second);
+ if(children.size() > 1) {
+ // multiple children found: this is not a chain!
+ const auto& segname = segment->first;
+ ROS_ERROR("The branch rooted at '%s' is not a chain: found child "
+ "segment '%s' with %lu children", base.c_str(), segname.c_str(),
+ children.size()
+ );
+ return false;
+ }
+ // there is a unique child: keep searching
+ segment = children[0];
+ }
+
+ // we got to the end of the chain
+ tip = segment->first;
+ ROS_INFO("Reached leaf segment '%s'", tip.c_str());
+ }
+
+ std::string grav_param;
+ std::vector grav(3, 0.);
+
+ if(!nh.searchParam("gravity", grav_param)) {
+ ROS_WARN("Could not locate 'gravity' in the parameter server (search "
+ "started from namespace '%s')\n** GRAVITY WILL BE SET TO ZERO **",
+ nh.getNamespace().c_str()
+ );
+ }
+ else if(!nh.getParam(grav_param, grav))
+ {
+ ROS_WARN("Failed to retrieve 'gravity' from parameter '%s' (maybe it is "
+ "not a list?)\n** GRAVITY WILL BE SET TO ZERO **", grav_param.c_str()
+ );
+ }
+
+ if(grav.size() != 3)
+ {
+ ROS_ERROR("Parameter 'gravity' found, but it has %lu values (3 expected)", grav.size());
+ return false;
+ }
+
+ KDL::Vector gravity;
+ gravity(0) = grav[0];
+ gravity(1) = grav[1];
+ gravity(2) = grav[2];
+ if(gravity.Norm() < 1e-6) {
+ ROS_INFO("Gravity compensation is OFF (due to zero gravity vector)");
+ }
+ else if(std::fabs(gravity.Norm()-9.806) > 0.1) {
+ ROS_WARN("Gravity vector on Earth is usually around 9.806 m/s^2 in norm. "
+ "Declared gravity has norm %f; if this value is correct, just ignore "
+ "this warning. Otherwise, check your setup to prevent damages.",
+ gravity.Norm()
+ );
+ }
+ ROS_DEBUG_STREAM("Gravity is set to " << gravity << " with respect to " << base);
+
+ // Should we do only gravity compensation?
+ nh.param("gravity_compensation_only", gravity_compensation_only_, false);
+ if(gravity_compensation_only_) {
+ if(gravity.Norm() < 1e-6) {
+ ROS_WARN("Requested to perform gravity compensation only, but the "
+ "gravity vector has zero-norm. Are you sure you properly configured "
+ "the controller?"
+ );
+ }
+ else {
+ ROS_INFO("Performing gravity compensation only.");
+ }
+ }
+
+ if(!tree.getChain(base, tip, chain_)) {
+ ROS_ERROR("Failed to extract chain with '%s' as base and '%s' as tip",
+ base.c_str(), tip.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("Chain extracted succesfully");
+
+ if(chain_.getNrOfJoints() == 0) {
+ ROS_ERROR("Chain from '%s' to '%s' does not contain any moving joint, "
+ "the dynamic solver cannot be initialized", base.c_str(), tip.c_str()
+ );
+ return false;
+ }
+
+ // Initialize joint names
+ joint_names_.clear();
+ joint_names_.reserve(chain_.getNrOfJoints());
+ for(const KDL::Segment& segment : chain_.segments)
+ if(segment.getJoint().getType() != KDL::Joint::None)
+ joint_names_.push_back(segment.getJoint().getName());
+ std::stringstream ss;
+ std::ostream_iterator out_it(ss, " ");
+ std::copy ( joint_names_.begin(), joint_names_.end(), out_it );
+ ROS_DEBUG("Found %d joints in chain: %s", (int)joint_names_.size(), ss.str().c_str());
+
+ // Resize internal data structures
+ rne_.reset(new KDL::ChainIdSolver_RNE(chain_, gravity));
+ wrenches_ = KDL::Wrenches(chain_.getNrOfSegments());
+ cfg_ = KDL::JntArrayAcc(chain_.getNrOfJoints());
+ efforts_ = KDL::JntArray(chain_.getNrOfJoints());
+
+ return true;
+}
+
+
+void KdlChainController::computeEfforts() {
+ // copy the current configuration of the chain
+ for(unsigned int i=0; iCartToJnt(cfg_.q, cfg_.qdot, cfg_.qdotdot, wrenches_, efforts_) < 0) {
+ // Failed for some reason: throw an exception
+ std::stringstream ss;
+ ss << "Failed to compute efforts using the Dynamic Model. Solver error: ";
+ ss << rne_->strError(rne_->getError());
+ throw std::runtime_error(ss.str());
+ }
+
+ // send computed commands to the hardware
+ for(unsigned int i=0; i
+#include
+#include
+#include
+// NOTE: this will be removed once the solver becomes available in Melodic
+#include "kdl/treeidsolver_recursive_newton_euler.hpp"
+
+
+namespace dynamics_controllers {
+
+KdlTreeController::KdlTreeController()
+: DynamicsControllerBase()
+{
+}
+
+
+bool KdlTreeController::initDynamics(ros::NodeHandle& nh) {
+ // Look for the parameter robot_description
+ std::string robot_description_param;
+ if(!nh.searchParam("robot_description", robot_description_param)) {
+ ROS_ERROR("Could not locate 'robot_description' in the parameter server ("
+ "search started from namespace '%s')", nh.getNamespace().c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("Parameter 'robot_description' found in %s", robot_description_param.c_str());
+
+ // Get the URDF
+ std::string robot_description_xml;
+ if(!nh.getParam(robot_description_param, robot_description_xml)) {
+ ROS_ERROR("Failed to retrieve 'robot_description' from the parameter '%s'",
+ robot_description_param.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("URDF retrieved succesfully");
+
+ KDL::Tree tree;
+ if(!kdl_parser::treeFromString(robot_description_xml, tree))
+ {
+ ROS_ERROR("Failed to build KDL::Tree object from URDF");
+ return false;
+ }
+ ROS_DEBUG("KDL parser: succesfully converted URDF into Tree");
+
+ // Look for the base and tip of the chain. By using "searchParam", it is
+ // possible to write these two parameters just once in the "common"
+ // controllers namespace.
+ std::string root_param, root_name(tree.getRootSegment()->first);
+
+ if(!nh.searchParam("tree_root", root_param)) {
+ ROS_INFO("Could not locate 'tree_root' in the parameter server (search "
+ "started from namespace '%s'). Assuming you want to start from the root "
+ "'%s' of the kinematic tree.", nh.getNamespace().c_str(), root_name.c_str()
+ );
+ }
+ else if(!nh.getParam(root_param, root_name)) {
+ ROS_ERROR("Failed to retrieve 'tree_root' from parameter '%s'",
+ root_param.c_str()
+ );
+ return false;
+ }
+ ROS_DEBUG("'root_name' is: '%s'", root_name.c_str());
+
+ // extract the tree from a given root
+ if(!internal::extractSubTree(tree, root_name, tree_)) {
+ ROS_ERROR("Failed to extrac sub-tree from segment '%s'", root_name.c_str());
+ return false;
+ }
+
+ // get gravity vector
+ std::string grav_param;
+ std::vector grav(3, 0.);
+
+ if(!nh.searchParam("gravity", grav_param)) {
+ ROS_WARN("Could not locate 'gravity' in the parameter server (search "
+ "started from namespace '%s')\n** GRAVITY WILL BE SET TO ZERO **",
+ nh.getNamespace().c_str()
+ );
+ }
+ else if(!nh.getParam(grav_param, grav))
+ {
+ ROS_WARN("Failed to retrieve 'gravity' from parameter '%s' (maybe it is "
+ "not a list?)\n** GRAVITY WILL BE SET TO ZERO **", grav_param.c_str()
+ );
+ }
+
+ if(grav.size() != 3)
+ {
+ ROS_ERROR("Parameter 'gravity' found, but it has %lu values (3 expected)", grav.size());
+ return false;
+ }
+
+ KDL::Vector gravity;
+ gravity(0) = grav[0];
+ gravity(1) = grav[1];
+ gravity(2) = grav[2];
+ if(gravity.Norm() < 1e-6) {
+ ROS_INFO("Gravity compensation is OFF (due to zero gravity vector)");
+ }
+ else if(std::fabs(gravity.Norm()-9.806) > 0.1) {
+ ROS_WARN("Gravity vector on Earth is usually around 9.806 m/s^2 in norm. "
+ "Declared gravity has norm %f; if this value is correct, just ignore "
+ "this warning. Otherwise, check your setup to prevent damages.",
+ gravity.Norm()
+ );
+ }
+ ROS_DEBUG_STREAM("Gravity is set to " << gravity << " with respect to " << root_name);
+
+ // Should we do only gravity compensation?
+ nh.param("gravity_compensation_only", gravity_compensation_only_, false);
+ if(gravity_compensation_only_) {
+ if(gravity.Norm() < 1e-6) {
+ ROS_WARN("Requested to perform gravity compensation only, but the "
+ "gravity vector has zero-norm. Are you sure you properly configured "
+ "the controller?"
+ );
+ }
+ else {
+ ROS_INFO("Performing gravity compensation only.");
+ }
+ }
+
+ // Initialize joint names
+ joint_names_.clear();
+ joint_names_.resize(tree_.getNrOfJoints());
+ for(const auto& elem : tree_.getSegments()) {
+ const auto& segment = GetTreeElementSegment(elem.second);
+ if(segment.getJoint().getType() != KDL::Joint::None) {
+ const auto& idx = GetTreeElementQNr(elem.second);
+ joint_names_[idx] = segment.getJoint().getName();
+ }
+ }
+
+ std::stringstream ss;
+ std::ostream_iterator out_it(ss, " ");
+ std::copy ( joint_names_.begin(), joint_names_.end(), out_it );
+ ROS_DEBUG("Found %d joints in chain: %s", (int)joint_names_.size(), ss.str().c_str());
+
+ // Resize internal data structures
+ rne_.reset(new KDL::TreeIdSolver_RNE(tree_, gravity));
+ wrenches_.clear();
+ cfg_ = KDL::JntArrayAcc(tree_.getNrOfJoints());
+ efforts_ = KDL::JntArray(tree_.getNrOfJoints());
+
+ return true;
+}
+
+
+void KdlTreeController::computeEfforts() {
+ // copy the current configuration of the chain
+ for(unsigned int i=0; iCartToJnt(cfg_.q, cfg_.qdot, cfg_.qdotdot, wrenches_, efforts_) < 0) {
+ // Failed for some reason: throw an exception
+ std::stringstream ss;
+ ss << "Failed to compute efforts using the Dynamic Model. Solver error: ";
+ ss << rne_->strError(rne_->getError());
+ throw std::runtime_error(ss.str());
+ }
+
+ // send computed commands to the hardware
+ for(unsigned int i=0; isecond).size(); i++) {
+ child = GetTreeElementChildren(root->second)[i];
+ // try to add the child
+ if (tree.addSegment(GetTreeElementSegment(child->second), hook_name)) {
+ ROS_DEBUG_STREAM("--> Appended segment " << child->first << " to " << hook_name);
+ // if child is added, add all the child's children
+ if (!expandTreeRecursive(tree, child, child->first)) {
+ // if it didn't work, return false
+ return false;
+ }
+ }
+ else {
+ // if the child could not be added, return false
+ return false;
+ }
+ }
+ return true;
+}
+
+
+bool extractSubTree(const KDL::Tree& tree, const std::string& root_name, KDL::Tree& subtree)
+{
+ // check if root_name exists
+ KDL::SegmentMap::const_iterator root = tree.getSegment(root_name);
+ if (root == tree.getSegments().end())
+ return false;
+ // init the subtree, root_name is the new root.
+ subtree = KDL::Tree(root->first);
+ return expandTreeRecursive(subtree, root, root_name);
+}
+
+} // end of namespace internal
+
+} // end of namespace dynamics_controllers
+
+PLUGINLIB_EXPORT_CLASS(dynamics_controllers::KdlTreeController, controller_interface::ControllerBase)