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[2])
    c = cos(q[2])

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

    qdd = -H\(C*qd + G - B*u[1])
    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[1], u[2], u[3]]
    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[4], u[5], u[6]]  # 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)

honglouyemeng / robotdynamics.jl Goto Github PK