klampt.control.simrobotinterface module¶
Used for testing code that works with the Klamp’t Robot Interface Layer on a simualted robot. Defines a variety of RobotInterfaceBase interfaces that work with Klamp’t simulations.
For each of the classes in this module, if you provide the simulator argument then this will automatically update your simulation upon each startStep() / endStep() pair. Otherwise, you will have to step the simulation manually.
Classes:
|
A very basic control interface that just sets the robot’s config to the last setPosition command. |
|
Adapts a SimRobotController to the RobotInterfaceBase class, accepting position control, move to control, velocity control, and torque control modes. |
|
Adapts a SimRobotController to the RobotInterfaceBase class in move-to control mode. |
|
Adapts a SimRobotController to the RobotInterfaceBase class in position control mode. |
|
Adapts a SimRobotController to the RobotInterfaceBase class in velocity control mode. |
-
class
klampt.control.simrobotinterface.
KinematicSimControlInterface
(robot, robotInfo=None)[source]¶ Bases:
klampt.control.robotinterface.RobotInterfaceBase
A very basic control interface that just sets the robot’s config to the last setPosition command. Can also perform kinematic simulation of simulators.
Also performs joint limit testing and self collision checking. These change the status of the interface to non-‘ok’ error codes.
Methods:
Retrieves the currently commanded joint position.
Returns the control rate, in Hz
Returns a list of names of enabled sensors on this robot.
endStep
()This is called after the commands sent at each time step
If applicable, returns the Klamp’t RobotModel associated with this controller.
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.
sensorMeasurements
(name)Returns the latest measurements from a sensor.
sensors
()Returns a list of names of possible sensors on this robot.
setPosition
(q)Sets an instantaneous position command.
status
()Returns a status string for the given part / 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.
-
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.
-
sensorMeasurements
(name)[source]¶ Returns the latest measurements from a sensor. Interpretation of the result is sensor-dependent.
-
-
class
klampt.control.simrobotinterface.
SimFullControlInterface
(sim_controller, simulator=None, robotInfo=None)[source]¶ Bases:
klampt.control.simrobotinterface._SimControlInterface
Adapts a SimRobotController to the RobotInterfaceBase class, accepting position control, move to control, velocity control, and torque control modes.
You should use
RobotInterfaceCompleter
to fill in move-to control, cartesian control, velocity control, etc.Methods:
Retrieves the currently commanded joint position.
Retrieves the currently commanded joint torque.
Retrieves the currently commanded joint velocity.
isMoving
([part, joint_idx])Returns true if the part / joint are currently moving
moveToPosition
(q[, speed])Sets a move-to position command.
Retrieves the currently sensed joint position.
Retrieves the currently sensed joint torque.
Retrieves the currently sensed joint velocity.
setPID
(q, dq[, t])Sets a PID command to configuration q, velocity dq, and feedforward torque t.
setPIDGains
(kP, kD, kI)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.
setTorque
(t[, ttl])Sets a instantaneous torque command.
setVelocity
(v[, ttl])Sets an instantaneous velocity command.
-
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.
-
setPID
(q, dq, t=None)[source]¶ Sets a PID command to configuration q, velocity dq, and feedforward torque t.
-
setPIDGains
(kP, kD, kI)[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.
-
-
class
klampt.control.simrobotinterface.
SimMoveToControlInterface
(sim_controller, simulator=None, robotInfo=None)[source]¶ Bases:
klampt.control.simrobotinterface._SimControlInterface
Adapts a SimRobotController to the RobotInterfaceBase class in move-to control mode.
Only implements moveToPosition, sensedPosition, and commandedPosition; you should use
RobotInterfaceCompleter
to fill in position control, cartesian control, velocity control, etc.Methods:
Retrieves the currently commanded joint position.
isMoving
([part, joint_idx])Returns true if the part / joint are currently moving
moveToPosition
(q[, speed])Sets a move-to position command.
Retrieves the currently sensed joint position.
-
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.
-
-
class
klampt.control.simrobotinterface.
SimPositionControlInterface
(sim_controller, simulator=None, robotInfo=None)[source]¶ Bases:
klampt.control.simrobotinterface._SimControlInterface
Adapts a SimRobotController to the RobotInterfaceBase class in position control mode.
Only implements setPosition, sensedPosition, and commandedPosition; you should use
RobotInterfaceCompleter
to fill in move-to control, cartesian control, velocity control, etc.Methods:
Retrieves the currently commanded joint position.
Retrieves the currently sensed joint position.
setPosition
(q)Sets an instantaneous position command.
-
class
klampt.control.simrobotinterface.
SimVelocityControlInterface
(sim_controller, simulator=None, robotInfo=None)[source]¶ Bases:
klampt.control.simrobotinterface._SimControlInterface
Adapts a SimRobotController to the RobotInterfaceBase class in velocity control mode.
Only implements setVelocity, sensedPosition, and commandedPosition; you should use
RobotInterfaceCompleter
to fill in move-to control, cartesian control, position control, etc.Methods:
Retrieves the currently commanded joint position.
Retrieves the currently sensed joint position.
setVelocity
(v[, ttl])Sets an instantaneous velocity command.