Actin  Version 5.5.1
Software for Robotics Simulation and Control
Actin Motion Control

Motion Control

Controlling the motion of joints and end effectors is done using the Actin Position Control System (EcPositionControlSystem). This class contains all the neccesary information to configure the motion of all the manipulators in an Actin EcSystemControlExecutive, and to configure how the inverse kinematics are calculated. The inverse kinematics solution can be explained simply as a set of constriants and optimizations. Constraints define how certain links should move. Optimizations configure how the other unconstrained links should move (how kinematic redundancy is handled).

Position Control System

There is one Position Control System per EcSystemControlExecutive.

Position Controller

Each position control system has a vector of position controllers (EcPositionController), one per manipulator. This handles robust integration of joint velocities from the inverse kinematics, closing the loop around the velocity controller.

Velocity Controller

Each position controller has one velocity controller (EcVelocityController). This contains all the components to solve the inverse kinematics, calculating the joint velocities to solve for the constrained end effector velocity.

End Effector Sets

End effector sets (EcEndEffectorSet) are used to conveniently store configurations of end effectors or constraints on the same manipulator. Different end effector sets are used for different purposes or task requirements. End effector sets can be switched at run time without dynamic memory allocation. Each velocity controller stores a vector of EcEndEffectorSets (EcEndEffectorSetVector), as well as a special EcEndEffectorSet for joint control which is used when jogging in joint space, or for joint motions.

For example, a robot may have two end effector sets, one for frame end effector, and one for free-spin-in-Z end effector, as well as the joint control end effector set.

Note
The special joint control end effector is not stored in the EcEndEffectorSetVector, it is stored directly in the EcVelocityController. When specifying the index of this EcEndEffectorSet, we use the EcVelocityController::JOINT_CONTROL_INDEX.

End Effectors

End Effectors (EcEndEffector) are used to constrain the motion of a link, usually a specific point on a link. Traditionally, the term "End Effector" is used to describe robot tooling, but in the Actin case it can be thought of as a constraint on the motion of a link, or a "motion constraint". EcEndEffectors are typically used to constrain the motion of an "End Effector", and as a result, we use the terms "End Effector" and "Motion Constraint" interchangeably.

End Effectors store a link Identifier string, which associates it with the EcManipulatorLink that it is constraining. Different Combinations of constraints within an EcEndEffectorSet on different links or on the same link can result in different behaviors.

End Effector Types

Below are the most common EcEndEffector types used for a variety of motion applications.

EcCoordinatedJointEndEffector

This end effector is used to constrain the motion of a set of joints. This constraint has many use cases:

Use case: Controlling a manipulator in joint space, with a syncrhonized trapezoidal velocity profile. This is useful for jogging in joint space, or for large joint motions.

Here is an example where the motion of the joints are constrained by an EcCoordinatedJointEndEffector and used to move in joint space

Use case: Preventing certain joints from moving, using this constraint as "brakes". For example if you have a bifurcating manipulator, and you want to only allow for the motion of one branch, a EcCoordinatedJointEndEffector configured with the links of the other branch will "lock" them and prevent them from moving.

Here is an example where the motion of the joints are constrained by an EcCoordinatedJointEndEffector to "lock" one branch of the manipulator

Use case: Locking "configuration" joints, which may not be a driven joint, but might be used to mock up different link lengths or configurations.

EcFrameEndEffector

This end effector constrains the link's motion in 6 degrees of freedom (X, Y, Z, Roll, Pitch, Yaw).

Use case: This constraint is commonly used for moving the last link of a manipulator around the workspace, often to move around some tooling.

Here is an example where the motion of the wrist links are constrained by an EcFrameEndEffector to move each link in Cartesian space

EcFreeSpinInZEndEffector

This end effector constrains the link's motion in 5 degrees of freedom (X, Y, Z, Roll, and Pitch). The link's Yaw is free to rotate and optimize.

Use case: This constraint is commonly used for positioning tooling with axial symmetry such as mig welders, extruders, pens/pencils, and spray nozzles. It can also be used for positioning cylindrical parts, grasping round holes or features.

Here is an example where the motion of the wrist link is constrained by an EcFreeSpinInZEndEffector to move the tool along a path in cartesian space

EcGeneralSpatialEndEffector

This end effector is configurable to constrain a link's motion in anywhere from 1 to 6 degrees of freedom, in either system frame, or "end effector" frame. This is useful to create a variety of behaviors:

Use case: This constraint can be configured to behave as an EcPointEndEffector by configuring the Axis flags to constrain X, Y, Z, in EE or System frame. This constraint can be configured to behave as a EcFreeSpinInZEndEffector by configuring the Axis flags to constrain X,Y,Z, Roll, Pitch, in EE frame.

Use case: To configure the end effector to have the z axis pointed down in system frame, configure the Axis flags to constrain only Pitch and Roll in System frame.

The constraint used to keep the welder pointed "down" is a EcGeneralSpatialEndEffector constraining Angular X and Y only. A Separate free spin in Z is used to move the tool along the path

Use case: To configure a "remote center of motion" which keeps the Y axis pointing through a certain point, we set up the Axis flags to constrain X, Z (not Y) in EE frame.

