robosuite.controllers.parts.arm package

robosuite.controllers.parts.arm package#

Submodules#

robosuite.controllers.parts.arm.ik module#

NOTE: IK is only supported for the following robots:

Baxter:

Sawyer:

Panda:

Attempting to run IK with any other robot will raise an error!

class robosuite.controllers.parts.arm.ik.InverseKinematicsController(sim, ref_name: List[str] | str, joint_indexes, robot_name, actuator_range, eef_rot_offset=None, bullet_server_id=0, policy_freq=20, load_urdf=True, ik_pos_limit=None, ik_ori_limit=None, interpolator_pos=None, interpolator_ori=None, converge_steps=5, kp: int = 100, kv: int = 10, control_delta: bool = True, **kwargs)#

Bases: JointPositionController

Controller for controlling robot arm via inverse kinematics. Allows position and orientation control of the robot’s end effector.

We use differential inverse kinematics with posture control in the null space to generate joint positions (see kevinzakka/mjctrl) which are fed to the JointPositionController.

NOTE: Control input actions are assumed to be relative to the current position / orientation of the end effector and are taken as the array (x_dpos, y_dpos, z_dpos, x_rot, y_rot, z_rot).

However, confusingly, x_dpos, y_dpos, z_dpos are relative to the mujoco world frame, while x_rot, y_rot, z_rot are relative to the current end effector frame.

Parameters:

  • sim (MjSim) – Simulator instance this controller will pull robot state updates from

  • ref_name – Name of controlled robot arm’s end effector (from robot XML)

  • joint_indexes (dict) –

    Each key contains sim reference indexes to relevant robot joint information, namely:

    ’joints’:

    list of indexes to relevant robot joints

    ’qpos’:

    list of indexes to relevant robot joint positions

    ’qvel’:

    list of indexes to relevant robot joint velocities

  • robot_name (str) – Name of robot being controlled. Can be {“Sawyer”, “Panda”, or “Baxter”}

  • actuator_range (2-tuple of array of float) – 2-Tuple (low, high) representing the robot joint actuator range

  • eef_rot_offset (4-array) – Quaternion (x,y,z,w) representing rotational offset between the final robot arm link coordinate system and the end effector coordinate system (i.e: the gripper)

  • policy_freq (int) – Frequency at which actions from the robot policy are fed into this controller

  • ik_pos_limit (float) – Limit (meters) above which the magnitude of a given action’s positional inputs will be clipped

  • ik_ori_limit (float) – Limit (radians) above which the magnitude of a given action’s orientation inputs will be clipped

  • interpolator (Interpolator) – Interpolator object to be used for interpolating from the current state to the goal state during each timestep between inputted actions

  • converge_steps (int) – How many iterations to run the inverse kinematics solver to converge to a solution

  • **kwargs – Does nothing; placeholder to “sink” any additional arguments so that instantiating this controller via an argument dict that has additional extraneous arguments won’t raise an error

Raises:

AssertionError – [Unsupported robot]

static compute_joint_positions(sim: ~robosuite.utils.binding_utils.MjSim, initial_joint: ~numpy.ndarray, joint_indices: ~numpy.ndarray, ref_name: ~typing.List[str] | str, control_freq: float, velocity_limits: ~numpy.ndarray | None = None, use_delta: bool = True, dpos: ~numpy.ndarray | None = None, drot: ~numpy.ndarray | None = None, Kn: ~numpy.ndarray | None = array([10., 10., 10., 10., 5., 5., 5.]), damping_pseudo_inv: float = 0.05, Kpos: float = 0.95, Kori: float = 0.95, jac: ~numpy.ndarray | None = None, joint_names: ~typing.List[str] | None = None, nullspace_joint_weights: ~typing.Dict[str, float] = Field(name=None, type=None, default=<dataclasses._MISSING_TYPE object>, default_factory=<class 'dict'>, init=True, repr=True, hash=None, compare=True, metadata=mappingproxy({}), _field_type=None), integration_dt: float = 0.1, damping: float = 0.5, max_dq: int = 4) → ndarray#

Returns joint positions to control the robot after the target end effector position and orientation are updated from arguments @dpos and @drot. If no arguments are provided, joint positions will be set as zero.

