Examples

All examples can be found in the examples directory under the Ravelin source code distribution.

Integrating the equations of motion for a pendulum

#include <iostream>
#include <boost/shared_ptr.hpp>
#include "integrate.h"  // defined in the examples directory
#include <Ravelin/RCArticulatedBodyd.h>
#include <Ravelin/RevoluteJointd.h>

using boost::shared_ptr;
using namespace Ravelin;

int main( void ) {
  const unsigned X = 0, Y = 1, Z = 2;
  const shared_ptr<const Pose3d> GLOBAL_3D;

  // current time
  double t = 0.0;

  // integration step size
  const double DT = 0.01;

// uncomment to log dynamics
//  Log<Ravelin::OutputToFile>::reporting_level = LOG_DYNAMICS;

  // the pendulum has a joint, so we need an articulated body 
  shared_ptr<RCArticulatedBodyd> pendulum( new RCArticulatedBodyd() );
  pendulum->body_id = "pendulum";

  // set the algorithm to the Composite Rigid Body Method
  pendulum->algorithm_type = RCArticulatedBodyd::eCRB;

  // set the dynamics computations to take place in link inertia frames
  // (these are frame defined at the center-of-mass of each link and changing
  //  orientation as the link changes orientation)
  pendulum->set_computation_frame_type(eLinkInertia);

  // vectors of links and joints used to define the body
  std::vector< shared_ptr<RigidBodyd> > links;
  std::vector< shared_ptr<Jointd > > joints;

  // create the fixed base;
  shared_ptr<RigidBodyd> base( new RigidBodyd() );
  {
    // identify the link
    base->body_id = "base";

    // this next line isn't necessary- it disables physics computation for the
    // base- because we will denote the articulated body as having a fixed
    // base, but it is good practice
    base->set_enabled( false );
 
    // set the pose of the base 
    Quatd ori(Quatd::normalize(Quatd(0,0,0,1)));
    Origin3d position(0,0,0);
    Pose3d pose( ori, position );
    base->set_pose( pose ); 

    // add the base to the set of links
    links.push_back( base );
  }

  // the pendulum body is a cylinder
  shared_ptr<RigidBodyd> link( new RigidBodyd() );
  {
    // setup cylinder radius and height
    const double R = 0.025;
    const double H = 1.0;

    // setup inertia for cylinder- it will be defined with respect to the
    // centroid of the cylinder
    SpatialRBInertiad J;
    J.pose = link->get_pose();
    J.m = 1.0;

    // inertia matrix for cylinder is taken from online resources; note that
    // primary axis of cylinder is aligned with y-axis 
    J.J.set_zero(3,3);
    J.J(X,X) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 
    J.J(Y,Y) = 0.5*J.m*R*R; 
    J.J(Z,Z) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 

    // assign the link parameters
    link->body_id = "link";                              // identify the link
    link->set_inertia( J );                // set the inertia of the cylinder
    link->set_enabled( true );                 // enable physics for the link
 
    // set the pose of the link 
    Quatd ori(0,0,0,1);
    Origin3d position(0,-0.5,0);
    Pose3d pose( ori, position );
    link->set_pose( pose ); 

    // add the link to the set of links
    links.push_back( link ); 
  }

  // create the revolute joint
  boost::shared_ptr<RevoluteJointd> revolute( new RevoluteJointd() );
  {
    // set the name of the joint
    revolute->joint_id = "q0";

    // set the position of the joint... center of the base w.r.t global frame
    Vector3d position(0.0, 0.0, 0.0, GLOBAL_3D);
    revolute->set_location( position, base, link );

    // set the axis of ori
    Vector3d axis(0,0,1.0,GLOBAL_3D);   // actuates around global z-axis

    // assign the revolute parameters
    // Note: set_location(...) must precede set_axis(...)
    revolute->set_axis( axis ); 

    // add the revolute to the set of joints
    joints.push_back( revolute );
  }

  // construct the pendulum articulated body from the sets of links and joints 
  pendulum->set_links_and_joints( links, joints );

  // pendulum has a fixed base
  pendulum->set_floating_base(false);

  // set the pendulum initial configuration so that it has potential energy
  VectorNd gc(1);
  gc[0] = M_PI_2;
  pendulum->set_generalized_coordinates_euler(gc);
 
  // start the main simulation loop
  while(true) {
    // integrate the body forward
    integrate_euler(pendulum, DT); 

    // get the pose of the link 
    boost::shared_ptr<const Pose3d> pose = link->get_pose();

    // copy the pose, b/c we want to alter its relative pose
    Pose3d P = *pose;

    // update the relative pose to be in the global frame
    P.update_relative_pose(GLOBAL_3D);

    // print the pose to the console 
    std::cout << "t: " << t << ", pose: " << P << std::endl;

    // update the time
    t += DT;
  }

  return 0;
}

