Simulate your robot and create a controller

Table of Contents

• Creating a ControlledSystem for our robot
• Simulate the forward dynamics
• Control and Linearization

Do you still remember the Basic Tutorials? 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.

Creating a ControlledSystem for our robot

Effectively, we want to turn our robot into a Ordinary Differential Equation (ODE) of the form

. 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

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

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

Simulate the forward dynamics

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

core::Integrator<STATE_DIM> integrator(dynamics, ct::core::RK4);

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

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

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.

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 LQR Control of a Damped Oscillator. Or maybe even want throw an MPC controller at it (see Model Predictive Control for a Damped Oscillator)? Do not forget to check out ct::rbd::FixBaseNLOC and ct::rbd::FloatingBaseNLOCContactModel to quickly generate a Nonlinear Optimal Control setup for your robot!

In most cases we need the linearization for our system. Here we have the same choices as indicated in Derivatives/Jacobian/Linearization Tutorial. 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.