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

Specific Domains Modelling & Simulations
1

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.

2

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

1 Like
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…

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.

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 JointIDs or over Joints (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
6

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