void JointTrajectoryController::getCubicSplineCoefficients(const double start_pos, const double start_vel,
const double end_pos, const double end_vel,
const double time, std::vector<double>& coefficients)
{
coefficients.resize(4);
if (time == 0.0)
{
coefficients[0] = end_pos;
coefficients[1] = end_vel;
coefficients[2] = 0.0;
coefficients[3] = 0.0;
}
else
{
double T[4];
generatePowers(3, time, T);
coefficients[0] = start_pos;
coefficients[1] = start_vel;
coefficients[2] = (-3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1]) / T[2];
coefficients[3] = (2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1]) / T[3];
}
}
void JointTrajectoryController::getQuinticSplineCoefficients(const double start_pos, const double start_vel,
const double start_acc, const double end_pos,
const double end_vel, const double end_acc,
const double time, std::vector<double>& coefficients)
{
coefficients.resize(6);
if (time == 0.0)
{
coefficients[0] = end_pos;
coefficients[1] = end_vel;
coefficients[2] = 0.5 * end_acc;
coefficients[3] = 0.0;
coefficients[4] = 0.0;
coefficients[5] = 0.0;
}
else
{
double T[6];
generatePowers(5, time, T);
coefficients[0] = start_pos;
coefficients[1] = start_vel;
coefficients[2] = 0.5 * start_acc;
coefficients[3] = (-20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] + end_acc * T[2]
- 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1]) / (2.0 * T[3]);
coefficients[4] = (30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] - 2.0 * end_acc * T[2]
+ 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1]) / (2.0 * T[4]);
coefficients[5] = (-12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] - 6.0 * start_vel * T[1]
- 6.0 * end_vel * T[1]) / (2.0 * T[5]);
}
}
void JointTrajectoryController::sampleQuinticSpline(const std::vector<double>& coefficients, const double time,
double& position, double& velocity, double& acceleration)
{
// create powers of time:
double t[6];
generatePowers(5, time, t);
position = t[0] * coefficients[0] + t[1] * coefficients[1] + t[2] * coefficients[2] + t[3] * coefficients[3]
+ t[4] * coefficients[4] + t[5] * coefficients[5];
velocity = t[0] * coefficients[1] + 2.0 * t[1] * coefficients[2] + 3.0 * t[2] * coefficients[3]
+ 4.0 * t[3] * coefficients[4] + 5.0 * t[4] * coefficients[5];
acceleration = 2.0 * t[0] * coefficients[2] + 6.0 * t[1] * coefficients[3] + 12.0 * t[2] * coefficients[4]
+ 20.0 * t[3] * coefficients[5];
}
// thread function, per-instance
// interpolates all joints of this instance
static int update(void *arg, const hal_funct_args_t *fa)
{
struct inst_data *ip = (struct inst_data *) arg;
double period = ((double) fa_period(fa)) * 1e-9;
int i;
if (segment_completed(ip, period)) {
// check for a new JointTrajectoryPoint
void *data;
ringsize_t size;
if (record_read(&ip->traj, (const void**)&data, &size) == 0) {
// protobuf-decode it
pb_istream_t stream = pb_istream_from_buffer(data, size);
pb_JointTrajectoryPoint rx = pb_JointTrajectoryPoint_init_zero;
if (!pb_decode(&stream, pb_JointTrajectoryPoint_fields, &rx)) {
rtapi_print_msg(RTAPI_MSG_ERR, "%s: pb_decode(JointTrajectoryPoint) failed: '%s'",
compname, PB_GET_ERROR(&stream));
} else {
// decode ok - start a new segment
double duration = *(ip->duration) = rx.time_from_start - ip->time_from_start;
// the very first point in the ringbuffer is not a segment.
// therefore we need to "jump" to these initial settings for the
// interpolator to calculate the correct path.
// for example, a path can start at position, velocity and acceleration
// who are non-zero. In a typical ROS message the first point has a
// duration of "0.0"
if (duration == 0.0) {
// set the start positions
// or try out to drop this point later on
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
*(jp->curr_pos) = *(jp->end_pos) = rx.positions[i];
*(jp->curr_vel) = *(jp->end_vel) = rx.velocities[i];
*(jp->curr_acc) = *(jp->end_acc) = rx.accelerations[i];
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
// so when we have read the first point, we need to discard everythin
// else and make sure we will read the second point, as to complete the
// first segment
} else {
generatePowers(*(ip->degree), duration, ip->powers);
ip->time_from_start = rx.time_from_start;
*(ip->progress) = 0.0;
for (i = 0; i < rx.positions_count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = true;
double pos2 = *(jp->end_pos) = rx.positions[i];
double vel2 = *(jp->end_vel) = rx.velocities[i];
double acc2 = *(jp->end_acc) = rx.accelerations[i];
double pos1 = *(jp->curr_pos);
double vel1 = *(jp->curr_vel);
double acc1 = *(jp->curr_acc);
switch (*(ip->degree)) {
case 1:
jp->coeff[0] = pos1;
jp->coeff[1] = (pos2 - pos1) / duration;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 3:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*ip->powers[1] - vel2*ip->powers[1]) / ip->powers[2];
jp->coeff[3] = (2.0*pos1 - 2.0*pos2 + vel1*ip->powers[1] + vel2*ip->powers[1]) / ip->powers[3];
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
break;
case 5:
jp->coeff[0] = pos1;
jp->coeff[1] = vel1;
jp->coeff[2] = 0.5 * acc1;
jp->coeff[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*ip->powers[2] + acc2*ip->powers[2] -
12.0*vel1*ip->powers[1] - 8.0*vel2*ip->powers[1]) / (2.0*ip->powers[3]);
jp->coeff[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*ip->powers[2] - 2.0*acc2*ip->powers[2] +
16.0*vel1*ip->powers[1] + 14.0*vel2*ip->powers[1]) / (2.0*ip->powers[4]);
jp->coeff[5] = (-12.0*pos1 + 12.0*pos2 - acc1*ip->powers[2] + acc2*ip->powers[2] -
6.0*vel1*ip->powers[1] - 6.0*vel2*ip->powers[1]) / (2.0*ip->powers[5]);
break;
}
}
}
}
record_shift(&ip->traj); // consume record
} else {
// segment completed and no new point in ringbuffer
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
*(jp->traj_busy) = false;
jp->coeff[0] = *(jp->end_pos);
jp->coeff[1] = 0.0;
jp->coeff[2] = 0.0;
jp->coeff[3] = 0.0;
jp->coeff[4] = 0.0;
jp->coeff[5] = 0.0;
}
}
}
*(ip->progress) += period;
generatePowers(*(ip->degree), *(ip->progress), ip->pnow);
for (i = 0; i < ip->count; i++) {
struct joint *jp = &ip->joints[i];
interpolate_joint(ip, jp, *(ip->progress), 0);
}
return 0;
}
