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); }