Actin®  Version 5.3.0
Software for Robotics Simulation and Control
Control System Modifier and Motion Library

Introduction

The control system modifier architechture (see the figure below) is designed to provide a set of unified and simpler APIs than directly using EcSystemControlExecutive (in real-time control) or Ec::Plugin (in ActinViewer). One of the major advantage of this architechture is due to the abstraction layer of control system modifiers, the APIs are the same for both EcSystemControlExecutive and Ec::Plugin.

ControlSystemModifier_Arch.png
The architechture of the control system modifier.

The basic workflow is illustrated in the figure below. The step of "Calculate State" is not needed if ActinViewer is being used.

EcControlSystemModifier_workflow.png
Basic controller workflow.
Note
To maximize portability, methods provided by EcControlSystemExecutiveModifier are not thread safe. Please use thread synchronization mechanisms provided by target operating systems to protect EcControlSystemExecutiveModifier.

Using Modifiers

See controlExecutiveModifierExampleMain.cpp for an example of using EcControlSystemExecutiveModifier. For complete API descriptions, see EcBaseControlSystemModifier, EcControlSystemExecutiveModifier, and EcControlSystemPluginModifier.

Create an Instance of Modifiers

The modifier classes act as a proxy for EcSystemControlExecutive or Ec::Plugin and do not have a default constructor. The EcControlSystemExecutiveModifier must be created with an EcSystemControlExecutive object:

The control system timestep should be set using

void EcControlSystemExecutiveModifier::setControlSystemTimeStep
(
const EcReal value
)

The EcControlSystemPluginModifier must be created with a plugin:

Set Motion Commands

See Motion Library or Group Motion Manager.

Calculate State

This step is needed and only needed when using EcControlSystemExecutiveModifier. After executing motion commands, the position states and velocity states can be calculated by calling EcControlSystemExecutiveModifier::calculateStateTimeSteps.

(
const EcU32 numTimeSteps = 1,
const EcReal extrapolationTime = 0.0
)

This method advances state by EcControlSystemExecutiveModifier::timeStep() \( \times \) numTimeSteps and does extrapolation if extrapolationTime > 0. In most cases, the default values should be used.

The initial position state of a manipulator can be set by calling EcControlSystemExecutiveModifier::setPositionState. This is usually needed when synchronizing with the actual hardware state.

pBasePose or pJointPositions can be NULL if either needs not be set. For example, the following code sets only the joint positions of Manipulator 0 to be all 0, but does not change its base pose:

EcU32 jointDof;
controlExecModifier.getManipulatorJointDof(0, jointDof);
EcRealVector initialJoints(jointDof, 0);
controlExecModifier.setPositionState(0, EcNULL, &initialJoints);

Similarly, if needed, the initial velocity a manipulator can be set by calling EcControlSystemExecutiveModifier::setVelocityState.

Motion Library

The motion library provides rich motion primitives and logics to control manipulators. It can be used by writing EcScript (see Page EcScript) or programmatically. This section explains how to use the motion library programmatically. All motion classes are subclasses of EcMotionScriptObject. Each motion has an interal state. The motion runs and changes state by calling EcMotionScriptObject::run (must NOT be blocking).

EcScript_State.png
Motion object states.

The initial state is EcMotionScriptObject::NOT_STARTED when the motion object is created. After calling EcMotionScriptObject::run, if an error happens (for example, manipulator index is invalid), the state will transit to EcMotionScriptObject::FAILED. If nothing goes wrong, all motions that require multiple steps to finish will transit to EcMotionScriptObject::IN_PROGRESS. When the motion finishes, the state will become EcMotionScriptObject::SUCCEEDED. Calling EcMotionScriptObject::restart will restart the motion and reset the state to EcMotionScriptObject::NOT_STARTED.

Move Joint

To move a manipulator using a joint end-effector, use EcEndEffectorMotionMoveJoint.

The example below uses the default joint end-effector (end-effector set index = EcVelocityController::JOINT_CONTROL_INDEX, end-effector index = 0) when calling EcEndEffectorMotionMoveJoint::moveJoint.

// construct the motion object
const EcRealVector desiredJoint = boost::assign::list_of(0.0)(EcHalfPi)(0.0)(EcHalfPi)(0.0)(EcHalfPi);
moveJoint.setManipulatorIndex(0);
moveJoint.moveJoint(desiredJoint, 1.0, EcMICRO, EcTrue);
// control loop
EcMotionScriptObject::Status status = moveJoint.run(controlExecModifier, currentTime);
while (status == EcMotionScriptObject::IN_PROGRESS)
{
// keep running the motion
status = moveJoint.run(controlExecModifier, currentTime);
// update clock
currentTime += timeStep;
// calculate state
controlExecModifier.calculateState(currentTime, state);
}

It is possible to have a joint end-effector in other sets in Actin. To specify the end-effector set index and the end-effector index of a joint end-effector, do:

moveJoint.setEeSetIndex(EcVelocityController::JOINT_CONTROL_INDEX);
moveJoint.setEeIndex(0);
moveJoint.moveJoint(desiredJoint, 1.0, EcMICRO);

Move Pose

To move a manipulator using other type of end-effectors, use EcEndEffectorMotionMovePose.

// way point
EcOrientation orientation;
orientation.setFrom123Euler(0, -EcHalfPi, -EcHalfPi);
EcVector p0(0.1, 0, 0.35);
EcCoordinateSystemTransformation wp0(p0, orientation);
// move pose
movePose.setManipulatorIndex(0);
movePose.setEeSetIndex(0);
movePose.setEeIndex(0);
movePose.movePose(wp0, 1.0, EcMICRO);
// control loop omitted, see Move Joint above.

