Skip to content

Home

Jump to bottom Edit New page
Paul Wilkening edited this page Jul 24, 2015 · 8 revisions

sawConstraintController and Constrainted Optimization

Setup

To start with sawConstraintController, it is recommended you clone the cisst-saw repository (you do not need to use most of the modules, but this setup is the easiest way to integrate saw components with cisst).

The instructions for configuring cisst can be found on the cisst wiki page, but the necessary information for use with sawConstraintController is included below:

  1. When configuring cisst with ccmake for the first time, ensure that you have cisstNetlib. The instructions for doing so can be found here.
  2. Ensure that the required cisst libraries for saw are turned on in ccmake. Other libraries can be turned off if unnecessary. The required libraries are: cisstCommon cisstVector cisstOSAbstraction cisstMultiTask cisstParameterTypes cisstRobot cisstNumerical
  3. Turn on SAW_sawConstraintController
  4. Configure, generate, and make the cisst-saw project
  5. To use one or more of the sawConstraintController classes, simply add the cisst-saw folder to your project via cmake and add #include <sawConstraintController/X.h> (where X is the class name) where you need to use the specified class.

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

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:

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

Plane Constraint

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

Joint Limit Constraint

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

Control Flow of the Optimizer

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.

Setup

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

Example: Force Compliance

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.

Overview

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

Robot Task Header Logic

Here we define our objects being used. This includes the VF Controller and any kinematics or sensor objects that aren't defined in another process.

    	    prmSensorState ForceSensorState;
    	        // Force sensor state information
    	        	    
    	    prmKinematicsState EEKinematics;
    	        // Kinematics object for end effector
    	        	    
    	    mtsVFDataSensorCompliance ForceVF;
    	        // Force compliance virtual fixture data object
    	                
            // VFController, output vector, and output return status objects    	    
    	    mtsVFController VFController;  
    	    vctDoubleVec ControllerOutput;
    	    nmrConstraintOptimizer::STATUS OptimizerStatus;

Robot Task Initialization Logic

In the task's constructor, we initialize our objects. This includes putting the local kinematics, sensor, and virtual fixture objects into the VF Controller, as the VF Controller contains a list of pointers for these objects. Objects of these types that come from another process must be updated every tick, as this is the only way to update the pointers for such objects.

    	    VFController = mtsVFController(NB_Joints,mtsVFBase::JVEL);	
    	        // Create VFController using the number of variables to solve for (in this case joints) and the desired control mode (in this case joint velocity)
    	    	    
    	    ForceSensorState = prmSensorState("Force Sensor");           	    
    	        // Create the force sensor state object using a string name
    	        	    
    	    EEKinematics = prmKinematicsState("EE Kinematics");
    	        // Create the kinematics object using a string name
        	    
    	    ForceVF = mtsVFDataSensorCompliance("Force VF", EEKinematics.Name, ForceSensorState.Name, NB_Joints);	    	    	   
    	        // Create the force compliance virtual fixture by giving it a name, setting its dependencies, and giving it the number of objective rows to allocate
    		        	    
    	    VFController.Sensors.push_back(&ForceSensorState);
    	        // Add pointer to force sensor values into VFController's sensor list
    	        	    
    	    VFController.Kinematics.push_back(&EEKinematics);
    	        // Add pointer to end-effector kinematics values into VFController's kinematics list
    		        	    
    	    VFController.VFMap.push_back(&ForceVF);
    	        // Add pointer to force compliance virtual fixture information into VFController's virtual fixture list

Robot Task Run Loop Logic

