dynamics

Helpers to ease computation of kinematic and dynamic quantities.

Warning

These helpers must be used with caution. They are inefficient and some may not even work properly due to ground profile being partially supported.

jiminy_py.dynamics.SE3ToXYZRPY(M)[source]

Convert Pinocchio SE3 object to [X,Y,Z,Roll,Pitch,Yaw] vector.

Parameters:

M (SE3)

Return type:

ndarray

jiminy_py.dynamics.XYZRPYToSE3(xyzrpy)[source]

Convert [X,Y,Z,Roll,Pitch,Yaw] vector to Pinocchio SE3 object.

Parameters:

xyzrpy (ndarray)

Return type:

ndarray

jiminy_py.dynamics.XYZRPYToXYZQuat(xyzrpy)[source]

Convert [X,Y,Z,Roll,Pitch,Yaw] to [X,Y,Z,Qx,Qy,Qz,Qw].

Parameters:

xyzrpy (ndarray)

Return type:

ndarray

jiminy_py.dynamics.XYZQuatToXYZRPY(xyzquat)[source]

Convert [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].

Parameters:

xyzquat (ndarray)

Return type:

ndarray

jiminy_py.dynamics.velocityXYZRPYToXYZQuat(xyzrpy, dxyzrpy)[source]

Convert the derivative of [X,Y,Z,Roll,Pitch,Yaw] to [X,Y,Z,Qx,Qy,Qz,Qw].

Warning

Linear velocity in XYZRPY must be local-world-aligned frame, while returned linear velocity in XYZQuat is in local frame.

Parameters:

  • xyzrpy (ndarray)

  • dxyzrpy (ndarray)

Return type:

ndarray

jiminy_py.dynamics.velocityXYZQuatToXYZRPY(xyzquat, v)[source]

Convert the derivative of [X,Y,Z,Qx,Qy,Qz,Qw] to [X,Y,Z,Roll,Pitch,Yaw].

Warning

Linear velocity in XYZQuat must be local frame, while returned linear velocity in XYZRPY is in local-world-aligned frame.

Parameters:

  • xyzquat (ndarray)

  • v (ndarray)

Return type:

ndarray

class jiminy_py.dynamics.State(t, q, v=None, a=None, u=None, command=None, f_external=None, lambda_c=None)[source]

Bases: object

Basic data structure storing kinematics and dynamics information at a given time.

Note

The user is the responsible for keeping track to which robot the state is associated to as this information is not stored in the state itself.

Parameters:

  • t (float)

  • q (ndarray)

  • v (ndarray | None)

  • a (ndarray | None)

  • u (ndarray | None)

  • command (ndarray | None)

  • f_external (ndarray | None)

  • lambda_c (ndarray | None)

t: float

Time.

q: ndarray

Configuration vector.

v: ndarray | None = None

Velocity vector as a 1D array.

a: ndarray | None = None

Acceleration vector as a 1D array.

u: ndarray | None = None

Total joint efforts as a 1D array.

command: ndarray | None = None

Motor command as a 1D array.

f_external: ndarray | None = None

Joint external forces as a 2D array.

The first dimension corresponds to the N individual joints of the robot including ‘universe’, while the second gathers the 6 spatial force coordinates (Fx, Fy, Fz, Mx, My, Mz).

lambda_c: ndarray | None = None

Lambda multipliers associated with all the constraints as a 1D array.

class jiminy_py.dynamics.Trajectory(states, robot, use_theoretical_model)[source]

Bases: object

Trajectory of a robot.

This class is mostly a basic data structure storing a sequence of states along with the robot to which it is associated. On top of that, it provides helper methods to make it easier to manipulate these data, eg query the state at a given timestamp.

Parameters:

  • states (Tuple[State, ...]) – Trajectory data as a sequence of State instances of increasing time.

  • robot (Robot) – Robot associated with the trajectory.

  • use_theoretical_model (bool) – Whether the state sequence is associated with the theoretical dynamical model or extended simulation model of the robot.

states: Tuple[State, ...]

Sequence of states of increasing time.

Warning

The time may not be strictly increasing. There may be up to two consecutive data point associated with the same timestep because quantities may vary instantaneously at acceleration-level and higher.

