Skip to content

Using the MBD library

Jump to bottom Edit New page
Mikael Persson edited this page Jan 7, 2013 · 1 revision

#Using the MBD library

The ReaK multi-body dynamics library is a collection of classes that can be used to construct models for dynamics / kinematics systems. All classes are derived from the base-class kte::kte_map, which represents a kinetostatics transmission element (KTE). A KTE is a mapping of motion (kinematics) and of forces (statics) through input and output frames, where motion is propagated from inputs to outputs and forces are propagated in the reverse direction. A chain of KTEs (kte::kte_map_chain) can model any serial or tree-structured kinematic chain and be used to perform the forward kinematics and inverse dynamics calculations needed to compute the accelerations on its degrees of freedom. Closed kinematic chains can be modeled with stiff members (e.g., kte::flexible_beam_3D) making the link between serial chains.

The base-class for all KTEs is a simple abstract class with the following (simplified) interface:

class kte_map : public virtual named_object, public boost::noncopyable {
  public:
    
    /**
     * This method performs a motion calculation pass (i.e. computes the kinematics).
     * \pre All kinematics inputs have been set to the proper values.
     * \post All kinematics outputs have been set and the internal kinematics-related states are updated.
     */
    virtual void doMotion() = 0;
    
    /**
     * This method performs a force calculation pass (i.e. computes the dynamics).
     * \pre All dynamics inputs have been set to the proper values.
     * \post All dynamics outputs have been added to the input frames and the internal dynamics-related states are updated.
     */
    virtual void doForce() = 0;
    
    /**
     * This method performs a force clearance pass (i.e. resets all force values to zero).
     * \pre Any state, but normally before making a new doMotion + doForce pair of calculation passes.
     * \post All force outputs have been set to zero.
     */
    virtual void clearForce() = 0;
    
};

And thus, a typical forward kinematics and inverse dynamics computation would involve calling the doMotion() function first to do the forward kinematics pass, then calling the clearForce() function to clear the force values on the frames in preparation for the inverse dynamics, and finally calling the doForce() function to sum (accumulate) the forces on the kinetostatic frames. A chain of KTEs simply implements the doMotion() function by calling the same function on each of its KTEs in forward order, and similarly for the other function (but in reverse order), and thus, a KTE chain is itself a KTE. Furthermore, individual KTEs can be anything from a simple element (e.g., a spring force kte::spring_3D) to a complete model (e.g., classes derived from kte::inverse_dynamics_model), as long as they have the observable effects described in the kte::kte_map interface.

Now, one might point out that neither functions in the kte::kte_map interface take inputs or give an output. This is because data is passed between KTEs through kinetostatic frames that are shared between successive KTEs in a chain. Kinetostatic frames come in three types:

  • gen_coord<double>: Represents a generalized coordinate frame consisting of a single-valued position, velocity, acceleration and force.
  • frame_2D<double>: Represents a 2-dimensional coordinate frame consisting of a 2D vectored position, velocity, acceleration and force, and an orientation (2D rotation), angular velocity, angular acceleration and torque.
  • frame_3D<double>: Represents a 3-dimensional coordinate frame consisting of a 3D vectored position, velocity, acceleration and force, and an orientation (unit-quaternion), angular velocity vector, angular acceleration vector and torque vector.

These kinetostatic frames are managed with a ReaK::shared_ptr<> smart-pointer such that they can be shared between KTEs. Any given KTE type will typically have, besides their parameters, a number of input and output frames ("input" and "output" w.r.t. to kinematics), and it is really the sharing of those frames that create the links between the elements of a chain.

Now that the basic logic has been establish, we can proceed with an example for constructing a KTE chain that models a dynamic system, and simulating its motion.

##Libraries for MDB-KTE

Before we go on to the tutorial examples, we must settle some technical issues. In order to use the MBD-KTE library, one has to link to some static libraries:

  • libreak_core.a: The core elements of the ReaK library (including math functions, integrators, etc.).
  • libreak_mbd_kte.a: The collection of KTE classes that make up the ReaK MBD-KTE library.
  • libreak_kte_models.a: A collection of higher-level classes for standard purposes, notably, serial manipulator models.

These libraries should be generated from building the ReaK library and should appear in the lib directory at the same level as the src directory from the code repository. In the ReaK library's cmake scripts, each of the above libraries correspond to a target for the make command, but with the inter-dependencies, if one builds the reak_kte_models target (with make reak_kte_models), then all of the above libraries will be built.

In order to find the header files, one needs to add the directories path_to_repo/src/ReaK/core and path_to_repo/src/ReaK/ctrl, or if installed (make install), add the directories path_to_repo/include/ReaK/core and path_to_repo/include/ReaK/ctrl, where path_to_repo is the path to cloned github repository.

