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

moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Apr 18 2024 02:23:40