Integrating the equations of motion for a double pendulum

#include <iostream>
#include <boost/shared_ptr.hpp>
#include "integrate.h"  // defined in the examples directory
#include <Ravelin/RCArticulatedBodyd.h>
#include <Ravelin/RevoluteJointd.h>

using boost::shared_ptr;
using namespace Ravelin;

int main( void ) {
  const unsigned X = 0, Y = 1, Z = 2;
  const shared_ptr<const Pose3d> GLOBAL_3D;

  // current time
  double t = 0.0;

  // integration step size
  const double DT = 0.01;

// uncomment to log dynamics
//  Log<Ravelin::OutputToFile>::reporting_level = LOG_DYNAMICS;

  // the double pendulum has joints, so we need an articulated body 
  shared_ptr<RCArticulatedBodyd> pendulum( new RCArticulatedBodyd() );
  pendulum->body_id = "double-pendulum";

  // set the algorithm to the Composite Rigid Body Method
  pendulum->algorithm_type = RCArticulatedBodyd::eCRB;

  // set the dynamics computations to take place in link inertia frames
  // (these are frame defined at the center-of-mass of each link and changing
  //  orientation as the link changes orientation)
  pendulum->set_computation_frame_type(eLinkInertia);

  // vectors for references used in defining the body
  std::vector< shared_ptr<RigidBodyd> > links;
  std::vector< shared_ptr<Jointd > > joints;

  // create the fixed base;
  shared_ptr<RigidBodyd> base( new RigidBodyd() );
  {
    // identify the link
    base->body_id = "base";

    // this next line isn't necessary- it disables physics computation for the
    // base- because we will denote the articulated body as having a fixed
    // base, but it is good practice
    base->set_enabled( false );
 
    // set the pose of the base 
    Quatd ori(Quatd::normalize(Quatd(0,0,0,1)));
    Origin3d position(0,0,0);
    Pose3d pose( ori, position );
    base->set_pose( pose ); 

    // add the base to the set of links
    links.push_back( base );
  }

  // the pendulum body is a cylinder
  shared_ptr<RigidBodyd> link1( new RigidBodyd() );
  {
    // setup cylinder radius and height
    const double R = 0.025;
    const double H = 1.0;

    // setup inertia for cylinder- it will be defined with respect to the
    // centroid of the cylinder
    SpatialRBInertiad J;
    J.pose = link1->get_pose();
    J.m = 1.0;

    // inertia matrix for cylinder is taken from online resources; note that
    // primary axis of cylinder is aligned with y-axis 
    J.J.set_zero(3,3);
    J.J(X,X) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 
    J.J(Y,Y) = 0.5*J.m*R*R; 
    J.J(Z,Z) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 

    // assign the link1 parameters
    link1->body_id = "link1";                              // identify the link
    link1->set_inertia( J );                // set the inertia of the cylinder
    link1->set_enabled( true );                 // enable physics for the link
 
    // set the pose of the link
    // the height of the cylinder is 1.0, and the first joint will be located
    // at (0,0,0). In the zero configuration, the centroid of the cylinder will
    // therefore be located at (0,-0.5,0)
    Quatd ori(0,0,0,1);
    Origin3d position(0,-H/2.0,0);
    Pose3d pose( ori, position );
    link1->set_pose( pose ); 

    // add the link to the set of links
    links.push_back( link1 ); 
  }

  // the pendulum body is a cylinder
  shared_ptr<RigidBodyd> link2( new RigidBodyd() );
  {
    // setup cylinder radius and height
    const double R = 0.025;
    const double H = 1.0;

    // setup inertia for cylinder- it will be defined with respect to the
    // centroid of the cylinder
    SpatialRBInertiad J;
    J.pose = link2->get_pose();
    J.m = 1.0;

    // inertia matrix for cylinder is taken from online resources; note that
    // primary axis of cylinder is aligned with y-axis 
    J.J.set_zero(3,3);
    J.J(X,X) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 
    J.J(Y,Y) = 0.5*J.m*R*R; 
    J.J(Z,Z) = 1.0/12*J.m*H*H + 0.25*J.m*R*R; 

    // assign the link2 parameters
    link2->body_id = "link2";                              // identify the link
    link2->set_inertia( J );                // set the inertia of the cylinder
    link2->set_enabled( true );                 // enable physics for the link
 
    // set the pose of the link
    // the height of the cylinder is 1.0, and the second joint will be located
    // at (0,-1,0) at the zero configuration. In the zero configuration for
    // this joint, the centroid of the cylinder will therefore be located at 
    // (0,-1.5,0)
    Quatd ori(0,0,0,1);
    Origin3d position(0,-1.0-H/2.0,0);
    Pose3d pose( ori, position );
    link2->set_pose( pose ); 

    // add the link to the set of links
    links.push_back( link2 ); 
  }

  // create the first revolute joint
  boost::shared_ptr<RevoluteJointd> j1( new RevoluteJointd() );
  {
    // compute the position of the joint... center of the base w.r.t global frame
    Vector3d position(0.0, 0.0, 0.0, GLOBAL_3D);
    j1->set_location( position, base, link1 );

    // Note: set_location(...) must precede set_axis(...)
    // compute the axis of rotation
    Vector3d axis(0,0,1.0,GLOBAL_3D);   // actuates around global x-axis
    j1->set_axis( axis ); 

    // set the joint name 
    j1->joint_id = "j1";

    // add the j1 to the set of joints
    joints.push_back( j1 );
  }

  // create the second revolute joint
  boost::shared_ptr<RevoluteJointd> j2( new RevoluteJointd() );
  {
    // compute the position of the joint... center of the base w.r.t global frame
    Vector3d position(0.0, -1.0, 0.0, GLOBAL_3D);
    j2->set_location( position, link1, link2 );

    // compute the axis of rotation
    Vector3d axis(0,0,1.0,GLOBAL_3D);   // actuates around global x-axis
    j2->set_axis( axis ); 

    // set the joint name 
    j2->joint_id = "j1";

    // add j2 to the set of joints
    joints.push_back( j2 );
  }

  // construct the pendulum articulated body from the set of links and joints created above
  pendulum->set_links_and_joints( links, joints );

  // pendulum has a fixed base
  pendulum->set_floating_base(false);

  // set the initial configuration of the pendulum to give it potential energy
  VectorNd gc(2);
  gc[0] = M_PI_2;
  gc[1] = 0.0;
  pendulum->set_generalized_coordinates_euler(gc);
 
  // start the main simulation loop
  while(true) {
    // integrate the body forward
    integrate_euler(pendulum, DT); 

    // get and output the pendulum generalized coordinates
    pendulum->get_generalized_coordinates_euler(gc);
    std::cout << "t: " << t << " q: " << gc << std::endl;

    // update the time
    t += DT;
  }

  return 0;
}

