Does anyone know how to store joint transform while running a simulation in RigidBodyDynamics.jl? I know that getting a joint transform before and after `simulate()`

by using `joint_transform()`

. But, I don’t know how to do it in the middle of the simulation.

# How to store joint transform while running simulation in RigidBodyDynamics.jl

**HiroIshida**#1

**HiroIshida**#3

Thanks for your reply. Following your advice, I tried to do that via RigidBodySim.jl. But it seems that only I can access is a state vector, not the “state” of the mechanism. Of course, one can compute the joint transform given the state vector.

But the problem is I couldn’t understand how to know the definition of the state vector. I tried to find the definition in the tutorials and documentation, but I couldn’t find it yet. If you know the corresponding documentation, could you tell me?

I think it’s easy to infer the state vector definition if the model is fixed to the world. For example, in the Acrobot case, the 4-dim state vector is composed of [q1, q2, ω1, ω2]^T, the subscript of which specify the joint. But in the case of a floating model, it’s not that easy. In my floating model with 2-orthogonal-joint case (8dof), the state vector has 8 components. After some experiments, I found that the state vector seems to be composed of [θ, Φ, Ψ, x, y, z, q1, q2, (those time-derivetives)]^T, where the first 6 elements correspond to the state of root body. But still, the definition is unclear to me…

**tkoolen**#4

There is an overload for `copyto!`

that allows you to set a `MechanismState`

given the flat `Vector`

used for DifferentialEquations.jl. See e.g.

You can also go the other direction.

I’ll think about this question a bit more and give a more detailed answer later when I have time.

**tkoolen**#5

OK, so here are a few options, in no particular order:

- use a plain DifferentialEquations.jl callback with RigidBodySim. Either use
`SavingCallback`

or roll your own. Use the`copyto!`

method I mentioned in my previous post to initialize a separate`MechanismState`

and then use`joint_transform(state, [jointid or joint])`

while iterating over`JointID`

s or over`Joint`

s (the former being more efficient). - (ab?)use the
`control!`

argument of the`RigidBodySim.Dynamics`

constructor. Create a struct that stores the joint transforms and overload the call operator with arguments`(τ::AbstractVector, t::Number, state::MechanismState)`

. It will be called upon every evaluation of the dynamics. Just set`τ`

to whatever your controller would normally do and then additionally compute the`joint_transform`

function with the`state`

. This would allow you to reuse cached computation results in`state`

between the dynamics evaluation and the joint transform computation (probably a very minor speedup). - If you don’t want to use RigidBodySim (e.g. to reduce dependencies), you can create a new
`RigidBodyDynamics.OdeIntegrators.OdeResultsSink`

subtype that stores the transforms with RigidBodyDynamics.jl’s lower level ODE integration interface which underlies the`RigidBodyDynamics.simulate`

function. - Regardless of whether you use RigidBodySim or only RigidBodyDynamics, you could also do all of this as a post-processing step given the results of the ODE integration. E.g., with RigidBodySim, iterate over all of the
`u`

's you get back from the`solve`

call and then use`copyto!`

to initialize a`MechanismState`

and call`joint_transform`

.

Let me know if you need more info on any of these.

**HiroIshida**#6

I tried the second option, and yes, it works!

Thank you for taking the time to give an answer.