robot_trajectory.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, 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: Ioan Sucan, Adam Leeper */
36 
39 #include <tf2_eigen/tf2_eigen.h>
40 #include <boost/math/constants/constants.hpp>
41 #include <numeric>
42 
43 namespace robot_trajectory
44 {
45 RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group)
46  : robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group))
47 {
48 }
49 
50 RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model,
51  const robot_model::JointModelGroup* group)
52  : robot_model_(robot_model), group_(group)
53 {
54 }
55 
56 void RobotTrajectory::setGroupName(const std::string& group_name)
57 {
58  group_ = robot_model_->getJointModelGroup(group_name);
59 }
60 
61 const std::string& RobotTrajectory::getGroupName() const
62 {
63  if (group_)
64  return group_->getName();
65  static const std::string EMPTY;
66  return EMPTY;
67 }
68 
70 {
71  return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0);
72 }
73 
75 {
76  if (duration_from_previous_.empty())
77  return 0.0;
78  else
79  return getDuration() / static_cast<double>(duration_from_previous_.size());
80 }
81 
83 {
84  robot_model_.swap(other.robot_model_);
85  std::swap(group_, other.group_);
86  waypoints_.swap(other.waypoints_);
88 }
89 
90 void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t start_index, size_t end_index)
91 {
92  end_index = std::min(end_index, source.waypoints_.size());
93  if (start_index >= end_index)
94  return;
95  waypoints_.insert(waypoints_.end(), std::next(source.waypoints_.begin(), start_index),
96  std::next(source.waypoints_.begin(), end_index));
97  std::size_t index = duration_from_previous_.size();
99  std::next(source.duration_from_previous_.begin(), start_index),
100  std::next(source.duration_from_previous_.begin(), end_index));
101  if (duration_from_previous_.size() > index)
102  duration_from_previous_[index] += dt;
103 }
104 
106 {
107  std::reverse(waypoints_.begin(), waypoints_.end());
108  for (robot_state::RobotStatePtr& waypoint : waypoints_)
109  {
110  // reversing the trajectory implies inverting the velocity profile
111  waypoint->invertVelocity();
112  }
113  if (!duration_from_previous_.empty())
114  {
116  std::reverse(duration_from_previous_.begin(), duration_from_previous_.end());
117  duration_from_previous_.pop_back();
118  }
119 }
120 
122 {
123  if (waypoints_.empty())
124  return;
125 
126  const std::vector<const robot_model::JointModel*>& cont_joints =
127  group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
128 
129  for (std::size_t i = 0; i < cont_joints.size(); ++i)
130  {
131  // unwrap continuous joints
132  double running_offset = 0.0;
133  double last_value = waypoints_[0]->getJointPositions(cont_joints[i])[0];
134 
135  for (std::size_t j = 1; j < waypoints_.size(); ++j)
136  {
137  double current_value = waypoints_[j]->getJointPositions(cont_joints[i])[0];
138  if (last_value > current_value + boost::math::constants::pi<double>())
139  running_offset += 2.0 * boost::math::constants::pi<double>();
140  else if (current_value > last_value + boost::math::constants::pi<double>())
141  running_offset -= 2.0 * boost::math::constants::pi<double>();
142 
143  last_value = current_value;
144  if (running_offset > std::numeric_limits<double>::epsilon() ||
145  running_offset < -std::numeric_limits<double>::epsilon())
146  {
147  current_value += running_offset;
148  waypoints_[j]->setJointPositions(cont_joints[i], &current_value);
149  }
150  }
151  }
152  for (std::size_t j = 0; j < waypoints_.size(); ++j)
153  waypoints_[j]->update();
154 }
155 
157 {
158  if (waypoints_.empty())
159  return;
160 
161  const std::vector<const robot_model::JointModel*>& cont_joints =
162  group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
163 
164  for (std::size_t i = 0; i < cont_joints.size(); ++i)
165  {
166  double reference_value0 = state.getJointPositions(cont_joints[i])[0];
167  double reference_value = reference_value0;
168  cont_joints[i]->enforcePositionBounds(&reference_value);
169 
170  // unwrap continuous joints
171  double running_offset = reference_value0 - reference_value;
172 
173  double last_value = waypoints_[0]->getJointPositions(cont_joints[i])[0];
174  if (running_offset > std::numeric_limits<double>::epsilon() ||
175  running_offset < -std::numeric_limits<double>::epsilon())
176  {
177  double current_value = last_value + running_offset;
178  waypoints_[0]->setJointPositions(cont_joints[i], &current_value);
179  }
180 
181  for (std::size_t j = 1; j < waypoints_.size(); ++j)
182  {
183  double current_value = waypoints_[j]->getJointPositions(cont_joints[i])[0];
184  if (last_value > current_value + boost::math::constants::pi<double>())
185  running_offset += 2.0 * boost::math::constants::pi<double>();
186  else if (current_value > last_value + boost::math::constants::pi<double>())
187  running_offset -= 2.0 * boost::math::constants::pi<double>();
188 
189  last_value = current_value;
190  if (running_offset > std::numeric_limits<double>::epsilon() ||
191  running_offset < -std::numeric_limits<double>::epsilon())
192  {
193  current_value += running_offset;
194  waypoints_[j]->setJointPositions(cont_joints[i], &current_value);
195  }
196  }
197  }
198  for (std::size_t j = 0; j < waypoints_.size(); ++j)
199  waypoints_[j]->update();
200 }
201 
203 {
204  waypoints_.clear();
205  duration_from_previous_.clear();
206 }
207 
208 void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const
209 {
210  trajectory = moveit_msgs::RobotTrajectory();
211  if (waypoints_.empty())
212  return;
213  const std::vector<const robot_model::JointModel*>& jnt =
214  group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels();
215 
216  std::vector<const robot_model::JointModel*> onedof;
217  std::vector<const robot_model::JointModel*> mdof;
218  trajectory.joint_trajectory.joint_names.clear();
219  trajectory.multi_dof_joint_trajectory.joint_names.clear();
220 
221  for (std::size_t i = 0; i < jnt.size(); ++i)
222  if (jnt[i]->getVariableCount() == 1)
223  {
224  trajectory.joint_trajectory.joint_names.push_back(jnt[i]->getName());
225  onedof.push_back(jnt[i]);
226  }
227  else
228  {
229  trajectory.multi_dof_joint_trajectory.joint_names.push_back(jnt[i]->getName());
230  mdof.push_back(jnt[i]);
231  }
232  if (!onedof.empty())
233  {
234  trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
235  trajectory.joint_trajectory.header.stamp = ros::Time(0);
236  trajectory.joint_trajectory.points.resize(waypoints_.size());
237  }
238 
239  if (!mdof.empty())
240  {
241  trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
242  trajectory.multi_dof_joint_trajectory.header.stamp = ros::Time(0);
243  trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
244  }
245 
246  static const ros::Duration ZERO_DURATION(0.0);
247  double total_time = 0.0;
248  for (std::size_t i = 0; i < waypoints_.size(); ++i)
249  {
250  if (duration_from_previous_.size() > i)
251  total_time += duration_from_previous_[i];
252 
253  if (!onedof.empty())
254  {
255  trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
256  trajectory.joint_trajectory.points[i].velocities.reserve(onedof.size());
257 
258  for (std::size_t j = 0; j < onedof.size(); ++j)
259  {
260  trajectory.joint_trajectory.points[i].positions[j] =
261  waypoints_[i]->getVariablePosition(onedof[j]->getFirstVariableIndex());
262  // if we have velocities/accelerations/effort, copy those too
263  if (waypoints_[i]->hasVelocities())
264  trajectory.joint_trajectory.points[i].velocities.push_back(
265  waypoints_[i]->getVariableVelocity(onedof[j]->getFirstVariableIndex()));
266  if (waypoints_[i]->hasAccelerations())
267  trajectory.joint_trajectory.points[i].accelerations.push_back(
268  waypoints_[i]->getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
269  if (waypoints_[i]->hasEffort())
270  trajectory.joint_trajectory.points[i].effort.push_back(
271  waypoints_[i]->getVariableEffort(onedof[j]->getFirstVariableIndex()));
272  }
273  // clear velocities if we have an incomplete specification
274  if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
275  trajectory.joint_trajectory.points[i].velocities.clear();
276  // clear accelerations if we have an incomplete specification
277  if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
278  trajectory.joint_trajectory.points[i].accelerations.clear();
279  // clear effort if we have an incomplete specification
280  if (trajectory.joint_trajectory.points[i].effort.size() != onedof.size())
281  trajectory.joint_trajectory.points[i].effort.clear();
282 
283  if (duration_from_previous_.size() > i)
284  trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
285  else
286  trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION;
287  }
288  if (!mdof.empty())
289  {
290  trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
291  for (std::size_t j = 0; j < mdof.size(); ++j)
292  {
293  geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
294  trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform;
295  // TODO: currently only checking for planar multi DOF joints / need to add check for floating
296  if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::JointType::PLANAR))
297  {
298  const std::vector<std::string> names = mdof[j]->getVariableNames();
299  const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
300 
301  geometry_msgs::Twist point_velocity;
302 
303  for (std::size_t k = 0; k < names.size(); ++k)
304  {
305  if (names[k].find("/x") != std::string::npos)
306  {
307  point_velocity.linear.x = velocities[k];
308  }
309  else if (names[k].find("/y") != std::string::npos)
310  {
311  point_velocity.linear.y = velocities[k];
312  }
313  else if (names[k].find("/z") != std::string::npos)
314  {
315  point_velocity.linear.z = velocities[k];
316  }
317  else if (names[k].find("/theta") != std::string::npos)
318  {
319  point_velocity.angular.z = velocities[k];
320  }
321  }
322  trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
323  }
324  }
325  if (duration_from_previous_.size() > i)
326  trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
327  else
328  trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION;
329  }
330  }
331 }
332 
334  const trajectory_msgs::JointTrajectory& trajectory)
335 {
336  // make a copy just in case the next clear() removes the memory for the reference passed in
337  const robot_state::RobotState& copy = reference_state;
338  clear();
339  std::size_t state_count = trajectory.points.size();
340  ros::Time last_time_stamp = trajectory.header.stamp;
341  ros::Time this_time_stamp = last_time_stamp;
342 
343  for (std::size_t i = 0; i < state_count; ++i)
344  {
345  this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
346  robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
347  st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
348  if (!trajectory.points[i].velocities.empty())
349  st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
350  if (!trajectory.points[i].accelerations.empty())
351  st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
352  if (!trajectory.points[i].effort.empty())
353  st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
354  addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
355  last_time_stamp = this_time_stamp;
356  }
357 }
358 
360  const moveit_msgs::RobotTrajectory& trajectory)
361 {
362  // make a copy just in case the next clear() removes the memory for the reference passed in
363  const robot_state::RobotState& copy = reference_state;
364  clear();
365 
366  std::size_t state_count =
367  std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
368  ros::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
369  trajectory.multi_dof_joint_trajectory.header.stamp :
370  trajectory.joint_trajectory.header.stamp;
371  ros::Time this_time_stamp = last_time_stamp;
372 
373  for (std::size_t i = 0; i < state_count; ++i)
374  {
375  robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
376  if (trajectory.joint_trajectory.points.size() > i)
377  {
378  st->setVariablePositions(trajectory.joint_trajectory.joint_names,
379  trajectory.joint_trajectory.points[i].positions);
380  if (!trajectory.joint_trajectory.points[i].velocities.empty())
381  st->setVariableVelocities(trajectory.joint_trajectory.joint_names,
382  trajectory.joint_trajectory.points[i].velocities);
383  if (!trajectory.joint_trajectory.points[i].accelerations.empty())
384  st->setVariableAccelerations(trajectory.joint_trajectory.joint_names,
385  trajectory.joint_trajectory.points[i].accelerations);
386  if (!trajectory.joint_trajectory.points[i].effort.empty())
387  st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
388  this_time_stamp =
389  trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
390  }
391  if (trajectory.multi_dof_joint_trajectory.points.size() > i)
392  {
393  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
394  {
395  Eigen::Isometry3d t = tf2::transformToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
396  st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
397  }
398  this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
399  trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
400  }
401 
402  addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
403  last_time_stamp = this_time_stamp;
404  }
405 }
406 
408  const moveit_msgs::RobotState& state,
409  const moveit_msgs::RobotTrajectory& trajectory)
410 {
411  robot_state::RobotState st(reference_state);
413  setRobotTrajectoryMsg(st, trajectory);
414 }
415 
416 void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after,
417  double& blend) const
418 {
419  if (duration < 0.0)
420  {
421  before = 0;
422  after = 0;
423  blend = 0;
424  return;
425  }
426 
427  // Find indicies
428  std::size_t index = 0, num_points = waypoints_.size();
429  double running_duration = 0.0;
430  for (; index < num_points; ++index)
431  {
432  running_duration += duration_from_previous_[index];
433  if (running_duration >= duration)
434  break;
435  }
436  before = std::max<int>(index - 1, 0);
437  after = std::min<int>(index, num_points - 1);
438 
439  // Compute duration blend
440  double before_time = running_duration - duration_from_previous_[index];
441  if (after == before)
442  blend = 1.0;
443  else
444  blend = (duration - before_time) / duration_from_previous_[index];
445 }
446 
447 double RobotTrajectory::getWayPointDurationFromStart(std::size_t index) const
448 {
449  if (duration_from_previous_.empty())
450  return 0.0;
451  if (index >= duration_from_previous_.size())
452  index = duration_from_previous_.size() - 1;
453 
454  double time = 0.0;
455  for (std::size_t i = 0; i <= index; ++i)
456  time += duration_from_previous_[i];
457  return time;
458 }
459 
460 double RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const
461 {
462  return getWayPointDurationFromStart(index);
463 }
464 
465 bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
466  robot_state::RobotStatePtr& output_state) const
467 {
468  // If there are no waypoints we can't do anything
469  if (getWayPointCount() == 0)
470  return false;
471 
472  int before = 0, after = 0;
473  double blend = 1.0;
474  findWayPointIndicesForDurationAfterStart(request_duration, before, after, blend);
475  // ROS_DEBUG_NAMED("robot_trajectory", "Interpolating %.3f of the way between index %d and %d.", blend, before,
476  // after);
477  waypoints_[before]->interpolate(*waypoints_[after], blend, *output_state);
478  return true;
479 }
480 
481 } // end of namespace robot_trajectory
Core components of MoveIt!
void swap(robot_trajectory::RobotTrajectory &other)
std::deque< robot_state::RobotStatePtr > waypoints_
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
void setGroupName(const std::string &group_name)
const robot_model::JointModelGroup * group_
robot_model::RobotModelConstPtr robot_model_
RobotTrajectory(const robot_model::RobotModelConstPtr &robot_model, const std::string &group)
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
geometry_msgs::TransformStamped t
std::size_t getWayPointCount() const
const std::string & getGroupName() const
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
unsigned int index
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const
const std::string & getName() const
Get the name of the joint group.
Maintain a sequence of waypoints and the time durations between these waypoints.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr &output_state) const
Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time interpolation.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints...
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints). ...
void findWayPointIndicesForDurationAfterStart(const double &duration, int &before, int &after, double &blend) const
Finds the waypoint indicies before and after a duration from start.
double getWaypointDurationFromStart(std::size_t index) const
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
Add a point to the trajectory.
void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
const double * getJointPositions(const std::string &joint_name) const
Definition: robot_state.h:568
std::deque< double > duration_from_previous_
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
void append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Feb 17 2020 03:50:49