00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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], ¤t_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
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], ¤t_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], ¤t_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
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
00259 if (trajectory.joint_trajectory.points[i].velocities.size() != onedof.size())
00260 trajectory.joint_trajectory.points[i].velocities.clear();
00261
00262 if (trajectory.joint_trajectory.points[i].accelerations.size() != onedof.size())
00263 trajectory.joint_trajectory.points[i].accelerations.clear();
00264
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 tf::transformEigenToMsg(waypoints_[i]->getJointTransform(mdof[j]),
00278 trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
00279 if (duration_from_previous_.size() > i)
00280 trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
00281 else
00282 trajectory.multi_dof_joint_trajectory.points[i].time_from_start = zero_duration;
00283 }
00284 }
00285 }
00286
00287 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00288 const trajectory_msgs::JointTrajectory& trajectory)
00289 {
00290
00291 robot_state::RobotState copy = reference_state;
00292 clear();
00293 std::size_t state_count = trajectory.points.size();
00294 ros::Time last_time_stamp = trajectory.header.stamp;
00295 ros::Time this_time_stamp = last_time_stamp;
00296
00297 for (std::size_t i = 0; i < state_count; ++i)
00298 {
00299 this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
00300 robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
00301 st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
00302 if (!trajectory.points[i].velocities.empty())
00303 st->setVariableVelocities(trajectory.joint_names, trajectory.points[i].velocities);
00304 if (!trajectory.points[i].accelerations.empty())
00305 st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
00306 if (!trajectory.points[i].effort.empty())
00307 st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
00308 addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
00309 last_time_stamp = this_time_stamp;
00310 }
00311 }
00312
00313 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00314 const moveit_msgs::RobotTrajectory& trajectory)
00315 {
00316
00317 robot_state::RobotState copy = reference_state;
00318 clear();
00319
00320 std::size_t state_count =
00321 std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
00322 ros::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
00323 trajectory.multi_dof_joint_trajectory.header.stamp :
00324 trajectory.joint_trajectory.header.stamp;
00325 ros::Time this_time_stamp = last_time_stamp;
00326
00327 for (std::size_t i = 0; i < state_count; ++i)
00328 {
00329 robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
00330 if (trajectory.joint_trajectory.points.size() > i)
00331 {
00332 st->setVariablePositions(trajectory.joint_trajectory.joint_names,
00333 trajectory.joint_trajectory.points[i].positions);
00334 if (!trajectory.joint_trajectory.points[i].velocities.empty())
00335 st->setVariableVelocities(trajectory.joint_trajectory.joint_names,
00336 trajectory.joint_trajectory.points[i].velocities);
00337 if (!trajectory.joint_trajectory.points[i].accelerations.empty())
00338 st->setVariableAccelerations(trajectory.joint_trajectory.joint_names,
00339 trajectory.joint_trajectory.points[i].accelerations);
00340 if (!trajectory.joint_trajectory.points[i].effort.empty())
00341 st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
00342 this_time_stamp =
00343 trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
00344 }
00345 if (trajectory.multi_dof_joint_trajectory.points.size() > i)
00346 {
00347 for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
00348 {
00349 Eigen::Affine3d t;
00350 tf::transformMsgToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j], t);
00351 st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
00352 }
00353 this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
00354 trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
00355 }
00356
00357 addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
00358 last_time_stamp = this_time_stamp;
00359 }
00360 }
00361
00362 void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
00363 const moveit_msgs::RobotState& state,
00364 const moveit_msgs::RobotTrajectory& trajectory)
00365 {
00366 robot_state::RobotState st(reference_state);
00367 robot_state::robotStateMsgToRobotState(state, st);
00368 setRobotTrajectoryMsg(st, trajectory);
00369 }
00370
00371 void robot_trajectory::RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before,
00372 int& after, double& blend) const
00373 {
00374 if (duration < 0.0)
00375 {
00376 before = 0;
00377 after = 0;
00378 blend = 0;
00379 return;
00380 }
00381
00382
00383 std::size_t index = 0, num_points = waypoints_.size();
00384 double running_duration = 0.0;
00385 for (; index < num_points; ++index)
00386 {
00387 running_duration += duration_from_previous_[index];
00388 if (running_duration >= duration)
00389 break;
00390 }
00391 before = std::max<int>(index - 1, 0);
00392 after = std::min<int>(index, num_points - 1);
00393
00394
00395 double before_time = running_duration - duration_from_previous_[index];
00396 if (after == before)
00397 blend = 1.0;
00398 else
00399 blend = (duration - before_time) / duration_from_previous_[index];
00400 }
00401
00402 double robot_trajectory::RobotTrajectory::getWaypointDurationFromStart(std::size_t index) const
00403 {
00404 if (duration_from_previous_.empty())
00405 return 0.0;
00406 if (index >= duration_from_previous_.size())
00407 index = duration_from_previous_.size() - 1;
00408
00409 double time = 0.0;
00410 for (std::size_t i = 0; i <= index; ++i)
00411 time += duration_from_previous_[i];
00412 return time;
00413 }
00414
00415 bool robot_trajectory::RobotTrajectory::getStateAtDurationFromStart(const double request_duration,
00416 robot_state::RobotStatePtr& output_state) const
00417 {
00418
00419 if (getWayPointCount() == 0)
00420 return false;
00421
00422 int before = 0, after = 0;
00423 double blend = 1.0;
00424 findWayPointIndicesForDurationAfterStart(request_duration, before, after, blend);
00425
00426 waypoints_[before]->interpolate(*waypoints_[after], blend, *output_state);
00427 return true;
00428 }