Trajectory::Trajectory(const Trajectory& source_traj, const std::string& planning_group):
planning_group_name_(planning_group),
discretization_(source_traj.discretization_)
{
ROS_INFO("Pezzotto safe at ChomTrajectory constructor");
num_joints_ = source_traj.getNumJoints();
num_points_ = source_traj.num_points_;
start_index_ = source_traj.getStartIndex();
end_index_ = (num_points_ - 1);
duration_ = (num_points_ - 1)*discretization_;
// allocate the memory:
init();
full_trajectory_index_.resize(num_points_);
// now copy the trajectories over:
for (int i=0; i<num_points_; i++)
{
full_trajectory_index_[i] = i;
for (int j=0; j<num_joints_; j++)
{
(*this)(i,j) = source_traj(i, j);
}
}
}
Trajectory::Trajectory(const Trajectory& source_traj, const std::string& planning_group, int diff_rule_length):
planning_group_name_(planning_group),
discretization_(source_traj.discretization_)
{
num_joints_ = source_traj.getNumJoints();
// figure out the num_points_:
// we need diff_rule_length-1 extra points on either side:
int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_-1) - source_traj.end_index_);
num_points_ = source_traj.num_points_ + start_extra + end_extra;
start_index_ = diff_rule_length - 1;
end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
duration_ = (num_points_ - 1)*discretization_;
// allocate the memory:
init();
full_trajectory_index_.resize(num_points_);
// now copy the trajectories over:
for (int i=0; i<num_points_; i++)
{
int source_traj_point = i - start_extra;
if (source_traj_point < 0)
source_traj_point = 0;
if (source_traj_point >= source_traj.num_points_)
source_traj_point = source_traj.num_points_-1;
full_trajectory_index_[i] = source_traj_point;
for (int j=0; j<num_joints_; j++)
{
(*this)(i,j) = source_traj(source_traj_point, j);
}
}
}
