Motor Safety Limit

Implementation of basic Proportional-Derivative controller block compatible with gym_jiminy reinforcement learning pipeline environment design.

gym_jiminy.common.blocks.motor_safety_limit.apply_safety_limits(command, q_measured, v_measured, kp, kd, motors_soft_position_lower, motors_soft_position_upper, motors_velocity_limit, motors_effort_limit, out)

Clip the command torque to ensure safe operation.

It acts on each actuator independently and only activate close to the position or velocity limits. Basically, the idea to the avoid moving faster when some prescribed velocity limit or exceeding soft position bounds by forcing the command torque to act against it. Still, it may not be enough to prevent such issue in practice as the command torque is bounded.

Warning

All the input arguments must be in motor order, including the measured position and velocity of the actuators. In practice, those measurements comes from the encoder sensors. If so, then the measurements must be re-ordered if necessary to match motor order instead of sensor order.

See also

See MotorSafetyLimit documentation for details.

Parameters:

• command (ndarray) – Desired command to will be updated in-place if necessary.

• q_measured (ndarray) – Current position of the actuators.

• v_measured (ndarray) – Current velocity of the actuators.

• kp (ndarray) – Scale of the velocity bound triggered by position limits.

• kd (ndarray) – Scale of the effort bound triggered by velocity limits.

• motors_soft_position_lower (ndarray) – Soft lower position limit of the actuators.

• motors_soft_position_upper (ndarray) – Soft upper position limit of the actuators.

• motors_velocity_limit (ndarray) – Maximum velocity of the actuators.

• motors_effort_limit (ndarray) – Maximum effort that the actuators can output. The command torque cannot exceed this limits, not even if needed to enforce safe operation.

• out (ndarray) – Pre-allocated memory to store the command motor torques.

Return type:

None

class gym_jiminy.common.blocks.motor_safety_limit.MotorSafetyLimit(name, env, *, kp, kd, soft_position_margin=0.0, soft_velocity_max=inf)

Bases: BaseControllerBlock[ndarray, ndarray, BaseObsT, ndarray]

Safety mechanism primarily designed to prevent hardware damage and premature wear, but also to temper violent, sporadic and dangerous motions.

A velocity limit v+ is enforced by bounding the commanded effort such that no effort can be applied to push the joint beyond the velocity limit, and a damping effort is applied if the joint is moving at a velocity beyond the limit, ie -kd * (v - v+).

When the joint is near the soft limits x+/-, the velocities are bounded to keep the position from crossing the soft limits. The k_position term determines the scale of the bound on velocity, ie v+/- = -kp * (x - x+/-). These bounds on velocity are the ones determining the bounds on effort.

Note

The prescribed position and velocity limits may be more respective that the actual hardware specification of the robot.

Warning

It must be connected directly to the environment without any other intermediary controller.

See also

See official ROS documentation and implementation for details: https://wiki.ros.org/pr2_controller_manager/safety_limits https://github.com/PR2/pr2_mechanism/blob/melodic-devel/pr2_mechanism_model/src/joint.cpp

Parameters:

• name (str) – Name of the block.

• env (InterfaceJiminyEnv[BaseObsT, BaseActT]) – Environment to connect with.

• kp (float) – Scale of the velocity bound triggered by position limits.

• kd (float) – Scale of the effort bound triggered by velocity limits.

• soft_position_margin (float) – Minimum distance of the current motor positions from their respective bounds before starting to break.

• soft_velocity_max (float) – Maximum velocity of the motor before starting to break.

property fieldnames: List[str]

Get mapping between each scalar element of the action space of the controller block and the associated fieldname for logging.

It is expected to return an object with the same structure than the action space, but having lists of string as leaves. Generic fieldnames are used by default.

compute_command(action, command)

Apply safety limits to the desired motor torques right before sending it to the robot so as to avoid exceeded prescribed position and velocity limits.

Parameters:

• action (ndarray) – Desired motor torques to apply on the robot.

• command (ndarray) – Current motor torques that will be updated in-place.

Return type:

None