dynamics

Helpers to ease computation of kinematic and dynamic quantities.

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, tau=None, contact_frames=None, f_ext=None, copy=False, **kwargs)[source]

Bases: object

Store the kinematics and dynamics data of the robot at a given time.

Parameters:
  • t (float) – Time.

  • q (ndarray) – Configuration vector.

  • v (ndarray | None) – Velocity vector.

  • a (ndarray | None) – Acceleration vector.

  • tau (ndarray | None) – Joint efforts.

  • contact_frames (Sequence[str] | None) – Name of the contact frames.

  • f_ext (Sequence[ndarray] | None) – Joint external forces.

  • copy (bool) – Force to copy the arguments.

  • kwargs (Any) –

class jiminy_py.dynamics.TrajectoryDataType[source]

Bases: TypedDict

Basic data structure storing the required information about a trajectory to later replay it using jiminy_py.viewer.play_trajectories.

jiminy_py.dynamics.update_quantities(robot, position, velocity=None, acceleration=None, update_physics=True, update_com=True, update_energy=True, update_jacobian=False, update_collisions=True, 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 spatial transforms, - body spatial velocities, - body spatial drifts, - body transform acceleration, - body transform jacobians, - 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.

Warning

It does not called overloaded pinocchio methods provided by jiminy_py.core but the original pinocchio methods instead. As a result, it does not take into account the rotor inertias / armatures. One is responsible of calling the appropriate methods manually instead of this one if necessary. This behavior is expected to change in the future.

Parameters:
  • robot (Model) – Jiminy robot.

  • position (ndarray) – Robot position vector.

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

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

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

  • update_com (bool) – Whether to compute the COM of the robot AND each link individually. The global COM is the first index. 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 jacobians. Optional: False by default.

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

  • update_collisions (bool) –

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 robot’s state. Optional: True by default.

  • 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 robot’s state. 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 robot’s state. 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 infered 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 robot’s state. Must be False if fixed_body_name is not speficied. 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 robot’s state. 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 anyaltical 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 actual one. Optional: False by default.

Returns:

motor torques.

Return type:

ndarray

jiminy_py.dynamics.compute_freeflyer(trajectory_data, freeflyer_continuity=True)[source]

Compute the freeflyer positions and velocities.

Warning

This function modifies the internal robot data.

Parameters:
  • trajectory_data (TrajectoryDataType) – Sequence of States for which to retrieve the freeflyer.

  • freeflyer_continuity (bool) – Whether to enforce the continuity in position of the freeflyer. Optional: True by default.

Return type:

None

jiminy_py.dynamics.compute_efforts(trajectory_data)[source]

Compute the efforts in the trajectory using RNEA method.

Parameters:

trajectory_data (TrajectoryDataType) – Sequence of States for which to compute the efforts.

Return type:

None