klampt.control.robotinterface module

The main module for Klampt’s Robot Interface Layer.

class klampt.control.robotinterface.RobotInterfaceBase(**properties)

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():

• parts(), indices(), numJoints(), and jointName(): should be constant. (At the very least, no parts should be destroyed)

• partInterface().

• partToRobotConfig() and robotToPartConfig(): utility methods.

• klamptModel(): if available, must be constant.

• configToKlampt(), configFromKlampt(), velocityToKlampt(), and velocityFromKlampt(): utility methods.

• cartesianPosition(), cartesianVelocity(), and cartesianForce(): utility methods.

• controlRate(): if available, should be constant.

• sensors(), hasSensor(): should be constant. If a sensor can be hot-swapped, it should be in the list of sensors but may appear/ disappear from enabledSensors().

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:

• numJoints() or klamptModel()

• Either clock() or controlRate()

• Either setPosition(), moveToPosition(), setVelocity(), setTorque(), or setPID()

• Either sensedPosition() or commandedPosition()

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:

• Global settings, which overload setSetting() and getSetting(). Unlike properties, settings are expected to be changeable through the life of the interface.

• Sensors, which overload sensors(), enableSensor(), sensorMeasurements(), and sensorUpdateTime().

• State queries, which overload stateValue().

• Control modes, which overload setControlMode().

• Custom calls and event triggers, which overload functionCall()

properties

a dict from string key to property value. Properties are application-dependent and constant through the life of the interface. Examples 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.

• ‘klamptModelCartesianLink’ (int): the link index of the end

  effector in the Klamp’t model.

• ‘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.

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

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

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

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

Type:

dict

initialize()

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()

Cleanly shuts down any resources acquired using initialize().

Return type:

bool

startStep()

Deprecated. use beginStep instead.

Return type:

None

beginStep()

This is called before the commands sent at each time step

Return type:

None

endStep()

This is called after the commands sent at each time step

Return type:

None

numJoints(part=None)

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)

Returns a string naming the given joint

Return type:

str

parts()

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)

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

Return type:

List[int]

partInterface(part, joint_idx=None)

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()

Returns the control rate, in Hz

Return type:

float

clock()

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

Return type:

float

status(joint_idx=None)

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

Return type:

str

estop()

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

Return type:

None

softStop()

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()

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()

Retrieves an implementation-dependent dict of possible settings.

Return type:

Dict[str, Any]

getSetting(name)

Retrieves an implementation-dependent setting.

setSetting(name, value)

Sets an implementation-dependent setting.

Return type:

None

sensors()

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

Return type:

list

enabledSensors()

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

Return type:

list

hasSensor(sensor)

Returns true if the given sensor can be enabled.

Return type:

bool

enableSensor(sensor, enabled=True)

Enables / disables a sensor. Returns true if successful.

Return type:

bool

sensorMeasurements(name)

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

sensorUpdateTime(name)

Returns the clock time of the last sensor update.

Return type:

float

setControlMode(mode, *args, **kwargs)

Enables a custom control mode.

functionCall(proc, *args, **kwargs)

Enables a custom one-off function call.

isMoving(joint_idx=None)

Returns true if the robot / joint are currently moving

Return type:

bool

sensedPosition()

Retrieves the currently sensed joint position.

Return type:

Sequence[float]

sensedVelocity()

Retrieves the currently sensed joint velocity.

Return type:

Sequence[float]

sensedTorque()

Retrieves the currently sensed joint torque.

Return type:

Sequence[float]

commandedPosition()

Retrieves the currently commanded joint position.

Return type:

Sequence[float]

commandedVelocity()

Retrieves the currently commanded joint velocity.

Return type:

Sequence[float]

commandedTorque()

Retrieves the currently commanded joint torque.

Return type:

Sequence[float]

destinationPosition()

Retrieves the destination of a motion queue controller.

Return type:

Sequence[float]

destinationVelocity()

Retrieves the final velocity of a motion queue controller.

Return type:

Sequence[float]

destinationTime()

Retrieves the final clock time of a motion queue controller.

Return type:

float

queuedTrajectory()

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

Return type:

tuple

Returns:

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

Return type:

tuple

stateValue(name)

Retrieves some custom state value

Return type:

Union[float, Sequence[float]]

cartesianPosition(q, frame='world')

Converts from a joint position vector to a cartesian position.

Return type:

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

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

cartesianVelocity(q, dq, frame='world')

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

Return type:

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

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)

cartesianForce(q, t, frame='world')

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

Return type:

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

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)

sensedCartesianPosition(frame='world')

Return type:

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

sensedCartesianVelocity(frame='world')

Return type:

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

sensedCartesianForce(frame='world')

Return type:

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

commandedCartesianPosition(frame='world')

Return type:

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

commandedCartesianVelocity(frame='world')

Return type:

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

commandedCartesianForce(frame='world')

Return type:

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

destinationCartesianPosition(frame='world')

Retrieves the Cartesian destination of a motion queue controller.

Return type:

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

Parameters:

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

destinationCartesianVelocity(frame='world')

Retrieves the final Cartesian velocity of a motion queue controller.

Return type:

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

Parameters:

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

queuedCartesianTrajectory(frame='world')

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

Return type:

tuple

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

setPosition(q)

Sets an instantaneous position command.

Return type:

None

Parameters:

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

setVelocity(v, ttl=None)

Sets an instantaneous velocity command.

Return type:

None

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.

setTorque(t, ttl=None)

Sets a instantaneous torque command.

Return type:

None

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.

setPID(q, dq, t=None)

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

Return type:

None

setPIDGains(kP, kI, kD)

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

Return type:

None

getPIDGains()

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)

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

Return type:

None

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.

setPiecewiseLinear(ts, qs, relative=True)

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

Return type:

None

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().

setPiecewiseCubic(ts, qs, vs)

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

Return type:

None

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().

setToolCoordinates(xtool_local)

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

Return type:

None

getToolCoordinates()

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])

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

Return type:

None

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.

getGravityCompensation()

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

Return type:

tuple

setCartesianPosition(xparams, frame='world')

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.

Return type:

None

Parameters:

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

• commands

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

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

moveToCartesianPosition(xparams, speed=1.0, frame='world')

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.

Return type:

None

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’

moveToCartesianPositionLinear(xparams, speed=1.0, frame='world')

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.

Return type:

None

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’

setCartesianVelocity(dxparams, ttl=None, frame='world')

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

Return type:

None

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’

setCartesianForce(fparams, ttl=None, frame='world')

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

Return type:

None

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’

partToRobotConfig(pconfig, part, robotConfig, joint_idx=None)

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)

Retrieves a part’s configuration from a robot configuration

Return type:

Sequence[float]

klamptModel()

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)

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)

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

Return type:

Sequence[float]

configToKlampt(config, klamptConfig=None, part=None, joint_idx=None)

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. :rtype: Sequence[float]

Note

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

velocityToKlampt(velocity, klamptVelocity=None, part=None, joint_idx=None)

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. :rtype: Sequence[float]

Note

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

wait(timeout=None, condition=None, pollRate=None)

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

Return type:

float

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