40 #include <kdl/velocityprofile_trap.hpp> 48 for (
unsigned int i=0; i<size; i++)
61 ROS_DEBUG(
"Generating trajectory for list of points of size %d", (
int) traj_in.points.size());
64 if (traj_in.points.size() < 2){
65 ROS_WARN(
"Trajectory message should contain at least two points, but it contains %d points. Returning original trajectory", (
int)traj_in.points.size());
67 throw ros::Exception(
"Trajectory contains fewer than two points.");
71 trajectory_msgs::JointTrajectory traj_res = traj_in;
72 traj_res.points.clear();
73 trajectory_msgs::JointTrajectoryPoint points_tmp;
76 points_tmp.accelerations.resize(
generators_.size());
78 double initial_time = traj_in.points[0].time_from_start.toSec();
80 for (
unsigned int pnt=0; pnt<traj_in.points.size()-1; pnt++){
82 if (traj_in.points[pnt].positions.size() !=
generators_.size() ||
83 traj_in.points[pnt+1].positions.size() !=
generators_.size()){
84 ROS_ERROR(
"The point lists in the trajectory do not have the same size as the generators");
90 generators_[i]->SetProfile(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i]);
93 double max_time = (traj_in.points[pnt+1].time_from_start - traj_in.points[pnt].time_from_start).toSec();
100 generators_[i]->SetProfileDuration(traj_in.points[pnt].positions[i], traj_in.points[pnt+1].positions[i], max_time);
103 unsigned int steps = fmax(10, (
unsigned int)(max_time / 0.1));
105 for (
unsigned int s=0;
s<=steps;
s++){
107 points_tmp.positions[i] =
generators_[i]->Pos(time);
108 points_tmp.velocities[i] =
generators_[i]->Vel(time);
109 points_tmp.accelerations[i] =
generators_[i]->Acc(time);
112 traj_res.points.push_back(points_tmp);
113 time += max_time/(double)steps;
115 initial_time += max_time;
TrajectoryGenerator(double max_vel, double max_acc, unsigned int size)
void generate(const trajectory_msgs::JointTrajectory &traj_in, trajectory_msgs::JointTrajectory &traj_out)
std::vector< KDL::VelocityProfile * > generators_