Model a Rigid Body Dynamics system

Table of Contents

• Creating an new model
  • Step 1: Create your model using RobCoGen
  • Step 2: Create a model for CT

In this tutorial, we will create a simple Rigid Body Dynamics system.

Creating an new model

The Rigid Body Dynamics module of the Control Toolbox relies on RobCoGen, a code-generation framework for Rigid Body Dynamics and Kinematics. This involves a separate code generation step for creating the dynamics and kinematics equations prior to using your model in the Control Toolbox. However, as of writing this documentation, RobCoGen is the only Rigid Body Dynamics framework that supports Auto-Differentiation, a major ingredient for fast Optimal Control. We plan to support more modelling frameworks in the future.

RobCoGen requires a special robot description format (kindsl). If you already have a URDF description of your robot, you can use the urdf2robcogen converter.

Step 1: Create your model using RobCoGen

As a first step, download RobCoGen version 0.4ad.0 from the official website. Follow the instruction on the website for creating your kindsl and dtdsl files. Generate all C++ outputs.

Step 2: Create a model for CT

As a next step, we suggest to first create a new package

cd catkin_ws/src
catkin_create_pkg my_robot ct_rbd

As you can see, we made our package depend on ct_rbd. Next, we create the folder structure

cd my_robot
mkdir -p include/my_robot/generated

Copy all files generated by RobCoGen into this folder. We generally recommend adding your kindsl/dtdsl files to a subfolder called "model" as well for later reference.

Next, edit the CMakeLists.txt in your package and add the following lines:

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

Next, create a header file for your robot in include/my_robot/MyRobot.h with the following content

#pragma once

#include <Eigen/Core>
#include <Eigen/StdVector>

#include "generated/declarations.h"
#include "generated/jsim.h"
#include "generated/jacobians.h"
#include "generated/traits.h"
#include "generated/forward_dynamics.h"
#include "generated/inertia_properties.h"
#include "generated/inverse_dynamics.h"
#include "generated/transforms.h"
#include "generated/link_data_map.h"

// define namespace and base
#define ROBCOGEN_NS ct_my_robot
#define TARGET_NS MyRobot

// define the links
#define CT_BASE fr_MyRobotBase
#define CT_L0 fr_Shoulder_AA
#define CT_L1 fr_Shoulder_FE
#define CT_L2 fr_Humerus_R
#define CT_L3 fr_Elbow_FE
#define CT_L4 fr_Wrist_R
#define CT_L5 fr_Wrist_FE

// define single end effector (could also be multiple)
#define CT_N_EE 1
#define CT_EE0 fr_ee
#define CT_EE0_IS_ON_LINK 5
#define CT_EE0_FIRST_JOINT 0
#define CT_EE0_LAST_JOINT 5

#include <ct/rbd/robot/robcogen/robcogenHelpers.h>

You need to adjust a few variables. First, set the namespace (ROBCOGEN_NS) which the RobCoGen files have been created in. Also, you can define a target namespace (MyRobot). Next, you need to define your base link (CT_BASE, which is either floating or fixed, depending on your kindsl). Afterwards, define all links (CT_L0, CT_L1, CT_L2, ...) by using the frames you defined in RobCoGen. Last but not least, you can but do not have to specify endeffectors. Endeffectors can be useful for contacts or optimal control. If in doubt, skip these for now.

Once you need end-effectors, specify their number (CT_N_EE). Then, for each end-effector, specify its name in RobCoGen (CT_EEx) and which link it is rigidly attached to (CT_EEx_IS_ON_LINK). Then specify the first joint in the kinematic chain leading up to the end-effector (CT_EEx_FIRST_JOINT, often but not necessarily 0) as well as the last joint (CT_EEx_LAST_JOINT). The Control Toolbox assumes that all joint numbers in between those two also influence the end-effector.

Above steps automatically create the following useful types:

ct::rbd::MyRobot::Dynamics myRobotDynamics; // an implementation of ct::rbd::Dynamics for your robot
ct::rbd::MyRobot::tpl::Dynamics<SCALAR> myRobotDynamicsTpl; // an implementation of ct::rbd::Dynamics for your robot templated on the scalar type!

ct::rbd::MyRobot::Kinematics myRobotKinematics; // an implementation of ct::rbd::Kinematics for your robot

The Dynamics and Kinematics class have some useful functions including computing forward and inverse dynamics as well as transforms and Jacobians. Please refer to the documentation for more details.

That's it! You can verify your setup by looking into the ct models package, e.g. at the HyA robot. Feel free to check out our tutorials on how to Simulate your robot and create a controller as well as Useful RBD types.