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:
A RobotInterfaceBase that consists of multiple parts that are addressed separately. |
|
|
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
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().
Retrieves the currently commanded joint position.
Retrieves the currently commanded joint velocity.
Returns the control rate, in Hz
Retrieves the Cartesian destination of a motion queue controller
Retrieves the final Cartesian velocity of a motion queue controller
Retrieves the destination of a motion queue controller.
Retrieves the final velocity of a motion queue controller.
enableSensor
(sensor)Enables / disables a sensor.
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.
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.
Retrieves the currently sensed joint position.
Retrieves the currently sensed joint torque.
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.
Tells the robot to start a piecewise cubic trajectory command.
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).
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 )
-
cartesianVelocity
(q, dq)[source]¶ Converts from a joint position / velocity vector to a cartesian velocity
-
destinationCartesianPosition
()[source]¶ Retrieves the Cartesian destination of a motion queue controller
-
destinationCartesianVelocity
()[source]¶ Retrieves the final Cartesian velocity of a motion queue controller
-
initialize
()[source]¶ Tries to connect to the robot. Returns true if ready to send commands. This should probably be the first method called.
-
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.
-
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.
-
-
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
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().
Retrieves the currently commanded joint position.
Retrieves the currently commanded joint torque.
Retrieves the currently commanded joint velocity.
Returns the control rate, in Hz
Retrieves the Cartesian destination of a motion queue controller
Retrieves the final Cartesian velocity of a motion queue controller
Retrieves the destination of a motion queue controller.
Retrieves the final clock time of a motion queue controller.
Retrieves the final velocity of a motion queue controller.
enableSensor
(sensor)Enables / disables a sensor.
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])Gets the tool coordinates of this robot relative to its end effector link.
hasSensor
(sensor)Returns true if the given sensor can be enabled.
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
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.
reset
()If the robot has a non-normal status code, attempt to reset it to normal operation.
Retrieves the currently sensed joint position.
Retrieves the currently sensed joint torque.
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).
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[, …])-
cartesianVelocity
(q, dq)[source]¶ Converts from a joint position / velocity vector to a cartesian velocity
-
destinationCartesianPosition
()[source]¶ Retrieves the Cartesian destination of a motion queue controller
-
destinationCartesianVelocity
()[source]¶ Retrieves the final Cartesian velocity of a motion queue controller
-
getToolCoordinates
()[source]¶ Gets the tool coordinates of this robot relative to its end effector link.
-
initialize
()[source]¶ Tries to connect to the robot. Returns true if ready to send commands. This should probably be the first method called.
-
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.
-
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.
-
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.