Parameters:

  • sim (MjSim) – The simulation object.

  • initial_joint (np.array) – Initial joint positions.

  • joint_indices (np.array) – Indices of the joints.

  • ref_name – Reference site name.

  • control_freq (float) – Control frequency.

  • velocity_limits (np.array) – Limits on joint velocities.

  • use_delta – Whether or not to use delta commands. If so, use dpos, drot to compute joint positions. If not, assume dpos, drot are absolute commands (in mujoco world frame).

  • dpos (Optional[np.array]) – Desired change in end-effector position if use_delta=True, otherwise desired end-effector position.

  • drot (Optional[np.array]) – Desired change in orientation for the reference site (e.g end effector) if use_delta=True, otherwise desired end-effector orientation.

  • update_targets (bool) – Whether to update ik target pos / ori attributes or not.

  • Kpos (float) – Position gain. Between 0 and 1. 0 means no movement, 1 means move end effector to desired position in one integration step.

  • Kori (float) – Orientation gain. Between 0 and 1.

  • jac (Optional[np.array]) – Precomputed jacobian matrix.

Returns:

A flat array of joint position commands to apply to try and achieve the desired input control.

Return type:

np.array

property control_limits#

The limits over this controller’s action space, as specified by self.ik_pos_limit and self.ik_ori_limit and overriding the superclass method

Returns:

  • (np.array) minimum control values

  • (np.array) maximum control values

Return type:

2-tuple

get_control(dpos=None, rotation=None, update_targets=False)#

Returns joint positions to control the robot after the target end effector position and orientation are updated from arguments @dpos and @rotation. If no arguments are provided, joint positions will be computed based on the previously recorded target.

Parameters:

  • dpos (np.array) – a 3 dimensional array corresponding to the desired change in x, y, and z end effector position.

  • rotation (np.array) – a rotation matrix of shape (3, 3) corresponding to the desired rotation from the current orientation of the end effector.

  • update_targets (bool) – whether to update ik target pos / ori attributes or not

Returns:

a flat array of joint position commands to apply to try and achieve the desired input control.

Return type:

np.array

property name#

Name of this controller

Returns:

controller name

Return type:

str

reset_goal()#

Resets the goal to the current pose of the robot

run_controller()#

Calculates the torques required to reach the desired setpoint

Returns:

Command torques

Return type:

np.array

set_goal(delta, set_ik=None)#

Sets the internal goal state of this controller based on @delta

Note that this controller wraps a PositionController, and so determines the desired positions to achieve the inputted pose, and sets its internal setpoint in terms of joint positions

TODO: Add feature so that using @set_ik automatically sets the target values to these absolute values

Parameters:

  • delta (Iterable) – Desired relative position / orientation goal state

  • set_ik (Iterable) – If set, overrides @delta and sets the desired global position / orientation goal state

update_initial_joints(initial_joints)#

Updates the internal attribute self.initial_joints. This is useful for updating changes in controller-specific behavior, such as with OSC where self.initial_joints is used for determine nullspace actions

This function can also be extended by subclassed controllers for additional controller-specific updates

Parameters:

initial_joints (Iterable) – Array of joint position values to update the initial joints

robosuite.controllers.parts.arm.osc module#

class robosuite.controllers.parts.arm.osc.OperationalSpaceController(sim, ref_name, joint_indexes, actuator_range, input_max=1, input_min=-1, output_max=(0.05, 0.05, 0.05, 0.5, 0.5, 0.5), output_min=(-0.05, -0.05, -0.05, -0.5, -0.5, -0.5), kp=150, damping_ratio=1, impedance_mode='fixed', kp_limits=(0, 300), damping_ratio_limits=(0, 100), policy_freq=20, position_limits=None, orientation_limits=None, interpolator_pos=None, interpolator_ori=None, control_ori=True, input_type='delta', input_ref_frame='base', uncouple_pos_ori=True, lite_physics=True, **kwargs)#

Bases: Controller

Controller for controlling robot arm via operational space control. Allows position and / or orientation control of the robot’s end effector. For detailed information as to the mathematical foundation for this controller, please reference http://khatib.stanford.edu/publications/pdfs/Khatib_1987_RA.pdf

NOTE: Control input actions can either be taken to be relative to the current position / orientation of the end effector or absolute values. In either case, a given action to this controller is assumed to be of the form: (x, y, z, ax, ay, az) if controlling pos and ori or simply (x, y, z) if only controlling pos