Update the local state data and virtual fixtures, and update the VF Controller by re-calculating its optimizer data using the latest state data and virtual fixtures. Then, solve the optimization problem and check the output code for errors. If it is satisfactory, use the optimizer output for the next incremental motion of the robot.

    	    ForceSensorState.Values = GetForceTorque();	               
    	        // Update force sensor state
    	    	    
    	    EEKinematics.Jacobian = GetJacobianTip();
    	        // Update the kinematics object
    		        	    
    	    ForceVF.Gain = GetForceGain();
    	        // Update force compliance gain
    	    	    
    	    VFController.UpdateOptimizer(this->GetPeriodicity());
    	        /* 
    	            This function does the following:	
    	             1 - Initialize optimization problem
    	                     Goes through each active virtual fixture and keeps track of the space needed in the optimizer's data
    	                     Allocates the optimizer's data fields
    	             2 - Fills in the optimizer's data
    	                     Goes through each active virtual fixture, makes sure its state data dependencies are up to date, and gives the virtual fixtures references to pieces of the optimizer's data fields
    	                     Each virtual fixture is responsible for using its internal data and state data dependencies to fill in these references
                */
    	    
    	    OptimizerStatus = VFController.Solve(ControllerOutput);	    
    	        // Solve the optimization problem and check the return code for any errors/warnings
    	        
    	    if(OptimizerStatus == nmrConstraintOptimizer::NMR_OK)
    	    {
    		      SetJointVelocity(ControllerOutput);
    	    }
    	    else
    	    {
    		      CMN_LOG_CLASS_RUN_ERROR << OptimizerStatus << std::endl;
    	    }

Force Compliance Virtual Fixture Class

When the VF Controller is passed a virtual fixture data object, it creates or overwrites a virtual fixture implementation class. In order to enact a force compliance virtual fixture in this manner, we need to write a force compliance virtual fixture implementation class that will use the proper logic to set up the optimization problem the constraint optimizer will solve.

Header File

The header file defines a set of variables used to fill in the given references with a combination of state data and virtual fixture data.

    mtsVFDataSensorCompliance* SensorComplianceData;
        // A pointer to the virtual fixture data object
        
    vctDoubleMat* JacP;
        // A pointer to a jacobian
       
    vctDynamicVector<double>* SensorValues;
        // A pointer to the force sensor values
        
    vctDoubleMat* Gain;
        // A pointer to the gain stored in the data object

FillInTableauRefs

To use logic specific to this virtual fixture to fill in the optimizer's data, we overwrite the FillInTableauRefs() method. Each loop, the VF Controller ensures that its set of matrices and vectors have allocated enough space for the entire optimization problem (determined by the virtual fixtures that are active in the current iteration) and assigns references to each virtual fixture according to the variables that specify the space required in their data objects. These references are filled in by FillInTableauRefs. The implementation of FillInTableauRefs for mtsVFSensorCompliance is outlined below.

    // We are trying to express the equation:
    // min || J(q)*dq - Gain*SensorValues ||
    // by filling in ObjectiveMatrixRef with J(q)
    // and filling in ObjectiveVectorRef with OverallGain*SensorValues
    
    // Check if we have the kinematics object for its jacobian and the sensor object
    if(Kinematics.size() < 1 || Sensors.size() < 1)
    {
        CMN_LOG_CLASS_RUN_ERROR << "Error: Sensor Compliance VF missing dependency data" << std::endl;
        cmnThrow("Error: Sensor Compliance VF missing dependency data");
    }
        
    SensorComplianceData = (mtsVFDataSensorCompliance *)(Data);
        // Assign the pointer to the virtual fixture data object (Convert base data class to mtsVFDataSensorCompliance in order to access its force compliance-specific data)
        
    JacP = &(Kinematics.at(0)->Jacobian);
        // Assign the pointer to the jacobian
        
    SensorValues = &(Sensors.at(0)->Values);
        // Assign the pointer to the force sensor values
        
    Gain = SensorComplianceData->Gain;       
        // Assign the pointer to the gain stored in the data object
        
    ObjectiveVectorRef.Assign((*Gain)*(*SensorValues));
        // Set the vector reference to the right hand side of the above equation (Gain*Force)
        
    ObjectiveMatrixRef.Assign(*JacP);    
        // Set the matrix reference to the left hand side of the above equation (Jacobian)
        
    ConvertRefs(Mode,TickTime);
        // Convert the assigned references, if necessary. If the VFController's mode is joint velocity, then no conversion is necessary. If it is incremental joint position, the values will be multiplied by the TickTime passed in.
Add a custom footer
Add a custom sidebar
Clone this wiki locally