예제 #1
0
void perturb_initial_conditions()
{
  const double LV_PERTURB = 0.1;
  const double AV_PERTURB = 0.05;
  const double ELLIPSE_RADIUS_PERTURB = 0.1;
  VectorNd gc, gv;
  Point3d dummy1, dummy2;

  // perturb the initial position
  ellipse->get_generalized_coordinates_euler(gc);
  gc[0] += normal_gen();
  gc[1] += normal_gen();

  // perturb the initial rotation
  Quatd quat = Matrix3d::rot_Z(randu() * M_PI * 2.0);
  gc[3] = quat.x;  
  gc[4] = quat.y;  
  gc[5] = quat.z;  
  gc[6] = quat.w;  

  // perturb the ellipse radius
  std::ostringstream x_rad, y_rad;
//  x_rad << (randu() * ELLIPSE_RADIUS_PERTURB * 2.0 - ELLIPSE_RADIUS_PERTURB);
//  y_rad << (randu() * ELLIPSE_RADIUS_PERTURB * 2.0 - ELLIPSE_RADIUS_PERTURB);
x_rad << "1.0";
y_rad << "2.0";
  setenv("ELLIPSE_X_RAD", x_rad.str().c_str(), 1);
  setenv("ELLIPSE_Y_RAD", y_rad.str().c_str(), 1);

  // ensure that the initial configuration is not in contact with the ground
  VectorNd newgc = gc;
  ellipse->set_generalized_coordinates_euler(newgc);

  // get the collision detector
  shared_ptr<CollisionDetection> coldet = sim->get_collision_detection();

  // get the collision geometries for the ellipse and the ground
  shared_ptr<CollisionGeometry> ground_cg = ground->geometries.front();
  shared_ptr<CollisionGeometry> ellipse_cg = ellipse->geometries.front();

  // check for contact between ground and ellipse
  if (coldet->calc_signed_dist(ground_cg, ellipse_cg, dummy1, dummy2) <= 0.0)
  {
    // reset the generalized coords
    ellipse->set_generalized_coordinates_euler(gc);

    // try again
    perturb_initial_conditions();
    return;
  }

  // perturb the initial velocity 
  ellipse->get_generalized_velocity(DynamicBodyd::eSpatial, gv);
  for (unsigned i=0; i< 3; i++)
    gv[i+3] += normal_gen()*LV_PERTURB; 
    gv[5] += normal_gen()*AV_PERTURB; 
  ellipse->set_generalized_velocity(DynamicBodyd::eSpatial, gv);
}
예제 #2
0
// 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();
}
예제 #3
0
// 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();
}
// 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();
}
예제 #5
0
// 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));
}
예제 #7
0
// 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();
}
예제 #8
0
// 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);
  }
}
예제 #9
0
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map, double time)
{
  const unsigned Z = 2;

  // overwrite the energy and velocity files
  std::ofstream out("energy.dat");
  out.close();
  out.open("velocity.dat");
  out.close();
  out.open("cvio.dat");
  out.close();

  // If use robot is active also init dynamixel controllers
  // 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 == "WHEEL")
      wheel = boost::dynamic_pointer_cast<RigidBody>(i->second);
    if (i->first == "GROUND")
      ground = 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->post_mini_step_callback_fn = &post_ministep_callback;

  // initialize the system to the fixed point
//  double theta = M_PI/N_SPOKES;
  double theta = 0.0;
  const double ALPHA = 0.2;
  const double LAMBDA_SQ = 2.0/3;
  const double MU = 1.0 + LAMBDA_SQ * (std::cos(2*M_PI/N_SPOKES) - 1.0);
  assert(MU > 0.0 && MU < 1.0);
//  double theta_dot = std::sqrt((4.0*MU*MU*LAMBDA_SQ*std::sin(M_PI/N_SPOKES)*std::sin(ALPHA))/(1.0 - MU*MU));

  // get theta dot
  char* theta_dot_str = getenv("RIMLESS_WHEEL_THETAD");
  if (!theta_dot_str) 
  {
    std::cerr << "RIMLESS_WHEEL_THETAD not defined!" << std::endl;
    exit(-1);
  }
  double theta_dot = std::atof(theta_dot_str);
  out.open("system.state", std::ostream::app);
  out << theta_dot << " ";
  out.close();

  // get the distance per revolution
  const double DIST_PER_REV = 2*M_PI*R;
  const double REV_PER_SEC = theta_dot / (M_PI*2.0);
  const double DIST_PER_SEC = DIST_PER_REV * REV_PER_SEC;

  // set the rotation about y
  Quatd q_wheel = Matrix3d::rot_Y(theta);
  Vector3d lvel(DIST_PER_SEC, 0.0, 0.0, wheel->get_velocity().pose); 
  Vector3d avel(0, theta_dot, 0, wheel->get_velocity().pose);
  Pose3d P;
  P.x.set_zero();
  P.x[Z] = 0.866025403784439; 
  P.q = q_wheel;
  wheel->set_pose(P);
  SVelocityd v;
  v.set_angular(avel);
  v.set_linear(lvel);
  wheel->set_velocity(v);
/*
  // Set initial conditions from ruina paper
  Ravelin::VectorNd x,xd;
  part->get_generalized_coordinates( Moby::DynamicBody::eEuler,x);
  part->get_generalized_velocity( Moby::DynamicBody::eSpatial,xd);

  x[1] = 0; // x
  x[2] = 0; // y
  x[3] = 0.1236; // z

//     9.866765986740000e-002
//    -9.248610676160000e-003
//    -1.601658349552200e-001
//     3.435833890385830e+000
//    -1.322096551035500e-001
//    -1.990961987794000e-002
//     4.712423746697700e-001
//    -3.925591686648300e-001

  double  PHI = 9.866765986740000e-002,  // yaw
          THE = -1.601658349552200e-001, // pitch + alpha
          PSI = -9.248610676160000e-003; // roll
  Ravelin::Matrix3d Rz = Ravelin::Matrix3d::rot_Z(PHI),
                    Ry = Ravelin::Matrix3d::rot_Y(THE),
                    Rx = Ravelin::Matrix3d::rot_X(PSI),
                    Rzxy = Ry.mult(Rx).mult(Rz);
  Quatd q = Quatd(Rzxy);

  x[4] = q[0];
  x[5] = q[1];
  x[6] = q[2];
  x[7] = q[3];
  x[0] =  3.435833890385830e+000 + THETA_SW_OFFSET; // Theta_sw

  // Convert Time derivative of RPY to angular velocity (w)
  double  dPHI = -1.322096551035500e-001, // d yaw / dt
          dTHE =  4.712423746697700e-001, // d pitch / dt
          dPSI = -1.990961987794000e-002; // d roll / dt
  //  a_dot is time derivative of RPY
  // Numerically derive skew symmetric
  // angular velocity tensor matrix W
  double h = 1e-6;
  Matrix3d
      Rz2 = Ravelin::Matrix3d::rot_Z(PHI + h*dPHI),
      Rx2 = Ravelin::Matrix3d::rot_X(PSI + h*dPSI),
      Ry2 = Ravelin::Matrix3d::rot_Y(THE + h*dTHE);
   Matrix3d
      Rzxy2 = Ry2.mult(Rx2).mult(Rz2);
   Matrix3d
      W = ((Rzxy2-Rzxy)/h).mult_transpose(Rzxy);

  // w is angular velocity
  Vector3d w((W(2,1)-W(1,2))/2,(W(0,2)-W(2,0))/2,(W(1,0)-W(0,1))/2);
  xd.set_zero();
  xd.set_sub_vec(4,w);
  xd[0] = -3.925591686648300e-001; // Theta_sw

  part->set_generalized_coordinates( Moby::DynamicBody::eEuler,x);
  part->set_generalized_velocity( Moby::DynamicBody::eSpatial,xd);
*/
}