klampt.control.robotinterface module

The main module for Klampt’s Robot Interface Layer.

class klampt.control.robotinterface.RobotInterfaceBase(**properties)[source]

Bases: object

The main class for the Klamp’t Robot Interface Layer. Defines a unifying API to interface with a robot’s motor controller, whether it’s simulated or a real robot.

Note

The API may look intimidating, but a subclass implementer is free to set up as many or as few of the given methods as the robot’s motor controller truly implements. The RobotInterfaceCompleter class will fill in the remainder of derived methods. See the Functionalities section for more details.

Each of these methods should be synchronous calls, called at a single time step. The calling convention is:

from klampt.control.utils import TimedLooper

interface = MyRobotInterface(...args...)
if not interface.initialize():  #should be called first
    raise RuntimeError("There was some problem initializing interface "+str(interface))

dt = 1.0/interface.controlRate()
looper = TimedLooper(dt)
while looper:           #this will run as close to the control rate as possible
    try:
        interface.beginStep()
        if interface.status() != 'ok':  #no error handling done here... could also try interface.reset()
            raise RuntimeError("Some error occurred: {}".format(interface.status()))
        [do any state queries or commands here comprising the control loop]
        interface.endStep()
    except Exception as e:
        print("Terminating on exception:",e)
        looper.stop()

interface.close()   #cleanly shut down the interface

To accept asynchronous commands, a RobotInterfaceBase subclass can be wrapped by a ThreadedRobotInterface or client (e.g., XMLRPCClientRobotInterface). Asynchronous usage allows sending commands in a procedural fashion, e.g.:

interface = MyRobotInterface(...args...)
if not interface.initialize():  #should be called first
    raise RuntimeError("There was some problem initializing interface "+str(interface))

time.sleep(whatever)
[any state queries or commands here]

interface.moveToPosition(qtarget)
poll_rate = 0.05
while interface.isMoving()
    time.sleep(poll_rate)

if interface.status() != 'ok':  #no error handling done here... could also try interface.reset()
    raise RuntimeError("Some error occurred: {}".format(interface.status()))

time.sleep(whatever)
[any state queries or commands here]

interface.close()   #cleanly shut down the interface

DOFs and Parts

The number of DOFs is assumed equal to the number of joint actuators / encoders. If the robot has fewer actuators than encoders, the commands for unactuated joints should just be ignored. If the robot corresponds to a Klampt model (typical), then the number of DOFs should be model.numDrivers()

A robot can have “parts”, which are named groups of DOFs. For example, a robot with a gripper can have parts “arm” and “gripper”, which can be potentially controlled separately. You may retrieve part names using parts(), part indices using indices(), and (potentially) access a RIL interface to a part using partInterface().

It is suggested that these parts correspond with parts in the robot’s RobotInfo structure.

Semantics of methods

In the strictest sense, no methods are assumed to be available before initialize() is called (and returns True) .

The following methods should be available after initialize() and do not need to be protected by beginStep() / endStep():

Most implementations will have these methods available even before initialize() is called, but to be safe a caller shouldn’t count on it. For example, if a robot tests to see which parts are connected at runtime, then parts(), numJoints(), klamptModel(), etc will not be available until afterwards.

All other methods should be placed within a beginStep() / endStep() pair.

Minimal functionality (For implementers)

There are a few functions your subclass will need to fill out:

Pass your RobotInterfaceBase subclass to RobotInterfaceCompleter to complete the implementation of as many of the remaining items as possible.

See the simrobotinterface.SimPositionControlInterface class for an example that passes commands to a Klamp’t physics simulator.

Cartesian control

Cartesian position, velocity, and force control may be implemented. The interpretation of these arguments is implementation-dependent, but we support two standard modes: 6DOF position/orientation and 3DOF position-only mode.

The reference frame for these quantities is specified by a frame argument, which is ‘world’ by default:

  • ‘world’: The world frame. Equal to ‘base’ for a top-level robot, but

    differs if this interface refers to a part of a larger robot.

  • ‘base’: The base of this part. Equal to ‘world’ for a top-level robot,

    but differs if this interface refers to a part of a larger robot.

  • ‘end effector’: relative to the origin of the end effector frame.

  • ‘tool’: relative to the current tool frame (the tool is an offset from

    the end effector frame)

Note that an interface might only support commands to one type of frame, and raise NotImplementedError() for other frames.

Real-time filters

