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 jointcontrolled. This is a highly configurable and reliable solver suitable for longduration, precise cartesian motions. Specifically, it handles subtleties with outofrange 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 nonactive joints.

robot
¶ the robot that will be used.
 Type

positionTolerance
¶ IK position tolerance, default 1e4
 Type
float

rotationTolerance
¶ IK orientation tolerance, default 1e3
 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 3vectors

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 (3vector or list of 3vectors, 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 (3vector or list of 3vectors) – 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 (3vector or list of 3vectors) – 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.
