void Trajectory::overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj) {
for(unsigned int i = 1; i <= traj.points.size(); i++) {
for(unsigned int j = 0; j < traj.joint_names.size(); j++) {
trajectory_(i,j) = traj.points[i-1].positions[j];
}
}
}
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);
}
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;
ROS_INFO_STREAM("Num planning joints " << planning_group_->num_joints_ << " num trajectory joints " << traj.joint_names.size());
num_points_ = traj.points.size();
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;
}
}
std::vector<int> ind;
for(unsigned int j = 0; j < traj.joint_names.size(); j++) {
int kdl_number = robot_model_->urdfNameToKdlNumber(traj.joint_names[j]);
if(kdl_number == 0) {
ROS_WARN_STREAM("Can't find kdl index for joint " << traj.joint_names[j]);
}
ind.push_back(kdl_number);
}
for(unsigned int i = 0; i < traj.points.size(); i++) {
for(unsigned int j = 0; j < traj.joint_names.size(); j++) {
trajectory_(i,ind[j]) = traj.points[i].positions[j];
}
}
}
void StompTrajectory::overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj)
{
std::vector<int> ind;
for(unsigned int j = 0; j < traj.joint_names.size(); j++) {
int kdl_number = robot_model_->urdfNameToKdlNumber(traj.joint_names[j]);
if(kdl_number == 0) {
ROS_WARN_STREAM("Can't find kdl index for joint " << traj.joint_names[j]);
}
ind.push_back(kdl_number);
}
for(unsigned int i = 1; i <= traj.points.size(); i++) {
for(unsigned int j = 0; j < traj.joint_names.size(); j++) {
trajectory_(i,ind[j]) = traj.points[i-1].positions[j];
}
}
}