Filters perform real-time enforcement of joint limits, velocity limits, collision checks, and signal processing. Typically filters are handled in software using a robotinterfaceutils.RobotInterfaceCompleter but an implementation is also allowed to send filter parameters to the hardware.

Users can activate and deactivate filters using the setFilter() method. A filter has an name, operation, input arguments, and output arguments, and its operation will usually yield an output or raise an exception, e.g., if a commanded position / velocity exceeds the limits, it can raise a FilterSoftStop exception, or it can just alter arguments, e.g., clamping to valid ranges.

Common examples include the following (using filter operations available in robotinterfaceutils):

  • setFilter('joint_limit',LimitClampFilter(qmin,qmax)): clamps commanded position to limits.

  • setFilter('joint_limit',LimitStopFilter(qmin,qmax)): stops when commanded position exceeds limits.

  • setFilter('velocity_limit',LimitClampFilter(qmin,qmax)): clamps commanded velocity to limits.

  • setFilter('velocity_limit',LimitStopFilter(qmin,qmax)): stops when commanded velocity exceeds limits.

  • setFilter('collision',CollisionStopFilter(collider)): stops when the robot is about to collide.

  • setFilter('position_tracking',DifferenceStopFilter(math.radians(15))): stops when the tracking error between the commanded and sensed position exceeds 15 radians.

  • setFilter('position_delta',DifferenceStopFilter(math.radians(5))): stops when the commanded position jumps more than 5 radians in a single time step.

  • setFilter(SENSOR_NAME,FIRFilter(b),output=OUT_NAME): adds a FIR filter to sensor SENSOR_NAME and outputs it to OUT_NAME.

  • setFilter('custom_filter', block, ['sensedPosition','sensedVelocity'], 'positionCommand'): sets a custom filter that uses a Block of two arguments (sensed position and velocity) and outputs one value (a position command).

To disable a filter, pass None as the second argument.

Customization

Certain controllers will provide custom API methods that fall outside of the standard API. Although an RIL subclass is free to implement any methods desired, for maximum compatibility with utility classes (e.g., RobotInterfaceCompleter), the subclass should provide uniform access to these custom methods through the following standard methods:

properties

a dict from string key to property value. Properties are application-dependent and constant through the life of the interface. Examples may include:

  • ‘name’ (str): the name of the robot

  • ‘version’ (str): a version of this interface

  • ‘simulated’ (bool): whether the “robot” is simulated vs physical

  • ‘klamptModelFile’ (str): the file name of the Klamp’t model file.

  • ‘asynchronous’ (bool): if True, beginStep/endStep are not needed to communicate with the robot. Networked controllers are often asynchronous.

  • ‘complete’ (bool): if True, all methods are implemented.

  • ‘part’ (bool): if True, this is not a top-level interface.

  • ‘joint_limits’ (pair of Vector): the hardware joint limits, not overridable by software limits.

  • ‘velocity_limits’ (pair of Vector): the hardware velocity limits, not overridable by software limits.

  • ‘acceleration_limits’ (pair of Vector): the hardware accel limits, not overridable by software limits.

  • ‘torque_limits’ (pair of Vector): the hardware torque limits, not overridable by software limits.

Type

dict

initialize()[source]

Tries to connect to the robot. Returns true if ready to send commands. This should probably be the first method called.

Return type

bool

close()[source]

Cleanly shuts down any resources acquired using initialize().

Return type

bool

startStep()[source]

Deprecated. use beginStep instead.

Return type

None

beginStep()[source]

This is called before the commands sent at each time step

Return type

None

endStep()[source]

This is called after the commands sent at each time step

Return type

None

numJoints(part=None)[source]

Returns the number of joints of the given part. By default, this returns the number of actuated DOFs in the Klamp’t model.

Return type

int

jointName(joint_idx)[source]

Returns a string naming the given joint

Return type

str

parts()[source]

Returns a dictionary of (part-name,configuration index list) pairs defining the named parts of the robot.

Since this will be used a lot, make sure to declare your override with @functools.lru_cache.

Return type

Dict[Any, List[int]]

indices(part=None, joint_idx=None)[source]

Helper: returns a list of indices for the given part / joint index

Return type

List[int]

partInterface(part, joint_idx=None)[source]

Returns a RobotInterfaceBase that allows control of the given part/joint. If no such controller exists, raises a NotImplementedError.

The part/joint controller should operate on exactly the DOFs specified by self.indices(part,joint_idx).

Return type

RobotInterfaceBase

controlRate()[source]

Returns the control rate, in Hz

Return type

float

clock()[source]

