Simulating with DifferentialEquations.jl
Getting started
To simulate the dynamics of the system, DifferentialEquations.jl can be used. The extension VMRobotControlDifferentialEquationsExt.jl is loaded when both VMRobotControl.jl and DifferentialEquation.jl are loaded.
using DifferentialEquations
using VMRobotControl
using StaticArraysDifferentialEquations.jl expects a function with the signature f(du, u, p, t), where u is the state, t is the current time, p is an optional vector of parameters (which we can ignore), and the answer is returned by modifying du in-place, to hold the derivative of u. The state is composed of the configurations and velocities of each joint, and the du is composed of the velocities and accelerations.
The function get_ode_dynamics(dynamics_cache, gravity; [f_setup, f_control]) returns a function with the correct call signature. The dynamics cache will be mutated while simulating, so take care to avoid memory aliasing if your are multithreading. The optional functions f_setup and f_control are called before starting and at every time step respectively. Their type signatures can be seen by looking at the help for DEFAULT_F_SETUP and DEFAULT_F_CONTROL.
get_ode_dynamics works the same way for mechanisms and virtual mechanism systems.
mechanism = parseRSON("../../../RSONs/rsons/franka_panda/pandaSurgical.rson")
m = compile(mechanism)
dcache = new_dynamics_cache(m)
gravity = SVector{3, Float64}(0.0, 0.0, -10.0)
f_dynamics = get_ode_dynamics(dcache, gravity;)(::VMRobotControl.var"#ode_dynamics!#226"{typeof(VMRobotControl.DEFAULT_F_CONTROL), VMRobotControl.MechanismCacheBundle{CompiledMechanism{Float64, VMRobotControl.RBTree{Float64, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledMechanismJoint(Float64, RevoluteData{Float64})}, Vector{CompiledMechanismJoint(Float64, Rigid{Float64})}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{CompiledCoord{JointSubspace{VMRobotControl.CompiledJointID{CompiledMechanismJoint(Float64, RevoluteData{Float64})}}}}, Vector{CompiledCoord{FrameAngularVelocity{CompiledFrameID}}}, Vector{CompiledCoord{FramePoint{Float64, CompiledFrameID}}}}}}, VMRobotControl.TypeStableCollections.TypeStableCollection{Tuple{Vector{Visual{GeometryBasics.Mesh{3, Float32, GeometryBasics.TriangleP{3, Float32, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}}, GeometryBasics.FaceView{GeometryBasics.TriangleP{3, Float32, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}}, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}, GeometryBasics.TriangleFace{GeometryBasics.ZeroIndex{Int32}}, StructArrays.StructVector{GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}, @NamedTuple{position::Vector{Point{3, Float32}}, normals::Vector{Vec{3, Float32}}}, Int64}, Vector{GeometryBasics.TriangleFace{GeometryBasics.ZeroIndex{Int32}}}}}, CompiledFrameID}}, Vector{Visual{GeometryBasics.Mesh{3, Float32, GeometryBasics.TriangleP{3, Float32, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}}, GeometryBasics.FaceView{GeometryBasics.TriangleP{3, Float32, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}}, GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}, GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}, StructArrays.StructVector{GeometryBasics.PointMeta{3, Float32, Point{3, Float32}, (:normals,), Tuple{Vec{3, Float32}}}, @NamedTuple{position::Vector{Point{3, Float32}}, normals::Vector{Vec{3, Float32}}}, Int64}, Vector{GeometryBasics.NgonFace{3, GeometryBasics.OffsetInteger{-1, UInt32}}}}}, CompiledFrameID}}, Vector{Inertia{Float64, CompiledCoordID{FrameAngularVelocity{CompiledFrameID}}}}, Vector{PointMass{Float64, CompiledCoordID{FramePoint{Float64, CompiledFrameID}}}}}}}, VMRobotControl.MechDynamicsCache{Float64}}, Nothing, Vector{Float64}, Vector{Float64}, Base.RefValue{Float64}, UnitRange{Int64}, UnitRange{Int64}}) (generic function with 1 method)Once we have the dynamics function, we can make an ODE problem, but we need to define the initial state vector. We can use two functions to convert between q, q̇ and a state vector u: assemble_state and state_idxs. The usage of these functions differs slightly depending on if you are working with a mechanism or virtual mechanism system, as you will have an extra configuration and velocity vector for the virtual mechanism if you are working with a virtual mechanism system.
q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
q̇ = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
tspan = 5.0
u0 = assemble_state(m, q, q̇)
prob = ODEProblem(f_dynamics, u0, tspan)
sol = solve(prob, Tsit5())
sol.retcodeReturnCode.Success = 1Alternatively you can the step of defining f_dynamics, and use the function get_ode_problem from to do everything in one go:
prob = get_ode_problem(dcache, gravity, q, q̇, tspan)
sol = solve(prob, Tsit5())
sol.retcodeReturnCode.Success = 1