34 #include <boost/shared_ptr.hpp>
53 for (
int i=1;
i<=n;
i++)
55 powers[
i] = powers[
i-1]*x;
60 double end_pos,
double end_vel,
double end_acc,
double time, std::vector<double>& coefficients)
62 coefficients.resize(6);
66 coefficients[0] = end_pos;
67 coefficients[1] = end_vel;
68 coefficients[2] = 0.5*end_acc;
69 coefficients[3] = 0.0;
70 coefficients[4] = 0.0;
71 coefficients[5] = 0.0;
78 coefficients[0] = start_pos;
79 coefficients[1] = start_vel;
80 coefficients[2] = 0.5*start_acc;
81 coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
82 12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
83 coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
84 16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
85 coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
86 6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
94 double& position,
double& velocity,
double& acceleration)
100 position = t[0]*coefficients[0] +
101 t[1]*coefficients[1] +
102 t[2]*coefficients[2] +
103 t[3]*coefficients[3] +
104 t[4]*coefficients[4] +
105 t[5]*coefficients[5];
107 velocity = t[0]*coefficients[1] +
108 2.0*t[1]*coefficients[2] +
109 3.0*t[2]*coefficients[3] +
110 4.0*t[3]*coefficients[4] +
111 5.0*t[4]*coefficients[5];
113 acceleration = 2.0*t[0]*coefficients[2] +
114 6.0*t[1]*coefficients[3] +
115 12.0*t[2]*coefficients[4] +
116 20.0*t[3]*coefficients[5];
120 double end_pos,
double end_vel,
double time, std::vector<double>& coefficients)
122 coefficients.resize(4);
126 coefficients[0] = end_pos;
127 coefficients[1] = end_vel;
128 coefficients[2] = 0.0;
129 coefficients[3] = 0.0;
136 coefficients[0] = start_pos;
137 coefficients[1] = start_vel;
138 coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
139 coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
145 : loop_count_(0), robot_(NULL)
173 for (
int i = 0;
i < joint_names.
size(); ++
i)
176 if (name_value.
getType() != XmlRpcValue::TypeString)
178 ROS_ERROR(
"Array of joint names should contain all strings. (namespace: %s)",
185 ROS_ERROR(
"Joint not found: %s. (namespace: %s)",
197 ROS_ERROR(
"Joint %s was not calibrated (namespace: %s)",
204 std::string gains_ns;
216 traj[0].duration = 0.0;
217 traj[0].splines.resize(
joints_.size());
218 for (
size_t j = 0; j <
joints_.size(); ++j)
219 traj[0].splines[j].coef[0] = 0.0;
232 (
node_,
"state", 1));
234 for (
size_t j = 0; j <
joints_.size(); ++j)
253 for (
size_t i = 0;
i <
pids_.size(); ++
i)
260 hold[0].duration = 0.0;
261 hold[0].splines.resize(
joints_.size());
262 for (
size_t j = 0; j <
joints_.size(); ++j)
263 hold[0].splines[j].coef[0] =
joints_[j]->position_;
279 ROS_FATAL(
"The current trajectory can never be null");
286 while (seg + 1 < (
int)traj.size() &&
287 traj[seg+1].start_time < time.
toSec())
294 if (traj.size() == 0)
295 ROS_ERROR(
"No segments in the trajectory");
297 ROS_ERROR(
"No earlier segments. First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.
toSec());
303 for (
size_t i = 0;
i <
q.size(); ++
i)
306 time.
toSec() - traj[seg].start_time,
327 for (
size_t j = 0; j <
joints_.size(); ++j)
347 ROS_DEBUG(
"Figuring out new trajectory at %.3lf, with data from %.3lf",
348 time.
toSec(),
msg->header.stamp.toSec());
355 if (
msg->points.empty())
363 std::vector<int> lookup(
joints_.size(), -1);
364 for (
size_t j = 0; j <
joints_.size(); ++j)
366 for (
size_t k = 0;
k <
msg->joint_names.size(); ++
k)
368 if (
msg->joint_names[
k] ==
joints_[j]->joint_->name)
377 ROS_ERROR(
"Unable to locate joint %s in the commanded trajectory.",
joints_[j]->joint_->name.c_str());
388 ROS_FATAL(
"The current trajectory can never be null");
396 int first_useful = -1;
397 while (first_useful + 1 < (
int)prev_traj.size() &&
398 prev_traj[first_useful + 1].start_time <= time.
toSec())
404 int last_useful = -1;
405 double msg_start_time;
406 if (
msg->points.size() > 0)
407 msg_start_time = (
msg->header.stamp +
msg->points[0].time_from_start).toSec();
409 msg_start_time = std::max(time.
toSec(),
msg->header.stamp.toSec());
411 while (last_useful + 1 < (
int)prev_traj.size() &&
412 prev_traj[last_useful + 1].start_time < msg_start_time)
417 if (last_useful < first_useful)
418 first_useful = last_useful;
421 for (
int i = std::max(first_useful,0);
i <= last_useful; ++
i)
423 new_traj.push_back(prev_traj[
i]);
428 if (new_traj.size() == 0)
429 new_traj.push_back(prev_traj[prev_traj.size() - 1]);
434 Segment &last = new_traj[new_traj.size() - 1];
435 std::vector<double> prev_positions(
joints_.size());
436 std::vector<double> prev_velocities(
joints_.size());
437 std::vector<double> prev_accelerations(
joints_.size());
439 ROS_DEBUG(
"Initial conditions for new set of splines:");
444 prev_positions[
i], prev_velocities[
i], prev_accelerations[
i]);
445 ROS_DEBUG(
" %.2lf, %.2lf, %.2lf (%s)", prev_positions[
i], prev_velocities[
i],
446 prev_accelerations[
i],
joints_[
i]->joint_->name.c_str());
451 std::vector<double> positions;
452 std::vector<double> velocities;
453 std::vector<double> accelerations;
455 std::vector<double> durations(
msg->points.size());
456 durations[0] =
msg->points[0].time_from_start.toSec();
457 for (
size_t i = 1;
i <
msg->points.size(); ++
i)
458 durations[
i] = (
msg->points[
i].time_from_start -
msg->points[
i-1].time_from_start).toSec();
461 std::vector<double> wrap(
joints_.size(), 0.0);
462 assert(!
msg->points[0].positions.empty());
463 for (
size_t j = 0; j <
joints_.size(); ++j)
465 if (
joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
468 wrap[j] = (prev_positions[j] + dist) -
msg->points[0].positions[j];
472 for (
size_t i = 0;
i <
msg->points.size(); ++
i)
476 seg.
start_time = (
msg->header.stamp +
msg->points[
i].time_from_start).toSec() - durations[
i];
482 if (
msg->points[
i].accelerations.size() != 0 &&
msg->points[
i].accelerations.size() !=
joints_.size())
484 ROS_ERROR(
"Command point %d has %d elements for the accelerations", (
int)
i, (
int)
msg->points[
i].accelerations.size());
487 if (
msg->points[
i].velocities.size() != 0 &&
msg->points[
i].velocities.size() !=
joints_.size())
489 ROS_ERROR(
"Command point %d has %d elements for the velocities", (
int)
i, (
int)
msg->points[
i].velocities.size());
492 if (
msg->points[
i].positions.size() !=
joints_.size())
494 ROS_ERROR(
"Command point %d has %d elements for the positions", (
int)
i, (
int)
msg->points[
i].positions.size());
500 accelerations.resize(
msg->points[
i].accelerations.size());
501 velocities.resize(
msg->points[
i].velocities.size());
502 positions.resize(
msg->points[
i].positions.size());
503 for (
size_t j = 0; j <
joints_.size(); ++j)
505 if (!accelerations.empty()) accelerations[j] =
msg->points[
i].accelerations[lookup[j]];
506 if (!velocities.empty()) velocities[j] =
msg->points[
i].velocities[lookup[j]];
507 if (!positions.empty()) positions[j] =
msg->points[
i].positions[lookup[j]] + wrap[j];
512 for (
size_t j = 0; j <
joints_.size(); ++j)
514 if (prev_accelerations.size() > 0 && accelerations.size() > 0)
517 prev_positions[j], prev_velocities[j], prev_accelerations[j],
518 positions[j], velocities[j], accelerations[j],
522 else if (prev_velocities.size() > 0 && velocities.size() > 0)
525 prev_positions[j], prev_velocities[j],
526 positions[j], velocities[j],
529 seg.
splines[j].coef.resize(6, 0.0);
533 seg.
splines[j].coef[0] = prev_positions[j];
534 if (durations[
i] == 0.0)
537 seg.
splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[
i];
547 new_traj.push_back(seg);
551 prev_positions = positions;
552 prev_velocities = velocities;
553 prev_accelerations = accelerations;
560 ROS_ERROR(
"The new trajectory was null!");
565 ROS_DEBUG(
"The new trajectory has %d segments", (
int)new_traj.size());
567 for (
size_t i = 0;
i < std::min((
size_t)20,new_traj.size()); ++
i)
569 ROS_DEBUG(
"Segment %2d: %.3lf for %.3lf",
i, new_traj[
i].start_time, new_traj[
i].duration);
570 for (
size_t j = 0; j < new_traj[
i].splines.size(); ++j)
572 ROS_DEBUG(
" %.2lf %.2lf %.2lf %.2lf , %.2lf %.2lf(%s)",
573 new_traj[
i].splines[j].coef[0],
574 new_traj[
i].splines[j].coef[1],
575 new_traj[
i].splines[j].coef[2],
576 new_traj[
i].splines[j].coef[3],
577 new_traj[
i].splines[j].coef[4],
578 new_traj[
i].splines[j].coef[5],
579 joints_[j]->joint_->name_.c_str());
586 pr2_controllers_msgs::QueryTrajectoryState::Request &req,
587 pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
593 ROS_FATAL(
"The current trajectory can never be null");
600 while (seg + 1 < (
int)traj.size() &&
601 traj[seg+1].start_time < req.time.toSec())
608 for (
size_t i = 0;
i <
q.size(); ++
i)
613 resp.name.resize(
joints_.size());
614 resp.position.resize(
joints_.size());
615 resp.velocity.resize(
joints_.size());
616 resp.acceleration.resize(
joints_.size());
617 for (
size_t j = 0; j <
joints_.size(); ++j)
619 resp.name[j] =
joints_[j]->joint_->name;
621 req.time.toSec() - traj[seg].start_time,
622 resp.position[j], resp.velocity[j], resp.acceleration[j]);
629 const std::vector<double>& coefficients,
double duration,
double time,
630 double& position,
double& velocity,
double& acceleration)
639 else if (time > duration)
649 position, velocity, acceleration);