// setup simulator callback
void post_step_callback(Simulator* sim)
{
// output the sliding velocity at the contact
std::ofstream out("ke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
}
// setup simulator callback
void post_step_callback(Simulator* sim)
{
const unsigned X = 0, Y = 1, Z = 2;
// setup the box height
const double H = 1.0;
// get the bottoms of the box
Transform3d wTs = Pose3d::calc_relative_pose(box->get_pose(), GLOBAL);
Vector3d p1 = wTs.transform_point(Vector3d(-.5, -.5, -.5, box->get_pose()));
Vector3d p2 = wTs.transform_point(Vector3d(-.5, -.5, .5, box->get_pose()));
Vector3d p3 = wTs.transform_point(Vector3d(.5, -.5, -.5, box->get_pose()));
Vector3d p4 = wTs.transform_point(Vector3d(.5, -.5, .5, box->get_pose()));
// get the bottoms of the box in the global frame
shared_ptr<Pose3d> P1(new Pose3d), P2(new Pose3d), P3(new Pose3d), P4(new Pose3d);
P1->x = Origin3d(p1);
P2->x = Origin3d(p2);
P3->x = Origin3d(p3);
P4->x = Origin3d(p4);
// get the velocity of the box at the contact points
SVelocityd v = box->get_velocity();
Vector3d xd1 = Pose3d::calc_relative_pose(v.pose, P1).transform(v).get_linear();
Vector3d xd2 = Pose3d::calc_relative_pose(v.pose, P2).transform(v).get_linear();
Vector3d xd3 = Pose3d::calc_relative_pose(v.pose, P3).transform(v).get_linear();
Vector3d xd4 = Pose3d::calc_relative_pose(v.pose, P4).transform(v).get_linear();
/*
SVelocityd v = box->get_velocity();
Origin3d xd(v.get_linear());
Origin3d omega(v.get_angular());
Origin3d s(1.0, 0.0, 0.0);
Origin3d t(0.0, 1.0, 0.0);
Origin3d crosss = Origin3d::cross(-wTs.x, s);
Origin3d crosst = Origin3d::cross(-wTs.x, t);
*/
// output the sliding velocity at the contact
std::ofstream out("contactv.dat", std::ostream::app);
out << sim->current_time << " " << xd1[X] << " " << xd1[Y] << " " << xd2[X] << " " << xd2[Y] << " " << xd3[X] << " " << xd3[Y] << " " << xd4[X] << " " << xd4[Y] << std::endl;
// out << sim->current_time << " " << (s.dot(xd) + crosss.dot(omega)) << " " << (t.dot(xd) + crosst.dot(omega)) << std::endl;
// out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("ke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
}
// simulator callback
void post_step_callback(Simulator* sim)
{
// output the sliding velocity at the contact
std::ofstream out("rke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
// save the generalized coordinates of the box
out.open("telemetry.box", std::ostream::app);
VectorNd q;
box->get_generalized_coordinates_euler(q);
out << sim->current_time;
for (unsigned i=0; i< q.size(); i++)
out << " " << q[i];
out << std::endl;
out.close();
}
// setup simulator callback
void post_step_callback(Simulator* s)
{
const unsigned X = 0, Y = 1, Z = 2;
// setup the sphere radius
const double R = 1.0;
// get the bottom of the sphere
Transform3d wTs = Pose3d::calc_relative_pose(sphere->get_pose(), GLOBAL);
shared_ptr<Pose3d> Pbot(new Pose3d);
Pbot->rpose = GLOBAL;
Pbot->x = wTs.x;
Pbot->x[Z] -= R;
// get the velocity of the sphere at the contact point
SVelocityd v = sphere->get_velocity();
Transform3d botTv = Pose3d::calc_relative_pose(v.pose, Pbot);
SVelocityd xd = botTv.transform(v);
Vector3d linear = xd.get_linear();
/*
SVelocityd v = sphere->get_velocity();
Origin3d xd(v.get_linear());
Origin3d omega(v.get_angular());
Origin3d s(1.0, 0.0, 0.0);
Origin3d t(0.0, 1.0, 0.0);
Origin3d crosss = Origin3d::cross(-wTs.x, s);
Origin3d crosst = Origin3d::cross(-wTs.x, t);
*/
// output the sliding velocity at the contact
std::ofstream out("contactv.dat", std::ostream::app);
out << sim->current_time << " " << linear[X] << " " << linear[Y] << " " << linear[Z] << std::endl;
// out << sim->current_time << " " << (s.dot(xd) + crosss.dot(omega)) << " " << (t.dot(xd) + crosst.dot(omega)) << std::endl;
// out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("velocity.dat", std::ostream::app);
out << sim->current_time << " " << v[3] << " " << v[4] << " " << v[5] << " " << v[0] << " " << v[1] << " " << v[2] << std::endl;
out.close();
out.open("ke.dat", std::ostream::app);
out << sim->current_time << " " << sphere->calc_kinetic_energy() << std::endl;
out.close();
}
// setup simulator callback
void post_step_callback(Simulator* sim)
{
const unsigned Z = 2;
std::ofstream out("energy.dat", std::ostream::app);
double KE = wheel->calc_kinetic_energy();
Transform3d gTw = Pose3d::calc_relative_pose(wheel->get_pose(), GLOBAL);
double PE = wheel->get_inertia().m*gTw.x[Z]*-grav->gravity[Z];
out << sim->current_time << " " << KE << " " << PE << " " << (KE+PE) << std::endl;
out.close();
// see whether there is significant undesired rotation
AAngled aa = Pose3d::calc_relative_pose(wheel->get_pose(), GLOBAL).q;
out.open("angular.dat", std::ostream::app);
out << sim->current_time << " " << std::fabs(aa.x) << " " << std::fabs(aa.z) << " " << " " << std::fabs(aa.angle) << std::endl;
out.close();
// see whether we are in a ballistic flight phase
TimeSteppingSimulator* esim = (TimeSteppingSimulator*) sim;
boost::shared_ptr<CollisionDetection> coldet = esim->get_collision_detection();
CollisionGeometryPtr cgw = wheel->geometries.front();
CollisionGeometryPtr cgg = ground->geometries.front();
Point3d cpw, cpg;
double dist = coldet->calc_signed_dist(cgw, cgg, cpw, cpg);
if (dist > 1e-4)
std::cerr << "-- in a ballistic flight phase at time " << sim->current_time << std::endl;
// fast exit conditions
if (FIND_MAP)
{
if (KE < NEAR_ZERO)
{
std::cerr << "kinetic energy too small!" << std::endl;
out.open("system.state", std::ostream::app);
out << "0.0" << std::endl;
exit(0);
}
if (sim->current_time > 100.0)
{
std::cerr << "simulation ran too long!" << std::endl;
out.open("system.state", std::ostream::app);
out << "DnF" << std::endl;
exit(0);
}
}
// if we're finding the return map, see whether the next step has been
// encountered
if (FIND_MAP)
{
// look to see whether the last processed contact was a new spoke
std::ifstream in("IPC.token");
if (in.fail())
return;
in.close();
// it was, output the state of the system
std::ofstream out("system.state", std::ostream::app);
out << " " << wheel->get_velocity().get_angular()[1] << std::endl;
out.close();
exit(0);
}
}