##A Simple Pendulum Model

In this part of the tutorial, we will construct a simple 2D model of a pendulum falling under gravity. In other words, the model will consist of a thin rod on a free revolute joint and a lumped mass at the end of that rod. Starting from a horizontal position and simulating the system under a gravitational acceleration will cause the pendulum to swing downwards.

We start the program with some includes that will be needed for this pendulum model:

#include "mbd_kte/kte_map_chain.hpp"           // for building the KTE chain.
#include "mbd_kte/revolute_joint.hpp"          // for attaching the rod to the base-frame.
#include "mbd_kte/rigid_link.hpp"              // for modeling the rod as a rigid link.
#include "mbd_kte/inertia.hpp"                 // for mass at the end of the pendulum.
#include "mbd_kte/mass_matrix_calculator.hpp"  // to calculate the generalized mass-matrix.

#include "recorders/ssv_recorder.hpp"          // to record the results in a space-separated-value file.
#include "serialization/xml_archiver.hpp"      // to save / load the KTE model into an XML file.

using namespace ReaK; // for convenience, all classes in ReaK are in the 'ReaK' namespace.

The process of creating model (programmatically) is about constructing coordinate frames and KTEs that are linked in a logical order. A kinematic chain always starts from a fixed coordinate frame (or at least, a frame whose motion is determined upstream of the chain). In this case, we must apply an acceleration on the base frame to account for gravity (i.e., gravitational acceleration), and thus, we create the frame as follows:

shared_ptr< frame_2D<double> > base_frame( new frame_2D<double>() );
base_frame->Acceleration = vect<double,2>(0, 9.81); //add gravity (gravity force downwards == acceleration upwards)

Now, we have to create a revolute joint to attach the rod to the base frame. A revolute joint is a KTE with two input frames: the frame at the base of the joint, and the generalized coordinate of the joint itself. Then, it has one output frame: the frame at the end of the joint (after the joint rotation is applied). The base of the joint is the base frame created above (base_frame). So, we just need to create two more frames:

shared_ptr< frame_2D<double> > joint_frame( new frame_2D<double>() );
shared_ptr< gen_coord<double> > joint_coord( new gen_coord<double>() );

where joint_frame is the frame after the joint rotation is applied, and joint_coord is the coordinate of the revolute joint (this is a "free" degree-of-freedom).

Another important component of a revolute joint (and any other kind of joint) is that it leads to a jacobian element (or twist-shaping element). Jacobian elements are defined by the ReaK library (in the header kinetostatics/motion_jacobians.hpp) with the naming scheme jacobian_X_Y where X and Y can be either gen, 2D or 3D, for a generalized coordinate, a 2D frame or a 3D frame, respectively. X represents the input coordinate (e.g., joint coordinate), while Y represents the output coordinate (e.g., the output frame of the joint). In this case, we need a jacobian_gen_2D for our revolute joint:

shared_ptr< jacobian_gen_2D<double> > joint_jacobian( new jacobian_gen_2D<double>() );

which is going to be used by the revolute joint to record the current state of the Jacobian, mapping the velocity and acceleration of the joint into a velocity and acceleration effect on its output frame. This Jacobian information is useful for various higher-level classes that need to build complete or partial Jacobian matrices or twist-shaping matrices, most notably, the mass-matrix calculator that will be used later in this tutorial example.

We are now ready to create the revolute joint, which is simply a matter of assigning a name to it and providing all the frames and the jacobian that we just created:

//create revolute joint
shared_ptr< kte::revolute_joint_2D > rev_joint( new kte::revolute_joint_2D(
  "joint1",          // a name for the joint.
  joint_coord,       // the joint coordinate.
  base_frame,        // the base frame of the joint.
  joint_frame,       // the end frame of the joint.
  joint_jacobian));  // an object where the jacobian will be recorded.

where the revolute joint will automatically have the "z-axis" as an axis of rotation, because there is no other choice in a 2D world. A positive rotation is in the counter-clockwise direction.

At this stage, we move on to create to thin rod, i.e., a rigid link. A rigid link is defined by an input frame (one end), an output frame (the other end), and an offset to be applied. The offset is a pose offset, that is, a position and orientation offset to be applied to the input frame in order to get to the output frame. In this case, we will create a rod of half a meter in length along the x-axis of its base frame (the output frame of the revolute joint, joint_frame). And we won't apply any orientation offset. So, we get the following code:

