klampt.control.robotinterfaceutils module

Contains utilities for the Klampt Robot Interface Layer. These help implement:

  • Generalization of simple interfaces to handle advanced functions, e.g., motion queues and Cartesian controllers (RobotInterfaceCompleter ). This is useful when you have a position controlled robot (e.g., an Arduino- controlled set of motors) and want to do more sophisticated work with it.

  • Robots composed of separate independent parts MultiRobotInterface, such as a robot arm + gripper, each working on separate interfaces.

  • Logging… TODO

  • Interface with the Klampt ControllerBase controller API.

Classes:

MultiRobotInterface()

A RobotInterfaceBase that consists of multiple parts that are addressed separately.

RobotInterfaceCompleter(baseInterface)

Completes as much of the RobotInterfaceBase API as possible from a partially specified RobotInterfaceBase.

class klampt.control.robotinterfaceutils.MultiRobotInterface[source]

Bases: klampt.control.robotinterface.RobotInterfaceBase

A RobotInterfaceBase that consists of multiple parts that are addressed separately. For example, a mobile manipulator can consist of a base, arms, and grippers.

On startup, call addPart(name,interface,klamptModel,klamptIndices) for each part of the robot.

Note

TIP: wrap your interface with a RobotInterfaceCompleter before adding it so that all parts get a uniform interface.

The total configuration for the robot is a list of floats, segmented into parts. The ordering of the parts’ configurations is the same as how they were added.

Methods:

addPart(partName, partInterface[, …])

Adds a part partName with the given interface partInterface.

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.

commandedVelocity()

Retrieves the currently commanded joint velocity.

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.

destinationVelocity()

Retrieves the final velocity of a motion queue controller.

enableSensor(sensor)

Enables / disables a sensor.

enabledSensors()

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

endStep()

This is called after the commands sent at each time step

getPartInterface([part, joint_idx])

hasSensor(sensor)

Returns true if the given sensor can be enabled.

initialize()

Tries to connect to the robot.

isMoving()

Returns true if the part / joint are currently moving

join(qparts)

Joins a bunch of parts into a whole-body robot.

jointName(joint_idx[, part])

Returns a string naming the given joint

numJoints([part])

Returns the number of joints of the given part.

parts()

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

reset()

If the robot has a non-normal status code, attempt to reset it to normal operation.

sensedCartesianForce()

sensedCartesianPosition()

sensedCartesianVelocity()

sensedPosition()

Retrieves the currently sensed joint position.

sensedTorque()

Retrieves the currently sensed joint torque.

sensedVelocity()

Retrieves the currently sensed joint velocity.

sensors()

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

setCartesianForce(fparams[, ttl])

Sets a Cartesian torque command.

setCartesianPosition(xparams[, immediate])

Sets a Cartesian position command.

setCartesianVelocity(dxparams[, ttl])

Sets a Cartesian velocity command.

setPID(q, dq[, t])

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

setPIDGains(kP, kI, kD)

Sets the PID gains.

setPiecewiseCubic()

Tells the robot to start a piecewise cubic trajectory command.

setPiecewiseLinear()

Tells the robot to start a piecewise linear trajectory command.

setPosition(q[, immediate])

Sets an instantaneous position command.

setTorque(t[, ttl])

Sets a instantaneous torque command.

setVelocity(v[, ttl])

Sets an instantaneous velocity command.

split(q)

Splits a whole-body robot to parts (one per listed item).

startStep()

This is called before the commands sent at each time step

status()

Returns a status string for the given part / joint.

addPart(partName, partInterface, klamptModel=None, klamptIndices=None)[source]

Adds a part partName with the given interface partInterface. The DOFs of the unified robot range from N to N+Np where N is the current number of robot DOFs and Np is the number of part DOFs.

If klamptModel / klamptIndices are given, The part is associated with the given klamptModel, and the indices of the part’s sub-model are given by klamptIndices. (The indices of partInterface are indices into klamptIndices, not klamptModel )

cartesianForce(q, t)[source]

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

cartesianPosition(q)[source]

Converts from a joint position vector to a cartesian position

cartesianVelocity(q, dq)[source]

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

clock()[source]

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

close()[source]

Cleanly shuts down any resources acquired using initialize().

