Robot Interfaces

Interface for obtaining robot state and sending robot control commands. RobotInterfaces are specific to the world they inhabit, as well as the type of robot. They contain Controllers for converting desired goal states to a set of joint torques.

RobotInterface

class perls2.robots.robot_interface.RobotInterface(config, controlType=None)

Abstract interface to be implemented for each real and simulated robot.

action_set

flag describing if action for controller has been set.

Type

bool

config

Dictionary describing robot parameters.

Type

dict

control_type

Type of controller for robot to use e.g. “EEImpedance”, “JointVelocity”

Type

str

interpolator_pos

Interpolator for ee/joint position

Type

perls2.interpolator.linear_interpolator

interpolator_ori

Interpolator for ee orientation

Type

perls2.interpolator.linear_interpolator

model

a model of the robot state as defined by tq_control.

Type

Model

controller

tq_control object that takes robot states and compute torques.

Type

Controller

step()

Update the robot state and model, set torques from controller.

change_controller(next_type)

Change to a different controller type.

Parameters

next_type (str) – keyword for desired control type. Choose from: -‘EEImpedance’ -‘EEPosture’ -‘JointVelocity’ -‘JointImpedance’ -‘JointTorque’

update()

Update robot interface model with states for controller.

update_model()

Update robot model with states and dynamics.

make_controller(control_type)

Returns a new controller type based on specs.

Parameters

control_type (str) – name of the control type.

set_controller_goal(**kwargs)

Update model and set the goal for controller.

Parameters

kwargs (dict) – arguments specific to controller for setting goal.

move_ee_delta(delta, set_pos=None, set_ori=None)

Use controller to move end effector by some delta.

Parameters
  • delta (6f) – delta position (dx, dy, dz) concatenated with delta orientation. Orientation change is specified as an axis angle rotation from previous orientation.

  • set_pos (3f) – end effector position to maintain while changing orientation. [x, y, z]. If not None, the delta for position is ignored.

  • set_ori (4f) – end effector orientation to maintain while changing orientation as a quaternion [qx, qy, qz, w]. If not None, any delta for orientation is ignored.

Notes

To fix position or orientation, it is better to specify using the kwargs than to use a 0 for the corresponding delta. This prevents any error from accumulating in that dimension.

Examples

Specify delta holding fixed orientation:: self.robot_interface.move_ee_delta([0.1, 0, 0, 0, 0, 0], set_pos=None, set_ori=[0, 0, 0, 1])

set_ee_pose(set_pos, set_ori, **kwargs)

Use controller to set end effector pose.

Parameters
  • set_pos (list) – 3f [x, y , z] desired absolute ee position in world frame.

  • set_ori (list) – 4f [qx, qy, qz, w] desired absolute ee orientation in world frame.

  • **kwargs (dict) – used to catch all other arguments.

Returns

None

Notes: Only for use with EEImpedance and EEPosture controller.

set_joint_positions(set_qpos, **kwargs)

Use controller to set new joint positions.

Parameters

set_qpos – 7f absolute joint positions (rad)

Returns

None

Notes: Only for use with Joint Impedance Controller

set_joint_delta(delta, **kwargs)

Use controller to set new joint position with a delta.

Parameters

delta (ndarray) – 7f delta joint position (rad) from current joint position.

Returns: None

Notes: Only for use with JointImpedance controller.

Does not check for exceeding maximum joint limits. (TODO)

set_joint_velocities(velocities)

Use controller to set joint velocity of the robot.

Parameters
  • velocities (list) – 7f desired joint velocities (rad/s) for each joint.

  • 0 is the base (Joint) –

Returns: None.

Notes: Only for use with JointVelocity controller.

set_joint_torques(torques)

Set joint torques to new joint torques.

Parameters

torques (list) – 7f list of toruqes to command.

Notes

Only for use with Joint Torque Controlle

abstract open_gripper()

Open robot gripper.

abstract close_gripper()

Close robot gripper

abstract set_gripper_to_value(value)

Set gripper to desired open/close value

abstract property name

str of the name of the robot

abstract property ee_pose

list of seven floats [x, y, z, qx, qy, qz, qw] of the 6D pose of the end effector.

abstract property ee_position

list of three floats [x, y, z] of the position of the end-effector.

abstract property ee_orientation

list of four floats [qx, qy, qz, qw] of the orientation quaternion of the end-effector.

abstract property ee_v

list of seven floats [x, y, z, qx, qy, qz, qw] of the 6D pose of the end effector.

abstract property ee_w

list of seven floats [x, y, z, qx, qy, qz, qw] of the 6D pose of the end effector.

abstract property ee_twist

list of seven floats [x, y, z, qx, qy, qz, qw] of the 6D pose of the end effector.

abstract property q

List of 7f describing joint positions (rad) of the robot arm.

Ordered from base to end_effector

abstract property dq

List of 7f describing joint velocities (rad/s) of the robot arm.

Ordered from base to end_effector

abstract property jacobian

List of 7f describing joint velocities (rad/s) of the robot arm.

Ordered from base to end_effector

abstract property linear_jacobian

List of 7f describing joint velocities (rad/s) of the robot arm.

Ordered from base to end_effector

abstract property angular_jacobian

List of 7f describing joint velocities (rad/s) of the robot arm.

Ordered from base to end_effector

abstract property mass_matrix

List of 7f describing joint velocities (rad/s) of the robot arm.

Ordered from base to end_effector

BulletRobotInterface

Extends RobotInterface functionality for BulletWorlds to be used wity PyBullet. Connects to pybullet physics engine to obtain robot state and dynamics parameters as well as execute torque control.

class perls2.robots.bullet_robot_interface.BulletRobotInterface(physics_id, arm_id, config, controlType='JointVelocity')

Abstract class that extends RobotInterface for BulletWorlds.

physics_id

id for PyBullet physics client.

Type

int

arm_id

id for robot arm in PyBullet. Returned by pb.loadURDF

Type

int

config

config parameters for the perls2 env.

Type

dict

robot_cfg

config parameters specific to the robot.

Type

dict

BulletSawyerInterface

Extends BulletRobotInterface for functionality specific to Rethink Sawyer Arms.

class perls2.robots.bullet_sawyer_interface.BulletSawyerInterface(physics_id, arm_id, config, controlType='JointVelocity')

Class for Sawyer Robot Interface in Pybullet. This class provides the functionsfor information about the state of the robot as well as sending commands.

BulletPandaInterface

Extends BulletRobotInterface for functionality specific to Franka Panda arms.

class perls2.robots.bullet_panda_interface.BulletPandaInterface(physics_id, arm_id, config, controlType='EEImp')

Class for Panda Robot Interface in Pybullet. This class provides the functions for information about the state of the robot as well as sending commands.

physics_id

unique identifer for pybullet sim.

Type

int

arm_id

unique identifier produced by pybullet to id robot.

Type

int

config

dictionary with configuration params for robot

Type

dict

controlType

id for controlType (‘osc’, ‘joint_space’)

Type

str

limb_neutral_positiosn

list of joint angles for default

Type

list

RealRobotInterface

Extends RobotInterface for Real Robots.

RealSawyerInterface

Extends RealRobotInterface for REthink Sawyer Arms

class perls2.robots.real_sawyer_interface.RealSawyerInterface(config, use_safenet=True, use_moveit=True, controlType='EEImpedance', node_name='sawyer_interface')

Abstract interface to be implemented for each real and simulated robot.

RealPandaInterface

Extends RealRobot Interface for Franka Panda Arms

class perls2.robots.real_panda_interface.RealPandaInterface(config, controlType='EEImpedance')

Abstract interface to be implemented for each real and simulated robot.