Support coulomb and viscous joint friction

[diegoferigo]

This PR introduces the support of coulomb and viscous joint friction.

The friction model used by physics is the following:

$$\begin{align} & M(\mathbf{q}) \dot{\boldsymbol{\nu}} + h(\mathbf{q}, \boldsymbol{\nu}) = B \boldsymbol{\tau}_{references} + B \boldsymbol{\tau}_{friction} + J^{\top}_{\mathcal{L}} \mathbf{f}_{\mathcal{L}} \\\ & \boldsymbol{\tau}_{friction} = -K_c \text{sign}(\mathbf{s}) - K_v \dot{\mathbf{s}} \end{align}$$

where

$$\begin{align} K_c &= \text{diag}(\mathbf{k}_c) \\\ K_v &= \text{diag}(\mathbf{k}_v) \end{align}$$

are the static and viscous joint friction parameters, and $\mathbf{q} = ({}^W H_B, \mathbf{s}) \in SE(3) \times \mathbb{R}^n$.

Note that in presence of joint friction, the joint torque actuated by the physics engine differs from the torque reference specified by the user. This is historically a delicate subject in many physics engines. In order to simplify the implementation of torque/impedance controllers that require in their feedback term the knowledge of the real joint torques, our high-level classes already return this value in the StepData object returned by Model.integrate:

jaxsim/src/jaxsim/high_level/model.py

Lines 60 to 62 in 74b57e8

	t0_model_input_real: jaxsim.physics.model.physics_model_state.PhysicsModelInput = (
	dataclasses.field(repr=False)
	)

[traversaro]

Note that in presence of joint friction, the joint torque actuated by the physics engine differs from the torque reference specified by the user. This is historically a delicate subject in many physics engines. In order to simplify the implementation of torque/impedance controllers that require in their feedback term the knowledge of the real joint torques, our high-level classes already return this value in the StepData object returned by Model.integrate:

jaxsim/src/jaxsim/high_level/model.py

Lines 60 to 62 in 74b57e8

	t0_model_input_real: jaxsim.physics.model.physics_model_state.PhysicsModelInput = (
	dataclasses.field(repr=False)
	)

Sorry, they return "the joint torque actuated by the physics engine" or the "the torque reference specified by the user" ?

[diegoferigo]

Sorry, they return "the joint torque actuated by the physics engine" or the "the torque reference specified by the user"?

The "the torque references specified by the user" are part of the PhysicsModelInput class that is included in high_level.model.Model.data:

jaxsim/src/jaxsim/physics/model/physics_model_state.py

Lines 121 to 125 in 74b57e8

	@jax_dataclasses.pytree_dataclass
	class PhysicsModelInput:
	tau: jtp.VectorJax
	f_ext: jtp.MatrixJax

The first location where the torque references can be gathered is this one. In the case the user does not explicitly set the references (e.g. because there are PID/whole-body controllers), StepData contains in any case the following:

jaxsim/src/jaxsim/high_level/model.py

Lines 58 to 62 in 74b57e8

	# Starting model data and real input (tau, f_ext) computed at t0
	t0_model_data: ModelData = dataclasses.field(repr=False)
	t0_model_input_real: jaxsim.physics.model.physics_model_state.PhysicsModelInput = (
	dataclasses.field(repr=False)
	)

t0_model_input_real, as wrote in the first post, contains the real inputs. Real inputs are the real actuated torques (= references - friction - joint limits #20) and the real external forces (= those supplied by the user - ground reaction forces).

Then, to make StepData as standalone as possible (since it's possible to integrate over an horizon #10), it also includes the original PhysicsModelInput (= torque references and external forces supplied by the user) included in t0_model_data. A vectorized StepData over an horizon should include all the information to plot most of the meaningful trajectories (joint/base position/velocities/accelerations, torque references, actuated torques, total external forces, etc). Have a look to ode.dx_dt for further information, the construction of its return values could answer a lot.

We know well nowadays that often this information is easily lost in translation, and I tried to return as much data as possible to the users. There should be all the ingredients to produce a real joint torques feedback.

[diegoferigo]

It's worth mentioning that the adopted friction model is discontinuous. As soon as we start investigating AD support #4, we might need to smooth the friction model for better gradients.

[DanielePucci]

TLDR In practice it is sometimes useful to have different constants for both coulomb and viscous friction velocity directions. I do not know if the current PR supports this - reading fast it does not seem so

[diegoferigo]

@DanielePucci SDF models have joint/axis/dynamics/friction and joint/axis/dynamics/damping for specifying respectively the $k_c$ and $k_v$ parameters.

This PR actually does read these two values separately and implements the joint friction model described in the first post:

jaxsim/src/jaxsim/simulation/ode.py

Lines 108 to 120 in 1c1c1ea

	# ==============
	# Joint friction
	# ==============
	# Static and viscous joint friction parameters
	kc = jnp.array(list(physics_model._joint_friction_static.values()))
	kv = jnp.array(list(physics_model._joint_friction_viscous.values()))
	# Compute the joint friction torque
	tau_friction = -(
	jnp.diag(kc) @ jnp.sign(ode_state.physics_model.joint_positions)
	+ jnp.diag(kv) @ ode_state.physics_model.joint_velocities
	)

I'm not sure what you mean with velocity directions. You mean two different pairs of parameters for each joint used when $\dot{s}$ is either positive or negative? This is something I didn't see often around, and SDF does not support it ootb.

[DanielePucci]

@diegoferigo

You mean two different pairs of parameters for each joint used when $\dot{s}$ is either positive or negative?

Yes

[DanielePucci]

This is something I didn't see often around, and SDF does not support it ootb.

I understand, you may see current work in this direction in isorrentino/icub-firmware@73e405d robotology/icub-firmware@a5176f2

[diegoferigo]

Thanks for reporting, good to know. Updating Python code to support this feature would be really straightforward, currently the main limitation is the model description that does not allow it out of the box. I have plans that will be soon revealed for supporting the new custom SDF attributes, and this extension could be included in this way. The trade off is that models containing such elements would become simulator-specific.

In any case, let's keep this in mind if someone needs it in the future.

[diegoferigo]

The functionality mentioned in #21 (comment) could be implemented by finalizing ami-iit/rod#4. I plan to migrate soon our parsing logic to this new library.