// create a frame for the other end of the rod:
shared_ptr< frame_2D<double> > end_frame( new frame_2D<double>() );
// create link of length 0.5 meters:
shared_ptr< kte::rigid_link_2D > link1( new kte::rigid_link_2D(
  "link1",                                       // a name for the rigid-link.
  joint_frame,                                   // the base frame of the rod.
  end_frame,                                     // the end frame of the rod.
  pose_2D<double>(weak_ptr<pose_2D<double> >(),
    vect<double,2>(0.5,0.0),                     // the position offset: 0.5 meters along the x-axis of 'joint_frame'.
    rot_mat_2D<double>(0.0))));                  // the orientation offset: no rotation.

To complete the model of the pendulum, we need to add a mass at the end of the rod. So far, all elements where purely kinematic and did not generate forces (but transfer them). Now, we will add a purely dynamic element, an inertial element. An inertial element is attached to a coordinate frame, generates a force that is proportional to the acceleration felt on that coordinate frame, and then applies that force back onto that frame. In other words, it is a terminal KTE because it does not have an output frame.

Now, there is one additional complication here. The acceleration of the frame to which we attach the inertial element could depend on the acceleration of the joints on which that frame's motion depends. That dependency is already being recorded by the relevant Jacobians (associated with each joint), but they need to be listed and associated to the frame on which we will attach an inertial element. The list of joint jacobians is recorded in a map type class kte::jacobian_joint_map_2D for mapping generalized coordinates to 2D frames (but there are other types of jacobian maps for different combinations). And so, we can create the list of up-stream joints for our inertial element as follows:

kte::jacobian_joint_map_2D joint_map1;
joint_map1[joint_coord] = joint_jacobian;  // associate the joint jacobian with its joint coordinate.

Now, we can create an inertial element for a lumped mass of 1 kg (and zero moment of inertia) attached to the end of the rod (end_frame) as follows:

//create end mass of 1.0 kg (point mass only)
shared_ptr< kte::inertia_2D > mass1( new kte::inertia_2D(
  "mass1",              // a name for the inertial element.
  shared_ptr< kte::joint_dependent_frame_2D >( new kte::joint_dependent_frame_2D(
    end_frame,          // the frame on which the inertia element is attached.
    joint_map1)),       // the list of up-stream joint jacobians on which the frame depends.
  1.0,                  // the mass of the inertial element.
  0.0));                // the moment of inertial of the inertial element.

Once all the individual elements have been created, we can now create the KTE chain that assembles all these KTEs into a serial chain. The class used to represent a KTE chain is called kte::kte_map_chain and can be used with the left shift operator to add elements to the chain in an orderly fashion. Remember, the KTEs are processed in a forward kinematics / inverse dynamics fashion, it is thus important that they be chained up in the correct order, determined by their input and output frames. In this case, the order is simple: the revolute joint first, then the rigid link and finally the inertial element. We thus get:

shared_ptr< kte::kte_map_chain > pendulum( new kte::kte_map_chain("pendulum") );
(*pendulum) << rev_joint 
            << link1 
            << mass1;

At this point, we could use the doMotion(), clearForce() and doForce() functions on the object pendulum to perform the forward kinematics and inverse dynamics calculations on the pendulum model. However, in order to be able to determine the acceleration on the free joint coordinate, we need to have a generalized mass matrix for the system (in this case, the matrix dimension is 1x1 because the system has a single degree of freedom). To construct this mass matrix, we use a class called kte::mass_matrix_calc in which we can register a number of inertial elements and joint coordinates from the model (only those we which to include in the mass matrix calculation). As the joint coordinates are added to the mass matrix calculator, their order is kept and the computed mass matrix will be ordered by generalized coordinates, 2D and 3D frames, with each group ordered as they have been added to the kte::mass_matrix_calc object (the same ordering scheme goes for the rows of the twist-shaping matrix). For our pendulum model, we construct the mass matrix calculator as follows:

shared_ptr< kte::mass_matrix_calc > mass_mat1( new kte::mass_matrix_calc("mass_mat1") );
(*mass_mat1) << mass1;
(*mass_mat1) << joint_coord;

We have now completed all the elements needed to run a simulation with our pendulum model. For convenience, we can also serialize all the data into an XML file, such that the above code does not need to be created but instead loaded from a file. To save the model data to a file, we need to save only the elements that we need access to (and everything else will be save internally), which are, in this case, the joint coordinate, the end frame, the pendulum KTE chain, and the mass matrix calculator. So, we save the model as follows:

{
  serialization::xml_oarchive pendulum_arc("pendulum.xml");
  pendulum_arc << joint_coord 
               << pendulum 
               << end_frame 
               << mass_mat1;
};

And we can then load the entire model as so:

shared_ptr< gen_coord<double> > joint_coord;
shared_ptr< frame_2D<double> > end_frame;
shared_ptr< kte::mass_matrix_calc > mass_mat1;
shared_ptr< kte::kte_map_chain > pendulum;

