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

**URL:** <https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797>\
**Category:** Modelling & Simulations\
**Created:** [October 25, 2018, 6:15pm UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797 "2018-10-25T18:15:32Z")\
**Posts on this page:** 6\
**Page:** 1

<div class="post-metadata">

**Author:** ![HiroIshida](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/hiroishida/32/7424_2.png) [@HiroIshida](https://discourse.julialang.org/u/HiroIshida)\
**Post date:** [October 25, 2018, 6:15pm UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/1 "2018-10-25T18:15:32Z")

</div>

Does anyone know how to store joint transform while running a simulation in [RigidBodyDynamics.jl](https://github.com/JuliaRobotics/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.

---

<div class="post-metadata">

**Author:** ![phelipe](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/phelipe/32/14643_2.png) [@phelipe](https://discourse.julialang.org/u/phelipe)\
**Post date:** [October 26, 2018, 2:08pm UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/2 "2018-10-26T14:08:01Z")

</div>

I think if you use the [RigidBodySim.jl](https://github.com/JuliaRobotics/RigidBodySim.jl) package you can use a callback to do this.

---

<div class="post-metadata">

**Author:** ![HiroIshida](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/hiroishida/32/7424_2.png) [@HiroIshida](https://discourse.julialang.org/u/HiroIshida)\
**Post date:** [October 29, 2018, 7:58am UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/3 "2018-10-29T07:58:00Z")

</div>

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…

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [October 29, 2018, 11:27am UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/4 "2018-10-29T11:27:40Z")

</div>

There is an overload for `copyto!` that allows you to set a `MechanismState` given the flat `Vector` used for DifferentialEquations.jl. See e.g.

> <https://github.com/JuliaRobotics/RigidBodySim.jl/blob/e01174abfcf9877bdeba3cf74f4f9b760d1f12ba/src/core.jl#L80>

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.

---

<div class="post-metadata">

**Author:** ![tkoolen](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/tkoolen/32/1603_2.png) [@tkoolen](https://discourse.julialang.org/u/tkoolen)\
**Post date:** [October 29, 2018, 10:13pm UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/5 "2018-10-29T22:13:11Z")

</div>

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

- use a plain DifferentialEquations.jl callback with RigidBodySim. Either use [`SavingCallback`](https://docs.juliadiffeq.org/stable/features/callback_library.html#SavingCallback-1) 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])`](http://www.juliarobotics.org/RigidBodyDynamics.jl/stable/mechanismstate.html#RigidBodyDynamics.joint_transform) 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`](http://www.juliarobotics.org/RigidBodyDynamics.jl/stable/simulation.html#RigidBodyDynamics.OdeIntegrators.OdeResultsSink) subtype that stores the transforms with RigidBodyDynamics.jl’s [lower level ODE integration interface](http://www.juliarobotics.org/RigidBodyDynamics.jl/stable/simulation.html#Lower-level-ODE-integration-interface-1) 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.

---

<div class="post-metadata">

**Author:** ![HiroIshida](https://sea2.discourse-cdn.com/julialang/user_avatar/discourse.julialang.org/hiroishida/32/7424_2.png) [@HiroIshida](https://discourse.julialang.org/u/HiroIshida)\
**Post date:** [October 31, 2018, 1:54am UTC](https://discourse.julialang.org/t/how-to-store-joint-transform-while-running-simulation-in-rigidbodydynamics-jl/16797/6 "2018-10-31T01:54:48Z")

</div>

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