Trajectory::Trajectory(const planning_models::KinematicModel* robot_model,
const std::string& planning_group,
const trajectory_msgs::JointTrajectory& traj) :
planning_group_name_(planning_group)
{
std::map<std::string, planning_models::KinematicModel::JointModelGroup*> groupMap = robot_model->getJointModelGroupMap();
const planning_models::KinematicModel::JointModelGroup* modelGroup = groupMap[planning_group_name_];
num_joints_ = modelGroup->getJointModels().size();
double discretization = (traj.points[1].time_from_start-traj.points[0].time_from_start).toSec();
double discretization2 = (traj.points[2].time_from_start-traj.points[1].time_from_start).toSec();
if(fabs(discretization2-discretization) > .001) {
ROS_WARN_STREAM("Trajectory Discretization not constant " << discretization << " " << discretization2);
}
discretization_ = discretization;
num_points_ = traj.points.size()+1;
duration_ = (traj.points.back().time_from_start-traj.points[0].time_from_start).toSec();
start_index_ = 1;
end_index_ = num_points_-2;
init();
for(int i = 0; i < num_points_; i++) {
for(int j = 0; j < num_joints_; j++) {
trajectory_(i,j) = 0.0;
}
}
overwriteTrajectory(traj);
}
StompTrajectory::StompTrajectory(const StompRobotModel* robot_model,
const StompRobotModel::StompPlanningGroup* planning_group,
const trajectory_msgs::JointTrajectory& traj) :
robot_model_(robot_model),
planning_group_(planning_group),
num_joints_(robot_model_->getNumKDLJoints())
{
double discretization = (traj.points[1].time_from_start-traj.points[0].time_from_start).toSec();
double discretization2 = (traj.points[2].time_from_start-traj.points[1].time_from_start).toSec();
if(fabs(discretization2-discretization) > .001) {
ROS_WARN_STREAM("Trajectory Discretization not constant " << discretization << " " << discretization2);
}
discretization_ = discretization;
num_points_ = traj.points.size()+1;
duration_ = (traj.points.back().time_from_start-traj.points[0].time_from_start).toSec();
start_index_ = 1;
end_index_ = num_points_-2;
init();
for(int i = 0; i < num_points_; i++) {
for(int j = 0; j < num_joints_; j++) {
trajectory_(i,j) = 0.0;
}
}
overwriteTrajectory(traj);
}