-
Notifications
You must be signed in to change notification settings - Fork 6
Home
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 VF Controller class (or a custom subclass) to enact the constraints.
This page is a work in progress. The descriptions and examples shown below focus on a DVRK implementation.
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
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:
Fdes = FslaveFmaster-1pmaster
The desired frame is converted into a desired set of joint positions using the slave's inverse jacobian function. This is then used to express the objective as follows:
mindq || I*dq - (qdes - qcur) ||
Depending on the application, we may wish to express this virtual fixture using cartesian frames. The equation for that approach is expressed as:
mindq || J*dq - (xdes - xcur) ||
A plane passing through a point P(x,y,z) with a normal n(a,b,c) can be represented as:
n * P = d
ax + by + cz = d
If we are given a frame F(R,p) with a point on the plane p, we generate the plane equation as follows:
- We take the Z axis of the frame to be the normal of the plane, n
- The translation vector of the frame to be the point on the plane, p
- We can calculate the value of d, as follows:
d = ax + by + cz
d = n * p
- Given an arbitrary point X, the plane equation is:
n * X = n * p
Constraint: To constrain X to be on or above the plane, we have the following constraint:
n * X - n * p >= 0
For any kind of objective it is absolutely mandatory to limit the joint velocities. In this example, this is accomplished by limiting incremental positions based on the maximum and minimum allowable velocities on each joint. This is an example of a static constraint that only needs to be initialized and not updated each iteration of a loop like many of the others.
Constraint:
I*dq >= VelLower
-I*dq >= -VelUpper
The optimization problems outlined in this approach are solved by using a VF Controller object, which contains a list of virtual fixture objects, data dependencies of these virtual fixtures (kinematics and sensor data), and logic for feeding this data into its constraint optimizer. The constraint optimizer contains a series of matrices and vectors and is responsible for populating them with data representing the objectives and constraints. At every iteration of the run loop the VF Controller is used, the following steps must be taken:
- ProcessQueuedCommands is called. This ensures that any virtual fixtures or data dependencies sent to the process that contains the VF Controller are updated.
- The current robot state data is read/calculated. Any robot-specific data that is used by virtual fixtures must be updated before it is used to update the optimizer.
- The optimizer is updated. This means that the objective and constraint matrices and vectors that it contains are reallocated if necessary, and each virtual fixture is assigned a reference to a piece of matrices and vectors it needs to populate with data. Each virtual fixture is then called to fill in its data for the current iteration of the loop using the latest robot data.
- The optimizer's solve function is called. This means that the objectives are minimized while not violating constraints.
- The output of the optimizer is used for robot motion.
A typical run loop is given by the RobotTask class, but deviations from this outline can be constructed simply by using the VF Controller within a robot's run loop in this manner.
Setting up the VF controller and populating it with objectives and constraints is relatively simple and straightforward.
- Initialize necessary variables:
// VF Variables
prmKinematicsState CurrentSlaveKinematics;
prmKinematicsState DesiredSlaveKinematics;
mtsVFController VFController;
mtsVFDataFollow FollowData;
vctDoubleVec ControllerOutput;
nmrConstraintOptimizer::STATUS OptimizerStatus;
// Plane Constraint
mtsVFDataPlane PlaneData;
- Initialize the VF controller object:
VFController = mtsVFController(7,mtsVFBase::JPOS);
- Initialize the data object(s):
FollowData.Name = "FollowVF";
FollowData.ObjectiveRows = 7;
// Sample data object representing the objective without any constraints
PlaneData.Name = "PlaneVF";
PlaneData.IneqConstraintRows = 1;
// Sample data object representing the constraints without any objective
- Assign distinct names for the kinematics objects:
CurrentSlaveKinematics.Name = "CurrentSlaveKinematics";
DesiredSlaveKinematics.Name = "DesiredSlaveKinematics";
// Kinematics objects for follow VF and plane constraint
- Provide the data object with the names of the kinematics objects that it has to request from the VF controller:
FollowData.KinNames.clear();
FollowData.KinNames.push_back("CurrentSlaveKinematics");
FollowData.KinNames.push_back("DesiredSlaveKinematics");
// Kinematics required by the objective
PlaneData.KinNames.clear();
PlaneData.KinNames.push_back("CurrentSlaveKinematics");
// Kinematics required by the constraints
- Create respective functions for the data objects created. These functions will help the VF controller add the objective/constraint data into a map indexed by name. (These functions are to be created in a user-created subclass of mtsVFController):
// Function to add Follow objective
void mtsDaVinciVFController::AddVFFollow(const mtsVFDataBase & vf)
{
// If we can find the VF, only change its data.
// Otherwise, create a new VF object.
if (!SetVFData(vf, typeid(mtsVFFollowJacobian)))
{
VFMap.insert(std::pair<std::string,mtsVFFollowJacobian *>(vf.Name,new mtsVFFollowJacobian(vf.Name,new mtsVFDataBase(vf))));
// Adds a new virtual fixture to the active vector
IncrementUsers(vf.KinNames,vf.SensorNames);
// Increment users of each kinematics and sensor object found
}
}
// Function to add Plane constraint
void mtsDaVinciVFController::AddVFPlane(const mtsVFDataPlane & vf)
{
// If we can find the VF, only change its data. Otherwise, create a new VF object.
if (!SetVFDataPlane(vf, typeid(mtsVFPlane)))
{
VFMap.insert(std::pair<std::string,mtsVFPlane *>(vf.Name, new mtsVFPlane(vf.Name,new mtsVFDataPlane(vf))));
// Adds a new virtual fixture to the active vector
IncrementUsers(vf.KinNames,vf.SensorNames);
// Increment users of each kinematics and sensor object found
}
}
- Add the data objects to the VF controller:
VFController.AddVFFollow(FollowData);
// Add Follow objective data
VFController.AddVFPlane(PlaneData);
// Add Plane constraint data
- In the run loop, update the parameters required by the optimizer like kinematics, jacobian, ... etc:
...
...
VFController.SetKinematics(CurrentSlaveKinematics);
VFController.SetKinematics(DesiredSlaveKinematics);
// Update the kinematics
- Once the required variables in the VF Controller are updated, an optimization problem is formed by the controller and an optimal solution is computed by the optimizer. Appropriate errors/warnings are thrown if there are any inconsistencies in the inputs provided:
// Solve the optimization problem and check the return code for any errors/warnings
OptimizerStatus = VFController.Solve(ControllerOutput);
/*
enum STATUS {NMR_OK, NMR_EQ_CONTRADICTION, NMR_INEQ_CONTRADICTION, NMR_BOTH_CONTRADICTION, NMR_MALFORMED, NMR_EMPTY};
0 Both equality and inequality constraints are compatible and have been satisfied.
1 Equality constraints are contradictory. A generalized inverse solution of EX=F was used to minimize the residual vector length F-EX. In this sense, the solution is still meaningful.
2 Inequality constraints are contradictory.
3 Both equality and inequality constraints are contradictory.
4 Input has a NaN or INF
*/
- The Solve function will call the FillInTableuRef function of respective virtual fixture classes. (These classes are created by the user. Refer to the following section for an example).
The following VF Controller example implements a force compliance virtual fixture for an arbitrary robot. This virtual fixture is made up of an objective that generates joint velocities based on the readings of a force sensor. The VF Controller is put into a Robot Task class. This is a class we assume already has been implemented that contains a method which periodically calculates the next incremental motion of the robot. The following sections outline how this class was modified to use the VF Controller. Note that the framework allows for inter-process communication of data, but this example assumes the process that controls the robot is also able to read the force sensor in question.
-
Define objects needed for constraint optimization, including a sensor state object for the force sensor, a kinematics object that contains the jacobian needed, an object that contains the virtual fixture gains, and the VF Controller itself.
-
Initialize the objects in the task's constructor. This includes giving the force sensor state and kinematics objects names. These names are put into the force compliance virtual fixture data object to signify that they are dependencies of the virtual fixture to be created within the VF Controller. This association is done via a string to make it easier to match dependencies to a virtual fixture between processes. A number of rows is also assigned to the data object. This is one of three variables that determine the size of the objective, inequality constraint, and equality constraint respectively. These variables are important for allocating enough space in the constraint optimizer for the total number of active virtual fixture needed this iteration of the run loop. The VF Controller is initialized by specifying the number of variables that it should solve for, as well as a control mode. This mode represents the form of the output vector. It is typically incremental joint positions or joint velocities. Note that local state data pointers are defined here, as they only need to be assigned once.
-
In the task's run loop, process queued commands and update the latest state data and virtual fixture data objects. Note that the VF Controller has pointers to these objects, so updating them is enough for the VF Controller to have up-to-date data. Processing queued commands will update the VF Controller for remote state data and virtual fixtures. Then, solve the optimization problem for the newest joint velocities and check to see if the results are accurate. These values are then used to drive the incremental motion of the robot.