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,
209  const std::vector<std::string>& joint_filter) const
210 {
211  trajectory = moveit_msgs::RobotTrajectory();
212  if (waypoints_.empty())
213  return;
214  const std::vector<const robot_model::JointModel*>& jnt =
215  group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels();
216 
217  std::vector<const robot_model::JointModel*> onedof;
218  std::vector<const robot_model::JointModel*> mdof;
219  trajectory.joint_trajectory.joint_names.clear();
220  trajectory.multi_dof_joint_trajectory.joint_names.clear();
221 
222  for (std::size_t i = 0; i < jnt.size(); ++i)
223  {
224  // only consider joints listed in joint_filter
225  if (!joint_filter.empty() &&
226  std::find(joint_filter.begin(), joint_filter.end(), jnt[i]->getName()) == joint_filter.end())
227  continue;
228 
229  if (jnt[i]->getVariableCount() == 1)
230  {
231  trajectory.joint_trajectory.joint_names.push_back(jnt[i]->getName());
232  onedof.push_back(jnt[i]);
233  }
234  else
235  {
236  trajectory.multi_dof_joint_trajectory.joint_names.push_back(jnt[i]->getName());
237  mdof.push_back(jnt[i]);
238  }
239  }
240 
241  if (!onedof.empty())
242  {
243  trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
244  trajectory.joint_trajectory.header.stamp = ros::Time(0);
245  trajectory.joint_trajectory.points.resize(waypoints_.size());
246  }
247 
248  if (!mdof.empty())
249  {
250  trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
251  trajectory.multi_dof_joint_trajectory.header.stamp = ros::Time(0);
252  trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
253  }
254 
255  static const ros::Duration ZERO_DURATION(0.0);
256  double total_time = 0.0;
257  for (std::size_t i = 0; i < waypoints_.size(); ++i)
258  {
259  if (duration_from_previous_.size() > i)
260  total_time += duration_from_previous_[i];
261 
262  if (!onedof.empty())
263  {
264  trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
265  trajectory.joint_trajectory.points[i].velocities.reserve(onedof.size());
266 
267  for (std::size_t j = 0; j < onedof.size(); ++j)
268  {
269  trajectory.joint_trajectory.points[i].positions[j] =
270  waypoints_[i]->getVariablePosition(onedof[j]->getFirstVariableIndex());
271  // if we have velocities/accelerations/effort, copy those too
272  if (waypoints_[i]->hasVelocities())
273  trajectory.joint_trajectory.points[i].velocities.push_back(
274  waypoints_[i]->getVariableVelocity(onedof[j]->getFirstVariableIndex()));
275  if (waypoints_[i]->hasAccelerations())
276  trajectory.joint_trajectory.points[i].accelerations.push_back(
277  waypoints_[i]->getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
278  if (waypoints_[i]->hasEffort())
279  trajectory.joint_trajectory.points[i].effort.push_back(
280  waypoints_[i]->getVariableEffort(onedof[j]->getFirstVariableIndex()));
281  }
282  // clear velocities if we have an incomplete specification
283  if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
284  trajectory.joint_trajectory.points[i].velocities.clear();
285  // clear accelerations if we have an incomplete specification
286  if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
287  trajectory.joint_trajectory.points[i].accelerations.clear();
288  // clear effort if we have an incomplete specification
289  if (trajectory.joint_trajectory.points[i].effort.size() != onedof.size())
290  trajectory.joint_trajectory.points[i].effort.clear();
291 
292  if (duration_from_previous_.size() > i)
293  trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
294  else
295  trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION;
296  }
297  if (!mdof.empty())
298  {
299  trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
300  for (std::size_t j = 0; j < mdof.size(); ++j)
301  {
302  geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
303  trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform;
304  // TODO: currently only checking for planar multi DOF joints / need to add check for floating
305  if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::JointType::PLANAR))
306  {
307  const std::vector<std::string> names = mdof[j]->getVariableNames();
308  const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
309 
310  geometry_msgs::Twist point_velocity;
311 
312  for (std::size_t k = 0; k < names.size(); ++k)
313  {
314  if (names[k].find("/x") != std::string::npos)
315  {
316  point_velocity.linear.x = velocities[k];
317  }
318  else if (names[k].find("/y") != std::string::npos)
319  {
320  point_velocity.linear.y = velocities[k];
321  }
322  else if (names[k].find("/z") != std::string::npos)
323  {
324  point_velocity.linear.z = velocities[k];
325  }
326  else if (names[k].find("/theta") != std::string::npos)
327  {
328  point_velocity.angular.z = velocities[k];
329  }
330  }
331  trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
332  }
333  }
334  if (duration_from_previous_.size() > i)
335  trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
336  else
337  trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION;
338  }
339  }
340 }
341 
343  const trajectory_msgs::JointTrajectory& trajectory)
344 {
345  // make a copy just in case the next clear() removes the memory for the reference passed in
346  const robot_state::RobotState& copy = reference_state;
347  clear();
348  std::size_t state_count = trajectory.points.size();
349  ros::Time last_time_stamp = trajectory.header.stamp;
350  ros::Time this_time_stamp = last_time_stamp;
351 
352  for (std::size_t i = 0; i < state_count; ++i)
353  {
354  this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
355  robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
356  st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
357  if (!trajectory.points[i].velocities.empty())
358  st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
359  if (!trajectory.points[i].accelerations.empty())
360  st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
361  if (!trajectory.points[i].effort.empty())
362  st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
363  addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
364  last_time_stamp = this_time_stamp;
365  }
366 }
367 
369  const moveit_msgs::RobotTrajectory& trajectory)
370 {
371  // make a copy just in case the next clear() removes the memory for the reference passed in
372  const robot_state::RobotState& copy = reference_state;
373  clear();
374 
375  std::size_t state_count =
376  std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
377  ros::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
378  trajectory.multi_dof_joint_trajectory.header.stamp :
379  trajectory.joint_trajectory.header.stamp;
380  ros::Time this_time_stamp = last_time_stamp;
381 
382  for (std::size_t i = 0; i < state_count; ++i)
383  {
384  robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
385  if (trajectory.joint_trajectory.points.size() > i)
386  {
387  st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions);
388  if (!trajectory.joint_trajectory.points[i].velocities.empty())
389  st->setVariableVelocities(trajectory.joint_trajectory.joint_names,
390  trajectory.joint_trajectory.points[i].velocities);
391  if (!trajectory.joint_trajectory.points[i].accelerations.empty())
392  st->setVariableAccelerations(trajectory.joint_trajectory.joint_names,
393  trajectory.joint_trajectory.points[i].accelerations);
394  if (!trajectory.joint_trajectory.points[i].effort.empty())
395  st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
396  this_time_stamp =
397  trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
398  }
399  if (trajectory.multi_dof_joint_trajectory.points.size() > i)
400  {
401  for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
402  {
403  Eigen::Isometry3d t = tf2::transformToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
404  st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
405  }
406  this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
407  trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
408  }
409 
410  addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
411  last_time_stamp = this_time_stamp;
412  }
413 }
414 
416  const moveit_msgs::RobotState& state,
417  const moveit_msgs::RobotTrajectory& trajectory)
418 {
419  robot_state::RobotState st(reference_state);
421  setRobotTrajectoryMsg(st, trajectory);
422 }
423 
424 void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after,
425  double& blend) const
426 {
427  if (duration < 0.0)
428  {
429  before = 0;
430  after = 0;
431  blend = 0;
432  return;
433  }
434 
435  // Find indicies
436  std::size_t index = 0, num_points = waypoints_.size();
437  double running_duration = 0.0;
438  for (; index < num_points; ++index)
439  {
440  running_duration += duration_from_previous_[index];
441  if (running_duration >= duration)
442  break;
443  }
444  before = std::max<int>(index - 1, 0);
445  after = std::min<int>(index, num_points - 1);
446 
447  // Compute duration blend
448  double before_time = running_duration - duration_from_previous_[index];
449  if (after == before)
450  blend = 1.0;
451  else
452  blend = (duration - before_time) / duration_from_previous_[index];
453 }
454 
455 double RobotTrajectory::getWayPointDurationFromStart(std::size_t index) const
456 {
457  if (duration_from_previous_.empty())
458  return 0.0;
459  if (index >= duration_from_previous_.size())
460  index = duration_from_previous_.size() - 1;
461 
462  double time = 0.0;
463  for (std::size_t i = 0; i <= index; ++i)
464  time += duration_from_previous_[i];
465  return time;
466 }
467 
468 double RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const
469 {
470  return getWayPointDurationFromStart(index);
471 }
472 
473 bool RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
474  robot_state::RobotStatePtr& output_state) const
475 {
476  // If there are no waypoints we can't do anything
477  if (getWayPointCount() == 0)
478  return false;
479 
480  int before = 0, after = 0;
481  double blend = 1.0;
482  findWayPointIndicesForDurationAfterStart(request_duration, before, after, blend);
483  // ROS_DEBUG_NAMED("robot_trajectory", "Interpolating %.3f of the way between index %d and %d.", blend, before,
484  // after);
485  waypoints_[before]->interpolate(*waypoints_[after], blend, *output_state);
486  return true;
487 }
488 
489 } // 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)
unsigned int index
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 getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
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:590
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 Tue Sep 8 2020 04:12:45