Home

sawConstraintController and Constrainted Optimization

Overview

The sawConstraintController package is used to calculate the desired motion of a robot at a specific point in time given one or many ideal motions and a series of constraints on this motion. A series of linear equations is constructed that represents these terms, and a least squares solver is used to acquire the ``best'' motion of the robot in question. This page first explains the format of these equations, how to convert a given constraint into the necessary form, and then how to use the VFController class (or a subclass) to enact the constraints.

This page is a work in progress. The descriptions and examples shown below were adapted from a DVRK implementation.

Objectives and Constraints

An objective is an expression that the optimizer will try to minimize by its choice of a solution. Given multiple objectives, a solution will be found that best minimizes all of the corresponding expressions simultaneously. A constraint is a condition of an optimization problem that the solution must satisfy. The types of constraints available are inequality constraints and equality constraints.

• Objective: Given a vector of variables X, a matrix C, and a vector d, objectives are of the form:

CX - d

• Inequality Constraint: Given a vector of variables X, a matrix A, and a vector b, inequality constraints are of the form:

AX >= b

• Equality Constraint: Given a vector of variables X, a matrix E, and a vector f, equality constraints are of the form:

EX = f

Examples

Follow Objective

This objective ensures that any movement made by the master is also made by the slave. Its dependencies are a kinematics object for the current frame and a kinematics object for the desired frame. The desired frame is calculated as follows:

F_des = F_slaveF_master^-1p_master