From 75a24ba46c0370bae5f0b79d8ad71733fdf0734b Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Thu, 3 Oct 2019 17:43:04 +0200 Subject: [PATCH 01/12] Controllers based on the inverse dynamics --- dynamics_controllers/CMakeLists.txt | 36 +++ dynamics_controllers/README.md | 152 ++++++++++ dynamics_controllers/dynamics_controllers.xml | 23 ++ .../dynamics_controller_base.h | 130 +++++++++ .../kdl_chain_controller.h | 81 +++++ .../kdl_tree_controller.h | 88 ++++++ dynamics_controllers/package.xml | 20 ++ .../src/dynamics_controller_base.cpp | 249 ++++++++++++++++ .../src/kdl_chain_controller.cpp | 276 ++++++++++++++++++ .../src/kdl_tree_controller.cpp | 225 ++++++++++++++ 10 files changed, 1280 insertions(+) create mode 100644 dynamics_controllers/CMakeLists.txt create mode 100644 dynamics_controllers/README.md create mode 100644 dynamics_controllers/dynamics_controllers.xml create mode 100644 dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h create mode 100644 dynamics_controllers/include/dynamics_controllers/kdl_chain_controller.h create mode 100644 dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h create mode 100644 dynamics_controllers/package.xml create mode 100644 dynamics_controllers/src/dynamics_controller_base.cpp create mode 100644 dynamics_controllers/src/kdl_chain_controller.cpp create mode 100644 dynamics_controllers/src/kdl_tree_controller.cpp diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt new file mode 100644 index 000000000..c0d164397 --- /dev/null +++ b/dynamics_controllers/CMakeLists.txt @@ -0,0 +1,36 @@ +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 +) + +# 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) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md new file mode 100644 index 000000000..5732dd8bb --- /dev/null +++ b/dynamics_controllers/README.md @@ -0,0 +1,152 @@ +## 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. + + + +### 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. **NOTE**: currently, the parameter is ignored + and the generated tree is always rooted at the base of the URDF. +- `gravity_compensation_only`: as discussed above. Default: `false`. + + + +### Limitations + +There are some limitations for these controllers: +- 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. +- [ ] Additional parameters such as joint inertia and viscous friction + + +### Personal todo list: + +- How should the license be pasted? Some headers contain only WillowGarage, + some many institutions. Should I put myself? +- Check CMakeLists.txt (install targets etc) +- Tell about the KDL version that is needed! +- Add documentation as in other packages 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..439b689aa --- /dev/null +++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h @@ -0,0 +1,130 @@ +/********************************************************************* + * 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 - franco.fusco@ls2n.fr + */ + +#ifndef DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H +#define DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H + +#include +#include +#include +#include + + +namespace dynamics_controllers { + +class DynamicsControllerBase : public controller_interface::Controller { +public: + DynamicsControllerBase(); + + bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle& nh) override; + + void starting(const ros::Time& time) override; + + void update(const ros::Time& time, const ros::Duration& period) override; + + 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. + pluginlib::ClassLoader controller_loader_; + /// Sub-controller instance. + std::unique_ptr controller_; + /// Simplified replica of the available hardware, to be given to the sub-controller. + std::unique_ptr 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& 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. + }; + +}; + +} + +#endif 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..0ba866c1f --- /dev/null +++ b/dynamics_controllers/include/dynamics_controllers/kdl_chain_controller.h @@ -0,0 +1,81 @@ +/********************************************************************* + * 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 - franco.fusco@ls2n.fr + */ + +#ifndef DYNAMICS_CONTROLLERS_KDL_CHAIN_CONTROLLER_H +#define DYNAMICS_CONTROLLERS_KDL_CHAIN_CONTROLLER_H + +#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_; + + +}; + +} + +#endif 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..3198adb63 --- /dev/null +++ b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h @@ -0,0 +1,88 @@ +/********************************************************************* + * 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 - franco.fusco@ls2n.fr + */ + +#ifndef DYNAMICS_CONTROLLERS_KDL_TREE_CONTROLLER_H +#define DYNAMICS_CONTROLLERS_KDL_TREE_CONTROLLER_H + +#include +#include +#include +#include + + +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. + */ +KDL::Tree extractSubTree(const KDL::Tree& tree, const std::string& root); +} + +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_; +}; + +} + +#endif diff --git a/dynamics_controllers/package.xml b/dynamics_controllers/package.xml new file mode 100644 index 000000000..ed27113d3 --- /dev/null +++ b/dynamics_controllers/package.xml @@ -0,0 +1,20 @@ + + + dynamics_controllers + 0.0.0 + Adds a "dynamics control layer" to standard effort controllers. + + Franco FUSCO + Franco FUSCO + BSD + + catkin + + controller_interface + 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..d8c61dc72 --- /dev/null +++ b/dynamics_controllers/src/dynamics_controller_base.cpp @@ -0,0 +1,249 @@ +/********************************************************************* + * 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 - franco.fusco@ls2n.fr + */ + +#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; jgetHandle(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; + if(!cnh.getParam("type", ctype)) { + ROS_ERROR("Could not deduce type of sub-controller from %s/type", controller_ns.c_str()); + return false; + } + + try { + ROS_DEBUG("Generating sub-controller plugin instance of type %s", ctype.c_str()); + controller_.reset( controller_loader_.createUnmanagedInstance(ctype) ); + if(controller_ == nullptr) + throw std::runtime_error("pluginlib::ClassLoader::createUnmanagedInstance returned 'nullptr'"); + } + catch(std::exception& ex) { + ROS_ERROR("Exception while loading controller of type %s. What: %s", ctype.c_str(), ex.what()); + return false; + } + + try { + // initializes the sub-controller + ROS_DEBUG("Initializing sub-controller"); + ClaimedResources resources; + bool init_ok = controller_->initRequest(fake_hw_.get(), nh, cnh, resources); + + if(!init_ok) { + ROS_ERROR("Failed to initialize sub-controller of type %s", ctype.c_str()); + return false; + } + + // NOTE: I am not sure if the follwing is the correct way of checking that + // the sub-controller acquired all joint control resources... + // I expect the sub-controller to claim resources only from the + // EffortJointInterface of the FakeHW. Thus, give a warning when multiple + // interfaces are used. (but it should be impossible, should it?) + if(resources.size()!=1) { + std::stringstream ss; + for(const auto& r : resources) + ss << " " << r.hardware_interface; + ROS_WARN("Sub-controller claimed resources from multiple hardware " + "interfaces, which might lead to problems. Resources were claimed from " + "the following interfaces: %s", ss.str().c_str() + ); + } + + // The hardware interface should be the EffortJointInterface + if(resources[0].hardware_interface != "hardware_interface::EffortJointInterface") { + ROS_WARN("First hardware interface claimed by the sub-controller is %s, " + "but hardware_interface::EffortJointInterface was expected. This might " + "lead to undefined behavior...", resources[0].hardware_interface.c_str() + ); + } + + // Check that all joints of the controller have been claimed by the + // subcontroller as well. + for(const auto& joint : joint_names_) { + const auto& res = resources[0].resources; + auto it = std::find(res.begin(), res.end(), joint); + if(it == res.end()) { + ROS_ERROR("Sub-controller did not claim resource %s. Note that this " + "controller expects the sub-controller to claim the same resources", + joint.c_str() + ); + return false; + } + } + + ROS_DEBUG("Sub-controller initialized"); + } + catch(std::exception& ex) { + ROS_ERROR("Exception while initializing sub-controller of type %s. What: %s", ctype.c_str(), ex.what()); + return false; + } + + return true; +} + + +void DynamicsControllerBase::starting(const ros::Time& time) { + // forward the start-request to the low-level controller + if(!controller_->startRequest(time)) { + ROS_ERROR("Start request to low-level controller failed"); + throw std::runtime_error("Start request to low-level controller failed"); + } +} + + +void DynamicsControllerBase::update(const ros::Time& time, const ros::Duration& period) { + fake_hw_->copyState(joint_handles_); + controller_->update(time, period); + fake_hw_->getEfforts(sub_command_); + + try { + computeEfforts(); + } + 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 controller + if(!controller_->stopRequest(time)) { + ROS_ERROR("Stop request to low-level controller failed"); + throw std::runtime_error("Stop request to low-level controller failed"); + } +} + +} diff --git a/dynamics_controllers/src/kdl_chain_controller.cpp b/dynamics_controllers/src/kdl_chain_controller.cpp new file mode 100644 index 000000000..6eb36f9ab --- /dev/null +++ b/dynamics_controllers/src/kdl_chain_controller.cpp @@ -0,0 +1,276 @@ +/********************************************************************* + * 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 - 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 + + +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()); + + // TODO: extract the tree from a given root + tree_ = internal::extractSubTree(tree, root_name); + + // 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; i Date: Thu, 3 Oct 2019 17:48:50 +0200 Subject: [PATCH 02/12] updated README --- dynamics_controllers/README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index 5732dd8bb..29d830372 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -133,6 +133,7 @@ There are some limitations for these controllers: - The sub-controller must control all joints of the internal `Chain`/`Tree`. + ### Possible Improvements - [ ] Compensation of external payloads, mainly in two ways: @@ -140,7 +141,8 @@ There are some limitations for these controllers: - 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. -- [ ] Additional parameters such as joint inertia and viscous friction +- [ ] Additional parameters such as joint inertia and viscous friction. +- [ ] Allow multiple sub-controllers to manage different joints. ### Personal todo list: From 17afe8d07a6b5e4b89ded500746f37defae8c8b3 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Fri, 4 Oct 2019 08:26:38 +0200 Subject: [PATCH 03/12] Updated README --- dynamics_controllers/README.md | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index 29d830372..317a3e62b 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -18,6 +18,9 @@ Currently, there exist two controllers: `KdlChainController` and 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 @@ -147,8 +150,9 @@ There are some limitations for these controllers: ### Personal todo list: -- How should the license be pasted? Some headers contain only WillowGarage, - some many institutions. Should I put myself? -- Check CMakeLists.txt (install targets etc) -- Tell about the KDL version that is needed! -- Add documentation as in other packages +- [ ] How should the license be pasted? Some headers contain only WillowGarage, + some many institutions. Should I put myself as well? +- [ ] Check CMakeLists.txt (install targets etc). +- [x] Tell about the KDL version that is needed! +- [ ] Add documentation as in other ROS-controllers packages. +- [ ] Complete `KdlTreeController` so that proper sub-tree extraction is possible. From c9d9152f443cd4cbff24c29ac80df17d16974b90 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Fri, 4 Oct 2019 10:13:52 +0200 Subject: [PATCH 04/12] sub-tree extraction --- dynamics_controllers/README.md | 5 +- .../kdl_tree_controller.h | 2 +- .../src/kdl_tree_controller.cpp | 50 +++++++++++++++++-- 3 files changed, 48 insertions(+), 9 deletions(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index 317a3e62b..375008065 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -119,8 +119,7 @@ should not depend on a specific controller (at least in principle!). - `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. **NOTE**: currently, the parameter is ignored - and the generated tree is always rooted at the base of the URDF. + from the robot description. - `gravity_compensation_only`: as discussed above. Default: `false`. @@ -155,4 +154,4 @@ There are some limitations for these controllers: - [ ] Check CMakeLists.txt (install targets etc). - [x] Tell about the KDL version that is needed! - [ ] Add documentation as in other ROS-controllers packages. -- [ ] Complete `KdlTreeController` so that proper sub-tree extraction is possible. +- [x] Complete `KdlTreeController` so that proper sub-tree extraction is possible. diff --git a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h index 3198adb63..76dc51302 100644 --- a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h +++ b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h @@ -56,7 +56,7 @@ namespace internal { * 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. */ -KDL::Tree extractSubTree(const KDL::Tree& tree, const std::string& root); +bool extractSubTree(const KDL::Tree& tree, const std::string& root_name, KDL::Tree& subtree); } class KdlTreeController : public DynamicsControllerBase { diff --git a/dynamics_controllers/src/kdl_tree_controller.cpp b/dynamics_controllers/src/kdl_tree_controller.cpp index 4f80b45cf..c77f247be 100644 --- a/dynamics_controllers/src/kdl_tree_controller.cpp +++ b/dynamics_controllers/src/kdl_tree_controller.cpp @@ -101,8 +101,11 @@ bool KdlTreeController::initDynamics(ros::NodeHandle& nh) { } ROS_DEBUG("'root_name' is: '%s'", root_name.c_str()); - // TODO: extract the tree from a given root - tree_ = internal::extractSubTree(tree, root_name); + // 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; @@ -215,11 +218,48 @@ void KdlTreeController::computeEfforts() { } -KDL::Tree internal::extractSubTree(const KDL::Tree& tree, const std::string& root) +namespace internal +{ + +// Utility function that basically copies the private method "KDL::Tree::addTreeRecursive" (which is private). +bool expandTreeRecursive(KDL::Tree& tree, KDL::SegmentMap::const_iterator root, const std::string& hook_name) { - return tree; + // get iterator for root-segment + KDL::SegmentMap::const_iterator child; + // try to add all of root's children + for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) { + child = GetTreeElementChildren(root->second)[i]; + // try to add the child + if (tree.addSegment(GetTreeElementSegment(child->second), hook_name)) { + std::cout << "--> Appended segment " << child->first << " to " << hook_name << std::endl; + // 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; } -} // end of namespace + +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) From 7882749ba03b31aa0c10176ff5a8353a7a3e2adc Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Tue, 22 Oct 2019 13:04:29 +0200 Subject: [PATCH 05/12] Updated doc --- dynamics_controllers/CMakeLists.txt | 12 ++++++++ dynamics_controllers/README.md | 7 +++-- .../dynamics_controller_base.h | 28 +++++++++++++++++++ 3 files changed, 44 insertions(+), 3 deletions(-) diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt index c0d164397..7a965536f 100644 --- a/dynamics_controllers/CMakeLists.txt +++ b/dynamics_controllers/CMakeLists.txt @@ -34,3 +34,15 @@ add_library(${PROJECT_NAME} # 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} +) \ No newline at end of file diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index 375008065..cf80ace41 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -126,7 +126,6 @@ should not depend on a specific controller (at least in principle!). ### Limitations -There are some limitations for these controllers: - 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. @@ -143,6 +142,8 @@ There are some limitations for these controllers: - 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. @@ -151,7 +152,7 @@ There are some limitations for these controllers: - [ ] How should the license be pasted? Some headers contain only WillowGarage, some many institutions. Should I put myself as well? -- [ ] Check CMakeLists.txt (install targets etc). +- [x] Check CMakeLists.txt (install targets etc). - [x] Tell about the KDL version that is needed! -- [ ] Add documentation as in other ROS-controllers packages. - [x] Complete `KdlTreeController` so that proper sub-tree extraction is possible. +- [ ] Enforce effort limits diff --git a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h index 439b689aa..e98364272 100644 --- a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h +++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h @@ -50,16 +50,44 @@ 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: From bba9155179cbec99f09f5f3458a7ea65250065cf Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Thu, 24 Oct 2019 15:24:20 +0200 Subject: [PATCH 06/12] Removing external dependency on Tree dynamics solver --- dynamics_controllers/CMakeLists.txt | 1 + .../kdl_tree_controller.h | 13 +- dynamics_controllers/src/kdl/treeidsolver.hpp | 62 ++++++++ .../treeidsolver_recursive_newton_euler.cpp | 140 ++++++++++++++++++ .../treeidsolver_recursive_newton_euler.hpp | 84 +++++++++++ .../src/kdl_tree_controller.cpp | 2 + 6 files changed, 301 insertions(+), 1 deletion(-) create mode 100644 dynamics_controllers/src/kdl/treeidsolver.hpp create mode 100644 dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.cpp create mode 100644 dynamics_controllers/src/kdl/treeidsolver_recursive_newton_euler.hpp diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt index 7a965536f..d77d05fc2 100644 --- a/dynamics_controllers/CMakeLists.txt +++ b/dynamics_controllers/CMakeLists.txt @@ -30,6 +30,7 @@ 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 diff --git a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h index 76dc51302..5182902ce 100644 --- a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h +++ b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h @@ -45,8 +45,19 @@ #include #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 { 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_tree_controller.cpp b/dynamics_controllers/src/kdl_tree_controller.cpp index c77f247be..b3d721b5c 100644 --- a/dynamics_controllers/src/kdl_tree_controller.cpp +++ b/dynamics_controllers/src/kdl_tree_controller.cpp @@ -43,6 +43,8 @@ #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 { From 1fb22f8473153cdd918f5f34fe5ece857637a512 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Sat, 26 Oct 2019 12:54:16 +0200 Subject: [PATCH 07/12] Addressing first comments in PR review --- dynamics_controllers/README.md | 3 --- .../dynamics_controllers/dynamics_controller_base.h | 12 +++--------- .../dynamics_controllers/kdl_chain_controller.h | 10 ++-------- .../dynamics_controllers/kdl_tree_controller.h | 10 ++-------- dynamics_controllers/package.xml | 1 + .../src/dynamics_controller_base.cpp | 5 +---- dynamics_controllers/src/kdl_chain_controller.cpp | 5 +---- dynamics_controllers/src/kdl_tree_controller.cpp | 5 +---- 8 files changed, 11 insertions(+), 40 deletions(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index cf80ace41..ccc925b03 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -18,9 +18,6 @@ Currently, there exist two controllers: `KdlChainController` and 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 diff --git a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h index e98364272..35f4e1710 100644 --- a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h +++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h @@ -1,10 +1,7 @@ /********************************************************************* * 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 + * Copyright (c) 2019, Franco Fusco * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -39,8 +36,7 @@ * Author: Franco Fusco - franco.fusco@ls2n.fr */ -#ifndef DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H -#define DYNAMICS_CONTROLLERS_DYNAMICS_CONTROLLER_BASE_H +#pragma once #include #include @@ -54,7 +50,7 @@ namespace 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 @@ -154,5 +150,3 @@ class DynamicsControllerBase : public controller_interface::Controller #include @@ -77,5 +73,3 @@ class KdlChainController : public DynamicsControllerBase { }; } - -#endif diff --git a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h index 5182902ce..878f3b2ab 100644 --- a/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h +++ b/dynamics_controllers/include/dynamics_controllers/kdl_tree_controller.h @@ -1,10 +1,7 @@ /********************************************************************* * 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 + * Copyright (c) 2019, Franco Fusco * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -39,8 +36,7 @@ * Author: Franco Fusco - franco.fusco@ls2n.fr */ -#ifndef DYNAMICS_CONTROLLERS_KDL_TREE_CONTROLLER_H -#define DYNAMICS_CONTROLLERS_KDL_TREE_CONTROLLER_H +#pragma once #include #include @@ -95,5 +91,3 @@ class KdlTreeController : public DynamicsControllerBase { }; } - -#endif diff --git a/dynamics_controllers/package.xml b/dynamics_controllers/package.xml index ed27113d3..a8b57b95d 100644 --- a/dynamics_controllers/package.xml +++ b/dynamics_controllers/package.xml @@ -6,6 +6,7 @@ Franco FUSCO Franco FUSCO + Bence Magyar BSD catkin diff --git a/dynamics_controllers/src/dynamics_controller_base.cpp b/dynamics_controllers/src/dynamics_controller_base.cpp index d8c61dc72..6ec76de1b 100644 --- a/dynamics_controllers/src/dynamics_controller_base.cpp +++ b/dynamics_controllers/src/dynamics_controller_base.cpp @@ -1,10 +1,7 @@ /********************************************************************* * 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 + * Copyright (c) 2019, Franco Fusco * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/dynamics_controllers/src/kdl_chain_controller.cpp b/dynamics_controllers/src/kdl_chain_controller.cpp index 6eb36f9ab..6f651dbd0 100644 --- a/dynamics_controllers/src/kdl_chain_controller.cpp +++ b/dynamics_controllers/src/kdl_chain_controller.cpp @@ -1,10 +1,7 @@ /********************************************************************* * 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 + * Copyright (c) 2019, Franco Fusco * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/dynamics_controllers/src/kdl_tree_controller.cpp b/dynamics_controllers/src/kdl_tree_controller.cpp index b3d721b5c..59f6b1800 100644 --- a/dynamics_controllers/src/kdl_tree_controller.cpp +++ b/dynamics_controllers/src/kdl_tree_controller.cpp @@ -1,10 +1,7 @@ /********************************************************************* * 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 + * Copyright (c) 2019, Franco Fusco * All rights reserved. * * Redistribution and use in source and binary forms, with or without From 1a4f88041798e21bb14251140142c897bdb6a49c Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 28 Oct 2019 12:33:47 +0100 Subject: [PATCH 08/12] Multiple sub-controllers --- dynamics_controllers/README.md | 87 ++++-- .../dynamics_controller_base.h | 15 +- .../src/dynamics_controller_base.cpp | 252 +++++++++++++----- .../src/kdl_tree_controller.cpp | 2 +- 4 files changed, 262 insertions(+), 94 deletions(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index ccc925b03..dd982fda1 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -6,9 +6,9 @@ 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 +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. @@ -24,7 +24,7 @@ algorithm for the 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 +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. @@ -33,19 +33,42 @@ To enable gravity compensation only, you can specify the parameter +### 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 -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} +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 @@ -63,13 +86,15 @@ 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} + # 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 @@ -86,7 +111,7 @@ velocity_controller: Note that you will likely have to tune again the gains of the sub-controllers to achieve good performances. -#### Detailed Controllers Description +#### Detailed Controllers Configuration Below, parameters relative to each controller are listed. Those marked as "searched for" can live in any parent namespace since the @@ -96,8 +121,10 @@ 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`. +- `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]`. @@ -110,8 +137,10 @@ should not depend on a specific controller (at least in principle!). ##### KdlTreeController -- `sub_controller`: should contain the configuration of a controller of type - compatible with `EffortJointInterface`. +- `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]`. @@ -123,12 +152,16 @@ should not depend on a specific controller (at least in principle!). ### 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-controller must control all joints of the internal `Chain`/`Tree`. +- The sub-controllers must control all joints of the internal `Chain`/`Tree`. @@ -142,12 +175,14 @@ should not depend on a specific controller (at least in principle!). 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. + 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. ### Personal todo list: -- [ ] How should the license be pasted? Some headers contain only WillowGarage, +- [x] 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! diff --git a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h index 35f4e1710..e8fdf5ff9 100644 --- a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h +++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include @@ -123,13 +123,16 @@ class DynamicsControllerBase : public controller_interface::Controller controller_loader_; - /// Sub-controller instance. - std::unique_ptr controller_; - /// Simplified replica of the available hardware, to be given to the sub-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_; - /// A fake hardware used to excange information with a "sub-controller". + /// Allows to get a map of controllers names (key) with corresponding types (values). + static bool getSubControllersMap(ros::NodeHandle&, std::map&); + + /// 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. diff --git a/dynamics_controllers/src/dynamics_controller_base.cpp b/dynamics_controllers/src/dynamics_controller_base.cpp index 6ec76de1b..3f5b427a0 100644 --- a/dynamics_controllers/src/dynamics_controller_base.cpp +++ b/dynamics_controllers/src/dynamics_controller_base.cpp @@ -37,6 +37,7 @@ */ #include +#include namespace dynamics_controllers { @@ -79,6 +80,7 @@ void DynamicsControllerBase::FakeHW::copyState(const std::vector& joint_effort_command) { for(unsigned int j=0; j& joint_effor +/** The function looks for all parameters available in the provided NodeHandle's + * namespace and then stores those nested namespaces (assumed to be + * sub-controllers names) which contain a (string) 'type' parameter. + */ +bool DynamicsControllerBase::getSubControllersMap( + ros::NodeHandle& nh, + std::map& 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_("controller_interface", "controller_interface::ControllerBase") + : controller_loader_(new controller_manager::ControllerLoader("controller_interface", "controller_interface::ControllerBase")) { } @@ -99,6 +146,9 @@ DynamicsControllerBase::DynamicsControllerBase() 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)"); @@ -132,95 +182,165 @@ bool DynamicsControllerBase::init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle cnh(nh, controller_ns); std::string ctype; - if(!cnh.getParam("type", ctype)) { - ROS_ERROR("Could not deduce type of sub-controller from %s/type", controller_ns.c_str()); - return false; + 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 { - ROS_DEBUG("Generating sub-controller plugin instance of type %s", ctype.c_str()); - controller_.reset( controller_loader_.createUnmanagedInstance(ctype) ); - if(controller_ == nullptr) - throw std::runtime_error("pluginlib::ClassLoader::createUnmanagedInstance returned 'nullptr'"); + 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 loading controller of type %s. What: %s", ctype.c_str(), ex.what()); + 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 { - // initializes the sub-controller - ROS_DEBUG("Initializing sub-controller"); - ClaimedResources resources; - bool init_ok = controller_->initRequest(fake_hw_.get(), nh, cnh, resources); + 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(!init_ok) { - ROS_ERROR("Failed to initialize sub-controller of type %s", ctype.c_str()); - return false; - } + // 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()); + } - // NOTE: I am not sure if the follwing is the correct way of checking that - // the sub-controller acquired all joint control resources... - // I expect the sub-controller to claim resources only from the - // EffortJointInterface of the FakeHW. Thus, give a warning when multiple - // interfaces are used. (but it should be impossible, should it?) - if(resources.size()!=1) { - std::stringstream ss; - for(const auto& r : resources) - ss << " " << r.hardware_interface; - ROS_WARN("Sub-controller claimed resources from multiple hardware " - "interfaces, which might lead to problems. Resources were claimed from " - "the following interfaces: %s", ss.str().c_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 - if(resources[0].hardware_interface != "hardware_interface::EffortJointInterface") { - ROS_WARN("First hardware interface claimed by the sub-controller is %s, " - "but hardware_interface::EffortJointInterface was expected. This might " - "lead to undefined behavior...", resources[0].hardware_interface.c_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 all joints of the controller have been claimed by the - // subcontroller as well. - for(const auto& joint : joint_names_) { - const auto& res = resources[0].resources; - auto it = std::find(res.begin(), res.end(), joint); - if(it == res.end()) { - ROS_ERROR("Sub-controller did not claim resource %s. Note that this " - "controller expects the sub-controller to claim the same resources", - joint.c_str() - ); - return false; + // 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; } } - - ROS_DEBUG("Sub-controller initialized"); } catch(std::exception& ex) { - ROS_ERROR("Exception while initializing sub-controller of type %s. What: %s", ctype.c_str(), ex.what()); + 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 controller - if(!controller_->startRequest(time)) { - ROS_ERROR("Start request to low-level controller failed"); - throw std::runtime_error("Start request to low-level controller failed"); + // 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_); - controller_->update(time, period); + for(auto& c : sub_controllers_) + c.second->update(time, period); fake_hw_->getEfforts(sub_command_); + // perform sub-class specific dynamics try { computeEfforts(); } @@ -236,10 +356,20 @@ void DynamicsControllerBase::update(const ros::Time& time, const ros::Duration& void DynamicsControllerBase::stopping(const ros::Time& time) { - // forward the stop-request to the low-level controller - if(!controller_->stopRequest(time)) { - ROS_ERROR("Stop request to low-level controller failed"); - throw std::runtime_error("Stop request to low-level controller failed"); + // 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()); } } diff --git a/dynamics_controllers/src/kdl_tree_controller.cpp b/dynamics_controllers/src/kdl_tree_controller.cpp index 59f6b1800..eae8fb37d 100644 --- a/dynamics_controllers/src/kdl_tree_controller.cpp +++ b/dynamics_controllers/src/kdl_tree_controller.cpp @@ -230,7 +230,7 @@ bool expandTreeRecursive(KDL::Tree& tree, KDL::SegmentMap::const_iterator root, child = GetTreeElementChildren(root->second)[i]; // try to add the child if (tree.addSegment(GetTreeElementSegment(child->second), hook_name)) { - std::cout << "--> Appended segment " << child->first << " to " << hook_name << std::endl; + 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 From 58d3a9aa0dd32ded5b44ce517d3bf209f3ae45b6 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 28 Oct 2019 13:29:00 +0100 Subject: [PATCH 09/12] Effort limits (saturation) --- dynamics_controllers/README.md | 2 +- .../dynamics_controller_base.h | 7 +++ .../src/dynamics_controller_base.cpp | 50 +++++++++++++++++++ 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index dd982fda1..cf0b74501 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -187,4 +187,4 @@ by yourself or to find proper workarounds. - [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 +- [x] Enforce effort limits diff --git a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h index e8fdf5ff9..cefd38cfd 100644 --- a/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h +++ b/dynamics_controllers/include/dynamics_controllers/dynamics_controller_base.h @@ -129,9 +129,16 @@ class DynamicsControllerBase : public controller_interface::Controller 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: diff --git a/dynamics_controllers/src/dynamics_controller_base.cpp b/dynamics_controllers/src/dynamics_controller_base.cpp index 3f5b427a0..de0871e7a 100644 --- a/dynamics_controllers/src/dynamics_controller_base.cpp +++ b/dynamics_controllers/src/dynamics_controller_base.cpp @@ -38,6 +38,8 @@ #include #include +#include + namespace dynamics_controllers { @@ -163,6 +165,38 @@ bool DynamicsControllerBase::init(hardware_interface::EffortJointInterface *hw, 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(); @@ -343,6 +377,7 @@ void DynamicsControllerBase::update(const ros::Time& time, const ros::Duration& // perform sub-class specific dynamics try { computeEfforts(); + enforceLimits(); } catch(std::exception& e) { ROS_ERROR("Caught exception while computing efforts: %s", e.what()); @@ -373,4 +408,19 @@ void DynamicsControllerBase::stopping(const ros::Time& time) { } } + +void DynamicsControllerBase::enforceLimits() { + for(unsigned int i=0; i lim) + joint_handles_[i].setCommand(lim); + else if(cmd < -lim) + joint_handles_[i].setCommand(-lim); + } +} + } From 1de39879b1cd577d912d906fbbdfa858f0b7a37b Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Mon, 28 Oct 2019 13:35:58 +0100 Subject: [PATCH 10/12] Removed personal TODOs --- dynamics_controllers/README.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/dynamics_controllers/README.md b/dynamics_controllers/README.md index cf0b74501..a263ce6d9 100644 --- a/dynamics_controllers/README.md +++ b/dynamics_controllers/README.md @@ -178,13 +178,3 @@ by yourself or to find proper workarounds. 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. - - -### Personal todo list: - -- [x] 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. -- [x] Enforce effort limits From d6ebc7661df364d24fc7b71a7f71503fa519ef5d Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 26 Feb 2020 10:07:05 +0100 Subject: [PATCH 11/12] Added pluginlib as dependency to solve link errors --- dynamics_controllers/CMakeLists.txt | 4 +++- dynamics_controllers/package.xml | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt index d77d05fc2..07da7724e 100644 --- a/dynamics_controllers/CMakeLists.txt +++ b/dynamics_controllers/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(orocos_kdl REQUIRED) find_package(catkin REQUIRED COMPONENTS controller_interface kdl_parser + pluginlib ) # Declare catkin package @@ -22,6 +23,7 @@ catkin_package( CATKIN_DEPENDS controller_interface kdl_parser + pluginlib ) include_directories(include ${orocos_kdl_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) @@ -46,4 +48,4 @@ install(TARGETS ${PROJECT_NAME} install( FILES dynamics_controllers.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) \ No newline at end of file +) diff --git a/dynamics_controllers/package.xml b/dynamics_controllers/package.xml index a8b57b95d..018c3306d 100644 --- a/dynamics_controllers/package.xml +++ b/dynamics_controllers/package.xml @@ -12,6 +12,7 @@ catkin controller_interface + pluginlib orocos_kdl kdl_parser From 1e5f2ddf2ace831182ccf2021e91fb7a871c1ab8 Mon Sep 17 00:00:00 2001 From: Franco Fusco Date: Wed, 26 Feb 2020 10:48:37 +0100 Subject: [PATCH 12/12] Added controller_manager as dependency --- dynamics_controllers/CMakeLists.txt | 1 + dynamics_controllers/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/dynamics_controllers/CMakeLists.txt b/dynamics_controllers/CMakeLists.txt index 07da7724e..32183988a 100644 --- a/dynamics_controllers/CMakeLists.txt +++ b/dynamics_controllers/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(orocos_kdl REQUIRED) # Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS controller_interface + controller_manager kdl_parser pluginlib ) diff --git a/dynamics_controllers/package.xml b/dynamics_controllers/package.xml index 018c3306d..c7d0c4bd6 100644 --- a/dynamics_controllers/package.xml +++ b/dynamics_controllers/package.xml @@ -12,6 +12,7 @@ catkin controller_interface + controller_manager pluginlib orocos_kdl kdl_parser