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