Actin  Version 5.5.5
Software for Robotics Simulation and Control
Position Control System

Introduction to the Actin Control System

The Actin control system is both powerful and flexible. The position control system is used to constrain the motion of the arm in a desired manner, allowing the arm to obey certain constraints while avoiding joint limits, singularities, and collisions with objects in the environment. The control system plugin provides tools to configure the control system parameters for all the manipulators (objects and robots) in the simulation. The plugin allows the user to edit or configure most of the parameters in the position control system using the dialogs, rather than programmatically or editing an xml file of the whole system.

At the core of Actin's velocity control framework is the following Eq1.

\[{\bf{\dot q}} = \beta ({\bf{q}},{\bf{V}}){\left[ {\frac{{\bf{J}}}{{{\bf{N}}_{\bf{J}}^T{\bf{W}}}}} \right]^{ - 1}}\left[ {\frac{{\bf{V}}}{{ - \alpha \,{\bf{N}}_{\bf{J}}^T{\bf{F}}}}} \right]\]

where \({\bf{V}}\) is an \(m\)-length vector representation of the motion of the end-effectors; \({\bf{q}}\) is the \(n\)-length vector of joint positions (with \({\bf{\dot q}}\) being its time derivative); and \({\bf{J}}\) is the \(m \times n\) manipulator Jacobian. \({{\bf{N}}_{\bf{J}}}\) is an \(n \times (n - m)\) set of vectors that spans the null space of \({\bf{J}}\). That is, \({\bf{J}}{{\bf{N}}_{\bf{J}}} = 0\), and \({{\bf{N}}_{\bf{J}}}\) has rank \((n - m)\). \({\bf{W}}\) is a matrix function of \({\bf{q}}\); \({\bf{W}}\) is a vector function of \({\bf{q}}\); and \(\alpha \) is a scalar. Mathematically, it achieves the desired \({\bf{V}}\) while minimizing \(\frac{1}{2}{{\bf{\dot q}}^T}{\bf{W\dot q}} + \alpha \,{{\bf{F}}^T}{\bf{\dot q}}\).

The parameters \(\alpha \), \({\bf{W}}\) and \({\bf{F}}\) can be varied to give many different types of velocity control. \(\beta ({\bf{q}},{\bf{V}})\) is a robust extension factor defined to help provide robustness in the presence of kinematic singularities.

In the Actin toolkit, \(\beta ({\bf{q}},{\bf{V}})\) can be defined using two metrics. The first metric is a measure of joint rate motion, and the second metric is a measure of end-effector motion error due to the linearization performed in using the Jacobian. These two metrics or filters are usually daisy-chained to provide maximum robustness to the kinematic solution. An example of the velocity control algorithm that would avoid joint limits and minimize kinetic energy when not operating near limits is depicted below. Since it was designed for generality, the underlying velocity control algorithm in Actin can take on any form or structure. However, because the joint limit avoidance and collision avoidance are the two most frequently used optimization criteria, we limit our scope to configuring these two criteria. Specifically, velocity control algorithms whose structures resemble that shown in the figure below are supported by the dialogs. For those not supported by the dialogs, the plugin provides the user a means to edit them as XML in a text editor.

ControlTree.png
Example control tree to avoid joint limits and minimize kinetic energy when not operating near limits

On loading the control system plugin, the relevant menu items are found in the Position Control System menu and its sub-menus.

ControlSysMenu.png
Control System menu.

The Control System Parameters dialog allows you to configure and edit the many parameters of the position control system. The Active Control Parameters dialog allows you to switch between the different control system configurations of each manipulator, and scale time of the simulation. The "Load Position Control System" dialog allows you to load a previously saved position control system from a file and replace the current one, and the "Save Position Control System" dialog allows you to save the current position control system to a file for future use.

Position Control System Properties

Selecting the Control System Parameters menu will bring up the control system dialog. In this dialog, the user can turn on/off the position control system (EcPositionControlSystem), change the time step and maximum iterations per cycle of the position control system, enable/disable position controllers (EcPositionController), and configure the velocity controllers.

posControllerOff.png
Position Controller Turned Off

The velocity controllers (EcVelocityController) contains the control descriptions of all manipulators in the system. In fact, each manipulator can possess multiple control descriptions, one of which needs to be active. A control description is what governs the motion of the manipulator and is the underlying unit that computes the joint velocities, given the end effector velocities. In the dialog, the user can select which manipulator to manage the control descriptions with the drop-down box. Once selected, all the control descriptions associated with that manipulator will be shown in the table. The active control description will be marked by the check box in front. The user can change the active control description by simply checking the check box in front of the desired control description. The user can also create a new control description by clicking the New button under the "Control Expression Containers" table widget. This will create a new control description by copying the currently selected one. The current control description can be deleted by clicking the Delete button. Note that if there is only one control description left, it cannot be deleted. The user can also create, rename, and delete end effector sets. The user can create a new end effector set by clicking the New button under the "End Effector Sets" table widget. Note that the "Joint control end-effector set" cannot be deleted or renamed, and there must be at least one other end effector set. The "Joint control end-effector set" is configured with a coordinated joint end effector, which constrains the position, velocity, and acceleration of the joints, and is used with the manipulator configuration slider bars, and joint frame actions.

