Dynamic simulation

[phelipe]

Hi,

I was reading the documentation and did not find a function returning the dynamics of the system, the RigidBodyDynamics.dynamics! () function returns nothing. Is there any way to simulate the mechanism by choosing the torques at each joint?

[tkoolen]

To avoid allocations, the dynamics! methods take a DynamicsResult object that can be reused in subsequent calls to dynamics!. DynamicsResult contains the joint acceleration vector (v̇ field), among other things. Calling dynamics! results in this joint acceleration vector being set.

To simulate with e.g. a controller in the loop, you can pattern match simulate, but instead of calling dynamics!(result, state), call dynamics!(result, state, torques), where torques is the vector of joint torques coming from the controller.

Here's a full example including visualization:

# Pkg.checkout("RigidBodyTreeInspector", "v0.3.2") # to avoid deprecation warnings until 0.3.2 tag is merged
using RigidBodyDynamics, RigidBodyDynamics.OdeIntegrators
using RigidBodyTreeInspector

urdf = Pkg.dir("RigidBodyDynamics", "test", "urdf", "Acrobot.urdf")
mechanism = parse_urdf(Float64, urdf)
remove_fixed_tree_joints!(mechanism)

shoulder, elbow = joints(mechanism)
torques = similar(velocity(state))
function closed_loop_dynamics!(v̇::AbstractArray, ṡ::AbstractArray, t, state)
    torques[velocity_range(state, shoulder)] .= 10 * sin(t)
    torques[velocity_range(state, elbow)] .= -1 .* velocity(state, elbow)
    dynamics!(result, state, torques)
    copy!(v̇, result.v̇)
    copy!(ṡ, result.ṡ) # note: these are derivatives of additional states, currently used for the stateful contact model
    nothing
end

geometries = parse_urdf(urdf, mechanism)
vis = Visualizer(mechanism, geometries)

state = MechanismState(mechanism)
result = DynamicsResult(mechanism)
tableau = runge_kutta_4(Float64)
sink = DrakeVisualizerSink(vis)
integrator = MuntheKaasIntegrator(closed_loop_dynamics!, tableau, sink)
integrate(integrator, state, 10.0, 1e-3; max_realtime_rate = 1.)

I think I should streamline this by adding controller and sink as optional arguments to simulate. I should probably also add another example notebook and extra documentation.

Don't know if you noticed, but there's also a different dynamics! method for use with standard ODE integrators, if you prefer that over the custom MuntheKaasIntegrator.

[tkoolen]

See this new notebook for a new, easier way to do the same thing as the example in https://github.com/tkoolen/RigidBodyDynamics.jl/issues/341#issuecomment-331215128 (not yet released).

[phelipe]

Thank you! 😃