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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:45