dynamics_solver.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
38 
39 // KDL
40 #include <kdl/jntarray.hpp>
42 #include <kdl/chainiksolverpos_nr_jl.hpp>
43 #include <kdl/chainiksolvervel_pinv.hpp>
44 #include <kdl/chainfksolverpos_recursive.hpp>
45 #include <kdl/chainjnttojacsolver.hpp>
46 #include <kdl/tree.hpp>
47 
48 namespace dynamics_solver
49 {
50 namespace
51 {
52 inline geometry_msgs::Vector3 transformVector(const Eigen::Affine3d& transform, const geometry_msgs::Vector3& vector)
53 {
54  Eigen::Vector3d p;
55  p = Eigen::Vector3d(vector.x, vector.y, vector.z);
56  p = transform.rotation() * p;
57 
58  geometry_msgs::Vector3 result;
59  result.x = p.x();
60  result.y = p.y();
61  result.z = p.z();
62 
63  return result;
64 }
65 }
66 
67 DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
68  const geometry_msgs::Vector3& gravity_vector)
69 {
70  robot_model_ = robot_model;
71  joint_model_group_ = robot_model_->getJointModelGroup(group_name);
72  if (!joint_model_group_)
73  return;
74 
76  {
77  logError("moveit.dynamics_solver: Group '%s' is not a chain. Will not initialize dynamics solver",
78  group_name.c_str());
79  joint_model_group_ = NULL;
80  return;
81  }
82 
83  if (joint_model_group_->getMimicJointModels().size() > 0)
84  {
85  logError("moveit.dynamics_solver: Group '%s' has a mimic joint. Will not initialize dynamics solver",
86  group_name.c_str());
87  joint_model_group_ = NULL;
88  return;
89  }
90 
92  if (!joint->getParentLinkModel())
93  {
94  logError("moveit.dynamics_solver: Group '%s' does not have a parent link", group_name.c_str());
95  joint_model_group_ = NULL;
96  return;
97  }
98 
100 
102  logDebug("moveit.dynamics_solver: Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str());
103 
104  const urdf::ModelInterfaceSharedPtr urdf_model = robot_model_->getURDF();
105  const srdf::ModelConstSharedPtr srdf_model = robot_model_->getSRDF();
106  KDL::Tree tree;
107 
108  if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
109  {
110  logError("moveit.dynamics_solver: Could not initialize tree object");
111  joint_model_group_ = NULL;
112  return;
113  }
115  {
116  logError("moveit.dynamics_solver: Could not initialize chain object");
117  joint_model_group_ = NULL;
118  return;
119  }
122 
124  state_->setToDefaultValues();
125 
126  const std::vector<std::string>& joint_model_names = joint_model_group_->getJointModelNames();
127  for (std::size_t i = 0; i < joint_model_names.size(); ++i)
128  {
129  const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
130  if (ujoint && ujoint->limits)
131  max_torques_.push_back(ujoint->limits->effort);
132  else
133  max_torques_.push_back(0.0);
134  }
135 
136  KDL::Vector gravity(gravity_vector.x, gravity_vector.y,
137  gravity_vector.z); // \todo Not sure if KDL expects the negative of this (Sachin)
138  gravity_ = gravity.Norm();
139  logDebug("moveit.dynamics_solver: Gravity norm set to %f", gravity_);
140 
142 }
143 
144 bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
145  const std::vector<double>& joint_accelerations,
146  const std::vector<geometry_msgs::Wrench>& wrenches, std::vector<double>& torques) const
147 {
148  if (!joint_model_group_)
149  {
150  logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
151  return false;
152  }
153  if (joint_angles.size() != num_joints_)
154  {
155  logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
156  return false;
157  }
158  if (joint_velocities.size() != num_joints_)
159  {
160  logError("moveit.dynamics_solver: Joint velocities vector should be size %d", num_joints_);
161  return false;
162  }
163  if (joint_accelerations.size() != num_joints_)
164  {
165  logError("moveit.dynamics_solver: Joint accelerations vector should be size %d", num_joints_);
166  return false;
167  }
168  if (wrenches.size() != num_segments_)
169  {
170  logError("moveit.dynamics_solver: Wrenches vector should be size %d", num_segments_);
171  return false;
172  }
173  if (torques.size() != num_joints_)
174  {
175  logError("moveit.dynamics_solver: Torques vector should be size %d", num_joints_);
176  return false;
177  }
178 
179  KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_),
180  kdl_torques(num_joints_);
181  KDL::Wrenches kdl_wrenches(num_segments_);
182 
183  for (unsigned int i = 0; i < num_joints_; ++i)
184  {
185  kdl_angles(i) = joint_angles[i];
186  kdl_velocities(i) = joint_velocities[i];
187  kdl_accelerations(i) = joint_accelerations[i];
188  }
189 
190  for (unsigned int i = 0; i < num_segments_; ++i)
191  {
192  kdl_wrenches[i](0) = wrenches[i].force.x;
193  kdl_wrenches[i](1) = wrenches[i].force.y;
194  kdl_wrenches[i](2) = wrenches[i].force.z;
195 
196  kdl_wrenches[i](3) = wrenches[i].torque.x;
197  kdl_wrenches[i](4) = wrenches[i].torque.y;
198  kdl_wrenches[i](5) = wrenches[i].torque.z;
199  }
200 
201  if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
202  {
203  logError("moveit.dynamics_solver: Something went wrong computing torques");
204  return false;
205  }
206 
207  for (unsigned int i = 0; i < num_joints_; ++i)
208  torques[i] = kdl_torques(i);
209 
210  return true;
211 }
212 
213 bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, double& payload,
214  unsigned int& joint_saturated) const
215 {
216  if (!joint_model_group_)
217  {
218  logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
219  return false;
220  }
221  if (joint_angles.size() != num_joints_)
222  {
223  logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
224  return false;
225  }
226  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
227  std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
228 
229  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
230  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
231  return false;
232 
233  for (unsigned int i = 0; i < num_joints_; ++i)
234  {
235  if (fabs(zero_torques[i]) >= max_torques_[i])
236  {
237  payload = 0.0;
238  joint_saturated = i;
239  return true;
240  }
241  }
242 
243  state_->setJointGroupPositions(joint_model_group_, joint_angles);
244  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
245  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
246  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
247  wrenches.back().force.z = 1.0;
248  wrenches.back().force = transformVector(transform, wrenches.back().force);
249  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
250 
251  logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
252  wrenches.back().force.y, wrenches.back().force.z);
253 
254  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
255  return false;
256 
257  double min_payload = std::numeric_limits<double>::max();
258  for (unsigned int i = 0; i < num_joints_; ++i)
259  {
260  double payload_joint = std::max<double>((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]),
261  (-max_torques_[i] - zero_torques[i]) /
262  (torques[i] - zero_torques[i])); // because we set the payload to 1.0
263  logDebug("moveit.dynamics_solver: Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
264  max_torques_[i], zero_torques[i]);
265  logDebug("moveit.dynamics_solver: Joint: %d, Payload Allowed (N): %f", i, payload_joint);
266  if (payload_joint < min_payload)
267  {
268  min_payload = payload_joint;
269  joint_saturated = i;
270  }
271  }
272  payload = min_payload / gravity_;
273  logDebug("moveit.dynamics_solver: Max payload (kg): %f", payload);
274  return true;
275 }
276 
277 bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles, double payload,
278  std::vector<double>& joint_torques) const
279 {
280  if (!joint_model_group_)
281  {
282  logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. Check error logs.");
283  return false;
284  }
285  if (joint_angles.size() != num_joints_)
286  {
287  logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
288  return false;
289  }
290  if (joint_torques.size() != num_joints_)
291  {
292  logError("moveit.dynamics_solver: Joint torques vector should be size %d", num_joints_);
293  return false;
294  }
295  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
296  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
297  state_->setJointGroupPositions(joint_model_group_, joint_angles);
298 
299  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
300  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
301  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
302  wrenches.back().force.z = payload * gravity_;
303  wrenches.back().force = transformVector(transform, wrenches.back().force);
304  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
305 
306  logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
307  wrenches.back().force.y, wrenches.back().force.z);
308 
309  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques))
310  return false;
311  return true;
312 }
313 
314 const std::vector<double>& DynamicsSolver::getMaxTorques() const
315 {
316  return max_torques_;
317 }
318 }
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
Core components of MoveIt!
std::vector< Wrench > Wrenches
bool isChain() const
Check if this group is a linear chain.
bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
bool getPayloadTorques(const std::vector< double > &joint_angles, double payload, std::vector< double > &joint_torques) const
Get torques corresponding to a particular payload value. Payload is the weight that this group can ho...
std::shared_ptr< KDL::ChainIdSolver_RNE > chain_id_solver_
const std::vector< std::string > & getLinkModelNames() const
Get the names of the links that are part of this joint group.
unsigned int getNrOfSegments() const
robot_model::RobotModelConstPtr robot_model_
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
const std::vector< double > & getMaxTorques() const
Get maximum torques for this group.
double Norm() const
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
This namespace includes the dynamics_solver library.
unsigned int getNrOfJoints() const
A joint from the robot. Models the transform that this joint applies in the kinematic chain...
Definition: joint_model.h:108
robot_state::RobotStatePtr state_
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
const robot_model::JointModelGroup * joint_model_group_
bool getTorques(const std::vector< double > &joint_angles, const std::vector< double > &joint_velocities, const std::vector< double > &joint_accelerations, const std::vector< geometry_msgs::Wrench > &wrenches, std::vector< double > &torques) const
Get the torques (the order of all input and output is the same as the order of joints for this group ...
std::vector< double > max_torques_
const std::vector< const JointModel * > & getJointRoots() const
Unlike a complete kinematic model, a group may contain disconnected parts of the kinematic tree – a ...
const LinkModel * getParentLinkModel() const
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return a NULL pointer here.
Definition: joint_model.h:148
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
bool getMaxPayload(const std::vector< double > &joint_angles, double &payload, unsigned int &joint_saturated) const
Get the maximum payload for this group (in kg). Payload is the weight that this group can hold when t...
DynamicsSolver(const robot_model::RobotModelConstPtr &robot_model, const std::string &group_name, const geometry_msgs::Vector3 &gravity_vector)
Initialize the dynamics solver.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jan 21 2018 03:54:28