Controller¶
Every Robot is equipped with a controller, which determines both the action space as well as how its values are mapped into command torques. By default, all controllers have a predefined set of methods and properities, though specific controllers may extend and / or override the default functionality found in the base class.
Base Controller¶

class
robosuite.controllers.base_controller.
Controller
(sim, eef_name, joint_indexes, actuator_range)¶ General controller interface.
Requires reference to mujoco sim object, eef_name of specific robot, relevant joint_indexes to that robot, and whether an initial_joint is used for nullspace torques or not
 Parameters
sim (MjSim) – Simulator instance this controller will pull robot state updates from
eef_name (str) – 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
actuator_range (2tuple of array of float) – 2Tuple (low, high) representing the robot joint actuator range

abstract
run_controller
()¶ Abstract method that should be implemented in all subclass controllers, and should convert a given action into torques (pre gravity compensation) to be executed on the robot. Additionally, resets the self.new_update flag so that the next self.update call will occur

scale_action
(action)¶ Clips @action to be within self.input_min and self.input_max, and then rescale the values to be within the range self.output_min and self.output_max
 Parameters
action (Iterable) – Actions to scale
 Returns
Rescaled action
 Return type
np.array

update
(force=False)¶ Updates the state of the robot arm, including end effector pose / orientation / velocity, joint pos/vel, jacobian, and mass matrix. By default, since this is a nonnegligible computation, multiple redundant calls will be ignored via the self.new_update attribute flag. However, if the @force flag is set, the update will occur regardless of that state of self.new_update. This base class method of @run_controller resets the self.new_update flag
 Parameters
force (bool) – Whether to force an update to occur or not

update_base_pose
(base_pos, base_ori)¶ Optional function to implement in subclass controllers that will take in @base_pos and @base_ori and update internal configuration to account for changes in the respective states. Useful for controllers e.g. IK, which is based on pybullet and requires knowledge of simulator state deviations between pybullet and mujoco
 Parameters
base_pos (3tuple) – x,y,z position of robot base in mujoco world coordinates
base_ori (4tuple) – x,y,z,w orientation or robot base in mujoco world coordinates

update_initial_joints
(initial_joints)¶ Updates the internal attribute self.initial_joints. This is useful for updating changes in controllerspecific 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 controllerspecific updates
 Parameters
initial_joints (Iterable) – Array of joint position values to update the initial joints

clip_torques
(torques)¶ Clips the torques to be within the actuator limits
 Parameters
torques (Iterable) – Torques to clip
 Returns
Clipped torques
 Return type
np.array

reset_goal
()¶ Resets the goal – usually by setting to the goal to all zeros, but in some cases may be different (e.g.: OSC)

static
nums2array
(nums, dim)¶ Convert input @nums into numpy array of length @dim. If @nums is a single number, broadcasts it to the corresponding dimension size @dim before converting into a numpy array
 Parameters
nums (numeric or Iterable) – Either single value or array of numbers
dim (int) – Size of array to broadcast input to env.sim.data.actuator_force
 Returns
Array filled with values specified in @nums
 Return type
np.array

property
torque_compensation
¶ Gravity compensation for this robot arm
 Returns
torques
 Return type
np.array

property
actuator_limits
¶ Torque limits for this controller
 Returns
(np.array) minimum actuator torques
(np.array) maximum actuator torques
 Return type
2tuple

property
control_limits
¶ Limits over this controller’s action space, which defaults to input min/max
 Returns
(np.array) minimum action values
(np.array) maximum action values
 Return type
2tuple

property
name
¶ Name of this controller
 Returns
controller name
 Return type
str
Joint Position Controller¶

class
robosuite.controllers.joint_pos.
JointPositionController
(sim, eef_name, joint_indexes, actuator_range, input_max=1, input_min= 1, output_max=0.05, output_min= 0.05, kp=50, damping_ratio=1, impedance_mode='fixed', kp_limits=0, 300, damping_ratio_limits=0, 100, policy_freq=20, qpos_limits=None, interpolator=None, **kwargs)¶ Controller for controlling robot arm via impedance control. Allows position control of the robot’s joints.
NOTE: Control input actions assumed to be taken relative to the current joint positions. A given action to this controller is assumed to be of the form: (dpos_j0, dpos_j1, … , dpos_jn1) for an njoint robot
 Parameters
