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  CONSOLE_BRIDGE_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  CONSOLE_BRIDGE_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  CONSOLE_BRIDGE_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  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Base name: '%s', Tip name: '%s'", base_name_.c_str(),
103  tip_name_.c_str());
104 
105  const urdf::ModelInterfaceSharedPtr urdf_model = robot_model_->getURDF();
106  const srdf::ModelConstSharedPtr srdf_model = robot_model_->getSRDF();
107  KDL::Tree tree;
108 
109  if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree))
110  {
111  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Could not initialize tree object");
112  joint_model_group_ = NULL;
113  return;
114  }
116  {
117  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Could not initialize chain object");
118  joint_model_group_ = NULL;
119  return;
120  }
123 
125  state_->setToDefaultValues();
126 
127  const std::vector<std::string>& joint_model_names = joint_model_group_->getJointModelNames();
128  for (std::size_t i = 0; i < joint_model_names.size(); ++i)
129  {
130  const urdf::Joint* ujoint = urdf_model->getJoint(joint_model_names[i]).get();
131  if (ujoint && ujoint->limits)
132  max_torques_.push_back(ujoint->limits->effort);
133  else
134  max_torques_.push_back(0.0);
135  }
136 
137  KDL::Vector gravity(gravity_vector.x, gravity_vector.y,
138  gravity_vector.z); // \todo Not sure if KDL expects the negative of this (Sachin)
139  gravity_ = gravity.Norm();
140  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Gravity norm set to %f", gravity_);
141 
143 }
144 
145 bool DynamicsSolver::getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
146  const std::vector<double>& joint_accelerations,
147  const std::vector<geometry_msgs::Wrench>& wrenches, std::vector<double>& torques) const
148 {
149  if (!joint_model_group_)
150  {
151  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. "
152  "Check error logs.");
153  return false;
154  }
155  if (joint_angles.size() != num_joints_)
156  {
157  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
158  return false;
159  }
160  if (joint_velocities.size() != num_joints_)
161  {
162  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint velocities vector should be size %d", num_joints_);
163  return false;
164  }
165  if (joint_accelerations.size() != num_joints_)
166  {
167  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint accelerations vector should be size %d", num_joints_);
168  return false;
169  }
170  if (wrenches.size() != num_segments_)
171  {
172  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Wrenches vector should be size %d", num_segments_);
173  return false;
174  }
175  if (torques.size() != num_joints_)
176  {
177  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Torques vector should be size %d", num_joints_);
178  return false;
179  }
180 
181  KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_),
182  kdl_torques(num_joints_);
183  KDL::Wrenches kdl_wrenches(num_segments_);
184 
185  for (unsigned int i = 0; i < num_joints_; ++i)
186  {
187  kdl_angles(i) = joint_angles[i];
188  kdl_velocities(i) = joint_velocities[i];
189  kdl_accelerations(i) = joint_accelerations[i];
190  }
191 
192  for (unsigned int i = 0; i < num_segments_; ++i)
193  {
194  kdl_wrenches[i](0) = wrenches[i].force.x;
195  kdl_wrenches[i](1) = wrenches[i].force.y;
196  kdl_wrenches[i](2) = wrenches[i].force.z;
197 
198  kdl_wrenches[i](3) = wrenches[i].torque.x;
199  kdl_wrenches[i](4) = wrenches[i].torque.y;
200  kdl_wrenches[i](5) = wrenches[i].torque.z;
201  }
202 
203  if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
204  {
205  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Something went wrong computing torques");
206  return false;
207  }
208 
209  for (unsigned int i = 0; i < num_joints_; ++i)
210  torques[i] = kdl_torques(i);
211 
212  return true;
213 }
214 
215 bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, double& payload,
216  unsigned int& joint_saturated) const
217 {
218  if (!joint_model_group_)
219  {
220  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. "
221  "Check error logs.");
222  return false;
223  }
224  if (joint_angles.size() != num_joints_)
225  {
226  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
227  return false;
228  }
229  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
230  std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
231 
232  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
233  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
234  return false;
235 
236  for (unsigned int i = 0; i < num_joints_; ++i)
237  {
238  if (fabs(zero_torques[i]) >= max_torques_[i])
239  {
240  payload = 0.0;
241  joint_saturated = i;
242  return true;
243  }
244  }
245 
246  state_->setJointGroupPositions(joint_model_group_, joint_angles);
247  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
248  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
249  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
250  wrenches.back().force.z = 1.0;
251  wrenches.back().force = transformVector(transform, wrenches.back().force);
252  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
253 
254  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
255  wrenches.back().force.y, wrenches.back().force.z);
256 
257  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
258  return false;
259 
260  double min_payload = std::numeric_limits<double>::max();
261  for (unsigned int i = 0; i < num_joints_; ++i)
262  {
263  double payload_joint = std::max<double>((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]),
264  (-max_torques_[i] - zero_torques[i]) /
265  (torques[i] - zero_torques[i])); // because we set the payload to 1.0
266  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i,
267  torques[i], max_torques_[i], zero_torques[i]);
268  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Joint: %d, Payload Allowed (N): %f", i, payload_joint);
269  if (payload_joint < min_payload)
270  {
271  min_payload = payload_joint;
272  joint_saturated = i;
273  }
274  }
275  payload = min_payload / gravity_;
276  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Max payload (kg): %f", payload);
277  return true;
278 }
279 
280 bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles, double payload,
281  std::vector<double>& joint_torques) const
282 {
283  if (!joint_model_group_)
284  {
285  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: Did not construct DynamicsSolver object properly. "
286  "Check error logs.");
287  return false;
288  }
289  if (joint_angles.size() != num_joints_)
290  {
291  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint angles vector should be size %d", num_joints_);
292  return false;
293  }
294  if (joint_torques.size() != num_joints_)
295  {
296  CONSOLE_BRIDGE_logError("moveit.dynamics_solver: Joint torques vector should be size %d", num_joints_);
297  return false;
298  }
299  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
300  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
301  state_->setJointGroupPositions(joint_model_group_, joint_angles);
302 
303  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
304  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
305  Eigen::Affine3d transform = tip_frame.inverse() * base_frame;
306  wrenches.back().force.z = payload * gravity_;
307  wrenches.back().force = transformVector(transform, wrenches.back().force);
308  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
309 
310  CONSOLE_BRIDGE_logDebug("moveit.dynamics_solver: New wrench (local frame): %f %f %f", wrenches.back().force.x,
311  wrenches.back().force.y, wrenches.back().force.z);
312 
313  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques))
314  return false;
315  return true;
316 }
317 
318 const std::vector<double>& DynamicsSolver::getMaxTorques() const
319 {
320  return max_torques_;
321 }
322 }
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.
KDL_PARSER_PUBLIC 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.
#define CONSOLE_BRIDGE_logDebug(fmt,...)
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_
#define CONSOLE_BRIDGE_logError(fmt,...)
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:110
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 Wed Apr 18 2018 02:49:03