Parameters:

  • sim (MjSim) – Simulator instance this controller will pull robot state updates from

  • joint_indexes (dict) –

    Each key contains sim reference indexes to relevant robot joint information, namely:

    ’joints’:

    list of indexes to relevant robot joints

    ’qpos’:

    list of indexes to relevant robot joint positions

    ’qvel’:

    list of indexes to relevant robot joint velocities

  • actuator_range (2-tuple of array of float) – 2-Tuple (low, high) representing the robot joint actuator range

  • input_max (float or Iterable of float) – Maximum above which an inputted action will be clipped. Can be either be a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the latter, dimension should be the same as the control dimension for this controller

  • input_min (float or Iterable of float) – Minimum below which an inputted action will be clipped. Can be either be a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the latter, dimension should be the same as the control dimension for this controller

  • output_max (float or Iterable of float) – Maximum which defines upper end of scaling range when scaling an input action. Can be either be a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the latter, dimension should be the same as the control dimension for this controller

  • output_min (float or Iterable of float) – Minimum which defines upper end of scaling range when scaling an input action. Can be either be a scalar (same value for all action dimensions), or a list (specific values for each dimension). If the latter, dimension should be the same as the control dimension for this controller

  • kp (float or Iterable of float) – positional gain for determining desired torques based upon the pos / ori error. Can be either be a scalar (same value for all action dims), or a list (specific values for each dim)

  • damping_ratio (float or Iterable of float) – used in conjunction with kp to determine the velocity gain for determining desired torques based upon the joint pos errors. Can be either be a scalar (same value for all action dims), or a list (specific values for each dim)

  • impedance_mode (str) – Impedance mode with which to run this controller. Options are {“fixed”, “variable”, “variable_kp”}. If “fixed”, the controller will have fixed kp and damping_ratio values as specified by the @kp and @damping_ratio arguments. If “variable”, both kp and damping_ratio will now be part of the controller action space, resulting in a total action space of (6 or 3) + 6 * 2. If “variable_kp”, only kp will become variable, with damping_ratio fixed at 1 (critically damped). The resulting action space will then be (6 or 3) + 6.

  • kp_limits (2-list of float or 2-list of Iterable of floats) – Only applicable if @impedance_mode is set to either “variable” or “variable_kp”. This sets the corresponding min / max ranges of the controller action space for the varying kp values. Can be either be a 2-list (same min / max for all kp action dims), or a 2-list of list (specific min / max for each kp dim)

  • damping_ratio_limits (2-list of float or 2-list of Iterable of floats) – Only applicable if @impedance_mode is set to “variable”. This sets the corresponding min / max ranges of the controller action space for the varying damping_ratio values. Can be either be a 2-list (same min / max for all damping_ratio action dims), or a 2-list of list (specific min / max for each damping_ratio dim)

  • policy_freq (int) – Frequency at which actions from the robot policy are fed into this controller

  • position_limits (2-list of float or 2-list of Iterable of floats) – Limits (m) below and above which the magnitude of a calculated goal eef position will be clipped. Can be either be a 2-list (same min/max value for all cartesian dims), or a 2-list of list (specific min/max values for each dim)

  • orientation_limits (2-list of float or 2-list of Iterable of floats) – Limits (rad) below and above which the magnitude of a calculated goal eef orientation will be clipped. Can be either be a 2-list (same min/max value for all joint dims), or a 2-list of list (specific min/mx values for each dim)

  • interpolator_pos (Interpolator) – Interpolator object to be used for interpolating from the current position to the goal position during each timestep between inputted actions

  • interpolator_ori (Interpolator) – Interpolator object to be used for interpolating from the current orientation to the goal orientation during each timestep between inputted actions

  • control_ori (bool) – Whether inputted actions will control both pos and ori or exclusively pos

  • input_type (str) – Whether to control the robot using delta (“delta”) or absolute commands (“absolute”). This is wrt the contorller reference frame (see input_ref_frame field)

  • input_ref_frame (str) – Reference frame for controller. Current supported options are: “base”: actions are wrt to the robot body (i.e., the base) “world”: actions are wrt the world coordinate frame

  • uncouple_pos_ori (bool) – Whether to decouple torques meant to control pos and torques meant to control ori

  • lite_physics (bool) – Whether to optimize for mujoco forward and step calls to reduce total simulation overhead. Set to False to preserve backward compatibility with datasets collected in robosuite <= 1.4.1.

  • **kwargs – Does nothing; placeholder to “sink” any additional arguments so that instantiating this controller via an argument dict that has additional extraneous arguments won’t raise an error

