dynamics

jiminy_py.dynamics.SE3ToXYZRPY(M)[source]

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

jiminy_py.dynamics.XYZRPYToSE3(xyzrpy)[source]

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

jiminy_py.dynamics.XYZRPYToXYZQuat(xyzrpy)[source]

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

jiminy_py.dynamics.XYZQuatToXYZRPY(xyzquat)[source]

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

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 (numpy.ndarray) –

  • dxyzrpy (numpy.ndarray) –

Return type

numpy.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 (numpy.ndarray) –

  • v (numpy.ndarray) –

Return type

numpy.ndarray

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 (jiminy_py.core.Model) – Jiminy robot.

  • position (numpy.ndarray) – Robot position vector.

  • velocity (Optional[numpy.ndarray]) – Robot velocity vector.

  • acceleration (Optional[numpy.ndarray]) – Robot acceleration vector.

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

  • update_com (bool) – Whether or not 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 or not to compute the energy of the robot. Optional: False by default

  • update_jacobian (bool) – Whether or not 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 (jiminy_py.core.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

pinocchio.pinocchio_pywrap.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 (jiminy_py.core.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

pinocchio.pinocchio_pywrap.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 (jiminy_py.core.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

pinocchio.pinocchio_pywrap.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 (jiminy_py.core.Model) – Jiminy robot.

  • ground_profile (Optional[Callable[numpy.ndarray, Tuple[float, numpy.ndarray]]]) – 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

pinocchio.pinocchio_pywrap.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 (jiminy_py.core.Model) – Jiminy robot.

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

  • velocity (Optional[numpy.ndarray]) – See position.

  • acceleration (Optional[numpy.ndarray]) – See position.

  • fixed_body_name (Optional[str]) – 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 (Optional[Callable[numpy.ndarray, Tuple[float, numpy.ndarray]]]) – Ground profile callback.

  • use_theoretical_model (Optional[bool]) – 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

str

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 (jiminy_py.core.Model) – Jiminy robot.

  • position (numpy.ndarray) – Robot configuration vector.

  • velocity (numpy.ndarray) – Robot velocity vector.

  • acceleration (numpy.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[numpy.ndarray, pinocchio.pinocchio_pywrap.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 (jiminy_py.core.Model) – Jiminy robot.

  • position (numpy.ndarray) – Robot configuration vector.

  • velocity (numpy.ndarray) – Robot velocity vector.

  • acceleration (numpy.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

numpy.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 (jiminy_py.log.TrajectoryDataType) – Sequence of States for which to retrieve the freeflyer.

  • freeflyer_continuity (bool) – Whether or not 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 (jiminy_py.log.TrajectoryDataType) – Sequence of States for which to compute the efforts.

Return type

None