void verlet(const data* dat, output_function output)
{
int i;
const double dt = dat->eta * delta_t_factor; // konstant
vector *a = (vector*) malloc((dat->N)*sizeof(vector)),
*a_1 = (vector*) malloc((dat->N)*sizeof(vector)),
*r_p = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n-1), not initialized
*r = init_r(dat), // r_(n)
*r_n = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n+1), not initialized
*v = init_v(dat); // v_(n)
double time = 0;
output(time, dt, dat, r, v);
// set initial data
accelerations(dat, r, a); // a_0
adots(dat, r, v, a_1);
for (i = 0; i < dat->N; i++)
{
// set r_(-1)
r_p[i] = vector_add(r[i],
vector_add(scalar_mult(dt, v[i]),
scalar_mult(0.5 * dt * dt, a[i])));
// set r_1
r_n[i] = vector_add(scalar_mult(2, r[i]),
vector_add(scalar_mult(-1, r_p[i]),
scalar_mult(dt * dt, a[i])));
}
//time += dt;
// regular timesteps
while (time < dat->t_max)
{
accelerations(dat, r_n, a); // a_n+1 (gets shifted to a_n)
for (i = 0; i < dat->N; i++)
{
// shift indexes n+1 -> n
r_p[i] = r[i];
r[i] = r_n[i];
r_n[i] = vector_add(scalar_mult(2, r[i]),
vector_add(scalar_mult(-1, r_p[i]),
scalar_mult(dt * dt, a[i])));
v[i] = scalar_mult(0.5 / dt, vector_diff(r_n[i], r_p[i]));
}
adots(dat, r, v, a_1);
time += dt;
output(time, dt, dat, r, v);
}
free(a); free(r_p); free(r); free(r_n); free(v);
}
void euler_cromer(const data* dat, output_function output)
{
int i;
double dt = dat->eta * delta_t_factor;
vector *a = (vector*) malloc((dat->N)*sizeof(vector)),
*a_1 = (vector*) malloc((dat->N)*sizeof(vector)),
*r = init_r(dat),
*v = init_v(dat);
double time = 0;
output(time, dt, dat, r, v);
while (time < dat->t_max)
{
accelerations(dat, r, a);
adots(dat, r, v, a_1);
for (i = 0; i < dat->N; i++)
{
vector v_neu = vector_add(v[i], scalar_mult(dt, a[i])),
r_neu = vector_add(r[i], scalar_mult(0.5 * dt, vector_add(v[i], v_neu)));
r[i] = r_neu;
v[i] = v_neu;
}
dt = delta_t(dat, a, a_1);
time += dt;
output(time, dt, dat, r, v);
}
free(a); free(r); free(v);
}
bool HuboPath::Trajectory::interpolate()
{
if( elements.size() < 2 )
return true;
if( HUBO_PATH_RAW == params.interp )
{
return true;
}
std::vector<size_t> joint_mapping;
get_active_indices(joint_mapping);
std::vector<hubo_joint_limits_t> limits;
if(!get_active_joint_limits(limits))
{
std::cout << "Cannot interpolate without knowing joint limits!" << std::endl;
return false;
}
Eigen::VectorXd velocities(joint_mapping.size());
Eigen::VectorXd accelerations(joint_mapping.size());
for(size_t j=0; j<joint_mapping.size(); ++j)
{
velocities[j] = limits[j].nominal_speed;
accelerations[j] = limits[j].nominal_accel;
}
double frequency = desc.okay()? desc.params.frequency : params.frequency;
if( 0 == frequency )
{
std::cout << "Warning: Your Trajectory contained a frequency parameter of 0\n"
" -- We will default this to 200" << std::endl;
frequency = 200;
}
if( HUBO_PATH_OPTIMAL == params.interp )
{
return _optimal_interpolation(velocities, accelerations, frequency);
}
else if( HUBO_PATH_SPLINE == params.interp )
{
return _spline_interpolation(velocities, accelerations, frequency);
}
else if( HUBO_PATH_DENSIFY == params.interp )
{
return _densify();
}
return false;
}
Vec3 Accelerometer::get_raw_acceleration() const
{
const int bytesPerAxis = 2;
const int totalNumOfBytes = bytesPerAxis * 3;
byte buffer[totalNumOfBytes];
I2C::read_from_register(DEVICE, DATAX0, totalNumOfBytes, buffer);
const short x = (((short)buffer[1]) << 8) | buffer[0];
const short y = (((short)buffer[3]) << 8) | buffer[2];
const short z = (((short)buffer[5]) << 8) | buffer[4];
Vec3 accelerations(x, y, z);
//Converts to Gs. Refer to the manual.
return accelerations * 0.004;
}
Vector<Acceleration, Frame> Ephemeris<Frame>::
ComputeGravitationalAccelerationOnMasslessBody(
Position<Frame> const& position,
Instant const& t) const {
std::vector<Vector<Acceleration, Frame>> accelerations(1);
std::vector<typename ContinuousTrajectory<Frame>::Hint> hints(bodies_.size());
ComputeMasslessBodiesGravitationalAccelerations(
t,
{position},
&accelerations,
&hints);
return accelerations[0];
}
void leap_frog(const data* dat, output_function output)
{
int i;
double dt = dat->eta * delta_t_factor;
vector *a = (vector*) malloc((dat->N)*sizeof(vector)),
*a_1 = (vector*) malloc((dat->N)*sizeof(vector)),
*r_p = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n+1/2), not initialized
*r = init_r(dat), // r_(n+1)
*r_n = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n+3/2), not initialized
*v = init_v(dat); // v_(n+1)
double time = 0;
output(time, dt, dat, r, v);
// initial step
for (i = 0; i < dat->N; i++)
// set r_(1/2)
r_n[i] = vector_add(r[i], scalar_mult(0.5 * dt, v[i]));
// regular timesteps
while (time < dat->t_max)
{
// a_(n+1/2)
accelerations(dat, r_n, a);
adots(dat, r_n, v, a_1);
for (i = 0; i < dat->N; i++)
{
// store previous values as r_(n+1/2)
r_p[i] = r_n[i];
// v_(n+1)
vector_add_to(&v[i], scalar_mult(dt, a[i]));
// r_(n+3/2)
vector_add_to(&r_n[i], scalar_mult(dt, v[i]));
// build r_(n+1)
r[i] = scalar_mult(0.5, vector_add(r_p[i], r_n[i]));
}
dt = delta_t(dat, a, a_1);
time += dt;
output(time, dt, dat, r, v);
}
free(a); free(r_p); free(r); free(r_n); free(v);
}
bool MotomanJointTrajectoryStreamer::create_message_ex(int seq, const motoman_msgs::DynamicJointPoint &point, SimpleMessage *msg)
{
JointTrajPtFullEx msg_data_ex;
JointTrajPtFullExMessage jtpf_msg_ex;
std::vector<industrial::joint_traj_pt_full::JointTrajPtFull> msg_data_vector;
JointData values;
int num_groups = point.num_groups;
for (int i = 0; i < num_groups; i++)
{
JointTrajPtFull msg_data;
motoman_msgs::DynamicJointsGroup pt;
motoman_msgs::DynamicJointPoint dpoint;
pt = point.groups[i];
if (pt.positions.size() < 10)
{
int size_to_complete = 10 - pt.positions.size();
std::vector<double> positions(size_to_complete, 0.0);
std::vector<double> velocities(size_to_complete, 0.0);
std::vector<double> accelerations(size_to_complete, 0.0);
pt.positions.insert(pt.positions.end(), positions.begin(), positions.end());
pt.velocities.insert(pt.velocities.end(), velocities.begin(), velocities.end());
pt.accelerations.insert(pt.accelerations.end(), accelerations.begin(), accelerations.end());
}
// copy position data
if (!pt.positions.empty())
{
if (VectorToJointData(pt.positions, values))
msg_data.setPositions(values);
else
ROS_ERROR_RETURN(false, "Failed to copy position data to JointTrajPtFullMessage");
}
else
msg_data.clearPositions();
// copy velocity data
if (!pt.velocities.empty())
{
if (VectorToJointData(pt.velocities, values))
msg_data.setVelocities(values);
else
ROS_ERROR_RETURN(false, "Failed to copy velocity data to JointTrajPtFullMessage");
}
else
msg_data.clearVelocities();
// copy acceleration data
if (!pt.accelerations.empty())
{
if (VectorToJointData(pt.accelerations, values))
msg_data.setAccelerations(values);
else
ROS_ERROR_RETURN(false, "Failed to copy acceleration data to JointTrajPtFullMessage");
}
else
msg_data.clearAccelerations();
// copy scalar data
msg_data.setRobotID(pt.group_number);
msg_data.setSequence(seq);
msg_data.setTime(pt.time_from_start.toSec());
// convert to message
msg_data_vector.push_back(msg_data);
}
msg_data_ex.setMultiJointTrajPtData(msg_data_vector);
msg_data_ex.setNumGroups(num_groups);
msg_data_ex.setSequence(seq);
jtpf_msg_ex.init(msg_data_ex);
return jtpf_msg_ex.toRequest(*msg); // assume "request" COMM_TYPE for now
}
Vector<Acceleration, Frame> Ephemeris<Frame>::
ComputeGravitationalAccelerationOnMassiveBody(
not_null<MassiveBody const*> const body,
Instant const& t) const {
bool const body_is_oblate = body->is_oblate();
// |reodered_bodies| is |bodies_| with |body| moved to position 0. Index 0 in
// |positions| and |accelerations| alos corresponds to |body|, the other
// indices to the other bodies in the same order as in |bodies| and |bodies_|.
std::vector<not_null<MassiveBody const*>> reodered_bodies;
std::vector<Position<Frame>> positions;
std::vector<Vector<Acceleration, Frame>> accelerations(bodies_.size());
// Make room for |body|.
reodered_bodies.push_back(body);
positions.resize(1);
// Evaluate the |positions|.
for (int b = 0; b < bodies_.size(); ++b) {
auto const& current_body = bodies_[b];
auto const& current_body_trajectory = trajectories_[b];
if (current_body.get() == body) {
positions[0] =
current_body_trajectory->EvaluatePosition(t, /*hint=*/nullptr);
} else {
reodered_bodies.push_back(current_body.get());
positions.push_back(
current_body_trajectory->EvaluatePosition(t, /*hint=*/nullptr));
}
}
if (body_is_oblate) {
ComputeGravitationalAccelerationByMassiveBodyOnMassiveBodies<
/*body1_is_oblate=*/true,
/*body2_is_oblate=*/true>(
/*body1=*/*body, /*b1=*/0,
/*bodies2=*/reodered_bodies,
/*b2_begin=*/1, /*b2_end=*/number_of_oblate_bodies_,
positions, &accelerations);
ComputeGravitationalAccelerationByMassiveBodyOnMassiveBodies<
/*body1_is_oblate=*/true,
/*body2_is_oblate=*/false>(
/*body1=*/*body, /*b1=*/0,
/*bodies2=*/reodered_bodies,
/*b2_begin=*/number_of_oblate_bodies_, /*b2_end=*/bodies_.size(),
positions, &accelerations);
} else {
ComputeGravitationalAccelerationByMassiveBodyOnMassiveBodies<
/*body1_is_oblate=*/false,
/*body2_is_oblate=*/true>(
/*body1=*/*body, /*b1=*/0,
/*bodies2=*/reodered_bodies,
/*b2_begin=*/1, /*b2_end=*/number_of_oblate_bodies_ + 1,
positions, &accelerations);
ComputeGravitationalAccelerationByMassiveBodyOnMassiveBodies<
/*body1_is_oblate=*/false,
/*body2_is_oblate=*/false>(
/*body1=*/*body, /*b1=*/0,
/*bodies2=*/reodered_bodies,
/*b2_begin=*/number_of_oblate_bodies_ + 1, /*b2_end=*/bodies_.size(),
positions, &accelerations);
}
return accelerations[0];
}
void runge_kutta(const data* dat, output_function output)
{
int i;
double dt = dat->eta * delta_t_factor;
vector *r = init_r(dat),
*v = init_v(dat),
*a = (vector*) malloc((dat->N)*sizeof(vector)),
*a_1 = (vector*) malloc((dat->N)*sizeof(vector));
// temporary r vector for acceleration calculations
vector *rt = (vector*) malloc((dat->N)*sizeof(vector));
vector *a1 = (vector*) malloc((dat->N)*sizeof(vector)),
*a2 = (vector*) malloc((dat->N)*sizeof(vector)),
*a3 = (vector*) malloc((dat->N)*sizeof(vector)),
*a4 = (vector*) malloc((dat->N)*sizeof(vector));
vector *r1 = (vector*) malloc((dat->N)*sizeof(vector)),
*r2 = (vector*) malloc((dat->N)*sizeof(vector)),
*r3 = (vector*) malloc((dat->N)*sizeof(vector)),
*r4 = (vector*) malloc((dat->N)*sizeof(vector));
vector *v1 = (vector*) malloc((dat->N)*sizeof(vector)),
*v2 = (vector*) malloc((dat->N)*sizeof(vector)),
*v3 = (vector*) malloc((dat->N)*sizeof(vector)),
*v4 = (vector*) malloc((dat->N)*sizeof(vector));
double time = 0;
while (time < dat->t_max)
{
accelerations(dat, r, a1);
for (i = 0; i < dat->N; i++)
{
v1[i] = scalar_mult(dt, a1[i]);
r1[i] = scalar_mult(dt, v[i]);
// temp r for a2
rt[i] = vector_add(r[i], scalar_mult(0.5, r1[i]));
}
accelerations(dat, rt, a2);
for (i = 0; i < dat->N; i++)
{
v2[i] = scalar_mult(dt, a2[i]);
r2[i] = scalar_mult(dt, vector_add(v[i], scalar_mult(0.5, v1[i])));
// temp r for a3
rt[i] = vector_add(r[i], scalar_mult(0.5, r2[i]));
}
accelerations(dat, rt, a3);
for (i = 0; i < dat->N; i++)
{
v3[i] = scalar_mult(dt, a3[i]);
r3[i] = scalar_mult(dt, vector_add(v[i], scalar_mult(0.5, v2[i])));
// temp r for a3
rt[i] = vector_add(r[i], scalar_mult(0.5, r3[i]));
}
accelerations(dat, rt, a4);
for (i = 0; i < dat->N; i++)
{
v4[i] = scalar_mult(dt, a4[i]);
r4[i] = scalar_mult(dt, vector_add(v[i], v3[i]));
// calculate v_(n+1) and r_(n+1)
vector_add_to(&v[i], vector_add(scalar_mult(1.0/6.0, v1[i]),
vector_add(scalar_mult(1.0/3.0, v2[i]),
vector_add(scalar_mult(1.0/3.0, v3[i]),
scalar_mult(1.0/6.0, v4[i])))));
vector_add_to(&r[i], vector_add(scalar_mult(1.0/6.0, r1[i]),
vector_add(scalar_mult(1.0/3.0, r2[i]),
vector_add(scalar_mult(1.0/3.0, r3[i]),
scalar_mult(1.0/6.0, r4[i])))));
}
// increase time
adots(dat, r, v, a_1);
dt = delta_t(dat, a1, a_1); // a1 = a(t_n), a_1 = a'(t_n)
time += dt;
output(time, dt, dat, r, v);
}
free(r); free(v); free(a); free(a_1);
free(rt);
free(r1); free(r2); free(r3); free(r4);
free(v1); free(v2); free(v3); free(v4);
free(a1); free(a2); free(a3); free(a4);
}
void hermite_iterated(const data* dat, output_function output, int iterations)
{
int i,j;
double dt = dat->eta * delta_t_factor;
vector *a = (vector*) malloc((dat->N)*sizeof(vector)), // a_n, not initialized
*a_p = (vector*) malloc((dat->N)*sizeof(vector)), // a_(n+1)^p, not initialized
*a_1 = (vector*) malloc((dat->N)*sizeof(vector)), // a_1_n, not initialized
*a_1_p = (vector*) malloc((dat->N)*sizeof(vector)), // a_1_(n+1)^p, not initialized
*r = init_r(dat), // r_n
*r_p = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n+1)^p, not initialized
*v = init_v(dat), // v_n
*v_p = (vector*) malloc((dat->N)*sizeof(vector)); // v_(n+1)^p, not initialized
double time = 0;
accelerations(dat, r, a);
adots(dat, r, v, a_1);
dt = delta_t(dat, a, a_1);
output(time, dt, dat, r, v);
while (time < dat->t_max)
{
// prediction step
for (i = 0; i < dat->N; i++)
{
v_p[i] = vector_add(v[i],
vector_add(scalar_mult(dt, a[i]),
scalar_mult(0.5 * dt * dt, a_1[i])));
r_p[i] = vector_add(r[i],
vector_add(scalar_mult(dt, v[i]),
vector_add(scalar_mult(0.5 * dt * dt, a[i]),
scalar_mult(dt * dt * dt / 6.0, a_1[i]))));
}
// iteration of correction step
for (j = 0; j < iterations; j++)
{
// predict a, a_1
accelerations(dat, r_p, a_p);
adots(dat, r_p, v_p, a_1_p);
for (i = 0; i < dat->N; i++)
{
// correction steps -> overwrite "prediction" variables for convenience,
// will get written in r,v after iteration is done
v_p[i] = vector_add(v[i],
vector_add(scalar_mult(dt / 2.0, vector_add(a_p[i], a[i])),
scalar_mult(dt * dt / 12.0, vector_diff(a_1_p[i], a_1[i]))));
r_p[i] = vector_add(r[i],
vector_add(scalar_mult(dt / 2.0, vector_add(v_p[i], v[i])),
scalar_mult(dt * dt / 12.0, vector_diff(a_p[i], a[i]))));
}
}
// apply iterated correction steps
for (i = 0; i < dat->N; i++)
{
r[i] = r_p[i];
v[i] = v_p[i];
}
time += dt;
// a/a_1 values for next step
accelerations(dat, r, a);
adots(dat, r, v, a_1);
// new dt
dt = delta_t(dat, a, a_1);
output(time, dt, dat, r, v);
}
free(a); free(a_p);
free(a_1); free(a_1_p);
free(r); free(r_p);
free(v); free(v_p);
}
void hermite(const data* dat, output_function output)
{
int i;
double dt = dat->eta * delta_t_factor;
vector *a = (vector*) malloc((dat->N)*sizeof(vector)), // a_n, not initialized
*a_p = (vector*) malloc((dat->N)*sizeof(vector)), // a_(n+1)^p, not initialized
*a_1 = (vector*) malloc((dat->N)*sizeof(vector)), // a_1_n, not initialized
*a_1_p = (vector*) malloc((dat->N)*sizeof(vector)), // a_1_(n+1)^p, not initialized
*a_2 = (vector*) malloc((dat->N)*sizeof(vector)), // a_(n+1)^(2), not initialized
*a_3 = (vector*) malloc((dat->N)*sizeof(vector)), // a_(n+1)^(3), not initialized
*r = init_r(dat), // r_n
*r_p = (vector*) malloc((dat->N)*sizeof(vector)), // r_(n+1)^p, not initialized
*v = init_v(dat), // v_n
*v_p = (vector*) malloc((dat->N)*sizeof(vector)); // v_(n+1)^p, not initialized
double time = 0;
accelerations(dat, r, a);
adots(dat, r, v, a_1);
dt = delta_t(dat, a, a_1);
output(time, dt, dat, r, v);
while (time < dat->t_max)
{
// prediction step
for (i = 0; i < dat->N; i++)
{
v_p[i] = vector_add(v[i],
vector_add(scalar_mult(dt, a[i]),
scalar_mult(0.5 * dt * dt, a_1[i])));
r_p[i] = vector_add(r[i],
vector_add(scalar_mult(dt, v[i]),
vector_add(scalar_mult(0.5 * dt * dt, a[i]),
scalar_mult(dt * dt * dt / 6.0, a_1[i]))));
}
// predict a^p, a_1^p
accelerations(dat, r_p, a_p);
adots(dat, r_p, v_p, a_1_p);
for (i = 0; i < dat->N; i++)
{
// predict 2nd and 3rd derivations
a_2[i] = vector_add(scalar_mult(2 * (-3) / dt / dt, vector_diff(a[i], a_p[i])),
scalar_mult(2 * (-1) / dt, vector_add(scalar_mult(2, a_1[i]), a_1_p[i])));
a_3[i] = vector_add(scalar_mult(6 * 2 / dt / dt / dt, vector_diff(a[i], a_p[i])),
scalar_mult(6 / dt / dt, vector_add(a_1[i], a_1_p[i])));
// correction steps
v[i] = vector_add(v_p[i],
vector_add(scalar_mult(dt * dt * dt / 6.0, a_2[i]),
scalar_mult(dt * dt * dt / 24.0, a_3[i])));
r[i] = vector_add(r_p[i],
vector_add(scalar_mult(dt * dt * dt * dt / 24.0, a_2[i]),
scalar_mult(dt * dt * dt * dt * dt / 120.0, a_3[i])));
}
time += dt;
// a/a_1 values for next step
accelerations(dat, r, a);
adots(dat, r, v, a_1);
// new dt
dt = delta_t_(dat, a, a_1, a_2, a_3);
output(time, dt, dat, r, v);
}
free(a); free(a_p);
free(a_1); free(a_1_p);
free(r); free(r_p);
free(v); free(v_p);
}
