40 #include <rclcpp/duration.hpp>
41 #include <rclcpp/logger.hpp>
42 #include <rclcpp/logging.hpp>
43 #include <rclcpp/time.hpp>
44 #include <tf2_eigen/tf2_eigen.hpp>
60 : robot_model_(robot_model), group_(nullptr)
65 : robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group))
71 : robot_model_(robot_model), group_(group)
81 for (
const auto& waypoint : other.waypoints_)
83 waypoints_.emplace_back(std::make_shared<moveit::core::RobotState>(*waypoint));
92 static const std::string EMPTY;
98 return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0);
103 if (duration_from_previous_.empty())
105 RCLCPP_WARN(
getLogger(),
"Too few waypoints to calculate a duration. Returning 0.");
110 if (duration_from_previous_[0] == 0)
112 if (duration_from_previous_.size() <= 1)
114 RCLCPP_WARN(
getLogger(),
"First and only waypoint has a duration of 0.");
118 return getDuration() /
static_cast<double>(duration_from_previous_.size() - 1);
121 return getDuration() /
static_cast<double>(duration_from_previous_.size());
126 robot_model_.swap(other.robot_model_);
127 std::swap(group_, other.group_);
128 waypoints_.swap(other.waypoints_);
129 duration_from_previous_.swap(other.duration_from_previous_);
134 end_index = std::min(end_index, source.waypoints_.size());
135 if (start_index >= end_index)
137 waypoints_.insert(waypoints_.end(), std::next(source.waypoints_.begin(), start_index),
138 std::next(source.waypoints_.begin(), end_index));
139 std::size_t index = duration_from_previous_.size();
140 duration_from_previous_.insert(duration_from_previous_.end(),
141 std::next(source.duration_from_previous_.begin(), start_index),
142 std::next(source.duration_from_previous_.begin(), end_index));
143 if (duration_from_previous_.size() > index)
144 duration_from_previous_[index] = dt;
151 std::reverse(waypoints_.begin(), waypoints_.end());
152 for (moveit::core::RobotStatePtr& waypoint : waypoints_)
155 waypoint->invertVelocity();
157 if (!duration_from_previous_.empty())
159 duration_from_previous_.push_back(duration_from_previous_.front());
160 std::reverse(duration_from_previous_.begin(), duration_from_previous_.end());
161 duration_from_previous_.pop_back();
169 if (waypoints_.empty())
172 const std::vector<const moveit::core::JointModel*>& cont_joints =
178 double running_offset = 0.0;
179 double last_value = waypoints_[0]->getJointPositions(cont_joint)[0];
180 cont_joint->enforcePositionBounds(&last_value);
181 waypoints_[0]->setJointPositions(cont_joint, &last_value);
183 for (std::size_t j = 1; j < waypoints_.size(); ++j)
185 double current_value = waypoints_[j]->getJointPositions(cont_joint)[0];
186 cont_joint->enforcePositionBounds(¤t_value);
187 if (last_value > current_value + M_PI)
189 running_offset += 2.0 * M_PI;
191 else if (current_value > last_value + M_PI)
193 running_offset -= 2.0 * M_PI;
196 last_value = current_value;
197 current_value += running_offset;
198 waypoints_[j]->setJointPositions(cont_joint, ¤t_value);
201 for (moveit::core::RobotStatePtr& waypoint : waypoints_)
211 if (waypoints_.empty())
214 const std::vector<const moveit::core::JointModel*>& cont_joints =
220 double reference_value = reference_value0;
221 cont_joint->enforcePositionBounds(&reference_value);
224 double running_offset = reference_value0 - reference_value;
226 double last_value = waypoints_[0]->getJointPositions(cont_joint)[0];
227 cont_joint->enforcePositionBounds(&last_value);
228 if (last_value > reference_value + M_PI)
230 running_offset -= 2.0 * M_PI;
232 else if (last_value < reference_value - M_PI)
234 running_offset += 2.0 * M_PI;
236 double current_start_value = last_value + running_offset;
237 waypoints_[0]->setJointPositions(cont_joint, ¤t_start_value);
239 for (std::size_t j = 1; j < waypoints_.size(); ++j)
241 double current_value = waypoints_[j]->getJointPositions(cont_joint)[0];
242 cont_joint->enforcePositionBounds(¤t_value);
243 if (last_value > current_value + M_PI)
245 running_offset += 2.0 * M_PI;
247 else if (current_value > last_value + M_PI)
249 running_offset -= 2.0 * M_PI;
252 last_value = current_value;
253 current_value += running_offset;
254 waypoints_[j]->setJointPositions(cont_joint, ¤t_value);
257 for (moveit::core::RobotStatePtr& waypoint : waypoints_)
266 const std::vector<std::string>& joint_filter)
const
268 trajectory = moveit_msgs::msg::RobotTrajectory();
269 if (waypoints_.empty())
271 const std::vector<const moveit::core::JointModel*>& jnts =
274 std::vector<const moveit::core::JointModel*> onedof;
275 std::vector<const moveit::core::JointModel*> mdof;
276 trajectory.joint_trajectory.joint_names.clear();
277 trajectory.multi_dof_joint_trajectory.joint_names.clear();
282 if (!joint_filter.empty() &&
283 std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end())
286 if (active_joint->getVariableCount() == 1)
288 trajectory.joint_trajectory.joint_names.push_back(active_joint->getName());
289 onedof.push_back(active_joint);
293 trajectory.multi_dof_joint_trajectory.joint_names.push_back(active_joint->getName());
294 mdof.push_back(active_joint);
300 trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
301 trajectory.joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
302 trajectory.joint_trajectory.points.resize(waypoints_.size());
307 trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
308 trajectory.multi_dof_joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME);
309 trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
312 static const auto ZERO_DURATION = rclcpp::Duration::from_seconds(0);
313 double total_time = 0.0;
314 for (std::size_t i = 0; i < waypoints_.size(); ++i)
316 if (duration_from_previous_.size() > i)
317 total_time += duration_from_previous_[i];
321 trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
322 trajectory.joint_trajectory.points[i].velocities.reserve(onedof.size());
324 for (std::size_t j = 0; j < onedof.size(); ++j)
326 trajectory.joint_trajectory.points[i].positions[j] =
327 waypoints_[i]->getVariablePosition(onedof[j]->getFirstVariableIndex());
329 if (waypoints_[i]->hasVelocities())
331 trajectory.joint_trajectory.points[i].velocities.push_back(
332 waypoints_[i]->getVariableVelocity(onedof[j]->getFirstVariableIndex()));
334 if (waypoints_[i]->hasAccelerations())
336 trajectory.joint_trajectory.points[i].accelerations.push_back(
337 waypoints_[i]->getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
339 if (waypoints_[i]->hasEffort())
341 trajectory.joint_trajectory.points[i].effort.push_back(
342 waypoints_[i]->getVariableEffort(onedof[j]->getFirstVariableIndex()));
346 if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
347 trajectory.joint_trajectory.points[i].velocities.clear();
349 if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
350 trajectory.joint_trajectory.points[i].accelerations.clear();
352 if (trajectory.joint_trajectory.points[i].effort.size() != onedof.size())
353 trajectory.joint_trajectory.points[i].effort.clear();
355 if (duration_from_previous_.size() > i)
357 trajectory.joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time);
361 trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION;
366 trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
367 for (std::size_t j = 0; j < mdof.size(); ++j)
369 geometry_msgs::msg::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
370 trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform;
372 if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == moveit::core::JointModel::JointType::PLANAR))
374 const std::vector<std::string> names = mdof[j]->getVariableNames();
375 const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
377 geometry_msgs::msg::Twist point_velocity;
379 for (std::size_t k = 0; k < names.size(); ++k)
381 if (names[k].find(
"/x") != std::string::npos)
383 point_velocity.linear.x = velocities[k];
385 else if (names[k].find(
"/y") != std::string::npos)
387 point_velocity.linear.y = velocities[k];
389 else if (names[k].find(
"/z") != std::string::npos)
391 point_velocity.linear.z = velocities[k];
393 else if (names[k].find(
"/theta") != std::string::npos)
395 point_velocity.angular.z = velocities[k];
398 trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
401 if (duration_from_previous_.size() > i)
403 trajectory.multi_dof_joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time);
407 trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION;
414 const trajectory_msgs::msg::JointTrajectory& trajectory)
419 std::size_t state_count = trajectory.points.size();
420 rclcpp::Time last_time_stamp = trajectory.header.stamp;
421 rclcpp::Time this_time_stamp = last_time_stamp;
423 for (std::size_t i = 0; i < state_count; ++i)
425 this_time_stamp = rclcpp::Time(trajectory.header.stamp) + trajectory.points[i].time_from_start;
426 auto st = std::make_shared<moveit::core::RobotState>(copy);
427 st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
428 if (!trajectory.points[i].velocities.empty())
429 st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
430 if (!trajectory.points[i].accelerations.empty())
431 st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
432 if (!trajectory.points[i].effort.empty())
433 st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
435 last_time_stamp = this_time_stamp;
442 const moveit_msgs::msg::RobotTrajectory& trajectory)
448 std::size_t state_count =
449 std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
450 rclcpp::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
451 trajectory.multi_dof_joint_trajectory.header.stamp :
452 trajectory.joint_trajectory.header.stamp;
453 rclcpp::Time this_time_stamp = last_time_stamp;
455 for (std::size_t i = 0; i < state_count; ++i)
457 auto st = std::make_shared<moveit::core::RobotState>(copy);
458 if (trajectory.joint_trajectory.points.size() > i)
460 st->setVariablePositions(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].positions);
461 if (!trajectory.joint_trajectory.points[i].velocities.empty())
463 st->setVariableVelocities(trajectory.joint_trajectory.joint_names,
464 trajectory.joint_trajectory.points[i].velocities);
466 if (!trajectory.joint_trajectory.points[i].accelerations.empty())
468 st->setVariableAccelerations(trajectory.joint_trajectory.joint_names,
469 trajectory.joint_trajectory.points[i].accelerations);
471 if (!trajectory.joint_trajectory.points[i].effort.empty())
472 st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
473 this_time_stamp = rclcpp::Time(trajectory.joint_trajectory.header.stamp) +
474 trajectory.joint_trajectory.points[i].time_from_start;
476 if (trajectory.multi_dof_joint_trajectory.points.size() > i)
478 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
480 Eigen::Isometry3d t = tf2::transformToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
481 st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
483 this_time_stamp = rclcpp::Time(trajectory.multi_dof_joint_trajectory.header.stamp) +
484 trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
488 last_time_stamp = this_time_stamp;
494 const moveit_msgs::msg::RobotState& state,
495 const moveit_msgs::msg::RobotTrajectory& trajectory)
514 std::size_t index = 0, num_points = waypoints_.size();
515 double running_duration = 0.0;
516 for (; index < num_points; ++index)
518 running_duration += duration_from_previous_[index];
519 if (running_duration >= duration)
522 before = std::max<int>(index - 1, 0);
523 after = std::min<int>(index, num_points - 1);
526 double before_time = running_duration - duration_from_previous_[index];
533 blend = (duration - before_time) / duration_from_previous_[index];
539 if (duration_from_previous_.empty())
541 if (index >= duration_from_previous_.size())
542 index = duration_from_previous_.size() - 1;
545 for (std::size_t i = 0; i <= index; ++i)
546 time += duration_from_previous_[i];
551 moveit::core::RobotStatePtr& output_state)
const
557 int before = 0, after = 0;
562 waypoints_[before]->interpolate(*waypoints_[after], blend, *output_state);
571 out <<
"Empty trajectory.";
575 std::ios::fmtflags old_settings = out.flags();
576 int old_precision = out.precision();
577 out << std::fixed << std::setprecision(3);
579 out <<
"Trajectory has " << num_points <<
" points over " <<
getDuration() <<
" seconds\n";
581 if (variable_indexes.empty())
590 variable_indexes.resize(robot_model_->getVariableCount());
591 std::iota(variable_indexes.begin(), variable_indexes.end(), 0);
595 for (
size_t p_i = 0; p_i < num_points; ++p_i)
598 out <<
" waypoint " << std::setw(3) << p_i;
601 for (
int index : variable_indexes)
608 for (
int index : variable_indexes)
616 for (
int index : variable_indexes)
624 for (
int index : variable_indexes)
632 out.flags(old_settings);
633 out.precision(old_precision);
639 trajectory.
print(out);
645 auto trajectory_length = 0.0;
648 const auto& first = trajectory.
getWayPoint(index - 1);
649 const auto& second = trajectory.
getWayPoint(index);
650 trajectory_length += first.
distance(second);
652 return trajectory_length;
674 double acos_value = (a * a + b * b - cdist * cdist) / (2.0 * a * b);
675 if (acos_value > -1.0 && acos_value < 1.0)
678 double angle = (M_PI - acos(acos_value));
681 double u = 2.0 * angle;
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::string & getName() const
Get the name of the joint group.
const std::vector< const JointModel * > & getContinuousJointModels() const
Get the array of continuous joints used in this group (may include mimic joints).
const std::vector< int > & getVariableIndexList() const
Get the index locations in the complete robot state for all the variables in this group.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Representation of a robot's state. This includes position, velocity, acceleration and effort.
double getVariableAcceleration(const std::string &variable) const
Get the acceleration of a particular variable. An exception is thrown if the variable is not known.
const double * getJointPositions(const std::string &joint_name) const
bool hasVelocities() const
By default, if velocities are never set or initialized, the state remembers that there are no velocit...
double getVariableVelocity(const std::string &variable) const
Get the velocity of a particular variable. An exception is thrown if the variable is not known.
bool hasAccelerations() const
By default, if accelerations are never set or initialized, the state remembers that there are no acce...
bool hasEffort() const
By default, if effort is never set or initialized, the state remembers that there is no effort set....
double * getVariableEffort()
Get raw access to the effort of the variables that make up this state. The values are in the same ord...
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & reverse()
void swap(robot_trajectory::RobotTrajectory &other)
const std::string & getGroupName() const
void findWayPointIndicesForDurationAfterStart(double duration, int &before, int &after, double &blend) const
Finds the waypoint indices before and after a duration from start.
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
RobotTrajectory(const moveit::core::RobotModelConstPtr &robot_model)
construct a trajectory for the whole robot
double getAverageSegmentDuration() const
void print(std::ostream &out, std::vector< int > variable_indexes=std::vector< int >()) const
Print information about the trajectory.
bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr &output_state) const
Gets a robot state corresponding to a supplied duration from start for the trajectory,...
void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
RobotTrajectory & unwind()
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
RobotTrajectory & 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...
std::size_t getWayPointCount() const
RobotTrajectory & clear()
const moveit::core::RobotState & getWayPoint(std::size_t index) const
double getDuration() const
rclcpp::Logger getLogger()
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
std::optional< double > waypointDensity(const RobotTrajectory &trajectory)
Calculate the waypoint density of a trajectory.
std::optional< double > smoothness(const RobotTrajectory &trajectory)
Calculate the smoothness of a given trajectory.
double pathLength(const RobotTrajectory &trajectory)
Calculate the path length of a given trajectory based on the accumulated robot state distances....
std::ostream & operator<<(std::ostream &out, const RobotTrajectory &trajectory)
Operator overload for printing trajectory to a stream.