Details
ODE problem creation
RigidBodySim.Core.Dynamics
— Type.Dynamics(mechanism)
Dynamics(mechanism, control!; setparams!)
Create a Dynamics
object, representing either the passive or closed-loop dynamics of a RigidBodyDynamics.Mechanism
.
The control!
argument is a callable with the signature control!(τ, t, state)
, where τ
is the torque vector to be set in the body of control!
, t
is the current time, and state
is a MechanismState
object. By default, control!
is zero_control!
(resulting in the passive dynamics).
The setparams!
keyword argument is a callable with the signature setparams!(state, p)
where state
is a MechanismState
and p
is a vector of parameters, as used in OrdinaryDiffEq.jl.
DiffEqBase.ODEProblem
— Method.ODEProblem(dynamics, x0, tspan)
ODEProblem(dynamics, x0, tspan, p; callback, kwargs...)
Create a DiffEqBase.ODEProblem
associated with the dynamics of a RigidBodyDynamics.Mechanism
.
The initial state x0
can be either a RigidBodyDynamics.MechanismState
), or an AbstractVector
containing the initial state represented as [q; v; s]
, where q
is the configuration vector, v
is the velocity vector, and s
is the vector of additional states.
The callback
keyword argument can be used to pass in additional DifferentialEquations.jl callbacks.
Control
RigidBodySim.Core.zero_control!
— Function.zero_control!(τ, t, state)
A 'zero' controller, i.e. one that sets all control torques to zero at all times.
RigidBodySim.Core.controlcallback
— Function.controlcallback(control!)
Can be used to create a callback associated with a given controller.
struct PeriodicController{Tau<:(AbstractArray{T,1} where T), T<:Number, C, I}
A PeriodicController
can be used to simulate a digital controller that runs at a fixed rate (in terms of simulation time). It does so by performing a zero-order hold on a provided control function.
PeriodicController
s can be constructed using
PeriodicController(τ, Δt, control!; initialize = DiffEqBase.INITIALIZE_DEFAULT, save_positions = (false, false))
where control!
is a controller satisfying the standard RigidBodySim controller signature (control!(τ, Δt, state)
), Δt
is the simulation time interval between calls to the control!
function, and τ
is used to call control!
. The initialize
and save_positions
keyword arguments are documented in the DiscreteCallback
section of the DifferentialEquations documentation.
PeriodicController
s are callable objects, and themselves fit the standard RigidBodySim controller signature.
A DiffEqCallbacks.PeriodicCallback
can be created from a PeriodicController
, and is used to stop ODE integration exactly every Δt
seconds, so that the control!
function can be called. Typically, users will not have to explicitly create this PeriodicCallback
, as it is automatically created and added to the ODEProblem
when the PeriodicController
is passed into the RigidBodySim-provided DiffEqBase.ODEProblem
constructor overload.
Examples
In the following example, a PeriodicController
is used to simulate a digital PD controller running at a fixed rate of 200 Hz.
julia> using RigidBodySim, RigidBodyDynamics, OrdinaryDiffEq
julia> mechanism = parse_urdf(Float64, joinpath(dirname(pathof(RigidBodySim)), "..", "test", "urdf", "Acrobot.urdf"));
julia> state = MechanismState(mechanism);
julia> set_configuration!(state, [0.1; 0.2]);
julia> controlcalls = Ref(0);
julia> pdcontrol!(τ, t, state) = (controlcalls[] += 1; τ .= -20 .* velocity(state) .- 100 .* configuration(state));
julia> τ = zero(velocity(state)); Δt = 1 / 200
0.005
julia> problem = ODEProblem(Dynamics(mechanism, PeriodicController(τ, Δt, pdcontrol!)), state, (0., 5.));
julia> sol = solve(problem, Tsit5());
julia> @assert all(x -> isapprox(x, 0, atol = 1e-4), sol.u[end]) # ensure state converges to zero
julia> controlcalls[]
1001
struct SumController{Tau<:(AbstractArray{T,1} where T), C<:Tuple}
A SumController
can be used to combine multiple controllers, summing the control torques that each of these controllers produces.
Examples
julia> using RigidBodySim, RigidBodyDynamics
julia> mechanism = parse_urdf(Float64, joinpath(dirname(pathof(RigidBodySim)), "..", "test", "urdf", "Acrobot.urdf"));
julia> state = MechanismState(mechanism);
julia> c1 = (τ, t, state) -> τ .= t;
julia> c2 = (τ, t, state) -> τ .= 2 * t;
julia> sumcontroller = SumController(similar(velocity(state)), (c1, c2))
julia> τ = similar(velocity(state))
julia> controller(τ, 1.0, state);
julia> @assert all(τ .== 3.0);
Visualization
RigidBodySim uses MeshCatMechanisms.jl for 3D visualization.
RigidBodySim.Visualization.GUI
— Type.GUI(visualizer; usernode)
Create a new RigidBodySim graphical user interface given a visualizer (e.g. a MeshCatMechanisms.MechanismVisualizer
).
The visualizer must support:
Base.copyto!(visualizer, state::Union{MechanismState, AbstractVector})
Base.wait(visualizer)
MeshCatMechanisms.visualizer(visualizer)
, which should return aMeshCat.Visualizer
.
Use open(gui)
to open the GUI in a standalone window.
GUI(mechanism, args; usernode)
Create a new RigidBodySim graphical user interface for the given Mechanism. All arguments are passed on to the MeshCatMechanisms.MechanismVisualizer
constructor.
Use open(gui)
to open the GUI in a standalone window.
SimulationControls()
Create a new SimulationControls
object, which may be used to pause and terminate the simulation.
The controls can be displayed in a standalone window using open(controls, Blink.Window())
.
DiffEqBase.CallbackSet
— Method.CallbackSet(vis; max_fps)
Create the DifferentialEquations.jl callbacks needed for publishing to a visualizer during simulation.
max_fps
is the maximum number of frames per second (in terms of wall time) to draw. Default: 60.0
.
DiffEqBase.CallbackSet
— Method.CallbackSet(gui; max_fps)
Create the DifferentialEquations.jl callbacks associated with the GUI
.
max_fps
is the maximum number of frames per second (in terms of wall time) to draw. Default: 60.0
.
MeshCat.setanimation!
— Method.setanimation!(vis, sol; max_fps, realtime_rate, pause_pollint)
Play back a visualization of a RigidBodySim.jl simulation.
Positional arguments:
vis
is aMeshCatMechanisms.MechanismVisualizer
sol
is aDiffEqBase.ODESolution
obtained from a RigidBodySim.jl simulation.
setanimation
accepts the following keyword arguments:
max_fps
: the maximum number of frames per second to draw. Default:60.0
.realtime_rate
: can be used to slow down or speed up playback compared to wall time. Arealtime_rate
of2
will result in playback that is sped up 2x. Default:1.0
.
Examples
Visualizing the result of a simulation of the passive dynamics of an Acrobot (double pendulum) at half speed:
using RigidBodySim, RigidBodyDynamics, MeshCatMechanisms, Blink
urdf = joinpath(dirname(pathof(RigidBodySim)), "..", "test", "urdf", "Acrobot.urdf")
mechanism = parse_urdf(Float64, urdf)
state = MechanismState(mechanism)
set_configuration!(state, [0.1; 0.2])
problem = ODEProblem(Dynamics(mechanism), state, (0., 2.))
sol = solve(problem, Vern7())
vis = MechanismVisualizer(mechanism, URDFVisuals(urdf))
# open(vis, Window()) # uncomment to open the visualizer window
setanimation!(vis, sol; realtime_rate = 0.5);
Utilities
RigidBodySim.Core.configuration_renormalizer
— Function.configuration_renormalizer(state)
configuration_renormalizer(state, condition)
configuration_renormalizer
can be used to create a callback that projects the configuration of a mechanism's state onto the configuration manifold. This may be necessary for mechanism's with e.g. quaternion-parameterized orientations as part of their joint configuration vectors, as numerical integration can cause the configuration to drift away from the unit norm constraints.
The callback is implemented as a DiffEqCallbacks.DiscreteCallback
By default, it is called at every integrator time step.
RigidBodySim.Core.RealtimeRateLimiter
— Method.RealtimeRateLimiter(; max_rate, poll_interval, save_positions, reset_interval)
RealtimeRateLimiter(; max_rate = 1., poll_interval = 1 / 30; save_positions = (false, false))
A DiscreteCallback
that limits the rate of integration so that integration time t
increases at a rate no higher than max_rate
compared to wall time.
A RealtimeRateLimiter
can be used, for example, if you want to simulate a physical system including its timing characteristics. Specific use cases may include realtime animation and user interaction during the simulation.
The poll_interval
keyword argument can be used to control how often the integration is stopped to check whether to sleep (and for how long). Specifically, this operation happens every poll_interval / max_rate
in terms of integration time, which corresponds to approximately every poll_interval
seconds wall time if max_rate
is actually achieved.