In this case, the EcGeneralSpatialEndEffector is used to keep the rod pivoting and sliding in the remote center of motion at the hole on top of the extrusion box

Use case: To configure a "look at" constraint, configure the same as above.

Here, a EcGeneralSpatialEndEffector is configured to "look at" the goal placement

EcLinearConstraintEndEffector

This end effector is used to constrain the motion of a specified. This constraint has many use cases:

Use case: This constraint is commonly used to constrain the joint position of one joint with a trapezoidal veloctity profile. This is useful to control a joint on a gripper for example, or to use the wrist to drive a tool.

Here, a EcLinearConstraintEndEffector is constraining the "waist" joint. When this is jogged, the waist joint turns.

Use case: Another use for this constraint is to lock the position of an individual joint.

Here, a EcLinearConstraintEndEffector is constraining the "elbow" joint. This locks the position of the elbow as the seperate EcPointEndEffector on the wrist is moved

Use case: This constraint can be used to configure one joint to be slaved from another joint. For example, if there exists a manipulator with 2 joints with one degree of freedom (maybe coupled by a cable drive or a gear) a linear constraint can be used to slave the position of joint A from joint B with a specific ratio.

Here, a EcLinearConstraintEndEffector is constraining the "distal" finger joint, weighted with the "proximal" joint (1,-1). This moves the "distal" joint to the same angle as the "proximal" joint when the "proximal" joint is moved.

EcOneDofGripperEndEffector

This end effector is used to constrain the motion of one degree of freedom similar to the EcLinearConstraintEndEffector. Mostly this is used for moving gripper fingers. Multiple slave links can be configured to move multiple joints from this one instance of the constraint.

Here, a EcOneDofGripperEndEffector is constraining the finger joints on each gripper (not the wrist motion).

EcOrientationEndEffector

This end effector is used to constrain the orientation of a link, just Roll, Pitch, and Yaw.

Use case: One use for this constraint is if you wish to keep a link "upright" as if holding a cup of liquid.

Use case: Another use is for floating base manipulators like quadrupeds, where it might be desirable to keep the torso level.

Here, the orientation is constrained on the "platform" .

EcPointEndEffector

This end effector is used to constrain the position of a link, just X, Y, and Z.

Use case: One use for this constraint is for grasping spherical objects. In this case the orientation of the link is free to optimize if its unconstrained.

Use case: Another use for this constraint is to constrain the motion of a ball joint, using a relative constraint.

Here, the orientation is constrained on the "platform" .

EcXEndEffector

This end effector is used to constrain only the X position of a link. This constraint is useful for constraining one finger of a gripper relative to the other finger, if positioning is desired based on distance rather than angle.

EcXyEndEffector

This end effector is used to constrain only the X and Y position of a link. This constraint can be thought of as a planar point end effector.

Use case: This constraint is useful for scara robots to position the end effector in X and Y.

Use case: This constraint is also used to constrain 4 bar linkages by making the constraint relative to another link on the mechanism. These links must all rotate in the same plane.

Individual closed chain 4 bar linkages are constrained using relative EcXyEndEffectors" .

End Effector Parameters

Tool Offsets

EcEndEffectors by default constrain the motion of a EcManipulatorLink at its Primary Frame. It is often desirable to control a link from some offset to account for attached tools, or target part positions. EcEndEffectors can be configured to use a Tool Offset. The EcVelocityController stores a vector of EcCoordinateSystemTransformations used as Tool Offsets. These are stored at the Velocity Controller level so that EcEndEffectors in each EcEndEffectorSet for a manipulator can use these common tool offsets. For example, a robot might have a gripper, which they need to constrain the motion in a variety of ways, but from the same offset. In this case there would be multiple EcEndEffectorSets with different types of EcEndEffector in each one. Each EcEndEffector would use the same tool offset to control the robot from the same offset. Tool offsets can change at runtime depending on the task required.

Here we can visualize 2 different tool offsets for different end effectors (on the same link in different end effector sets) .

Relative End Effectors

EcEndEffectors can be configured to be relative to another link on the same manipulator. This is useful for coordinating motion across branching manipulators, for coordinated lifts, handoffs, as well as coordinating control between arms and positioners ("external" axes). This is also used to control closed chain mechanisms like 4 bar linkages (EcXyEndEffector) and ball joints (EcPointEndEffector).

Here, a relative EcFrameEndEffector is used to position the 2 parts together in a coordinated way. Next a second relative constraint is added, and used to coordinate the welder motion with the other 2 arms holding the assembly, which is still constrained with the other relative end effector

Is End Effector Editable

EcEndEffectors can either be editable or not. This is largely used as a mechanism to "hide" hidden internal constraints used for 4 bar linkages and ball joints that are not supposed to be driven.

Allow Changing Desired Placement

EcEndEffectors can also have a fixed goal placement that is stored in the EcEndEffector itself. This requires setting the m_AllowChangingDesiredPlacement flag to false, and setting the m_StoredDesiredPlacement.

Is Hard Constraint