Integrating the equations of motion for a pendulum read from URDF

#include <Ravelin/SForced.h>
#include <Ravelin/URDFReaderd.h>
#include <Ravelin/RCArticulatedBodyd.h>
#include <Ravelin/Log.h>

using std::vector;
using boost::shared_ptr;
using namespace Ravelin;

void integrate(shared_ptr<RCArticulatedBodyd> pendulum, double dt)
{
  // clear all forces on the pendulum
  pendulum->reset_accumulators();

  // add gravity
  const double G = 9.8;
  shared_ptr<RigidBodyd> link1 = pendulum->get_links()[1];
  shared_ptr<RigidBodyd> link2 = pendulum->get_links()[2];
  const double mg1 = G*link1->get_inertia().m;
  const double mg2 = G*link2->get_inertia().m;
  SForced f1(0.0, -mg1, 0.0, 0.0, 0.0, 0.0, link1->get_mixed_pose());
  SForced f2(0.0, -mg2, 0.0, 0.0, 0.0, 0.0, link2->get_mixed_pose());
  link1->add_force(f1); 
  link2->add_force(f2); 

  // compute forward dynamics
  pendulum->calc_fwd_dyn();

  // integrate the generalized velocity forward
  VectorNd gc, gv, gve, ga;
  pendulum->get_generalized_acceleration(ga);
  pendulum->get_generalized_velocity(DynamicBodyd::eSpatial, gv);
  pendulum->get_generalized_velocity(DynamicBodyd::eSpatial, gve);
  ga *= dt;
  gv += ga;

  // integrate the generalized position forward
  pendulum->get_generalized_coordinates_euler(gc);
  static double t = 0.0;
  std::cout << t << " " << gc << std::endl;
  gve *= dt;
  gc += gve;
  pendulum->set_generalized_coordinates_euler(gc);
  pendulum->set_generalized_velocity(DynamicBodyd::eSpatial, gv);

  // update t
  t += dt;
} 

