// 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(); }
// 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 mini-callback void post_ministep_callback(ConstraintSimulator* sim) { const unsigned Y = 1; // see whether there is significant undesired rotation AAngled aa = Pose3d::calc_relative_pose(wheel->get_pose(), GLOBAL).q; if ((std::fabs(aa.x) > 1e-4 || std::fabs(aa.z) > 1e-4) && std::fabs(aa.angle) > 1e-6) { std::cerr << "excessive angular rotation out of the plane: " << std::max(std::fabs(aa.x), std::fabs(aa.z)) << std::endl; } // output the velocity std::ofstream out("velocity.dat", std::ostream::app); out << sim->current_time << " " << wheel->get_velocity().get_angular()[Y] << std::endl; out.close(); }
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map, double time) { // wipe out existing files std::ofstream out("rke.dat"); out.close(); out.open("telemetry.box"); out.close(); out.open("manifold.change"); out.close(); // get a reference to the TimeSteppingSimulator instance for (std::map<std::string, Moby::BasePtr>::const_iterator i = read_map.begin(); i !=read_map.end(); i++) { // Find the simulator reference if (!sim) sim = boost::dynamic_pointer_cast<TimeSteppingSimulator>(i->second); if (i->first == "box") box = boost::dynamic_pointer_cast<RigidBody>(i->second); if (!grav) grav = boost::dynamic_pointer_cast<GravityForce>(i->second); } sim->post_step_callback_fn = &post_step_callback; sim->constraint_callback_fn = &post_contact_callback; srand(0); // set the position, orientation, and velocities to some random values Pose3d x = *box->get_pose(); rand_pose(x); // add 2.0 to the initial pose for the box x.x[1] += 2.0; box->set_pose(x); // set random velocity for the box shared_ptr<const Pose3d> P = box->get_velocity().pose; box->set_velocity(rand_velocity(P)); }
// 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); } }