EcEndEffectors can be configured to be "Hard" or "Soft". If the constraint is "hard" then the motion of the link will not deviate from the calculated "straight line" path. If a collision or joint limit or singularity is encountered, then the motion will stop.

Here, a hard EcFrameEndEffector is used in an attempt to drive through an obstacle. Because it is not allowed to diverge from its nominal path, it collides.

If the end effector constraint is "Soft" then the motion is allowed to deviate from the straight path, and use optimizations to move around joint limits, singularities, and collisions. This can be used as a form of "local path planning". In essence, this forms a multidimensional artificial potential field, driven by attraction to the goal position and repulsion from collisions and joint limits (or any other configured optimizations). This allows the robot to plan around obstacles in real-time. The resulting motion appears as if the robot attempts to take a straight line path until collisions or joint limits force the path to deviate around obstacles.

Here, a soft EcFrameEndEffector is used in an attempt to drive through an obstacle. Because it is allowed to diverge from its nominal path, and the Control Expression Description has optimizations configured it avoids the collision.

Soft constraints can also be used when the desired behavior is for an end effector to stay static until a disturbance.

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.

Note
Soft constraints require using the most efficient shapes when using collision avoidance.

Control Expression Description

The Control Description (EcControlExpressionDescription) configures the optimizations used when solving for the end effector constraints. Each velocity controller stores a vector of control expression descriptions, which can be configured for different inverse kinematics behaviors, and switched at runtime.

Control Parameters

The control parameters store the maximum joint velocitiy limits, which are used to limit the jonit velocities when moving end effectors. The control parameters also store maximum acceleration and deceleration values for each joint. These accel/decel values are typically referenced and used by the EcCoordinatedJointEndEffector to set the limits of its joint level motion.

Joint Rate Filter

The joint rate filter limits the joint velocities of the manipulator, so that the maximums are not exceeded when the end effector is moved. The maximum joint velocities are stored in the Control Parameters.

End Effector Error Filter

The End Effector Error Filter is usually configured to stop the motion of the manipulator at a joint limit, or when the manipulator has a collision with either itself or another manipulator.

Weighting Matrix

The weighting matrix is used to weight how the kinematic redundancy is handled by favoring the motion of certain links over others. For example, if the desired motion for a 6 dof arm with a EcPointEndEffector is to favor the motion of the wrist joints over the shoulder joints, then the weightings of the first 3 joints should be larger than the last 3 joints ( 100 100 100 1 1 1 ).

Another common example is when configuring the motion of a 6 DOF arm on a rail, totalling 7 degrees of freedom. To favor the arm motion over the rail, weight the rail joint higher than the rest ( 1000 1 1 1 1 1 1 ). If the opposite is desired, and the rail motion is to be favored over the arm motion, then the weight of the rail joint should be decreased relative to the other weights ( 1 100 100 100 100 100 100 ).

Here, two motions are performed with different weighting matrices, one favoring the rail, one favoring the arm.

Scalar

The scalar value is the overall weight of the weighted sum of the vector of optimizations. This can be used to balance the optimizations against the Weighting Matrix.

Optimizations

Optimizations are stored in the control tree as part of the Control Expression Descriptions. These optimizations configure how kinematic redundancy is handled. Each optimization has a gain, which allows for balancing multiple optimizations. For example, you can prioritize joint limit avoidance over collision avoidance, by having a larger magnitude gain than other optimizations like collision avoidance.

Joint Limit Avoidance

The most common default optimization is joint limit avoidance. This is configured to avoid a zone at the end of the joint range, in SI units (radians/meters), or a percentage (0-1) if the "fractional" flag is set to true.

Here, you can see the motion avoid a joint limit on the rail

Collision Avoidance

Collision avoidance configures the manipulator to avoid collisions based on the bounding volumes used and collision exclusion maps used. This optimization can be configured to have the manipulator avoid "self" collisions (avoiding its own links) and "environment" collisions (other manipulators/links). There is also a configurable avoidance distance stored in meters.

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. Collision avoidance is used throughout.
Note
Some shapes are more efficient than others. If you plan on using collision avoidance, try and use EcSphere, EcCapsule, EcLozenge, and EcHalfSpace.

End Effector Placement

To set the goal position of an EcEndEffector, we use the EcEndEffectorPlacement and EcManipulatorEndEffectorPlacement classes. EcEndEffectorPlacement is used to define the placement for a single EcEndEffector. It stores an EcCoordinateSystemTransformation which is used to set the target for "cartesian" type end effector constratints like EcPointEndEffector, EcFrameEndEffector, EcFreeSpinInZEndEffector, EcGeneralSpatialEndEffector. EcEndEffectorPlacement also stores an EcXmlRealVector, which stores joint angle targets for constraints like EcCoordinatedJointEndEffector. Few of the one degree constraint end effectors (EcLinearConstraintEndEffector, EcOneDofGripperEndEffector) use the EcCoordinateSystemTransformation Z value only.

EcManipulatorEndEffectorPlacement stores a vector of EcEndEffectorPlacement (EcEndEffectorPlacementVector), one for each end effector on a given manipulator (in its active the end effector set).