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 
00051 namespace
00052 {
00053 inline geometry_msgs::Vector3 transformVector(const Eigen::Affine3d &transform,
00054                                               const geometry_msgs::Vector3 &vector)
00055 {
00056   Eigen::Vector3d p;
00057   p = Eigen::Vector3d(vector.x, vector.y, vector.z);
00058   p = transform.rotation()*p;
00059 
00060   geometry_msgs::Vector3 result;
00061   result.x = p.x();
00062   result.y = p.y();
00063   result.z = p.z();
00064 
00065   return result;
00066 }
00067 }
00068 
00069 DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr &robot_model,
00070                                const std::string &group_name,
00071                                const geometry_msgs::Vector3 &gravity_vector)
00072 {
00073   robot_model_ = robot_model;
00074   joint_model_group_ = robot_model_->getJointModelGroup(group_name);
00075   if (!joint_model_group_)
00076     return;
00077 
00078   if (!joint_model_group_->isChain())
00079   {
00080     logError("moveit.dynamics_solver: Group '%s' is not a chain. Will not initialize dynamics solver", group_name.c_str());
00081     joint_model_group_ = NULL;
00082     return;
00083   }
00084 
00085   if (joint_model_group_->getMimicJointModels().size() > 0)
00086   {
00087     logError("moveit.dynamics_solver: Group '%s' has a mimic joint. Will not initialize dynamics solver", group_name.c_str());
00088     joint_model_group_ = NULL;
00089     return;
00090   }
00091 
00092   const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0];
00093   if (!joint->getParentLinkModel())
00094   {
00095     logError("moveit.dynamics_solver: Group '%s' does not have a parent link", group_name.c_str());
00096     joint_model_group_ = NULL;
00097     return;
00098   }
00099   
00100   base_name_ = joint->getParentLinkModel()->getName();
00101 
00102   tip_name_ = joint_model_group_->getLinkModelNames().back();
00103   logDebug("moveit.dynamics_solver: Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());
00104 
00105   const boost::shared_ptr<const urdf::ModelInterface> urdf_model = robot_model_->getURDF();
00106   const boost::shared_ptr<const srdf::Model> srdf_model = robot_model_->getSRDF();
00107   KDL::Tree tree;
00108 
00109   if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
00110   {
00111     logError("moveit.dynamics_solver: Could not initialize tree object");
00112     joint_model_group_ = NULL;
00113     return;
00114   }
00115   if (!tree.getChain(base_name_, tip_name_, kdl_chain_))
00116   {
00117     logError("moveit.dynamics_solver: Could not initialize chain object");
00118     joint_model_group_ = NULL;
00119     return;
00120   }
00121   num_joints_ = kdl_chain_.getNrOfJoints();
00122   num_segments_ = kdl_chain_.getNrOfSegments();
00123 
00124   state_.reset(new robot_state::RobotState(robot_model_));
00125   state_->setToDefaultValues();
00126   
00127   const std::vector<std::string> &joint_model_names = joint_model_group_->getJointModelNames();
00128   for (std::size_t i = 0; i < joint_model_names.size(); ++i)
00129   {
00130     const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
00131     if (ujoint && ujoint->limits)
00132       max_torques_.push_back(ujoint->limits->effort);
00133     else
00134       max_torques_.push_back(0.0);
00135   }
00136 
00137   KDL::Vector gravity(gravity_vector.x, gravity_vector.y, 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,
00145                                 const std::vector<double> &joint_velocities,
00146                                 const std::vector<double> &joint_accelerations,
00147                                 const std::vector<geometry_msgs::Wrench> &wrenches,
00148                                 std::vector<double> &torques) const
00149 {
00150   if (!joint_model_group_)
00151   {
00152     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00153     return false;
00154   }
00155   if (joint_angles.size() != num_joints_)
00156   {
00157     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00158     return false;
00159   }
00160   if (joint_velocities.size() != num_joints_)
00161   {
00162     logError("moveit.dynamics_solver: Joint velocities vector should be size %d", num_joints_);
00163     return false;
00164   }
00165   if (joint_accelerations.size() != num_joints_)
00166   {
00167     logError("moveit.dynamics_solver: Joint accelerations vector should be size %d", num_joints_);
00168     return false;
00169   }
00170   if (wrenches.size() != num_segments_)
00171   {
00172     logError("moveit.dynamics_solver: Wrenches vector should be size %d", num_segments_);
00173     return false;
00174   }
00175   if (torques.size() != num_joints_)
00176   {
00177     logError("moveit.dynamics_solver: Torques vector should be size %d", num_joints_);
00178     return false;
00179   }
00180 
00181   KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_), kdl_torques(num_joints_);
00182   KDL::Wrenches kdl_wrenches(num_segments_);
00183 
00184   for (unsigned int i = 0; i < num_joints_; ++i)
00185   {
00186     kdl_angles(i) = joint_angles[i];
00187     kdl_velocities(i) = joint_velocities[i];
00188     kdl_accelerations(i) = joint_accelerations[i];
00189   }
00190 
00191   for (unsigned int i = 0; i < num_segments_; ++i)
00192   {
00193     kdl_wrenches[i](0) = wrenches[i].force.x;
00194     kdl_wrenches[i](1) = wrenches[i].force.y;
00195     kdl_wrenches[i](2) = wrenches[i].force.z;
00196 
00197     kdl_wrenches[i](3) = wrenches[i].torque.x;
00198     kdl_wrenches[i](4) = wrenches[i].torque.y;
00199     kdl_wrenches[i](5) = wrenches[i].torque.z;
00200   }
00201 
00202   if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
00203   {
00204     logError("moveit.dynamics_solver: Something went wrong computing torques");
00205     return false;
00206   }
00207 
00208   for (unsigned int i = 0; i < num_joints_; ++i)
00209     torques[i] = kdl_torques(i);
00210 
00211   return true;
00212 }
00213 
00214 bool DynamicsSolver::getMaxPayload(const std::vector<double> &joint_angles,
00215                                    double &payload,
00216                                    unsigned int &joint_saturated) const
00217 {
00218   if (!joint_model_group_)
00219   {
00220     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00221     return false;
00222   }
00223   if (joint_angles.size() != num_joints_)
00224   {
00225     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00226     return false;
00227   }
00228   std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00229   std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
00230 
00231   std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00232   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
00233     return false;
00234 
00235   for (unsigned int i = 0; i < num_joints_; ++i)
00236   {
00237     if (fabs(zero_torques[i]) >= max_torques_[i])
00238     {
00239       payload = 0.0;
00240       joint_saturated = i;
00241       return true;
00242     }
00243   }
00244 
00245   state_->setJointGroupPositions(joint_model_group_, joint_angles);
00246   const Eigen::Affine3d &base_frame = state_->getFrameTransform(base_name_);
00247   const Eigen::Affine3d &tip_frame = state_->getFrameTransform(tip_name_);
00248   Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
00249   wrenches.back().force.z = 1.0;
00250   wrenches.back().force = transformVector(transform, wrenches.back().force);
00251   wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00252 
00253   logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z);
00254 
00255   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
00256     return false;
00257 
00258   double min_payload = std::numeric_limits<double>::max();
00259   for (unsigned int i = 0; i < num_joints_; ++i)
00260   {
00261     double payload_joint = std::max<double>((max_torques_[i]-zero_torques[i])/(torques[i]-zero_torques[i]),(-max_torques_[i]-zero_torques[i])/(torques[i]-zero_torques[i]));//because we set the payload to 1.0
00262     logDebug("moveit.dynamics_solver: Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], max_torques_[i], zero_torques[i]);
00263     logDebug("moveit.dynamics_solver: Joint: %d, Payload Allowed (N): %f", i, payload_joint);
00264     if (payload_joint < min_payload)
00265     {
00266       min_payload = payload_joint;
00267       joint_saturated = i;
00268     }
00269   }
00270   payload = min_payload/gravity_;
00271   logDebug("moveit.dynamics_solver: Max payload (kg): %f", payload);
00272   return true;
00273 }
00274 
00275 bool DynamicsSolver::getPayloadTorques(const std::vector<double> &joint_angles,
00276                                        double payload,
00277                                        std::vector<double> &joint_torques) const
00278 {
00279   if (!joint_model_group_)
00280   {
00281     logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
00282     return false;
00283   }
00284   if (joint_angles.size() != num_joints_)
00285   {
00286     logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
00287     return false;
00288   }
00289   if (joint_torques.size() != num_joints_)
00290   {
00291     logError("moveit.dynamics_solver: Joint torques vector should be size %d", num_joints_);
00292     return false;
00293   }
00294   std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
00295   std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
00296   state_->setJointGroupPositions(joint_model_group_, joint_angles);
00297 
00298   const Eigen::Affine3d &base_frame = state_->getFrameTransform(base_name_);
00299   const Eigen::Affine3d &tip_frame = state_->getFrameTransform(tip_name_);
00300   Eigen::Affine3d transform = tip_frame.inverse()* base_frame;
00301   wrenches.back().force.z = payload * gravity_;
00302   wrenches.back().force = transformVector(transform, wrenches.back().force);
00303   wrenches.back().torque = transformVector(transform, wrenches.back().torque);
00304 
00305   logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z);
00306 
00307   if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques))
00308     return false;
00309   return true;
00310 }
00311 
00312 const std::vector<double>& DynamicsSolver::getMaxTorques() const
00313 {
00314   return max_torques_;
00315 }
00316 
00317 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:52