## RobotDynamics.jl

Author RoboticExplorationLab
Popularity
8 Stars
Updated Last
1 Year Ago
Started In
March 2020

# RobotDynamics.jl

This purpose of this package is to provide a common interface for calling systems with forced dynamics, i.e. dynamics of the form `xdot = f(x,u,t)` where `x` is an n-dimensional state vector and `u` is an m-dimensional control vector.

A model is defined by creating a custom type that inherits from `AbstractModel` and then defining a custom `dynamics` function on your model, along with `state_dim` and `control_dim` that gives n and m. With this information, `RobotDynamics.jl` provides methods for computing discrete dynamics as well as continuous and discrete time dynamics Jacobians.

This package also supports dynamics of rigid bodies and models whose state depends on one or more arbitrary 3D rotations. This package provides methods that correctly compute the dynamics Jacobians for the 3D attitude representions implemented in `Rotations.jl`.

Internally, this package makes use of the `Knotpoint` type that is a custom type that simply stores the state and control vectors along with the current time and time step length all together in a single type.

## Basic Example: Cartpole

```using RobotDynamics
using StaticArrays

# Define the model struct with parameters
struct Cartpole{T} <: AbstractModel
mc::T
mp::T
l::T
g::T
end

Cartpole() = Cartpole(1.0, 0.2, 0.5, 9.81)

# Define the continuous dynamics
function RobotDynamics.dynamics(model::Cartpole, x, u)
mc = model.mc  # mass of the cart in kg (10)
mp = model.mp   # mass of the pole (point mass at the end) in kg
l = model.l   # length of the pole in m
g = model.g  # gravity m/s^2

q = x[ @SVector [1,2] ]
qd = x[ @SVector [3,4] ]

s = sin(q)
c = cos(q)

H = @SMatrix [mc+mp mp*l*c; mp*l*c mp*l^2]
C = @SMatrix [0 -mp*qd*l*s; 0 0]
G = @SVector [0, mp*g*l*s]
B = @SVector [1, 0]

qdd = -H\(C*qd + G - B*u)
return [qd; qdd]
end

# Specify the state and control dimensions
RobotDynamics.state_dim(::Cartpole) = 4
RobotDynamics.control_dim(::Cartpole) = 1

# Create the model
model = Cartpole()
n,m = size(model)

# Generate random state and control vector
x,u = rand(model)
dt = 0.1  # time step (s)
z = KnotPoint(x,u,dt)

# Evaluate the continuous dynamics and Jacobian
ẋ = dynamics(model, x, u)
∇f = RobotDynamics.DynamicsJacobian(model)
jacobian!(∇f, model, z)

# Evaluate the discrete dynamics and Jacobian
x′ = discrete_dynamics(RK3, model, z)
discrete_jacobian!(RK3, ∇f, model, z)```

## Rigid Body Example: Satellite

```using RobotDynamics
using Rotations
using StaticArrays, LinearAlgebra
using BenchmarkTools

# Define the model struct to inherit from `RigidBody{R}`
struct Satellite{R,T} <: RigidBody{R}
mass::T
J::Diagonal{T,SVector{3,T}}
end
RobotDynamics.control_dim(::Satellite) = 6

# Define some simple "getter" methods that are required to evaluate the dynamics
RobotDynamics.mass(model::Satellite) = model.mass
RobotDynamics.inertia(model::Satellite) = model.J

# Define the 3D forces at the center of mass, in the world frame
function RobotDynamics.forces(model::Satellite, x::StaticVector, u::StaticVector)
q = orientation(model, x)
F = @SVector [u, u, u]
q*F  # world frame
end

# Define the 3D moments at the center of mass, in the body frame
function RobotDynamics.moments(model::Satellite, x::StaticVector, u::StaticVector)
return @SVector [u, u, u]  # body frame
end

# Build model
T = Float64
R = UnitQuaternion{T}
mass = 1.0
J = Diagonal(@SVector ones(3))
model = Satellite{R,T}(mass, J)

# Initialization
x,u = rand(model)   # generate a state with the rotation uniformly sampled from the space of rotations
dt = 0.1            # time step (s)
z = KnotPoint(x,u,dt)
∇f = RobotDynamics.DynamicsJacobian(model)

# Continuous dynamics
dynamics(model, x, u)
jacobian!(∇f, model, z)

# Discrete dynamics
discrete_dynamics(RK2, model, z)
discrete_jacobian!(RK2, ∇f, model, z)```