int main()
{
  // read in the pendulum file
  std::string fname = "pendulum.urdf";
  std::string name = "pendulum";
  vector<shared_ptr<RigidBodyd> > links;
  vector<shared_ptr<Jointd> > joints;
  URDFReaderd::read(fname, name, links, joints);

  // setup dynamics
  Log<OutputToFile>::reporting_level = LOG_DYNAMICS;

  // create a new articulated body
  shared_ptr<RCArticulatedBodyd> rcab(new RCArticulatedBodyd);
  rcab->set_floating_base(false);
  rcab->set_links_and_joints(links, joints); 
  rcab->set_computation_frame_type(eLinkCOM);
  rcab->algorithm_type = RCArticulatedBodyd::eCRB;

  for (unsigned i=0; i< 1000; i++)
    integrate(rcab, 1.0/1000);

}

Integrating the dynamics of a block falling due to gravity

#include <iostream>
#include <boost/shared_ptr.hpp>
#include <Ravelin/RigidBodyd.h>
#include <Ravelin/ArticulatedBodyd.h>
#include "integrate.h"  // defined in the examples directory

using boost::shared_ptr;
using boost::dynamic_pointer_cast;
using namespace Ravelin;

int main( void ) {

  const shared_ptr<const Pose3d> GLOBAL_3D;
  const unsigned X = 0, Y = 1, Z = 2;

  // integration step size
  const double DT = 1e-3;

  // current time
  double t = 0.0;

  // create the rigid body
  shared_ptr<RigidBodyd> rb( new RigidBodyd() );
  rb->body_id = "block";
  rb->set_enabled( true );
  rb->set_pose( Pose3d( Quatd(0,0,0,1), Origin3d(0,0,0) ) );

  // setup the inertia for the block; inertia is defined at the geometric
  // centroid of the block
  const double XSZ = 1.0, YSZ = 1.0, ZSZ = 1.0;  // box dimensions
  SpatialRBInertiad J;
  J.m = 100.0;
  J.J.set_zero();

  // inertia matrix definition comes from standard texts
  J.J(X,X) = (YSZ*YSZ + ZSZ*ZSZ); 
  J.J(Y,Y) = (XSZ*XSZ + ZSZ*ZSZ); 
  J.J(Z,Z) = (XSZ*XSZ + YSZ*YSZ); 
  J.J *= (2.0*J.m/12.0);

  // set the pose that the inertia is defined w.r.t. 
  J.pose = rb->get_pose();

  // set the inertia
  rb->set_inertia( J );

  while(true) {
    // integrate the equations of motion of the box forward in time
    integrate_euler( rb, DT );

    // get the pose of the box
    boost::shared_ptr<const Pose3d> pose = rb->get_pose();

    // copy the pose of the box, b/c we want to alter its relative pose
    Pose3d P = *pose;

    // update the relative pose to be in the global frame
    // (this is for pedagogical purposes- the relative pose of the box will 
    //  already be defined w.r.t. the global frame)
    P.update_relative_pose(GLOBAL_3D);
    std::cout << "t: " << t << " x: " << P.x << std::endl;

    // update current time
    t += DT;
  }

  return 0;
}