ForwardDynamicsSolver.cpp
1 //////////////////////////////////////////////////////////////////////////////// 2 // Copyright 2019 FZI Research Center for Information Technology 3 // 4 // Redistribution and use in source and binary forms, with or without 5 // modification, are permitted provided that the following conditions are met: 6 // 7 // 1. Redistributions of source code must retain the above copyright notice, 8 // this list of conditions and the following disclaimer. 9 // 10 // 2. Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // 14 // 3. Neither the name of the copyright holder nor the names of its 15 // contributors may be used to endorse or promote products derived from this 16 // software without specific prior written permission. 17 // 18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 // POSSIBILITY OF SUCH DAMAGE. 29 //////////////////////////////////////////////////////////////////////////////// 30 31 //----------------------------------------------------------------------------- 32 /*!\file ForwardDynamicsSolver.cpp 33 * 34 * \author Stefan Scherzinger <scherzin@fzi.de> 35 * \date 2020/03/24 36 * 37 */ 38 //----------------------------------------------------------------------------- 39 40 #include <cartesian_controller_base/ForwardDynamicsSolver.h> 41 42 #include <algorithm> 43 #include <kdl/framevel.hpp> 44 #include <kdl/jntarrayvel.hpp> 45 #include <map> 46 #include <pluginlib/class_list_macros.hpp> 47 #include <sstream> 48 49 /** 50 * \class cartesian_controller_base::ForwardDynamicsSolver 51 * 52 * Users may explicitly specify it with \a "forward_dynamics" as \a ik_solver 53 * in their controllers.yaml configuration file for each controller: 54 * 55 * \code{.yaml} 56 * <name_of_your_controller>: 57 * ros__parameters: 58 * ik_solver: "forward_dynamics" 59 * ... 60 * 61 * solver: 62 * ... 63 * forward_dynamics: 64 * link_mass: 0.5 65 * \endcode 66 * 67 */ 68 PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver, 69 cartesian_controller_base::IKSolver) 70 71 namespace cartesian_controller_base 72 { 73 ForwardDynamicsSolver::ForwardDynamicsSolver() {} 74 75 ForwardDynamicsSolver::~ForwardDynamicsSolver() {} 76 77 trajectory_msgs::msg::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds( 78 rclcpp::Duration period, const ctrl::Vector6D & net_force) 79 { 80 // Compute joint space inertia matrix with actualized link masses 81 buildGenericModel(); 82 m_jnt_space_inertia_solver->JntToMass(m_current_positions, m_jnt_space_inertia); 83 84 // Compute joint jacobian 85 m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian); 86 87 // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$ 88 m_current_accelerations.data = 89 m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force; 90 91 // Numerical time integration with the Euler forward method 92 m_current_positions.data = m_last_positions.data + m_last_velocities.data * period.seconds(); 93 m_current_velocities.data = 94 m_last_velocities.data + m_current_accelerations.data * period.seconds(); 95 m_current_velocities.data *= 0.9; // 10 % global damping against unwanted null space motion. 96 // Will cause exponential slow-down without input. 97 // Make sure positions stay in allowed margins 98 applyJointLimits(); 99 100 // Apply results 101 trajectory_msgs::msg::JointTrajectoryPoint control_cmd; 102 for (int i = 0; i < m_number_joints; ++i) 103 { 104 control_cmd.positions.push_back(m_current_positions(i)); 105 control_cmd.velocities.push_back(m_current_velocities(i)); 106 107 // Accelerations should be left empty. Those values will be interpreted 108 // by most hardware joint drivers as max. tolerated values. As a 109 // consequence, the robot will move very slowly. 110 } 111 control_cmd.time_from_start = period; // valid for this duration 112 113 // Update for the next cycle 114 m_last_positions = m_current_positions; 115 m_last_velocities = m_current_velocities; 116 117 return control_cmd; 118 } 119 120 bool ForwardDynamicsSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, 121 const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits, 122 const KDL::JntArray & lower_pos_limits) 123 { 124 IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits); 125 126 if (!buildGenericModel()) 127 { 128 RCLCPP_ERROR(nh->get_logger(), "Something went wrong in setting up the internal model."); 129 return false; 130 } 131 132 // Forward dynamics 133 m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain)); 134 m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain, KDL::Vector::Zero())); 135 m_jnt_jacobian.resize(m_number_joints); 136 m_jnt_space_inertia.resize(m_number_joints); 137 138 // Set the initial value if provided at runtime, else use default value. 139 m_min = auto_declare(m_params + ".link_mass", 0.1); 140 141 RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver initialized"); 142 RCLCPP_INFO(nh->get_logger(), "Forward dynamics solver has control over %i joints", 143 m_number_joints); 144 145 return true; 146 } 147 148 bool ForwardDynamicsSolver::buildGenericModel() 149 { 150 // Set all masses and inertias to minimal (yet stable) values. 151 double ip_min = 0.000001; 152 for (size_t i = 0; i < m_chain.segments.size(); ++i) 153 { 154 // Fixed joint segment 155 if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None) 156 { 157 m_chain.segments[i].setInertia(KDL::RigidBodyInertia::Zero()); 158 } 159 else // relatively moving segment 160 { 161 m_chain.segments[i].setInertia( 162 KDL::RigidBodyInertia(m_min, // mass 163 KDL::Vector::Zero(), // center of gravity 164 KDL::RotationalInertia(ip_min, // ixx 165 ip_min, // iyy 166 ip_min // izz 167 // ixy, ixy, iyz default to 0.0 168 ))); 169 } 170 } 171 172 // Only give the last segment a generic mass and inertia. 173 // See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting. 174 double m = 1; 175 double ip = 1; 176 m_chain.segments[m_chain.segments.size() - 1].setInertia( 177 KDL::RigidBodyInertia(m, KDL::Vector::Zero(), KDL::RotationalInertia(ip, ip, ip))); 178 179 return true; 180 } 181 182 } // namespace cartesian_controller_base