commandedCartesianForce()[source]
commandedCartesianPosition()[source]
commandedCartesianVelocity()[source]
commandedPosition()[source]

Retrieves the currently commanded joint position.

commandedVelocity()[source]

Retrieves the currently commanded joint velocity.

controlRate()[source]

Returns the control rate, in Hz

destinationCartesianPosition()[source]

Retrieves the Cartesian destination of a motion queue controller

destinationCartesianVelocity()[source]

Retrieves the final Cartesian velocity of a motion queue controller

destinationPosition()[source]

Retrieves the destination of a motion queue controller.

destinationVelocity()[source]

Retrieves the final velocity of a motion queue controller.

enableSensor(sensor)[source]

Enables / disables a sensor. Returns true if successful.

enabledSensors()[source]

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

endStep()[source]

This is called after the commands sent at each time step

getPartInterface(part=None, joint_idx=None)[source]
hasSensor(sensor)[source]

Returns true if the given sensor can be enabled.

initialize()[source]

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

isMoving()[source]

Returns true if the part / joint are currently moving

join(qparts)[source]

Joins a bunch of parts into a whole-body robot.

jointName(joint_idx, part=None)[source]

Returns a string naming the given joint

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.

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.

reset()[source]

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

sensedCartesianForce()[source]
sensedCartesianPosition()[source]
sensedCartesianVelocity()[source]
sensedPosition()[source]

Retrieves the currently sensed joint position.

sensedTorque()[source]

Retrieves the currently sensed joint torque.

sensedVelocity()[source]

Retrieves the currently sensed joint velocity.

sensors()[source]

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

setCartesianForce(fparams, ttl=None)[source]

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, immediate=False)[source]

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

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.

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

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

setPIDGains(kP, kI, kD)[source]

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

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

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

setPosition(q, immediate=False)[source]

Sets an instantaneous position command.

Parameters

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

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.

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.

split(q)[source]

Splits a whole-body robot to parts (one per listed item).

startStep()[source]

This is called before the commands sent at each time step

status()[source]

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

class klampt.control.robotinterfaceutils.RobotInterfaceCompleter(baseInterface)[source]

Bases: klampt.control.robotinterface.RobotInterfaceBase

Completes as much of the RobotInterfaceBase API as possible from a partially specified RobotInterfaceBase. For example, if your RobotInterfaceBase implements position control, then the Completer will provide velocity, piecewise linear, and piecewise cubic, and Cartesian control commands.

Note: Completing the base’s part interfaces is not implemented yet.

At a minimum, a base must implement sensedPosition() and one of the following:

This class will not emulate:

This class will also not emulate the following, although the basic RobotInterfaceBase class has default implementations that work OK:

It will emulate:

  • Clock time from control rate using integration

  • Control rate from clock time using differences

  • Joint velocities from joint positions using finite differences

  • Joint positions from joint velocities using integration

  • PID control from torque commands

  • Torque commands from PID control

  • Piecewise linear / piecewise cubic control from position / velocity / PID control.

  • Cartesian positions / velocities / forces from joint positions / velocities / torques using the Klamp’t model.

  • Cartesian control using the Klamp’t model and IK

Parameters

baseInterface (RobotInterfaceBase) – the partially-implemented interface.

Methods:

addPart(name, indices)

Can add new parts, e.g., for Cartesian control

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.

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)

Enables / disables a sensor.

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

fromKlamptConfig(klamptConfig[, part, joint_idx])

fromKlamptVelocity(klamptVelocity[, part, …])

getPartInterface([part, joint_idx])

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.

initialize()

Tries to connect to the robot.

isMoving()

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.

moveToCartesianPosition(xparams[, speed])

Sets a Cartesian move-to-position command.

moveToPosition(q[, speed])

Sets a move-to position command.

numJoints([part])

Returns the number of joints of the given part.

partToRobotConfig(pconfig, part, robotConfig)

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.

print_status()

reset()

If the robot has a non-normal status code, attempt to reset it to normal operation.

sensedCartesianForce()

sensedCartesianPosition()

sensedCartesianVelocity()

sensedPosition()

Retrieves the currently sensed joint position.

sensedTorque()

Retrieves the currently sensed joint torque.

sensedVelocity()