robot: Robot

Robot associated with the trajectory.

use_theoretical_model: bool

Whether to use the theoretical model or the extended simulation model.

property has_data: bool

Whether the trajectory has data, ie the state sequence is not empty.

property has_velocity: bool

Whether the trajectory contains the velocity vector.

property has_acceleration: bool

Whether the trajectory contains the acceleration vector.

property has_effort: bool

Whether the trajectory contains the effort vector.

property has_command: bool

Whether the trajectory contains motor commands.

property has_external_forces: bool

Whether the trajectory contains external forces.

property has_constraints: bool

Whether the trajectory contains lambda multipliers of constraints.

property time_interval: Tuple[float, float]

Time interval of the trajectory.

It raises an exception if no data is available.

get(t, mode='raise')[source]

Query the state at a given timestamp.

Internally, the nearest neighbor states are linearly interpolated, taking into account the corresponding Lie Group of all state attributes that are available.

Parameters:

  • t (float) – Time of the state to extract from the trajectory.

  • mode (Literal['raise', 'wrap', 'clip']) – Fallback strategy when the query time is not in the time interval ‘time_interval’ of the trajectory. ‘raise’ raises an exception if the query time is out-of-bound wrt the underlying state sequence of the selected trajectory. ‘clip’ forces clipping of the query time before interpolation of the state sequence. ‘wrap’ wraps around the query time wrt the time span of the trajectory. This is useful to store periodic trajectories as finite state sequences.

Return type:

State

jiminy_py.dynamics.update_quantities(robot, position, velocity=None, acceleration=None, f_external=None, update_dynamics=True, update_centroidal=True, update_energy=True, update_jacobian=False, update_collisions=False, use_theoretical_model=True)[source]

Compute all quantities using position, velocity and acceleration configurations.

Run multiple algorithms to compute all quantities which can be known with the model position, velocity and acceleration.

This includes: - body and frame spatial transforms, - body spatial velocities, - body spatial drifts, - body spatial acceleration, - joint transform jacobian matrices, - center-of-mass position, - center-of-mass velocity, - center-of-mass drift, - center-of-mass acceleration, - center-of-mass jacobian, - articular inertia matrix, - non-linear effects (Coriolis + gravity) - collisions and distances

Note

Computation results are stored internally in the robot, and can be retrieved with associated getters.

Warning

This function modifies the internal robot data.

Parameters:

  • robot (Model) – Jiminy robot.

  • position (ndarray) – Configuration vector.

  • velocity (ndarray | None) – Joint velocity vector.

  • acceleration (ndarray | None) – Joint acceleration vector.

  • f_external (List[ndarray] | StdVec_Force | None) – External forces applied on each joints.

  • update_dynamics (bool) – Whether to compute the non-linear effects and the joint internal forces. Optional: True by default.

  • update_centroidal (bool) – Whether to compute the centroidal dynamics (incl. CoM) of the robot. Optional: False by default.

  • update_energy (bool) – Whether to compute the energy of the robot. Optional: False by default

  • update_jacobian (bool) – Whether to compute the Jacobian matrices of the joint transforms. Optional: False by default.

  • update_collisions (bool) – Whether to detect collisions and compute distances between all the geometry objects. Optional: False by default.

  • use_theoretical_model (bool) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. Optional: True by default.

Return type:

None

jiminy_py.dynamics.get_body_world_transform(robot, body_name, use_theoretical_model=True, copy=True)[source]

Get the transform from world frame to body frame for a given body.

Warning

It is assumed that update_quantities has been called beforehand.

Parameters:

  • robot (Model) – Jiminy robot.

  • body_name (str) – Name of the body.

  • use_theoretical_model (bool) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot.

  • copy (bool) – Whether to return the internal buffers (which could be altered) or copy them. Optional: True by default. It is less efficient but safer.

Returns:

Body transform.

Return type:

SE3

jiminy_py.dynamics.get_body_world_velocity(robot, body_name, use_theoretical_model=True)[source]

Get the spatial velocity in world frame.

Warning

It is assumed that update_quantities has been called beforehand.

Parameters:

  • robot (Model) – Jiminy robot.

  • body_name (str) – Name of the body.

  • use_theoretical_model (bool) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. Optional: True by default.