Raises:

AssertionError – [Invalid impedance mode]

compute_goal_ori(delta, goal_update_mode=None)#

Compute new goal orientation, given a delta to update. Can either update the new goal based on current achieved position or current deisred goal. Updating based on current deisred goal can be useful if we want the robot to adhere with a sequence of target poses as closely as possible, without lagging or overshooting.

Parameters:

  • delta (np.array) – Desired relative change in orientation, in axis-angle form [ax, ay, az]

  • goal_update_mode (str) – either “achieved” (achieved position) or “desired” (desired goal)

Returns:

updated goal orientation in the controller frame

Return type:

np.array

compute_goal_pos(delta, goal_update_mode=None)#

Compute new goal position, given a delta to update. Can either update the new goal based on current achieved position or current deisred goal. Updating based on current deisred goal can be useful if we want the robot to adhere with a sequence of target poses as closely as possible, without lagging or overshooting.

Parameters:

  • delta (np.array) – Desired relative change in position [x, y, z]

  • goal_update_mode (str) – either “achieved” (achieved position) or “desired” (desired goal)

Returns:

updated goal position in the controller frame

Return type:

np.array

property control_limits#

Returns the limits over this controller’s action space, overrides the superclass property Returns the following (generalized for both high and low limits), based on the impedance mode:

Mode ‘fixed’:

[joint pos command]

Mode ‘variable’:

[damping_ratio values, kp values, joint pos command]

Mode ‘variable_kp’:

[kp values, joint pos command]

Returns:

  • (np.array) minimum action values

  • (np.array) maximum action values

Return type:

2-tuple

delta_to_abs_action(delta_ac, goal_update_mode)#

helper function that converts delta action into absolute action

goal_origin_to_eef_pose()#
property name#

Name of this controller

Returns:

controller name

Return type:

str

reset_goal(goal_update_mode='achieved')#

Resets the goal to the current state of the robot.

Parameters:

goal_update_mode (str) – set mode for updating controller goals, either “achieved” (achieved position) or “desired” (desired goal).

run_controller()#

Calculates the torques required to reach the desired setpoint.

Executes Operational Space Control (OSC) – either position only or position and orientation.

A detailed overview of derivation of OSC equations can be seen at: http://khatib.stanford.edu/publications/pdfs/Khatib_1987_RA.pdf

Returns:

Command torques

Return type:

np.array

set_goal(action)#

Sets goal based on input @action. If self.impedance_mode is not “fixed”, then the input will be parsed into the delta values to update the goal position / pose and the kp and/or damping_ratio values to be immediately updated internally before executing the proceeding control loop.

Note that @action expected to be in the following format, based on impedance mode!

Mode ‘fixed’:

[joint pos command]

Mode ‘variable’:

[damping_ratio values, kp values, joint pos command]

Mode ‘variable_kp’:

[kp values, joint pos command]

Parameters:

action (Iterable) – Desired relative joint position goal state

set_goal_update_mode(goal_update_mode)#
update_initial_joints(initial_joints)#

Updates the internal attribute self.initial_joints. This is useful for updating changes in controller-specific behavior, such as with OSC where self.initial_joints is used for determine nullspace actions

This function can also be extended by subclassed controllers for additional controller-specific updates

Parameters:

initial_joints (Iterable) – Array of joint position values to update the initial joints

update_origin(origin_pos, origin_ori)#

Optional function to implement in subclass controllers that will take in @origin_pos and @origin_ori and update internal configuration to account for changes in the respective states. Useful for controllers in which the origin is a frame of reference that is dynamically changing, e.g., adapting the arm to move along with a moving base.

Parameters:

  • origin_pos (3-tuple) – x,y,z position of controller reference in mujoco world coordinates

  • origin_ori (np.array) – 3x3 rotation matrix orientation of controller reference in mujoco world coordinates

world_to_origin_frame(vec)#

transform vector from world to reference coordinate frame

Module contents#