sim (MjSim) – Simulator instance this controller will pull robot state updates from
eef_name (str) – 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
actuator_range (2tuple of array of float) – 2Tuple (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 joint pos 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 num_joints * 3. If “variable_kp”, only kp will become variable, with damping_ratio fixed at 1 (critically damped). The resulting action space will then be num_joints * 2.
kp_limits (2list of float or 2list 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 2list (same min / max for all kp action dims), or a 2list of list (specific min / max for each kp dim)
damping_ratio_limits (2list of float or 2list 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 2list (same min / max for all damping_ratio action dims), or a 2list 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
qpos_limits (2list of float or 2list of Iterable of floats) – Limits (rad) below and above which the magnitude of a calculated goal joint position will be clipped. Can be either be a 2list (same min/max value for all joint dims), or a 2list of list (specific min/max values for each dim)
interpolator (Interpolator) – Interpolator object to be used for interpolating from the current joint position to the goal joint position during each timestep between inputted actions
**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]

set_goal
(action, set_qpos=None)¶ 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_qpos (Iterable) – If set, overrides @action and sets the desired absolute joint position goal state
 Raises
AssertionError – [Invalid action dimension size]

reset_goal
()¶ Resets joint position goal to be current position

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
2tuple
Joint Velocity Controller¶

class
robosuite.controllers.joint_vel.
JointVelocityController
(sim, eef_name, joint_indexes, actuator_range, input_max=1, input_min= 1, output_max=1, output_min= 1, kp=0.25, policy_freq=20, velocity_limits=None, interpolator=None, **kwargs)¶ Controller for controlling the robot arm’s joint velocities. This is simply a P controller with desired torques (pre gravity compensation) taken to be proportional to the velocity error of the robot joints.
NOTE: Control input actions assumed to be taken as absolute joint velocities. A given action to this controller is assumed to be of the form: (vel_j0, vel_j1, … , vel_jn1) for an njoint robot
 Parameters
sim (MjSim) – Simulator instance this controller will pull robot state updates from
eef_name (str) – 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
actuator_range (2tuple of array of float) – 2Tuple (low, high) representing the robot joint actuator range
input_max (float or list 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 list 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 list 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 list 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 list of float) – velocity gain for determining desired torques based upon the joint vel errors. Can be either be a scalar (same value for all action dims), or a list (specific values for each dim)
policy_freq (int) – Frequency at which actions from the robot policy are fed into this controller
velocity_limits (2list of float or 2list of list of floats) – Limits (m/s) below and above which the magnitude of a calculated goal joint velocity will be clipped. Can be either be a 2list (same min/max value for all joint dims), or a 2list of list (specific min/max values for each dim)
interpolator (Interpolator) – Interpolator object to be used for interpolating from the current joint velocities to the goal joint velocities during each timestep between inputted actions
**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

set_goal
(velocities)¶ Sets goal based on input @velocities.
 Parameters
velocities (Iterable) – Desired joint velocities
 Raises
AssertionError – [Invalid action dimension size]

reset_goal
()¶ Resets joint velocity goal to be all zeros
Joint Torque Controller¶

class
robosuite.controllers.joint_tor.
JointTorqueController
(sim, eef_name, joint_indexes, actuator_range, input_max=1, input_min= 1, output_max=0.05, output_min= 0.05, policy_freq=20, torque_limits=None, interpolator=None, **kwargs)¶ Controller for controlling the robot arm’s joint torques. As the actuators at the mujoco sim level are already torque actuators, this “controller” usually simply “passes through” desired torques, though it also includes the typical input / output scaling and clipping, as well as interpolator features seen in other controllers classes as well
NOTE: Control input actions assumed to be taken as absolute joint torques. A given action to this controller is assumed to be of the form: (torq_j0, torq_j1, … , torq_jn1) for an njoint robot
 Parameters
sim (MjSim) – Simulator instance this controller will pull robot state updates from
eef_name (str) – 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
actuator_range (2tuple of array of float) – 2Tuple (low, high) representing the robot joint actuator range
input_max (float or list 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 list 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 list 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 list 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
policy_freq (int) – Frequency at which actions from the robot policy are fed into this controller
torque_limits (2list of float or 2list of list of floats) – Limits (Nm) below and above which the magnitude of a calculated goal joint torque will be clipped. Can be either be a 2list (same min/max value for all joint dims), or a 2list of list (specific min/max values for each dim) If not specified, will automatically set the limits to the actuator limits for this robot arm
interpolator (Interpolator) – Interpolator object to be used for interpolating from the current joint torques to the goal joint torques during each timestep between inputted actions
**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

set_goal
(torques)¶ Sets goal based on input @torques.
 Parameters
torques (Iterable) – Desired joint torques
 Raises
AssertionError – [Invalid action dimension size]

reset_goal
()¶ Resets joint torque goal to be all zeros (precompensation)
Operation Space Controller¶

class
robosuite.controllers.osc.
OperationalSpaceController
(sim, eef_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, control_delta=True, uncouple_pos_ori=True, **kwargs)¶ 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
eef_name (str) – 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
actuator_range (2tuple of array of float) – 2Tuple (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 (2list of float or 2list 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 2list (same min / max for all kp action dims), or a 2list of list (specific min / max for each kp dim)
damping_ratio_limits (2list of float or 2list 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 2list (same min / max for all damping_ratio action dims), or a 2list 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 (2list of float or 2list 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 2list (same min/max value for all cartesian dims), or a 2list of list (specific min/max values for each dim)
orientation_limits (2list of float or 2list 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 2list (same min/max value for all joint dims), or a 2list 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
uncouple_pos_ori (bool) – Whether to decouple torques meant to control pos and torques meant to control ori
**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]

set_goal
(action, set_pos=None, set_ori=None)¶ 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_pos (Iterable) – If set, overrides @action and sets the desired absolute eef position goal state
set_ori (Iterable) – IF set, overrides @action and sets the desired absolute eef orientation goal state

reset_goal
()¶ Resets the goal to the current state of the robot

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
2tuple
Inverse Kinematics Controller¶

class
robosuite.controllers.ik.
InverseKinematicsController
(sim, eef_name, joint_indexes, robot_name, actuator_range, eef_rot_offset, 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, **kwargs)¶ Controller for controlling robot arm via inverse kinematics. Allows position and orientation control of the robot’s end effector.
Inverse kinematics solving is handled by pybullet.
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).
 Parameters
sim (MjSim) – Simulator instance this controller will pull robot state updates from
eef_name (str) – 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 (2tuple of array of float) – 2Tuple (low, high) representing the robot joint actuator range
eef_rot_offset (4array) – 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 pybullet 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]

setup_inverse_kinematics
(load_urdf=True)¶ This function is responsible for doing any setup for inverse kinematics.
Inverse Kinematics maps end effector (EEF) poses to joint angles that are necessary to achieve those poses.
 Parameters
load_urdf (bool) – specifies whether the robot urdf should be loaded into the sim. Useful flag that should be cleared in the case of multiarmed robots which might have multiple IK controller instances but should all reference the same (single) robot urdf within the bullet sim
 Raises
ValueError – [Invalid eef id]

sync_state
()¶ Syncs the internal Pybullet robot state to the joint positions of the robot being controlled.

sync_ik_robot
(joint_positions=None, simulate=False, sync_last=True)¶ Force the internal robot model to match the provided joint angles.
 Parameters
joint_positions (Iterable) – Array of joint positions. Default automatically updates to current mujoco joint pos state
simulate (bool) – If True, actually use physics simulation, else write to physics state directly.
sync_last (bool) – If False, don’t sync the last joint angle. This is useful for directly controlling the roll at the end effector.

ik_robot_eef_joint_cartesian_pose
()¶ Calculates the current cartesian pose of the last joint of the ik robot with respect to the base frame as a (pos, orn) tuple where orn is a xyzw quaternion
 Returns
(np.array) position
(np.array) orientation
 Return type
2tuple

get_control
(dpos=None, rotation=None, update_targets=False)¶ Returns joint velocities 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 velocities 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 velocity commands to apply to try and achieve the desired input control.
 Return type
np.array

inverse_kinematics
(target_position, target_orientation)¶ Helper function to do inverse kinematics for a given target position and orientation in the PyBullet world frame.
 Parameters
target_position (3tuple) – desired position
target_orientation (4tuple) – desired orientation quaternion
 Returns
list of size @num_joints corresponding to the joint angle solution.
 Return type
list

joint_positions_for_eef_command
(dpos, rotation, update_targets=False)¶ This function runs inverse kinematics to back out target joint positions from the provided end effector command.
 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 list of size @num_joints corresponding to the target joint angles.
 Return type
list

bullet_base_pose_to_world_pose
(pose_in_base)¶ Convert a pose in the base frame to a pose in the world frame.
 Parameters
pose_in_base (2tuple) – a (pos, orn) tuple.
 Returns
a (pos, orn) tuple reflecting robot pose in world coordinates
 Return type
2tuple

set_goal
(delta, set_ik=None)¶ Sets the internal goal state of this controller based on @delta
Note that this controller wraps a VelocityController, and so determines the desired velocities to achieve the inputted pose, and sets its internal setpoint in terms of joint velocities
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

reset_goal
()¶ Resets the goal to the current pose of the robot

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
2tuple

_clip_ik_input
(dpos, rotation)¶ Helper function that clips desired ik input deltas into a valid range.
 Parameters
dpos (np.array) – a 3 dimensional array corresponding to the desired change in x, y, and z end effector position.
rotation (np.array) – relative rotation in scaled axis angle form (ax, ay, az) corresponding to the (relative) desired orientation of the end effector.
 Returns
(np.array) clipped dpos
(np.array) clipped rotation
 Return type
2tuple

_make_input
(action, old_quat)¶ Helper function that returns a dictionary with keys dpos, rotation from a raw input array. The first three elements are taken to be displacement in position, and a quaternion indicating the change in rotation with respect to @old_quat. Additionally clips @action as well
 Parameters
action (np.array) – [dx, dy, dz, ax, ay, az] (orientation in scaled axisangle form)
old_quat (np.array) –

static
_get_current_error
(current, set_point)¶ Returns an array of differences between the desired joint positions and current joint positions. Useful for PID control.
 Parameters
current (np.array) – the current joint positions
set_point (np.array) – the joint positions that are desired as a numpy array
 Returns
the current error in the joint positions
 Return type
np.array