Returns:

Spatial velocity.

Return type:

SE3

jiminy_py.dynamics.get_body_world_acceleration(robot, body_name, use_theoretical_model=True)[source]

Get the body spatial acceleration in world frame.

The moment of this tensor (i.e linear part) is NOT the linear acceleration of the center of the body frame, expressed in the world frame.

Warning

It is assumed that update_quantities has been called.

Parameters:

  • robot (Model) – Jiminy robot.

  • body_name (str) – Name of the body.

  • use_theoretical_model (bool) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. Optional: True by default.

Returns:

Spatial acceleration.

Return type:

SE3

jiminy_py.dynamics.compute_transform_contact(robot, ground_profile=None)[source]

Compute the transform the apply to the freeflyer to touch the ground with up to 3 contact points.

This method can be used in conjunction with compute_freeflyer_state_from_fixed_body to ensures no contact points are going through the ground and up to three are in contact.

Warning

It is assumed that update_quantities has been called.

Parameters:

  • robot (Model) – Jiminy robot.

  • ground_profile (Callable[[ndarray], Tuple[float, ndarray]] | None) – Ground profile callback.

Returns:

The transform the apply in order to touch the ground. If the robot has no contact point, then the identity is returned.

Return type:

SE3

jiminy_py.dynamics.compute_freeflyer_state_from_fixed_body(robot, position, velocity=None, acceleration=None, fixed_body_name=None, ground_profile=None, use_theoretical_model=None)[source]

Fill rootjoint data from articular data when a body is fixed and aligned with world frame.

This method computes the position of freeflyer in the fixed body frame.

If fixed_body_name is omitted, it will default to either:

  • the set of three contact points

  • a single collision body

In such a way that no contact points nor collision bodies are going through the ground and at least contact points or a collision body are touching it.

None is returned if their is no contact frame or if the robot does not have a freeflyer.

Warning

This function modifies the internal robot data.

Warning

The method fills in freeflyer data instead of returning an updated copy for efficiency.

Parameters:

  • robot (Model) – Jiminy robot.

  • position (ndarray) – Must contain current articular data. The freeflyer data can contain any value, it will be ignored and replaced.

  • velocity (ndarray | None) – See position.

  • acceleration (ndarray | None) – See position.

  • fixed_body_name (str | None) – Name of the body frame that is considered fixed parallel to world frame. Optional: It will be inferred from the set of contact points and collision bodies.

  • ground_profile (Callable[[ndarray], Tuple[float, ndarray]] | None) – Ground profile callback.

  • use_theoretical_model (bool | None) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. Must be False if fixed_body_name is not specified. Optional: True by default if fixed_body_name is specified, False otherwise.

Returns:

Name of the contact frame, if any.

Return type:

None

jiminy_py.dynamics.compute_efforts_from_fixed_body(robot, position, velocity, acceleration, fixed_body_name, use_theoretical_model=True)[source]

Compute the efforts using RNEA method.

Warning

This function modifies the internal robot data.

Parameters:

  • robot (Model) – Jiminy robot.

  • position (ndarray) – Robot configuration vector.

  • velocity (ndarray) – Robot velocity vector.

  • acceleration (ndarray) – Robot acceleration vector.

  • fixed_body_name (str) – Name of the body frame.

  • use_theoretical_model (bool) – Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. Optional: True by default.

Returns:

articular efforts and external forces.

Return type:

Tuple[ndarray, Force]

jiminy_py.dynamics.compute_inverse_dynamics(robot, position, velocity, acceleration, use_theoretical_model=False)[source]

Compute the motor torques through inverse dynamics, assuming to external forces except the one resulting from the analytical constraints applied on the model.

Warning

This function modifies the internal robot data.

Parameters:

  • robot (Model) – Jiminy robot.

  • position (ndarray) – Robot configuration vector.

  • velocity (ndarray) – Robot velocity vector.

  • acceleration (ndarray) – Robot acceleration vector.

  • use_theoretical_model (bool) – Whether the position, velocity and acceleration are associated with the theoretical model instead of the extended one. Optional: False by default.

Returns:

motor torques.

Return type:

ndarray