void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i)
{
const Eigen::MatrixXd::RowXpr& point = group_trajectory.getTrajectoryPoint(i);
std::vector<double> joint_states;
for(int j = 0; j < group_trajectory.getNumJoints(); j ++)
{
joint_states.push_back(point(0,j));
}
//ros::WallTime timer = ros::WallTime::now();
planning_models::RobotState *::JointStateGroup* group = state_.getJointStateGroup(planning_group_);
group->setStateValues(joint_states);
//timer = ros::WallTime::now();
}
ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, int diff_rule_length) :
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);
}
}
}