controlSysDialog.png
Dialog for configuring control system parameters.

Configuring a Control System Description

To configure a control description, there are two options. The first option is to configure it by directly editing XML. This option is suitable for advanced users who are comfortable with XML and Actin control. When the 'XML Edit' button is clicked, the selected control description will be written as XML strings into the XML editor. Since XML can be verbose and long, the XML editor provides a way to quickly search for strings of interest. For example, if the user wishes to configure the joint rate filter, he can search for the word 'jointRateFilter' as illustrated below. The user can then edit the XML and click OK/Cancel to accept/discard the changes.

xmlDialog.png
XML Editor.

Another option of configuring a control description is to double-click on the desired control description itself. If it is not supported, a pop-up window will be displayed to inform the user that the selected control description is not supported and the user will be diverted to the XML editor. If it is supported by the dialogs (i.e. it has a structure similar to that shown above, then the Velocity Control Description dialog will be displayed.

VelocityControlDialog.png
Velocity Control Description dialog.

In this dialog, the user can change the label (name) of the control description. By clicking on the component of interest in the control algorithm, the user can also configure the parameters of that component. The properties of the selected component will be displayed in the table on the left. The following sections detail the parameters of all supported components.

End-Effector Error Filter

The end-effector error filter is typically at the highest level of the velocity control core hierarchy. It helps prevent excessive end-effector error. The figure below shows the configurable properties of the end-effector error filter. The end-effector error filter calculates the end-effector velocities at the start of the time step and the velocities at the end of the time step and differences them to form an error filter. The weighted two-norm of this error is then taken using the weights as a matrix or as a diagonal matrix when the weights form just a column. These diagonal entries can be made using the GUI. If this weighted two norm exceeds the threshold, then the joint rates are scaled to reduce the error to below the threshold. If the stopsAtLimits value is flagged, then the arm is stopped or scaled to avoid exceeding joint limits. If the stopsAtCollisions value is flagged, then the arm is stopped or scaled to avoid collisions.

EEErrorFilter.png
Configuring end-effector error filter.

Joint Rate Filter

The joint rate filter helps prevent the excessive joint rates (or velocities). Its properties can be configured through the dialog as illustrated below. For this filter, the weighted one-norm of the joint rates (with base linear and angular velocity appended for mobile-base systems) is then taken using the weights as an explicit square matrix or as an effective diagonal matrix when the weights form just a column. These diagonal entries can be made using the GUI, as shown. If this weighted norm exceeds the threshold, then the joint rates are scaled to reduce the measure to below the threshold.

jointRateFilter.png
Configuring joint-rate filter.

The maximum joint rates can be configured from the inverted "Weights", or they can use the Control Parameters which are specified in radians/second or meters/second.

useControlParams.png
Use Control Parameters Enabled
ControlParametersConfigGui.png
Control Parameters

Matrix

This is the matrix W of the core velocity control in the control tree diagram above. The user has an option of using the manipulator inertia matrix as W or defining his own diagonal matrix. To use the inertia matrix, check the massMatrix checkbox. The use of the inertia matrix will result in minimization of the kinetic energy when the robot moves. If the user un-hecks the massMatrix check box, the diagonal entry will be enabled and it will allow the user to enter the elements of the diagonal matrix in the comma-separated values.

matrixConfig.png
Configuring matrix in control core.

Scalar

The scalar constant is designated by a of the core velocity control in the control tree diagram above. This is simply a weighting factor between optimizing the matrix or the vector term.

scalarConfig.png
Configuring scalar in control core.

Vector

This is the vector \(F\) of the core velocity control in the control tree diagram above. This vector can be a single optimization criterion or a combination of multiple criteria. In this dialog, only the joint limit avoidance and obstacle avoidance criteria are supported. Selection of optimization criteria is performed by checking and un-checking the boxes in front of the criteria. The user can also specify the weighting factors for all the selected criteria by editing the weight values directly in the vector table. Mathematically, this means that the vector \(F\) is simply a weighted sum of all criteria:

\[F = \sum\limits_{i = 1}^n {{w_i}{F_i}} \]

The properties of each optimization criterion can also be configured. Figures 11 and 12 show the configurable parameters of the joint limit avoidance and collision avoidance criteria, respectively. For joint-limit avoidance, for each joint, a rate term is independently calculated that is a polynomial function of the proximity to a limit, as illustrated in the plot below. This is used with a negative scalar value to drive the manipulator away from joint limits. Each of these parameters can be configured in the dialog below. Normally, the avoidance zone is measured in absolute from the lower/upper limits. Therefore, for revolute joint the avoidance zone would be in radians and for prismatic joints, it would be in meters. Because of the unit variation and joint range variation (i.e. some joints have much larger ranges than others), the absolute avoidance zone may not be appropriate. The user can specified the default avoidance zone to be fractional by checking the fractional check box. As a result, the default avoidance zone will be proportional to the joint range.

jointLimitAvoid.png
Joint limit avoidance function.

jointLimitAvoidConfig.png
Configuring joint limit avoidance in control core.

To set the avoidance zone for each link, the Avoidance Zone Map GUI can be populated with the link label and the associated zone value. This GUI, shown below, is accessed via the Control System Plugin (Edit->Position Control System->Control System Parameters...), clicking on the relevant control expression description, and clicking on the joint limit avoidance filter. An “Edit…” button will appear if the 'avoidanceZones' property is clicked. Any link that is not specified will use the default 'avoidanceZone' property.

jointLimitAvoidZonesConfig.png
Configuring joint limit avoidance zones.

The collision avoidance algorithm employs a gradient-based method which seeks to minimize the function with the general form as follows:

\[f({\bf{q}}) = \sum\limits_{i = 1}^N {\sum\limits_{j = 1}^B {{F^p}(i,j)} } \]

where \(N\)is the number of links in the manipulator, \(B\) is the number of obstacles in the environment, \(p\) is a user-defined exponent, and \(F(i,j)\) is a measure of the proximity of the bounding volume of \(i\) to the bounding volume of link \(j\). \(F(i,j)\) is zero when the distance is larger than a user-specified threshold (avoidanceDistance), and within the threshold, it is just a scalar constant times the minimum distance needed to move one bounding volume to take it outside the threshold distance from the other bounding volume. This function is used with finite differencing to calculate its gradient, which is then used as the vector parameter to the core control system to drive the manipulators in a direction that avoids collisions. The user can choose to have the manipulator avoid self-collision and/or collisions with other manipulators by checking the checkSelfCollision and checkEnvironmentCollisions check boxes, respectively.

CollisionAvoidConfig

CollisionAvoidanceConfig.png
Configuring collision avoidance in control core.

Soft Constraint Handler

Normally, the end-effectors are defined as hard constraints and therefore are embedded in the constraint portion (i.e., \({\bf{J}}\) and \({\bf{V}}\)) of the formulation in Equation 1. The implication is that the joint velocities will be solved to obtain the exact values of the given end-effectors' velocities, if possible. This will result in a more accurate but sometimes more fragile equation, i.e. the solution may not exist. If an end-effector is defined instead as a soft constraint, it will be moved from the constraint portion to the optimization portion (i.e., \(\alpha \), \({\bf{W}}\), and \({\bf{W}}\)). Practically, this means that the solution may not yield the exact end-effector velocity, but it will not fail to find a solution. Every end-effector now has a flag indicating whether it is a hard or a soft constraint. When an end effector is flagged as a soft constraint, it is not included in the Jacobian equation. Instead, it is incorporated into \(\alpha \), \({\bf{W}}\), and \({\bf{W}}\)). through a soft-constraint handler.

The soft-constraint handler is an object that resides in a container that is a member variable of the class implementing the core velocity algorithm. This is illustrated in the figure below.

softConstraint.png
An illustration of the control system with the soft-constraint handler.

Let \({{\bf{J}}_s}({\bf{q}})\) be the Jacobian formed from all the soft-constrain end effectors in use, and let \({{\bf{V}}_s}\) be the concatenated desired velocity of those end effectors. Then an approach to handling the soft constraints is to minimize a measure of the soft end-effector error \({{\bf{E}}_s}\), defined as follows:

\[{{\bf{E}}_s} = {{\bf{J}}_s}({\bf{q}})\,{\bf{\dot q}} - {{\bf{V}}_s}\]

Let the measure of this error be defined through a weighted quadratic form. That is, for some positive definite weighting matrix \({{\bf{W}}_s}\), define the measure to be minimized as

\[{e_s} = {\bf{E}}_s^T{{\bf{W}}_s}{{\bf{E}}_s}\]

The method of Equation 1 minimizes

\[{e_o} = \frac{1}{2}{{\bf{\dot q}}^T}{\bf{W\dot q}} + \alpha \,{{\bf{F}}^T}{\bf{\dot q}}\]

based on values of \(\alpha \), \({\bf{W}}\), and \({\bf{W}}\) that have already been defined through the control-system database. To integrate the soft constraints, the default handler makes a trade-off based on a scalar parameter \(\sigma \,\). It simply changes \(\alpha \), \({\bf{W}}\), and \({\bf{W}}\) to minimize the following:

\[e = {e_o} + \sigma \,{e_s}\]

\(\sigma \,\) is essentially the weighting factor between the previous optimization and the soft-constraint optimization. A large value of \(\sigma \,\) will give more emphasis to the soft constraints and yield a solution that behaves more closely to what would be obtained if the end-effectors were hard-constraints. The user can configure the value of \(\sigma \,\) by changing the weight as seen in the dialog below.

softConstraintConfig.png
Configuring soft-constraint handler in control core.

Here, the first stage shows the right arm configured with a soft constraint, and the left arm constrained with a hard constraint. Then it shows both arms configured with hard constraints.

Finally, the user can configure any part of the control system through the plugin when the simulation is stopped. However, when the simulation is running, the user cannot load or save the control system as their menu items are disabled as illustrated below.

ControlSysMenuRunning.png
Control System menu while simulation is running.