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:

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()
while interface.status() == 'ok':  #no error handling done here...
    t0 = time.time()
    interface.startStep()
    [any getXXX or setXXX commands here comprising the control loop]
    interface.endStep()
    t1 = time.time()
    telapsed = t1 - t0
    [wait for time max(dt - telapsed,0)]
interface.close()   #cleanly shut down the interface

To accept asynchronous commands, a RobotInterfaceBase subclass can be passed to AsynchronousRobotInterface or RobotInterfaceServer.

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 controlled separately. You may retrieve part names using parts(), part indices using indices(), and access a RIL interface to a part using partController().

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

Functionalities

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 SimPositionControlInterface class for an example that passes commands to a Klamp’t physics simulator.

properties

a dict from string key to property value. Application dependent. Examples may include:

• ‘name’: str

• ‘version’: str

• ‘simulated’: bool

• ‘klamptModelFile’: str

Type

dict

cartesianForce(q, t)

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

cartesianPosition(q)

Converts from a joint position vector to a cartesian position

cartesianVelocity(q, dq)

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

clock()

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

close()

Cleanly shuts down any resources acquired using initialize().

commandedCartesianForce()

commandedCartesianPosition()

commandedCartesianVelocity()

commandedPosition()

Retrieves the currently commanded joint position.

commandedTorque()

Retrieves the currently commanded joint torque.

commandedVelocity()

Retrieves the currently commanded joint velocity.

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.

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.

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

controlRate()

Returns the control rate, in Hz

destinationCartesianPosition()

Retrieves the Cartesian destination of a motion queue controller

destinationCartesianVelocity()

Retrieves the final Cartesian velocity of a motion queue controller

destinationPosition()

Retrieves the destination of a motion queue controller.

destinationTime()

Retrieves the final clock time of a motion queue controller.

destinationVelocity()

Retrieves the final velocity of a motion queue controller.

enableSensor(sensor, enabled=True)

Enables / disables a sensor. Returns true if successful.

enabledSensors()

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

endStep()

This is called after the commands sent at each time step

estop()

Calls an emergency stop on the robot

getToolCoordinates()

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

hasSensor(sensor)

Returns true if the given sensor can be enabled.

indices(part=None, joint_idx=None)

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

initialize()

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

isMoving(part=None, joint_idx=None)

Returns true if the part / joint are currently moving

jointName(joint_idx)

Returns a string naming the given joint

klamptModel()

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

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

moveToCartesianPosition(xparams, speed=1.0)

Sets a Cartesian move-to-position command.

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.

moveToPosition(q, speed=1.0)

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.

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.

partInterface(part=None, 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).

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

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

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.

queuedCartesianTrajectory()

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

Returns

either (ts,Ts) or (ts,Ts,dTs) representing a piecewise linear or a piecewise-cubic trajectory.

Return type

tuple

queuedTrajectory()

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

reset()

If the robot has a non-normal status code, attempt to reset it to normal operation. Returns true on success, false on failure.

robotToPartConfig(robotConfig, part, joint_idx=None)

Retrieves a part’s configuration from a robot configuration

sensedCartesianForce()

sensedCartesianPosition()

sensedCartesianVelocity()

sensedPosition()

Retrieves the currently sensed joint position.

sensedTorque()

Retrieves the currently sensed joint torque.

sensedVelocity()

Retrieves the currently sensed joint velocity.

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.

sensors()

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

setCartesianForce(fparams, ttl=None)

Sets a Cartesian torque command.

Parameters

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

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

setCartesianPosition(xparams)

Sets a Cartesian position command. Like setPosition, this command is sent in immediate mode.

Parameters

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

setCartesianVelocity(dxparams, ttl=None)

Sets a Cartesian velocity command.

Parameters

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

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

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.

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.

setPID(q, dq, t=None)

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

setPIDGains(kP, kI, kD)

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

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.

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

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.

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

setPosition(q)

Sets an instantaneous position command.

Parameters

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

setToolCoordinates(xtool_local)

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

setTorque(t, ttl=None)

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.

setVelocity(v, ttl=None)

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.

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.

startStep()

This is called before the commands sent at each time step

status(part=None, joint_idx=None)

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

velocityFromKlampt(klamptVelocity, part=None, joint_idx=None)

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

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.

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