klampt.control.cartesian_drive module

class klampt.control.cartesian_drive.CartesianDriveSolver(robot)

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

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:

RobotModel or SubRobotModel

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:

IKSolver

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)

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.

setTolerance(positionTolerance, rotationTolerance=None)

setMaxIters(maxIters)

setActiveDofs(activeDofs)

setJointLimits(qmin, qmax=None)

setVelocityLimits(vmin=None, vmax=None)

drive(qcur, angVel, vel, dt)

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)

Retrieves an entire trajectory produced by stepping numSteps steps at resolution dt.

If reset is true (on by default), resets the driveTransforms and driveSpeedAdjustment to the initial state after the trajectory is computed.

klampt.control.cartesian_drive.axis_rotation_magnitude(R, a)

Given a rotation matrix R and a unit axis a, determines how far R rotates about a. Specifically, if R = from_axis_angle((a,v)) this should return v (modulo pi).

klampt.control.cartesian_drive.normalize_rotation(R)

Given a matrix close to a proper (orthogonal) rotation matrix, returns a true orthogonal matrix.