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