Returns the current time on the robot’s clock, in seconds

Return type

float

status(joint_idx=None)[source]

Returns a status string for the robot / given joint. ‘ok’ means everything is OK.

Return type

str

estop()[source]

Calls an emergency stop on the robot. Default uses the soft stop.

Return type

None

softStop()[source]

Calls a software E-stop on the robot (braking as quickly as possible). Default implementation stops robot at current position; a better solution would slow the robot down.

Return type

None

reset()[source]

If the robot has a non-normal status code, attempt to reset it to normal operation. The caller should poll until status()==’ok’

Return type

None

getSettings()[source]

Retrieves an implementation-dependent dict of possible settings.

Return type

Dict[str, Any]

getSetting(name)[source]

Retrieves an implementation-dependent setting.

setSetting(name, value)[source]

Sets an implementation-dependent setting.

Return type

None

sensors()[source]

Returns a list of names of possible sensors on this robot.

Return type

list

enabledSensors()[source]

Returns a list of names of enabled sensors on this robot.

Return type

list

hasSensor(sensor)[source]

Returns true if the given sensor can be enabled.

Return type

bool

enableSensor(sensor, enabled=True)[source]

Enables / disables a sensor. Returns true if successful.

Return type

bool

sensorMeasurements(name)[source]

Returns the latest measurements from a sensor. Interpretation of the result is sensor-dependent.

sensorUpdateTime(name)[source]

Returns the clock time of the last sensor update.

Return type

float

setControlMode(mode, *args, **kwargs)[source]

Enables a custom control mode.

functionCall(proc, *args, **kwargs)[source]

Enables a custom one-off function call.

isMoving(joint_idx=None)[source]

Returns true if the robot / joint are currently moving

Return type

bool

sensedPosition()[source]

Retrieves the currently sensed joint position.

Return type

Sequence[float]

sensedVelocity()[source]

Retrieves the currently sensed joint velocity.

Return type

Sequence[float]

sensedTorque()[source]

Retrieves the currently sensed joint torque.

Return type

Sequence[float]

commandedPosition()[source]

Retrieves the currently commanded joint position.

Return type

Sequence[float]

commandedVelocity()[source]

Retrieves the currently commanded joint velocity.

Return type

Sequence[float]

commandedTorque()[source]

Retrieves the currently commanded joint torque.

Return type

Sequence[float]

destinationPosition()[source]

Retrieves the destination of a motion queue controller.

Return type

Sequence[float]

destinationVelocity()[source]

Retrieves the final velocity of a motion queue controller.

Return type

Sequence[float]

destinationTime()[source]

Retrieves the final clock time of a motion queue controller.

Return type

float

queuedTrajectory()[source]

Returns a trajectory starting from the current time representing all commands in a motion queue controller.

Returns

either (ts,qs) or (ts,qs,vs) representing a piecewise linear or a piecewise-cubic trajectory.

Return type

tuple

Return type

tuple

stateValue(name)[source]

Retrieves some custom state value

Return type

Union[float, Sequence[float]]

cartesianPosition(q, frame='world')[source]

Converts from a joint position vector to a cartesian position.

Parameters
  • q (vector) – the (whole) robot’s joint positions.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’. Note: ‘end effector’ and ‘tool’ don’t make any sense here, since the tool frame is constant relative to these frames…

Returns

specifies end effector Cartesian transform relative to the given frame.

Return type

Klampt se3 element

Return type

Tuple[Sequence[float], Sequence[float]]

cartesianVelocity(q, dq, frame='world')[source]

Converts from a joint position / velocity vector to a cartesian velocity.

Parameters
  • q (vector) – the (whole) robot’s joint positions.

  • dq (vector) – the (whole) robot’s joint velocities.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’. Note: if ‘base’ is specified, the velocity of the base is subtracted from the reported speed.

Returns

specifies end effector Cartesian angular velocity/velocity relative to the given frame.

Return type

(w,v)

Return type

Tuple[Sequence[float], Sequence[float]]

cartesianForce(q, t, frame='world')[source]

Converts from a joint position / torque vector to a cartesian force.

Parameters
  • q (vector) – the (whole) robot’s joint positions.

  • t (vector) – the (whole) robot’s joint torques.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’.

Returns

specifies end effector Cartesian torque/force relative to given frame.

Return type

(t,f)

Return type

Tuple[Sequence[float], Sequence[float]]

