コード例 #1
0
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);
    }
  }
}
コード例 #2
0
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);
    }
  }
}