Skip to content

Commit

Permalink
added some doc strings. there's a new TODO of inverse dynamics but no…
Browse files Browse the repository at this point in the history
… algorithsm are using this yet.
  • Loading branch information
klowrey committed Dec 26, 2019
1 parent 5e38a34 commit 237309d
Showing 1 changed file with 65 additions and 3 deletions.
68 changes: 65 additions & 3 deletions src/mjsim.jl
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,17 @@ struct MJSim{MN, DN, S, SE, A}
skip::Int
end

"""
MJSim(m::jlModel, d::jlData; skip::Integer=1)
Construct an MJSim struct with a user specified jlModel and jlData. See MuJoCo.jl for more info on
these structures.
'skip' is defined as the number of times the simulation is integrated. The ctrl field in jlData is
held as the same value for each integration steps. The effective timestep of the simulation thus becomes
skip * m.opt.timestep
"""
function MJSim(m::jlModel, d::jlData; skip::Integer=1)
skip > 0 || error("`skip` must be > 0")

Expand Down Expand Up @@ -97,6 +108,13 @@ function MJSim(m::jlModel, d::jlData; skip::Integer=1)
forward!(sim)
end

"""
MJSim(modelpath::String, args...; kwargs...)
This function automatically constructs a jlModel, jlData for a given MuJoCo XML model file path.
args, kwargs allows for customized sim constructors to be created. For instance, a function may be defined for an environment that allows for dynamic model parameter randomization at load time; the fields and values can be passed in thusly.
"""
function MJSim(modelpath::String, args...; kwargs...)
m = jlModel(modelpath)
d = jlData(m)
Expand Down Expand Up @@ -254,13 +272,13 @@ Step the simulation by `skip` steps, where `skip` defaults to `MJSim.skip`.
State-dependent controls (e.g. MJSim.d.{ctrl, xfrc_applied, qfrc_applied})
should be set before calling `step!`.
"""
function step!(sim::MJSim)
function step!(sim::MJSim, skip::Integer=sim.skip)
# According to MuJoCo docs order must be:
# 1. mj_step1
# 2. set_{ctrl, xfrc_applied, qfrc_applied}
# 3. mj_step2
# This implies user should call setaction! then step!
for _=1:sim.skip
for _=1:skip
mj_step2(sim.m, sim.d)
mj_step1(sim.m, sim.d)
end
Expand All @@ -270,7 +288,14 @@ end
step!(sim::MJSim, a) = (setaction!(sim, a); step!(sim))


"""
zeroctrl!(sim::MJSim) = (fill!(sim.d.ctrl, zero(mjtNum)); sim)
Zero out the mjData fields thats contribute to forward dynamics calculations, namely:
d.ctrl
d.qfrc_applied
d.xfrc_applied
"""
@inline zeroctrl!(sim::MJSim) = (fill!(sim.d.ctrl, zero(mjtNum)); sim)
function zerofullctrl!(sim::MJSim)
fill!(sim.d.ctrl, zero(mjtNum))
Expand All @@ -279,6 +304,11 @@ function zerofullctrl!(sim::MJSim)
sim
end

"""
masscenter(sim::MJSim)
Calculate the center of mass by summing up the Cartesian coordinates of each body.
"""
function masscenter(sim::MJSim)
mcntr = zeros(SVector{3, Float64})
mtotal = 0.0
Expand All @@ -293,15 +323,47 @@ function masscenter(sim::MJSim)
mcntr / mtotal
end

"""
forward!(sim::MJSim) = (mj_forward(sim.m, sim.d); sim)
"""
@inline forward!(sim::MJSim) = (mj_forward(sim.m, sim.d); sim)
"""
forwardskip!(sim::MJSim, skipstage::MJCore.mjtStage=MJCore.mjSTAGE_NONE, skipsensor::Bool=false)
Wraps mj_forwardSkip, to allow for optimization of forward dynamics calculations by skipping appropriate calculation steps.
The enum selecting for which stages are skipped are:
@cenum mjtStage::Cint begin # computation stage
mjSTAGE_NONE = 0 # no computations
mjSTAGE_POS # position-dependent computations
mjSTAGE_VEL # velocity-dependent computations
mjSTAGE_ACC # acceleration/force-dependent computations
end
skipsensor == true will avoid the sensor computations.
More information can be found at http://mujoco.org/book/programming.html under 'Forward Dynamics'
"""
@inline function forwardskip!(sim::MJSim, skipstage::MJCore.mjtStage=MJCore.mjSTAGE_NONE, skipsensor::Bool=false)
mj_forwardSkip(sim.m, sim.d, skipstage, skipsensor)
sim
end

#TODO inverse and inverseskip

"""
timestep(sim::MJSim) = sim.m.opt.timestep
"""
timestep(sim::MJSim) = sim.m.opt.timestep
"""
effective_timestep(sim::MJSim) = timestep(sim) * sim.skip
"""
effective_timestep(sim::MJSim) = timestep(sim) * sim.skip
"""
Base.time(sim::MJSim) = sim.d.time
"""
Base.time(sim::MJSim) = sim.d.time

_maybecopyto!(::Shapes.Length{0}, src, dest) = src
_maybecopyto!(::Shapes.Length, src, dest) = copyto!(src, dest)
_maybecopyto!(::Shapes.Length, src, dest) = copyto!(src, dest)

0 comments on commit 237309d

Please sign in to comment.