sensedCartesianPosition(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

sensedCartesianVelocity(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

sensedCartesianForce(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

commandedCartesianPosition(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

commandedCartesianVelocity(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

commandedCartesianForce(frame='world')[source]
Return type

Tuple[Sequence[float], Sequence[float]]

destinationCartesianPosition(frame='world')[source]

Retrieves the Cartesian destination of a motion queue controller.

Parameters

frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

Tuple[Sequence[float], Sequence[float]]

destinationCartesianVelocity(frame='world')[source]

Retrieves the final Cartesian velocity of a motion queue controller.

Parameters

frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

Tuple[Sequence[float], Sequence[float]]

queuedCartesianTrajectory(frame='world')[source]

Returns the Cartesian trajectory starting from the current time representing all commands in a motion queue controller.

Parameters

frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Returns

either (ts,Ts) or (ts,Ts,dTs) representing a piecewise linear or a piecewise-cubic trajectory. Ts is a list of Klampt se3 elements. dTs is a list of (angular velocity,velocity) pairs.

Return type

tuple

Return type

tuple

setPosition(q)[source]

Sets an instantaneous position command.

Parameters

q (list of floats) – A list of floats giving the desired configuration of the robot.

Return type

None

setVelocity(v, ttl=None)[source]

Sets an instantaneous velocity command.

Parameters
  • v (list of floats) – A list of floats giving the desired velocity of each joint.

  • ttl (float, optional) – A time-to-live for this command.

Return type

None

setTorque(t, ttl=None)[source]

Sets a instantaneous torque command.

Parameters
  • t (list of floats) – A list of floats giving the desired torques at each joint.

  • ttl (float, optional) – A time-to-live for this command.

Return type

None

setPID(q, dq, t=None)[source]

Sets a PID command to configuration q, velocity dq, and feedforward torque t.

Return type

None

setPIDGains(kP, kI, kD)[source]

Sets the PID gains. Some controllers might not implement this even if they implement setPID…

Return type

None

getPIDGains()[source]

Gets the PID gains (kP,kI,kD) as set to a prior call to setPIDGains. Some controllers might not implement this even if they implement setPIDGains…

Return type

Tuple[List[float], List[float], List[float]]

moveToPosition(q, speed=1.0)[source]

Sets a move-to position command. The trajectory that the robot will take on should be extractable through getMoveToTrajectory(q).

Parameters
  • q (list of floats) – A list of floats giving the desired configuration of the robot.

  • speed (float, optional) – The speed at which the position should be reached.

Return type

None

setPiecewiseLinear(ts, qs, relative=True)[source]

Tells the robot to start a piecewise linear trajectory command. The first milestone will be interpolated from the current commanded configuration.

Parameters
  • ts (list of floats) – times of the trajectory’s milestones

  • qs (list of floats, or list of list of floats) – list of the trajectory’s milestones

  • relative (bool) – if true, the times in ts are assumed to start at current time 0. Otherwise, they must all be greater than the current time retrieved by clock().

Return type

None

setPiecewiseCubic(ts, qs, vs)[source]

Tells the robot to start a piecewise cubic trajectory command. The first milestone will be interpolated from the current commanded configuration / velocity.

Parameters
  • ts (list of floats) – times of the trajectory’s milestones

  • qs (list of floats, or list of list of floats) – list of the trajectory’s milestones

  • vs (list of floats, or list of list of floats) – list of the trajectory’s derivatives at the milestones.

  • relative (bool) – if true, the times in ts are assumed to start at current time 0. Otherwise, they must all be greater than the current time retrieved by clock().

Return type

None

setToolCoordinates(xtool_local)[source]

Sets the tool coordinates of this robot relative to its end effector link.

Return type

None

getToolCoordinates()[source]

Gets the tool coordinates of this robot relative to its end effector link.

Return type

Sequence[float]

setGravityCompensation(gravity=[0, 0, - 9.8], load=0.0, load_com=[0, 0, 0])[source]

Sets up gravity compensation with a given gravity vector and end effector load.

Parameters
  • gravity (list of 3 floats, optional) – the gravity vector in the base frame, in m/s^2.

  • load (float, optional) – a weight attached to the end effector, in kg.

  • load_com (list of 3 floats, optional) – the COM of the load, expressed relative to the end-effector link frame.

Return type

None

getGravityCompensation()[source]

Gets (gravity,load,load_com) as from a prior call to setGravityCompensation.

Return type

tuple

setCartesianPosition(xparams, frame='world')[source]

Sets a Cartesian position command. The tool is commanded to reach the given coordinates relative to the given frame. Like setPosition, this command is sent in immediate mode.

Parameters
  • xparams – a klampt.math.se3 object for position / orientation

  • or a 3-vector for position-only. (commands,) –

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

None

moveToCartesianPosition(xparams, speed=1.0, frame='world')[source]

Sets a Cartesian move-to-position command. The tool is commanded to reach the given coordinates relative to the given frame. The movement is accomplished via an arbitrary joint space motion, and is not necessarily linear in Cartesian space.

Parameters
  • xparams – typically a klampt.math.se3 object for position / orientation commands, or a 3-vector for position-only.

  • speed (float, optional) – The speed at which the position should be reached.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

None

moveToCartesianPositionLinear(xparams, speed=1.0, frame='world')[source]

Sets a Cartesian move-to-position command. The tool is commanded to reach the given coordinates relative to the given frame via a linear Cartesian path.

Parameters
  • xparams – typically a klampt.math.se3 object for position / orientation commands, or a 3-vector for position-only.

  • speed (float, optional) – The speed at which the position should be reached.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

None

setCartesianVelocity(dxparams, ttl=None, frame='world')[source]

Sets a Cartesian velocity command. The tool is commanded to move along the given velocity(s) relative to the given frame.

Parameters
  • dxparams – typically an (angular velocity, translational velocity) pair for translation / orientation commands. A 3-vector for translation-only.

  • ttl (float, optional) – A time-to-live for this command.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

None

setCartesianForce(fparams, ttl=None, frame='world')[source]

Sets a Cartesian torque command. The tool is commanded to exert the given force or (torque, force) relative to the given frame.

Parameters
  • fparams – typically an (torque, force) pair for translation / orientation commands. A 3-vector for translation-only.

  • ttl (float, optional) – A time-to-live for this command.

  • frame (str) – either ‘world’, ‘base’, ‘end effector’, or ‘tool’

Return type

None

partToRobotConfig(pconfig, part, robotConfig, joint_idx=None)[source]

Fills a configuration vector for the whole robot given the configuration pconfig for a part

Return type

Sequence[float]

robotToPartConfig(robotConfig, part, joint_idx=None)[source]

Retrieves a part’s configuration from a robot configuration

Return type

Sequence[float]

klamptModel()[source]

If applicable, returns the Klamp’t RobotModel associated with this controller. Default tries to load from properties[‘klamptModelFile’].

Note that the configuration DOFs in the RIL robot correspond to the robot’s DRIVERs, not its links.

Note: the result of the default implementation is cached, so this can be called multiple times without a performance hit.

Return type

Union[RobotModel, SubRobotModel, None]

configFromKlampt(klamptConfig, part=None, joint_idx=None)[source]

Extracts a RobotInterfaceBase configuration from a configuration of the Klampt model.

Note: the configuration of the model in self.klamptModel() is overridden.

Return type

Sequence[float]

velocityFromKlampt(klamptVelocity, part=None, joint_idx=None)[source]

Extracts a RobotInterfaceBase velocity from a velocity of the Klampt model.

Return type

Sequence[float]

configToKlampt(config, klamptConfig=None, part=None, joint_idx=None)[source]

Creates a configuration vector for the Klamp’t model using the RobotInterfaceBase configuration.

If klamptConfig is given, then these values are used for the non-part configuration values. Otherwise, the robot’s current configuration from self.klamptModel() is used.

Note

The configuration of the model in self.klamptModel() is overridden.

Return type

Sequence[float]

velocityToKlampt(velocity, klamptVelocity=None, part=None, joint_idx=None)[source]

Creates a velocity vector for a Klamp’t model using the joint velocity.

If klamptVelocity is given, then these values are used for the non-part configuration values. Otherwise, the robot’s current velocity from self.klamptModel() is used.

Note

The velocity of the model in self.klamptModel() is overridden.

Return type

Sequence[float]

wait(timeout=None, condition=None, pollRate=None)[source]

Sleeps the caller until isMoving() returns False. This should only be called outside of a beginStep()/endStep() pair.

Parameters
  • timeout (float, optional) – if given, will break after this amount of time has elapsed (in s)

  • condition (callable, optional) – if given, overrides the test self.isMoving(). Instead, will stop when condition() returns True.

  • pollRate (float, optional) – time between isMoving checks, in Hz. If None, polls at the controller’s natural rate.

Note

pollRate should only be non-None if the interface is asynchronous. Otherwise, steps may be skipped.

Returns

the approximate time in s that was waited.

Return type

float

Return type

float