robot_trajectory.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Adam Leeper */
00036 
00037 #include <moveit/robot_trajectory/robot_trajectory.h>
00038 #include <moveit/robot_state/conversions.h>
00039 #include <eigen_conversions/eigen_msg.h>
00040 #include <boost/math/constants/constants.hpp>
00041 #include <numeric>
00042 
00043 robot_trajectory::RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model,
00044                                                    const std::string& group)
00045   : robot_model_(robot_model), group_(group.empty() ? NULL : robot_model->getJointModelGroup(group))
00046 {
00047 }
00048 
00049 robot_trajectory::RobotTrajectory::RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model,
00050                                                    const robot_model::JointModelGroup* group)
00051   : robot_model_(robot_model), group_(group)
00052 {
00053 }
00054 
00055 void robot_trajectory::RobotTrajectory::setGroupName(const std::string& group_name)
00056 {
00057   group_ = robot_model_->getJointModelGroup(group_name);
00058 }
00059 
00060 const std::string& robot_trajectory::RobotTrajectory::getGroupName() const
00061 {
00062   if (group_)
00063     return group_->getName();
00064   static const std::string empty;
00065   return empty;
00066 }
00067 
00068 double robot_trajectory::RobotTrajectory::getAverageSegmentDuration() const
00069 {
00070   if (duration_from_previous_.empty())
00071     return 0.0;
00072   else
00073     return std::accumulate(duration_from_previous_.begin(), duration_from_previous_.end(), 0.0) /
00074            (double)duration_from_previous_.size();
00075 }
00076 
00077 void robot_trajectory::RobotTrajectory::swap(robot_trajectory::RobotTrajectory& other)
00078 {
00079   robot_model_.swap(other.robot_model_);
00080   std::swap(group_, other.group_);
00081   waypoints_.swap(other.waypoints_);
00082   duration_from_previous_.swap(other.duration_from_previous_);
00083 }
00084 
00085 void robot_trajectory::RobotTrajectory::append(const RobotTrajectory& source, double dt)
00086 {
00087   waypoints_.insert(waypoints_.end(), source.waypoints_.begin(), source.waypoints_.end());
00088   std::size_t index = duration_from_previous_.size();
00089   duration_from_previous_.insert(duration_from_previous_.end(), source.duration_from_previous_.begin(),
00090                                  source.duration_from_previous_.end());
00091   if (duration_from_previous_.size() > index)
00092     duration_from_previous_[index] += dt;
00093 }
00094 
00095 void robot_trajectory::RobotTrajectory::reverse()
00096 {
00097   std::reverse(waypoints_.begin(), waypoints_.end());
00098   if (!duration_from_previous_.empty())
00099   {
00100     duration_from_previous_.push_back(duration_from_previous_.front());
00101     std::reverse(duration_from_previous_.begin(), duration_from_previous_.end());
00102     duration_from_previous_.pop_back();
00103   }
00104 }
00105 
00106 void robot_trajectory::RobotTrajectory::unwind()
00107 {
00108   if (waypoints_.empty())
00109     return;
00110 
00111   const std::vector<const robot_model::JointModel*>& cont_joints =
00112       group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
00113 
00114   for (std::size_t i = 0; i < cont_joints.size(); ++i)
00115   {
00116     // unwrap continuous joints
00117     double running_offset = 0.0;
00118     double last_value = waypoints_[0]->getJointPositions(cont_joints[i])[0];
00119 
00120     for (std::size_t j = 1; j < waypoints_.size(); ++j)
00121     {
00122       double current_value = waypoints_[j]->getJointPositions(cont_joints[i])[0];
00123       if (last_value > current_value + boost::math::constants::pi<double>())
00124         running_offset += 2.0 * boost::math::constants::pi<double>();
00125       else if (current_value > last_value + boost::math::constants::pi<double>())
00126         running_offset -= 2.0 * boost::math::constants::pi<double>();
00127 
00128       last_value = current_value;
00129       if (running_offset > std::numeric_limits<double>::epsilon() ||
00130           running_offset < -std::numeric_limits<double>::epsilon())
00131       {
00132         current_value += running_offset;
00133         waypoints_[j]->setJointPositions(cont_joints[i], &current_value);
00134       }
00135     }
00136   }
00137   for (std::size_t j = 0; j < waypoints_.size(); ++j)
00138     waypoints_[j]->update();
00139 }
00140 
00141 void robot_trajectory::RobotTrajectory::unwind(const robot_state::RobotState& state)
00142 {
00143   if (waypoints_.empty())
00144     return;
00145 
00146   const std::vector<const robot_model::JointModel*>& cont_joints =
00147       group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
00148 
00149   for (std::size_t i = 0; i < cont_joints.size(); ++i)
00150   {
00151     double reference_value0 = state.getJointPositions(cont_joints[i])[0];
00152     double reference_value = reference_value0;
00153     cont_joints[i]->enforcePositionBounds(&reference_value);
00154 
00155     // unwrap continuous joints
00156     double running_offset = reference_value0 - reference_value;
00157 
00158     double last_value = waypoints_[0]->getJointPositions(cont_joints[i])[0];
00159     if (running_offset > std::numeric_limits<double>::epsilon() ||
00160         running_offset < -std::numeric_limits<double>::epsilon())
00161     {
00162       double current_value = last_value + running_offset;
00163       waypoints_[0]->setJointPositions(cont_joints[i], &current_value);
00164     }
00165 
00166     for (std::size_t j = 1; j < waypoints_.size(); ++j)
00167     {
00168       double current_value = waypoints_[j]->getJointPositions(cont_joints[i])[0];
00169       if (last_value > current_value + boost::math::constants::pi<double>())
00170         running_offset += 2.0 * boost::math::constants::pi<double>();
00171       else if (current_value > last_value + boost::math::constants::pi<double>())
00172         running_offset -= 2.0 * boost::math::constants::pi<double>();
00173 
00174       last_value = current_value;
00175       if (running_offset > std::numeric_limits<double>::epsilon() ||
00176           running_offset < -std::numeric_limits<double>::epsilon())
00177       {
00178         current_value += running_offset;
00179         waypoints_[j]->setJointPositions(cont_joints[i], &current_value);
00180       }
00181     }
00182   }
00183   for (std::size_t j = 0; j < waypoints_.size(); ++j)
00184     waypoints_[j]->update();
00185 }
00186 
00187 void robot_trajectory::RobotTrajectory::clear()
00188 {
00189   waypoints_.clear();
00190   duration_from_previous_.clear();
00191 }
00192 
00193 void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const
00194 {
00195   trajectory = moveit_msgs::RobotTrajectory();
00196   if (waypoints_.empty())
00197     return;
00198   const std::vector<const robot_model::JointModel*>& jnt =
00199       group_ ? group_->getActiveJointModels() : robot_model_->getActiveJointModels();
00200 
00201   std::vector<const robot_model::JointModel*> onedof;
00202   std::vector<const robot_model::JointModel*> mdof;
00203   trajectory.joint_trajectory.joint_names.clear();
00204   trajectory.multi_dof_joint_trajectory.joint_names.clear();
00205 
00206   for (std::size_t i = 0; i < jnt.size(); ++i)
00207     if (jnt[i]->getVariableCount() == 1)
00208     {
00209       trajectory.joint_trajectory.joint_names.push_back(jnt[i]->getName());
00210       onedof.push_back(jnt[i]);
00211     }
00212     else
00213     {
00214       trajectory.multi_dof_joint_trajectory.joint_names.push_back(jnt[i]->getName());
00215       mdof.push_back(jnt[i]);
00216     }
00217   if (!onedof.empty())
00218   {
00219     trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
00220     trajectory.joint_trajectory.header.stamp = ros::Time(0);
00221     trajectory.joint_trajectory.points.resize(waypoints_.size());
00222   }
00223 
00224   if (!mdof.empty())
00225   {
00226     trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
00227     trajectory.multi_dof_joint_trajectory.header.stamp = ros::Time(0);
00228     trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
00229   }
00230 
00231   static const ros::Duration zero_duration(0.0);
00232   double total_time = 0.0;
00233   for (std::size_t i = 0; i < waypoints_.size(); ++i)
00234   {
00235     if (duration_from_previous_.size() > i)
00236       total_time += duration_from_previous_[i];
00237 
00238     if (!onedof.empty())
00239     {
00240       trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
00241       trajectory.joint_trajectory.points[i].velocities.reserve(onedof.size());
00242 
00243       for (std::size_t j = 0; j < onedof.size(); ++j)
00244       {
00245         trajectory.joint_trajectory.points[i].positions[j] =
00246             waypoints_[i]->getVariablePosition(onedof[j]->getFirstVariableIndex());
00247         // if we have velocities/accelerations/effort, copy those too
00248         if (waypoints_[i]->hasVelocities())
00249           trajectory.joint_trajectory.points[i].velocities.push_back(
00250               waypoints_[i]->getVariableVelocity(onedof[j]->getFirstVariableIndex()));
00251         if (waypoints_[i]->hasAccelerations())
00252           trajectory.joint_trajectory.points[i].accelerations.push_back(
00253               waypoints_[i]->getVariableAcceleration(onedof[j]->getFirstVariableIndex()));
00254         if (waypoints_[i]->hasEffort())
00255           trajectory.joint_trajectory.points[i].effort.push_back(
00256               waypoints_[i]->getVariableEffort(onedof[j]->getFirstVariableIndex()));
00257       }
00258       // clear velocities if we have an incomplete specification
00259       if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
00260         trajectory.joint_trajectory.points[i].velocities.clear();
00261       // clear accelerations if we have an incomplete specification
00262       if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
00263         trajectory.joint_trajectory.points[i].accelerations.clear();
00264       // clear effort if we have an incomplete specification
00265       if (trajectory.joint_trajectory.points[i].effort.size() != onedof.size())
00266         trajectory.joint_trajectory.points[i].effort.clear();
00267 
00268       if (duration_from_previous_.size() > i)
00269         trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
00270       else
00271         trajectory.joint_trajectory.points[i].time_from_start = zero_duration;
00272     }
00273     if (!mdof.empty())
00274     {
00275       trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
00276       for (std::size_t j = 0; j < mdof.size(); ++j)
00277       {
00278         tf::transformEigenToMsg(waypoints_[i]->getJointTransform(mdof[j]),
00279                                 trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
00280         // TODO: currently only checking for planar multi DOF joints / need to add check for floating
00281         if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::PLANAR))
00282         {
00283           const std::vector<std::string> names = mdof[j]->getVariableNames();
00284           const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);
00285 
00286           geometry_msgs::Twist point_velocity;
00287 
00288           for (std::size_t k = 0; k < names.size(); ++k)
00289           {
00290             if (names[k].find("/x") != std::string::npos)
00291             {
00292               point_velocity.linear.x = velocities[k];
00293             }
00294             else if (names[k].find("/y") != std::string::npos)
00295             {
00296               point_velocity.linear.y = velocities[k];
00297             }
00298             else if (names[k].find("/z") != std::string::npos)
00299             {
00300               point_velocity.linear.z = velocities[k];
00301             }
00302             else if (names[k].find("/theta") != std::string::npos)
00303             {
00304               point_velocity.angular.z = velocities[k];
00305             }
00306           }
00307           trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity);
00308         }
00309       }
00310       if (duration_from_previous_.size() > i)
00311         trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
00312       else
00313         trajectory.multi_dof_joint_trajectory.points[i].time_from_start = zero_duration;
00314     }
00315   }
00316 }
00317 
00318 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00319                                                               const trajectory_msgs::JointTrajectory& trajectory)
00320 {
00321   // make a copy just in case the next clear() removes the memory for the reference passed in
00322   robot_state::RobotState copy = reference_state;
00323   clear();
00324   std::size_t state_count = trajectory.points.size();
00325   ros::Time last_time_stamp = trajectory.header.stamp;
00326   ros::Time this_time_stamp = last_time_stamp;
00327 
00328   for (std::size_t i = 0; i < state_count; ++i)
00329   {
00330     this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
00331     robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
00332     st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
00333     if (!trajectory.points[i].velocities.empty())
00334       st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
00335     if (!trajectory.points[i].accelerations.empty())
00336       st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
00337     if (!trajectory.points[i].effort.empty())
00338       st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
00339     addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
00340     last_time_stamp = this_time_stamp;
00341   }
00342 }
00343 
00344 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00345                                                               const moveit_msgs::RobotTrajectory& trajectory)
00346 {
00347   // make a copy just in case the next clear() removes the memory for the reference passed in
00348   robot_state::RobotState copy = reference_state;
00349   clear();
00350 
00351   std::size_t state_count =
00352       std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
00353   ros::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
00354                                   trajectory.multi_dof_joint_trajectory.header.stamp :
00355                                   trajectory.joint_trajectory.header.stamp;
00356   ros::Time this_time_stamp = last_time_stamp;
00357 
00358   for (std::size_t i = 0; i < state_count; ++i)
00359   {
00360     robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
00361     if (trajectory.joint_trajectory.points.size() > i)
00362     {
00363       st->setVariablePositions(trajectory.joint_trajectory.joint_names,
00364                                trajectory.joint_trajectory.points[i].positions);
00365       if (!trajectory.joint_trajectory.points[i].velocities.empty())
00366         st->setVariableVelocities(trajectory.joint_trajectory.joint_names,
00367                                   trajectory.joint_trajectory.points[i].velocities);
00368       if (!trajectory.joint_trajectory.points[i].accelerations.empty())
00369         st->setVariableAccelerations(trajectory.joint_trajectory.joint_names,
00370                                      trajectory.joint_trajectory.points[i].accelerations);
00371       if (!trajectory.joint_trajectory.points[i].effort.empty())
00372         st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
00373       this_time_stamp =
00374           trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
00375     }
00376     if (trajectory.multi_dof_joint_trajectory.points.size() > i)
00377     {
00378       for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
00379       {
00380         Eigen::Affine3d t;
00381         tf::transformMsgToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j], t);
00382         st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
00383       }
00384       this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
00385                         trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
00386     }
00387 
00388     addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
00389     last_time_stamp = this_time_stamp;
00390   }
00391 }
00392 
00393 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00394                                                               const moveit_msgs::RobotState& state,
00395                                                               const moveit_msgs::RobotTrajectory& trajectory)
00396 {
00397   robot_state::RobotState st(reference_state);
00398   robot_state::robotStateMsgToRobotState(state, st);
00399   setRobotTrajectoryMsg(st, trajectory);
00400 }
00401 
00402 void robot_trajectory::RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before,
00403                                                                                  int& after, double& blend) const
00404 {
00405   if (duration < 0.0)
00406   {
00407     before = 0;
00408     after = 0;
00409     blend = 0;
00410     return;
00411   }
00412 
00413   // Find indicies
00414   std::size_t index = 0, num_points = waypoints_.size();
00415   double running_duration = 0.0;
00416   for (; index < num_points; ++index)
00417   {
00418     running_duration += duration_from_previous_[index];
00419     if (running_duration >= duration)
00420       break;
00421   }
00422   before = std::max<int>(index - 1, 0);
00423   after = std::min<int>(index, num_points - 1);
00424 
00425   // Compute duration blend
00426   double before_time = running_duration - duration_from_previous_[index];
00427   if (after == before)
00428     blend = 1.0;
00429   else
00430     blend = (duration - before_time) / duration_from_previous_[index];
00431 }
00432 
00433 double robot_trajectory::RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const
00434 {
00435   if (duration_from_previous_.empty())
00436     return 0.0;
00437   if (index >= duration_from_previous_.size())
00438     index = duration_from_previous_.size() - 1;
00439 
00440   double time = 0.0;
00441   for (std::size_t i = 0; i <= index; ++i)
00442     time += duration_from_previous_[i];
00443   return time;
00444 }
00445 
00446 bool robot_trajectory::RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
00447                                                                     robot_state::RobotStatePtr& output_state) const
00448 {
00449   // If there are no waypoints we can't do anything
00450   if (getWayPointCount() == 0)
00451     return false;
00452 
00453   int before = 0, after = 0;
00454   double blend = 1.0;
00455   findWayPointIndicesForDurationAfterStart(request_duration, before, after, blend);
00456   // logDebug("Interpolating %.3f of the way between index %d and %d.", blend, before, after);
00457   waypoints_[before]->interpolate(*waypoints_[after], blend, *output_state);
00458   return true;
00459 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36