Retrieves the currently sensed joint velocity.

sensors()

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

setCartesianForce(fparams[, ttl])

Sets a Cartesian torque command.

setCartesianPosition(xparams)

Sets a Cartesian position command.

setCartesianVelocity(dxparams[, ttl])

Sets a Cartesian velocity command.

setGravityCompensation([gravity, load, load_com])

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

setPID(q, dq[, t])

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

setPIDGains(kP, kI, kD)

Sets the PID gains.

setPiecewiseCubic(ts, qs, vs[, relative])

Tells the robot to start a piecewise cubic trajectory command.

setPiecewiseLinear(ts, qs[, relative])

Tells the robot to start a piecewise linear trajectory command.

setPosition(q)

Sets an instantaneous position command.

setToolCoordinates(xtool_local)

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

setTorque(t[, ttl])

Sets a instantaneous torque command.

setVelocity(v[, ttl])

Sets an instantaneous velocity command.

softStop()

Calls a software E-stop on the robot (braking as quickly as possible).

startStep()

This is called before the commands sent at each time step

status()

Returns a status string for the given part / joint.

toKlamptConfig(config[, klamptConfig, part, …])

toKlamptVelocity(velocity, klamptVelocity[, …])

addPart(name, indices)[source]

Can add new parts, e.g., for Cartesian control

cartesianForce(q, t)[source]

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

cartesianPosition(q)[source]

Converts from a joint position vector to a cartesian position

cartesianVelocity(q, dq)[source]

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

clock()[source]

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

close()[source]

Cleanly shuts down any resources acquired using initialize().

commandedCartesianForce()[source]
commandedCartesianPosition()[source]
commandedCartesianVelocity()[source]
commandedPosition()[source]

Retrieves the currently commanded joint position.

commandedTorque()[source]

Retrieves the currently commanded joint torque.

commandedVelocity()[source]

Retrieves the currently commanded joint velocity.

controlRate()[source]

Returns the control rate, in Hz

destinationCartesianPosition()[source]

Retrieves the Cartesian destination of a motion queue controller

destinationCartesianVelocity()[source]

Retrieves the final Cartesian velocity of a motion queue controller

destinationPosition()[source]

Retrieves the destination of a motion queue controller.

destinationTime()[source]

Retrieves the final clock time of a motion queue controller.

destinationVelocity()[source]

Retrieves the final velocity of a motion queue controller.

enableSensor(sensor)[source]

Enables / disables a sensor. Returns true if successful.

enabledSensors()[source]

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

endStep()[source]

This is called after the commands sent at each time step

estop()[source]

Calls an emergency stop on the robot

fromKlamptConfig(klamptConfig, part=None, joint_idx=None)[source]
fromKlamptVelocity(klamptVelocity, part=None, joint_idx=None)[source]
getPartInterface(part=None, joint_idx=None)[source]
getToolCoordinates()[source]

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

hasSensor(sensor)[source]

Returns true if the given sensor can be enabled.

initialize()[source]

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

isMoving()[source]

Returns true if the part / joint are currently moving

jointName(joint_idx)[source]

Returns a string naming the given joint

klamptModel()[source]

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

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

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.

partToRobotConfig(pconfig, part, robotConfig)[source]

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

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.

print_status()[source]
reset()[source]

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

sensedCartesianForce()[source]
sensedCartesianPosition()[source]
sensedCartesianVelocity()[source]
sensedPosition()[source]

Retrieves the currently sensed joint position.

sensedTorque()[source]

Retrieves the currently sensed joint torque.

sensedVelocity()[source]

Retrieves the currently sensed joint velocity.

sensors()[source]

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

setCartesianForce(fparams, ttl=None)[source]

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

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

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

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

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

setPIDGains(kP, kI, kD)[source]

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

setPiecewiseCubic(ts, qs, vs, relative=True)[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().

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

setPosition(q)[source]

Sets an instantaneous position command.

Parameters

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

setToolCoordinates(xtool_local)[source]

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

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.

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.

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.

startStep()[source]

This is called before the commands sent at each time step

status()[source]

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

toKlamptConfig(config, klamptConfig=None, part=None, joint_idx=None)[source]
toKlamptVelocity(velocity, klamptVelocity, part=None, joint_idx=None)[source]