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