{
  serialization::xml_iarchive pendulum_arc("pendulum.xml");
  pendulum_arc >> joint_coord 
               >> pendulum 
               >> end_frame 
               >> mass_mat1;
};

which gives us the four objects back from the file "pendulum.xml".

###Simple Simulation of the Pendulum

Now that we have a model for our pendulum, we can proceed to simulate the system. In this tutorial example, we will not use one of ReaK's numerical integrators but, instead, a simple Euler integration rule. In this section, it would be awkward to split the code into a step by step tutorial, so, instead the entire code is laid out in one shot with in-code comments.

mat<double,mat_structure::symmetric> M(1);  //< create a mass matrix (symmetric) of dimension 1.

// create a space-separated-value recorder to output the results:
recorder::ssv_recorder output_rec("pendulum_results.ssvdat");
output_rec << "time" << "q" << "qd" << "qdd" << "f" << recorder::data_recorder::end_name_row;

for (double sim_time = 0.0; sim_time < 50.0; sim_time += 0.001) {
  
  joint_coord->q_ddot = 0.0;    //< set the joint accelerations to zero.
  pendulum->doMotion();          //< propagate the motion (kinematics) throughout the KTE chain.
  pendulum->clearForce();        //< clear all the forces throughout the KTE chain.
  pendulum->doForce();           //< propagate the forces (statics) backwards in the KTE chain.
  
  mass_mat1->getMassMatrix(M);  //< compute the mass matrix.
  
  joint_coord->q_ddot = joint_coord->f / M(0,0);  //< apply "a = f / m" (Newton's second law).
  
  // record the results for this time point:
  output_rec << sim_time
             << joint_coord->q
             << joint_coord->q_dot
             << joint_coord->q_ddot
             << joint_coord->f
             << recorder::data_recorder::end_value_row;
  
  // apply a simple forward-Euler integration rule:
  joint_coord->q += joint_coord->q_dot * 0.001;
  joint_coord->q_dot += joint_coord->q_ddot * 0.001;
};

output_rec << recorder::data_recorder::close;

The ReaK library does include a number of numerical integrators that one should use instead of a home-made integration scheme as it was done above. To use these integrators, one must define a class that is derived from the base class state_rate_function or state_rate_function_with_io for a state-space system or a input-output system, respectively. Internally, one would simply use the lines of code used above to compute the joint acceleration and output that as the state vector's rate of change (x-dot). For convenience, one can use the class kte::manipulator_dynamics_model to provide the KTE chain, as well as the list of free joints (degrees of freedom), and some other things, which is a class derived from the state_rate_function_with_io base class and can thus be used to perform numerical integration with the integrators provided by the ReaK library (including equivalents to most integrators found in Matlab). For the pendulum system, we can create the following dynamics model:

// from #include <kte_models/manip_dynamics_model.hpp>

shared_ptr< kte::manipulator_dynamics_model > pendulum_mdl( new kte::manipulator_dynamics_model("pendulum_mdl") );
pendulum_mdl->setModel( pendulum ); // set the KTE chain of the dynamics model.
(*pendulum_mdl) << joint_coord;     // register the joint coordinate (will be added to the states).
(*pendulum_mdl) << mass1;           // register the inertia (will be added to the mass matrix calculator).

This dynamics model can be used with any integrator in the ReaK library (in folder core/integrators). For example, one could use the Dormand-Prince 45 integrator (the same method as in the "ode45()" function in Matlab/Octave):

// from #include <integrators/variable_step_integrators.hpp>

dormand_prince45_integrator<double> ode45_integ(
  "pendulum_ode45_integrator",
  pendulum_mdl->getJointStates(),   // initial state vector
  0.0,                   // start time
  0.00001,               // initial step-size
  pendulum_mdl,          // state-rate function
  0.001,                 // maximum step-size
  1e-6,                  // minimum step-size
  1e-4);                 // tolerance

// create a space-separated-value recorder to output the results:
recorder::ssv_recorder output_rec("pendulum_results_ode45.ssvdat");
output_rec << "time" << "q" << "qd" << recorder::data_recorder::end_name_row;

for (double sim_time = 0.0; sim_time < 50.0; sim_time += 0.001) {
  
  ode45_integ.integrate(sim_time);  //< integrate function up to 'sim_time'.
  
  // record the results for this time point:
  output_rec << sim_time
             << *(ode45_integ.getStateBegin())
             << *(ode45_integ.getStateBegin() + 1)
             << recorder::data_recorder::end_value_row;
};

output_rec << recorder::data_recorder::close;
Add a custom footer
Add a custom sidebar
Clone this wiki locally