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.linear() * 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  ROS_ERROR_NAMED("dynamics_solver", "Group '%s' is not a chain. Will not initialize dynamics solver",
78  group_name.c_str());
79  joint_model_group_ = nullptr;
80  return;
81  }
82 
84  {
85  ROS_ERROR_NAMED("dynamics_solver", "Group '%s' has a mimic joint. Will not initialize dynamics solver",
86  group_name.c_str());
87  joint_model_group_ = nullptr;
88  return;
89  }
90 
92  if (!joint->getParentLinkModel())
93  {
94  ROS_ERROR_NAMED("dynamics_solver", "Group '%s' does not have a parent link", group_name.c_str());
95  joint_model_group_ = nullptr;
96  return;
97  }
98 
100 
102  ROS_DEBUG_NAMED("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  ROS_ERROR_NAMED("dynamics_solver", "Could not initialize tree object");
111  joint_model_group_ = nullptr;
112  return;
113  }
115  {
116  ROS_ERROR_NAMED("dynamics_solver", "Could not initialize chain object");
117  joint_model_group_ = nullptr;
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  ROS_DEBUG_NAMED("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  ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
151  "Check error logs.");
152  return false;
153  }
154  if (joint_angles.size() != num_joints_)
155  {
156  ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
157  return false;
158  }
159  if (joint_velocities.size() != num_joints_)
160  {
161  ROS_ERROR_NAMED("dynamics_solver", "Joint velocities vector should be size %d", num_joints_);
162  return false;
163  }
164  if (joint_accelerations.size() != num_joints_)
165  {
166  ROS_ERROR_NAMED("dynamics_solver", "Joint accelerations vector should be size %d", num_joints_);
167  return false;
168  }
169  if (wrenches.size() != num_segments_)
170  {
171  ROS_ERROR_NAMED("dynamics_solver", "Wrenches vector should be size %d", num_segments_);
172  return false;
173  }
174  if (torques.size() != num_joints_)
175  {
176  ROS_ERROR_NAMED("dynamics_solver", "Torques vector should be size %d", num_joints_);
177  return false;
178  }
179 
180  KDL::JntArray kdl_angles(num_joints_), kdl_velocities(num_joints_), kdl_accelerations(num_joints_),
181  kdl_torques(num_joints_);
182  KDL::Wrenches kdl_wrenches(num_segments_);
183 
184  for (unsigned int i = 0; i < num_joints_; ++i)
185  {
186  kdl_angles(i) = joint_angles[i];
187  kdl_velocities(i) = joint_velocities[i];
188  kdl_accelerations(i) = joint_accelerations[i];
189  }
190 
191  for (unsigned int i = 0; i < num_segments_; ++i)
192  {
193  kdl_wrenches[i](0) = wrenches[i].force.x;
194  kdl_wrenches[i](1) = wrenches[i].force.y;
195  kdl_wrenches[i](2) = wrenches[i].force.z;
196 
197  kdl_wrenches[i](3) = wrenches[i].torque.x;
198  kdl_wrenches[i](4) = wrenches[i].torque.y;
199  kdl_wrenches[i](5) = wrenches[i].torque.z;
200  }
201 
202  if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0)
203  {
204  ROS_ERROR_NAMED("dynamics_solver", "Something went wrong computing torques");
205  return false;
206  }
207 
208  for (unsigned int i = 0; i < num_joints_; ++i)
209  torques[i] = kdl_torques(i);
210 
211  return true;
212 }
213 
214 bool DynamicsSolver::getMaxPayload(const std::vector<double>& joint_angles, double& payload,
215  unsigned int& joint_saturated) const
216 {
217  if (!joint_model_group_)
218  {
219  ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
220  "Check error logs.");
221  return false;
222  }
223  if (joint_angles.size() != num_joints_)
224  {
225  ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
226  return false;
227  }
228  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
229  std::vector<double> torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0);
230 
231  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
232  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques))
233  return false;
234 
235  for (unsigned int i = 0; i < num_joints_; ++i)
236  {
237  if (fabs(zero_torques[i]) >= max_torques_[i])
238  {
239  payload = 0.0;
240  joint_saturated = i;
241  return true;
242  }
243  }
244 
245  state_->setJointGroupPositions(joint_model_group_, joint_angles);
246  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
247  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
248  Eigen::Affine3d transform = tip_frame.inverse(Eigen::Isometry) * base_frame;
249  wrenches.back().force.z = 1.0;
250  wrenches.back().force = transformVector(transform, wrenches.back().force);
251  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
252 
253  ROS_DEBUG_NAMED("dynamics_solver", "New wrench (local frame): %f %f %f", wrenches.back().force.x,
254  wrenches.back().force.y, wrenches.back().force.z);
255 
256  if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques))
257  return false;
258 
259  double min_payload = std::numeric_limits<double>::max();
260  for (unsigned int i = 0; i < num_joints_; ++i)
261  {
262  double payload_joint = std::max<double>((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]),
263  (-max_torques_[i] - zero_torques[i]) /
264  (torques[i] - zero_torques[i])); // because we set the payload to 1.0
265  ROS_DEBUG_NAMED("dynamics_solver", "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i],
266  max_torques_[i], zero_torques[i]);
267  ROS_DEBUG_NAMED("dynamics_solver", "Joint: %d, Payload Allowed (N): %f", i, payload_joint);
268  if (payload_joint < min_payload)
269  {
270  min_payload = payload_joint;
271  joint_saturated = i;
272  }
273  }
274  payload = min_payload / gravity_;
275  ROS_DEBUG_NAMED("dynamics_solver", "Max payload (kg): %f", payload);
276  return true;
277 }
278 
279 bool DynamicsSolver::getPayloadTorques(const std::vector<double>& joint_angles, double payload,
280  std::vector<double>& joint_torques) const
281 {
282  if (!joint_model_group_)
283  {
284  ROS_DEBUG_NAMED("dynamics_solver", "Did not construct DynamicsSolver object properly. "
285  "Check error logs.");
286  return false;
287  }
288  if (joint_angles.size() != num_joints_)
289  {
290  ROS_ERROR_NAMED("dynamics_solver", "Joint angles vector should be size %d", num_joints_);
291  return false;
292  }
293  if (joint_torques.size() != num_joints_)
294  {
295  ROS_ERROR_NAMED("dynamics_solver", "Joint torques vector should be size %d", num_joints_);
296  return false;
297  }
298  std::vector<double> joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0);
299  std::vector<geometry_msgs::Wrench> wrenches(num_segments_);
300  state_->setJointGroupPositions(joint_model_group_, joint_angles);
301 
302  const Eigen::Affine3d& base_frame = state_->getFrameTransform(base_name_);
303  const Eigen::Affine3d& tip_frame = state_->getFrameTransform(tip_name_);
304  Eigen::Affine3d transform = tip_frame.inverse(Eigen::Isometry) * base_frame;
305  wrenches.back().force.z = payload * gravity_;
306  wrenches.back().force = transformVector(transform, wrenches.back().force);
307  wrenches.back().torque = transformVector(transform, wrenches.back().torque);
308 
309  ROS_DEBUG_NAMED("dynamics_solver", "New wrench (local frame): %f %f %f", wrenches.back().force.x,
310  wrenches.back().force.y, wrenches.back().force.z);
311 
312  return getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques);
313 }
314 
315 const std::vector<double>& DynamicsSolver::getMaxTorques() const
316 {
317  return max_torques_;
318 }
319 
320 } // end of namespace dynamics_solver
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
Core components of MoveIt!
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.
double Norm(double eps=epsilon) const
#define ROS_DEBUG_NAMED(name,...)
const std::vector< double > & getMaxTorques() const
Get maximum torques for this group.
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_
std::vector< Wrench > Wrenches
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 ...
#define ROS_ERROR_NAMED(name,...)
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:123
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 Oct 18 2020 13:16:33