dynamics_solver.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #include <moveit/dynamics_solver/dynamics_solver.h>
00038 
00039 // KDL
00040 #include <kdl/jntarray.hpp>
00041 #include <kdl_parser/kdl_parser.hpp>
00042 #include <kdl/chainiksolverpos_nr_jl.hpp>
00043 #include <kdl/chainiksolvervel_pinv.hpp>
00044 #include <kdl/chainfksolverpos_recursive.hpp>
00045 #include <kdl/chainjnttojacsolver.hpp>
00046 #include <kdl/tree.hpp>
00047 
00048 namespace dynamics_solver
00049 {
00050 namespace
00051 {
00052 inline geometry_msgs::Vector3 transformVector(const Eigen::Affine3d& transform, const geometry_msgs::Vector3& vector)
00053 {
00054   Eigen::Vector3d p;
00055   p = Eigen::Vector3d(vector.x, vector.y, vector.z);
00056   p = transform.rotation() * p;
00057 
00058   geometry_msgs::Vector3 result;
00059   result.x = p.x();
00060   result.y = p.y();
00061   result.z = p.z();
00062 
00063   return result;
00064 }
00065 }
00066 
00067 DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
00068                                const geometry_msgs::Vector3& gravity_vector)
00069 {
00070   robot_model_ = robot_model;
00071   joint_model_group_ = robot_model_->getJointModelGroup(group_name);
00072   if (!joint_model_group_)
00073     return;
00074 
00075   if (!joint_model_group_->isChain())
00076   {
00077     logError("moveit.dynamics_solver: Group '%s' is not a chain. Will not initialize dynamics solver",
00078              group_name.c_str());
00079     joint_model_group_ = NULL;
00080     return;
00081   }
00082 
00083   if (joint_model_group_->getMimicJointModels().size() > 0)
00084   {
00085     logError("moveit.dynamics_solver: Group '%s' has a mimic joint. Will not initialize dynamics solver",
00086              group_name.c_str());
00087     joint_model_group_ = NULL;
00088     return;
00089   }
00090 
00091   const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0];
00092   if (!joint->getParentLinkModel())
00093   {
00094     logError("moveit.dynamics_solver: Group '%s' does not have a parent link", group_name.c_str());
00095     joint_model_group_ = NULL;
00096     return;
00097   }
00098 
00099   base_name_ = joint->getParentLinkModel()->getName();
00100 
00101   tip_name_ = joint_model_group_->getLinkModelNames().back();
00102   logDebug("moveit.dynamics_solver: Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());
00103 
00104   const boost::shared_ptr<const urdf::ModelInterface> urdf_model = robot_model_->getURDF();
00105   const boost::shared_ptr<const srdf::Model> srdf_model = robot_model_->getSRDF();
00106   KDL::Tree tree;
00107 
00108   if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
00109   {
00110     logError("moveit.dynamics_solver: Could not initialize tree object");
00111     joint_model_group_ = NULL;
00112     return;
00113   }
00114   if (!tree.getChain(base_name_, tip_name_, kdl_chain_))
00115   {
00116     logError("moveit.dynamics_solver: Could not initialize chain object");
00117     joint_model_group_ = NULL;
00118     return;
00119   }
00120   num_joints_ = kdl_chain_.getNrOfJoints();
00121   num_segments_ = kdl_chain_.getNrOfSegments();
00122 
00123   state_.reset(new robot_state::RobotState(robot_model_));
00124   state_->setToDefaultValues();
00125 
00126   const std::vector<std::string>& joint_model_names = joint_model_group_->getJointModelNames();
00127   for (std::size_t i = 0; i < joint_model_names.size(); ++i)
00128   {
00129     const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
00130     if (ujoint && ujoint->limits)
00131       max_torques_.push_back(ujoint->limits->effort);
00132     else
00133       max_torques_.push_back(0.0);
00134   }
00135 
00136   KDL::Vector gravity(gravity_vector.x, gravity_vector.y,
00137                       gravity_vector.z);  // \todo Not sure if KDL expects the negative of this (Sachin)
00138   gravity_ = gravity.Norm();
00139   logDebug("moveit.dynamics_solver: Gravity norm set to %f", gravity_);
00140 
00141   chain_id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, gravity));
00142 }
00143 
00144 bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
00145                                 const std::vector<double>& joint_accelerations,
00146                                 const std::vector<geometry_msgs::Wrench>& wrenches, std::vector<double>& torques) const
00147 {
00148   if (!joint_model_group_)
00149   {
00150     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00151     return false;
00152   }
00153   if (joint_angles.size() != num_joints_)
00154   {
00155     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00156     return false;
00157   }
00158   if (joint_velocities.size() != num_joints_)
00159   {
00160     logError("moveit.dynamics_solver: Joint velocities vector should be size %d", num_joints_);
00161     return false;
00162   }
00163   if (joint_accelerations.size() != num_joints_)
00164   {
00165     logError("moveit.dynamics_solver: Joint accelerations vector should be size %d", num_joints_);
00166     return false;
00167   }
00168   if (wrenches.size() != num_segments_)
00169   {
00170     logError("moveit.dynamics_solver: Wrenches vector should be size %d", num_segments_);
00171     return false;
00172   }
00173   if (torques.size() != num_joints_)
00174   {
00175     logError("moveit.dynamics_solver: Torques vector should be size %d", num_joints_);
00176     return false;
00177   }
00178 
00179   KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_),
00180       kdl_torques(num_joints_);
00181   KDL::Wrenches kdl_wrenches(num_segments_);
00182 
00183   for (unsigned int i = 0; i < num_joints_; ++i)
00184   {
00185     kdl_angles(i) = joint_angles[i];
00186     kdl_velocities(i) = joint_velocities[i];
00187     kdl_accelerations(i) = joint_accelerations[i];
00188   }
00189 
00190   for (unsigned int i = 0; i < num_segments_; ++i)
00191   {
00192     kdl_wrenches[i](0) = wrenches[i].force.x;
00193     kdl_wrenches[i](1) = wrenches[i].force.y;
00194     kdl_wrenches[i](2) = wrenches[i].force.z;
00195 
00196     kdl_wrenches[i](3) = wrenches[i].torque.x;
00197     kdl_wrenches[i](4) = wrenches[i].torque.y;
00198     kdl_wrenches[i](5) = wrenches[i].torque.z;
00199   }
00200 
00201   if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
00202   {
00203     logError("moveit.dynamics_solver: Something went wrong computing torques");
00204     return false;
00205   }
00206 
00207   for (unsigned int i = 0; i < num_joints_; ++i)
00208     torques[i] = kdl_torques(i);
00209 
00210   return true;
00211 }
00212 
00213 bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, double& payload,
00214                                    unsigned int& joint_saturated) const
00215 {
00216   if (!joint_model_group_)
00217   {
00218     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00219     return false;
00220   }
00221   if (joint_angles.size() != num_joints_)
00222   {
00223     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00224     return false;
00225   }
00226   std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00227   std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
00228 
00229   std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00230   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
00231     return false;
00232 
00233   for (unsigned int i = 0; i < num_joints_; ++i)
00234   {
00235     if (fabs(zero_torques[i]) >= max_torques_[i])
00236     {
00237       payload = 0.0;
00238       joint_saturated = i;
00239       return true;
00240     }
00241   }
00242 
00243   state_->setJointGroupPositions(joint_model_group_, joint_angles);
00244   const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
00245   const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
00246   Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
00247   wrenches.back().force.z = 1.0;
00248   wrenches.back().force = transformVector(transform, wrenches.back().force);
00249   wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00250 
00251   logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
00252            wrenches.back().force.y, wrenches.back().force.z);
00253 
00254   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
00255     return false;
00256 
00257   double min_payload = std::numeric_limits<double>::max();
00258   for (unsigned int i = 0; i < num_joints_; ++i)
00259   {
00260     double payload_joint = std::max<double>((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]),
00261                                             (-max_torques_[i] - zero_torques[i]) /
00262                                                 (torques[i] - zero_torques[i]));  // because we set the payload to 1.0
00263     logDebug("moveit.dynamics_solver: Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
00264              max_torques_[i], zero_torques[i]);
00265     logDebug("moveit.dynamics_solver: Joint: %d, Payload Allowed (N): %f", i, payload_joint);
00266     if (payload_joint < min_payload)
00267     {
00268       min_payload = payload_joint;
00269       joint_saturated = i;
00270     }
00271   }
00272   payload = min_payload / gravity_;
00273   logDebug("moveit.dynamics_solver: Max payload (kg): %f", payload);
00274   return true;
00275 }
00276 
00277 bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles, double payload,
00278                                        std::vector<double>& joint_torques) const
00279 {
00280   if (!joint_model_group_)
00281   {
00282     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00283     return false;
00284   }
00285   if (joint_angles.size() != num_joints_)
00286   {
00287     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00288     return false;
00289   }
00290   if (joint_torques.size() != num_joints_)
00291   {
00292     logError("moveit.dynamics_solver: Joint torques vector should be size %d", num_joints_);
00293     return false;
00294   }
00295   std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00296   std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00297   state_->setJointGroupPositions(joint_model_group_, joint_angles);
00298 
00299   const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
00300   const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
00301   Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
00302   wrenches.back().force.z = payload * gravity_;
00303   wrenches.back().force = transformVector(transform, wrenches.back().force);
00304   wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00305 
00306   logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
00307            wrenches.back().force.y, wrenches.back().force.z);
00308 
00309   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques))
00310     return false;
00311   return true;
00312 }
00313 
00314 const std::vector<double>& DynamicsSolver::getMaxTorques() const
00315 {
00316   return max_torques_;
00317 }
00318 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49