ct_doc/doc/rbd_tutorial.dox

/*!
@page rbd_tutorials ct rbd Tutorials

- \subpage rbd_tut_modelling
- \subpage rbd_tut_forward_dynamics
- \subpage rbd_tut_types

This tutorial is specific for the RBD Module of the Control Toolbox. For installation instructions and an overview see the main CT documentation.

\page rbd_tut_modelling Model a Rigid Body Dynamics system
@tableofcontents

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

\section rbd_tut_modelling_robcogen Creating an new model

The Rigid Body Dynamics module of the Control Toolbox relies on <a href="https://robcogenteam.bitbucket.io">RobCoGen</a>, 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.

\subsection rbd_tut_robcogen Step 1: Create your model using RobCoGen
As a first step, download RobCoGen version 0.4ad.0 from the <a href="https://robcogenteam.bitbucket.io/binary.html">official website</a>. Follow the instruction on the website for creating your kindsl and
dtdsl files. Generate all C++ outputs.

\subsection rbd_tut_robcogen_import Step 2: Create a model for CT
As a next step, we suggest to first create a new package

\code{.sh}
cd catkin_ws/src
catkin_create_pkg my_robot ct_rbd
\endcode

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

\code{.sh}
cd my_robot
mkdir -p include/my_robot/generated
\endcode

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:

\code{.cmake}
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)
\endcode

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


\code{.cpp}

#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>

\endcode

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:

\code{.cpp}
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
\endcode

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 \ref rbd_tut_forward_dynamics as well as \ref rbd_tut_types.





\page rbd_tut_forward_dynamics Simulate your robot and create a controller
@tableofcontents

Do you still remember the \ref tut_basics? If not, refresh your mind really quickly now. Great! As you remember, to forward simulate a system or apply any control to it, we first have to make it a ct::core::System or if controllable a ct::core::ControlledSystem.

\section rbd_tut_create_system Creating a ControlledSystem for our robot
Effectively, we want to turn our robot into a Ordinary Differential Equation (ODE) of the form \f[ \dot(x) = f(x(t),u(t)) \f] . However, this raises the question what our state x and control u are. For a Rigid Body Dynamics system there are multiple choices. If we assume a kinematic model, the state consists of joint positions and control inputs are the joint velocities. If we use a dynamic model, the state will most likely be joint positions and velocities while the control input are joint forces/torques. This is the case that we will look at in this example. However, keep in mind there are multiple choices for formulating the ODE.

The Control Toolbox provides two handy wrappers, that transform the forward dynamics of a Rigid Body system into a ct::core::ControlledSystem. For fixed-base system, this is the class ct::rbd::FixBaseFDSystem and for floating-base system this is ct::rbd::FloatingBaseFDSystem. Take a look at those files to see the implementation details.

You can simply instantiate your dynamics using those classes

\code{.cpp}
  // obtain the state dimension
  const size_t STATE_DIM = FixBaseFDSystem<MyRobot::Dynamics>::STATE_DIM;

  // create an instance of the system
  std::shared_ptr<core::System<STATE_DIM>> dynamics(new FixBaseFDSystem<MyRobot::Dynamics>);
\endcode

Please look at the documentation to find other ct::rbd::Dynamics methods that you can potentially use.



\section rbd_tut_simulate_system Simulate the forward dynamics

In order to forward simulate the dynamics, we can simply call an integrator

\code{.cpp}
    core::Integrator<STATE_DIM> integrator(dynamics, ct::core::RK4);

    core::StateVector<12> state;
    state.setZero();

    integrator.integrate_n_steps(state, 0, 1000, 0.001);
\endcode

While we are using a regular integrator in the example above, all ct/rbd models can also be used with our ct::core::SymplecticIntegrator which allows for bigger step sizes.


\section rbd_tut_control Control and Linearization

Now that our robot is a ct::core::ControlledSystem we can throw all sorts of great control algorithms at it! If we want to compute an LQR controller for the forward dynamics, you can simply follow our tutorial on \ref optcon_tut_lqr. Or maybe even want throw an MPC controller at it (see \ref optcon_tut_mpc)? Do not forget to check out ct::rbd::FixBaseNLOC and ct::rbd::FloatingBaseNLOCContactModel to quickly generate a Nonlinear Optimal Control setup for your robot!

\warn In most cases we need the linearization for our system. Here we have the same choices as indicated in \ref core_tut_linearization. The same characteristics apply and for best accuracy and speed we recommend using Auto-Differentiation with code generation. However, for quick prototyping numerical differentiation can be appealing. Due to the special structure of Rigid Body Dynamics systems, we can compute a slightly better linearization which mixes analytic and numerical derivatives to improve accuracy and speed over pure numerical derivatives. This class is called ct::rbd::RbdLinearizer.



\page rbd_tut_types Useful RBD types
There are several useful types in the ct RBD package. This is just a loose collection of a few useful classes and types. Please refer to the individual documentation for more information.

* ct::rbd::Dynamics and ct::rbd::Kinematics - forward/inverse dynamics, transforms and Jacobians
* ct::rbd::RBDState - the state of a Rigid Body Dynamics system in minimal coordinates
* ct::rbd::RigidBodyState - the state of a single 6-DoF Rigid Body
* ct::rbd::IDControllerFB - a floating base inverse dynamics controller

*/