klampt.control.cartesian_drive module¶
-
class
klampt.control.cartesian_drive.
CartesianDriveSolver
(robot)[source]¶ Bases:
object
Allows continuous, smooth Cartesian commands to be sent to a robot that is only joint-controlled. This is a highly configurable and reliable solver suitable for long-duration, precise cartesian motions. Specifically, it handles subtleties with out-of-range commands, singularities, IK solve tolerances, and oscillations.
Most of the solver is stateless except for
driveTransforms
anddriveSpeedAdjustment
.Tests on an Intel i7 show that this can be run for 1 6DOF IK goal at ~200Hz in Python with default settings. Reducing the max # of iterations or increasing the tolerance can help with running time. There is also a C++ version in KrisLibrary/robotics/CartesianDrive.h if you need to squeak out extra performance. It includes some perforamcne enhancements, like avoiding forward kinematics updates for non-active joints.
-
robot
¶ the robot that will be used.
- Type
-
positionTolerance
¶ IK position tolerance, default 1e-4
- Type
float
-
rotationTolerance
¶ IK orientation tolerance, default 1e-3
- Type
float
-
links
¶ set of end effectors in the IK constraints.
- Type
list of ints
-
baseLinks
¶ if given, IK constraints will be treated relative to these links
- Type
list of ints
-
endEffectorOffsets
¶ the tool coordinates (local positions) for each constraint. Must be of size len(links).
- Type
list of 3-vectors
-
driveTransforms
¶ The current cartesian drive goals. Must be of size len(links).
- Type
list of se3 elements
-
ikGoals
¶ If set, must be of size len(links). The solver updates the end effector DOFs selected for by these goals. If not set, uses fixed position/orientation goals.
- Type
list of IKObjectives
-
qmin
¶ If set, overrides the robot’s default joint limits.
- Type
list of floats, optional
-
qmax
¶ If set, overrides the robot’s default joint limits.
- Type
list of floats, optional
-
vmin
¶ If set, overrides the robot’s default velocity limits.
- Type
list of floats, optional
-
vmax
¶ If set, overrides the robot’s default velocity limits.
- Type
list of floats, optional
-
solver
¶ the IK solver used. The user can configure parameters, e.g., the IK solve tolerance, active DOFs, joint limits, etc.
- Type
-
driveSpeedAdjustment
¶ internal speed adjustment in the range [0,1]. This helps keep the # of iterations consistent and low per time step.
- Type
float
Usage:
qstart = controller.getCommandedConfig() solver = CartesianDriveSolver(robot) solver.start(qstart,5) #initializes with 1 link qcur = qstart while not done: wdes,vdes = [desired angular and translational velocities] progress,qcmd = solver.drive(qcur,wdes,vdes,dt) if progress <= 0: print("Stopped") controller.setCommandedConfig(qcmd) qcur = qcmd
-
drive
(qcur, angVel, vel, dt)[source]¶ Drives the robot by an incremental time step to reach the desired Cartesian (angular/linear) velocities of the links previously specified in start().
- Parameters
qcur (list of floats) – The robot’s current configuration.
angVel (3-vector or list of 3-vectors) – the angular velocity of each driven link. Set angVel to None to turn off orientation control of every constraint. angVel[i] to None to turn off orientation control of constraint i.
vel (3-vector or list of 3-vectors) – the linear velocity of each driven link. Set vel to None to turn off position control of every constraint. Set vel[i] to None to turn off position control of constraint i.
dt (float) – the time step.
- Returns
A pair
(progress,qnext)
.progress
gives a value in the range [0,1] indicating indicating how far along the nominal drive amount the solver was able to achieve. If the result is < 0, this indicates that the solver failed to make further progress.qnext
is the resulting configuration that should be sent to the robot’s controller.- Return type
tuple
For longer moves, you should pass qnext back to this function as qcur.
-
getTrajectory
(qcur, angVel, vel, dt, numSteps, reset=True)[source]¶ Retrieves an entire trajectory produced by stepping
numSteps
steps at resolutiondt
.If
reset
is true (on by default), resets thedriveTransforms
anddriveSpeedAdjustment
to the initial state after the trajectory is computed.
-
start
(q, links, baseLinks=None, endEffectorPositions=None)[source]¶ Configures the IK solver with some set of links
After start() is called, you can set up additional settings for the IK solver by calling
setTolerance()
,setMaxIters()
,setActiveDofs()
.- Parameters
q (list of floats) – the robot’s current configuration.
links (int, str, list of int, or list of str) – the link or links to drive.
baseLinks (int, str, list of int, or list of str, optional) – If given, uses relative positioning mode. Each drive command is treated as relative to the given base link. A value of -1 or an empty str indicates no base link.
endEffectorPositions (3-vector or list of 3-vectors, optional) – Local offsets for the end effector positions, relative to the specified link. If not given, the offsets will be treated as zero.
-