Move Pose Sequence

To move the TCP of a manipulator through a sequence of waypoints, use EcEndEffectorMotionMovePoseSequence. The example below corresponds to the example in Pose Sequence EcScript. Note that setManipulatorIndex, setEeSetIndex, and setEeIndex must be called on movePoseSeq before calling movePose.

// pose sequence
movePoseSeq.setManipulatorIndex(0);
movePoseSeq.setEeSetIndex(1);
movePoseSeq.setEeIndex(0);
// add waypoints
EcOrientation orientation;
orientation.setFrom123Euler(0, 0, 90 * EcDEG2RAD);
{
// waypoint 0
const EcVector p0(0.1, -0.3, 0.1);
const EcCoordinateSystemTransformation wp0(p0, orientation);
const EcReal blendRadius = 0.03;
movePoseSeq.movePose(wp0, speedFactor, blendRadius);
}
{
// waypoint 1
const EcVector p1(0.1, -0.4, 0.1);
const EcCoordinateSystemTransformation wp1(p1, orientation);
const EcReal blendRadius = 0.03;
movePoseSeq.movePose(wp1, speedFactor, blendRadius);
}
{
// waypoint 2
const EcVector p2(0.0, -0.4, 0.1);
const EcCoordinateSystemTransformation wp2(p1, orientation);
const EcReal blendRadius = 0.03;
movePoseSeq.movePose(wp2, speedFactor, blendRadius);
}
{
// waypoint 3
const EcVector p3(0.0, -0.3, 0.1);
const EcCoordinateSystemTransformation wp3(p1, orientation);
const EcReal tolerance = 0.001;
movePoseSeq.movePose(wp3, speedFactor, tolerance);
}

Move Circular

To move the TCP of a manipulator along a circular path, use EcEndEffectorMotionMoveCircular. The example below corresponds to Move Circular EcScript.

In this example, the circle is defined by center point, normal direction, and arc length. The goal orientation is provided. The motion will interpolate orientation from the start to the goal.

const EcVector center(-0.2, 0.4, 0.1);
const EcVector normal(0, 0, 1);
const EcReal arcLength = 360 * EcDEG2RAD;
EcOrientation goalOrientation;
goalOrientation.setFrom123Euler(180 * EcDEG2RAD, 0, 180 * EcDEG2RAD);
const EcReal speedFactor = 0.1;
moveCircular.setManipulatorIndex(0);
moveCircular.setEeSetIndex(1);
moveCircular.setEeIndex(0);
moveCircular.moveCircular(center, normal, &goalOrientation, speedFactor);

If the goal orientation is not provided, the orientation will be calculated by rotating the start orientation about the circle normal. That is, the orientation will be constant relative to the trangent direction.

moveCircular.moveCircular(center, normal, EcNULL, speedFactor);

Interpolated Pose Sequence

To move the TCP of a manipulator through a sequence of linear motions and circular motions with desired speed limits configured by speed factors, use EcEndEffectorMotionInterpolatedPoseSequence. This motion uses a look ahead algorithm to maximize the feedrate within speed and acceleration limits. The example below corresponds to Interpolated Pose Sequence EcScript. Note that setManipulatorIndex, setEeSetIndex, and setEeIndex must be called on interpPoseSeq before calling moveLinear or moveCircular.

// interpolated pose sequence
interpPoseSeq.setManipulatorIndex(0);
interpPoseSeq.setEeSetIndex(1);
interpPoseSeq.setEeIndex(0);
// add move linear
{
const EcVector goal(150 * EcMILLI, 350 * EcMILLI, 100 * EcMILLI);
EcOrientation orientation;
orientation.setFrom123Euler(-180 * EcDEG2RAD, 0, 0);
const EcCoordinateSystemTransformation wp(goal, orientation);
interpPoseSeq.moveLinear(wp, 0.1);
}
// add move circular
{
const EcVector center(150 * EcMILLI, 400 * EcMILLI, 100 * EcMILLI);
const EcVector normal(0, 0, 1);
const EcReal arcLength = 90 * EcDEG2RAD;
interpPoseSeq.moveCircular(center, normal, arcLength, EcNULL, 0.1);
}

Speed

To control an end-effector (joint or any type) in speed mode, use EcEndEffectorMotionSpeed. The size of the desired speeds should match the DOF of the end-effector. If it is smaller, the rest of the DOF will use 0 as the target speed.

Asumming End-effector 0 of End-effector Set 0 is a frame end-effector, the following code will control the TCP to move along z-axis at 1mm/s. If it is a joint end-effector, the motion will control the 3rd joint to move at 0.001rad/s (revolute joint) or 1mm/s (prismatic joint).

EcRealVector desiredSpeed = boost::assign::list_of(0.0)(0.0)(0.001)(0.0)(0.0)(0.0);
speedEe.setEeSetIndex(0);
speedEe.setEeIndex(0);
speedEe.setSpeed(desiredSpeed, EcMICRO);

To stop an end-effector, simply use a zero-size real vector:

speedEe.setEeSetIndex(0);
speedEe.setEeIndex(0);
speedEe.setSpeed(EcRealVector(), EcMICRO);

Or use EcEndEffectorMotionSpeed::stop:

speedEe.setEeSetIndex(0);
speedEe.setEeIndex(0);
speedEe.stop(EcMICRO);
Created by Energid Technologies www.energid.com
Copyright © 2016 Energid. All trademarks mentioned in this document are property of their respective owners.