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

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.

I think if you use the RigidBodySim.jl package you can use a callback to do this.

1 Like

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…

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.

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.

2 Likes

I tried the second option, and yes, it works!
Thank you for taking the time to give an answer.