Exemplos de RigidBodyPtr::get_generalized_coordinates_euler em C++ (Cpp)

Exemplo n.º 1

Arquivo: ellipse-init.cpp Projeto: PositronicsLab/Moby

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

Exemplo n.º 2

Arquivo: rbox-plugin-adaptive.cpp Projeto: PositronicsLab/wafr16-experiments

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