Actin®  Version 5.2.0
Software for Robotics Simulation and Control
controlExecutiveModifierExampleMain.cpp

An example of control system executive modifier.
The header file for the controlExecutiveModifierExample class

#ifndef controlExecutiveModifierExample_H_
#define controlExecutiveModifierExample_H_
//------------------------------------------------------------------------------
// Copyright (c) 2018 Energid Technologies. All rights reserved.
//
//
//
//------------------------------------------------------------------------------
{
public:
(
);
(
);
(
const EcReal progress
);
};
#endif // controlExecutiveModifierExample_H_


The implementation file for the controlExecutiveModifierExample class

//------------------------------------------------------------------------------
// Copyright (c) 2018 Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
#include <boost/assign/list_of.hpp>
#include <boost/make_shared.hpp>
//------------------------------------------------------------------------------
void
(
)
{
//-----------------------------------------
// Configure system control executive
//-----------------------------------------
#if EC_HAS_boost_filesystem
// add the file location to the search path
EcString("/actinExamples"));
// declare the filename
EcString filename = "cytonRobotControlExec.xml";
// load the system control executive from an XML file
std::cout << "Loading system control executive from file: " << filename << "...";
EcBoolean result = EcXmlObjectReaderWriter::readFromFile(controlExec, filename);
if (!result)
{
std::cout << std::endl << "Could not load system control executive." << std::endl;
}
std::cout << "DONE" << std::endl;
#else
// load the system control executive from header
std::cout << "Loading system control executive from header...";
EcString stringBuffer(cytonRobotControlExec, cytonRobotControlExec + sizeof(cytonRobotControlExec));
EcBoolean result = EcXmlObjectReaderWriter::readFromBuffer(controlExec, stringBuffer);
if (!result)
{
std::cout << std::endl << "Could not load system control executive." << std::endl;
}
std::cout << "DONE" << std::endl;
#endif
// create the modifier
EcControlSystemExecutiveModifier controlExecModifier(controlExec);
// index of the relevant manipulator. cytonRobot simulation has only one manipulator
// 7dof cyton robot with a two finger gripper
const EcU32 manipIndex = 0;
// cytonRobot model has two motion constraint sets.
// Point EE set has point motion constraint attached to wrist-roll link.
// Frame EE Set has frame constraint attached to the wrist-roll link.
const EcU32 frameEeSetIndex = 1;
const EcU32 eeIndex = 0;
//-----------------------------------------
// Configure the motion sequence
//
// (motion_seq
// (move_joint 0 (-102.72deg 26.62deg 45.66deg -40.41deg -70.715deg -60.149deg 43.256deg) 1.0 1e-6)
// (pose_seq 0 1 0
// (move_pose 0 1 0 ((0.1 0 0.35)(0 -90deg -90deg)) 1.0 0.02)
// (move_pose 0 1 0 ((0.1 0.1 0.25)(0 -90deg -90deg)) 1.0 0.02)
// (move_pose 0 1 0 ((0.1 0.1 0.35)(0 -90deg -90deg)) 1.0 0.02)
// (move_pose 0 1 0 ((0.1 0 0.35)(0 -90deg -90deg)) 1.0 0.02)
// )
// )
//
//-----------------------------------------
EcMotionScriptSequenceSharedPtr pMotionSeq = boost::make_shared<EcMotionScriptSequence>();
const EcReal speedFactor = 1.0;
// move joint
{
EcEndEffectorMotionMoveJointSharedPtr pMoveJoint = boost::make_shared<EcEndEffectorMotionMoveJoint>();
pMoveJoint->setManipulatorIndex(manipIndex);
EcRealVector desiredJoint = boost::assign::list_of(-102.72)(26.62)(45.66)(-40.41)(-70.715)(-60.149)(43.256);
for (EcSizeT ii = 0; ii < desiredJoint.size(); ++ii)
{
desiredJoint[ii] *= EcDEG2RAD;
}
pMoveJoint->moveJoint(desiredJoint, speedFactor, EcMICRO, EcTrue);
pMotionSeq->addMotion(pMoveJoint);
}
// pose sequence
{
EcEndEffectorMotionMovePoseSequenceSharedPtr pMovePoseSeq = boost::make_shared<EcEndEffectorMotionMovePoseSequence>();
pMovePoseSeq->setManipulatorIndex(manipIndex);
pMovePoseSeq->setEeSetIndex(frameEeSetIndex);
pMovePoseSeq->setEeIndex(eeIndex);
EcOrientation orientation;
orientation.setFrom123Euler(0, -EcPi * 0.5, -EcPi * 0.5);
EcVector p0(0.1, 0, 0.35);
EcCoordinateSystemTransformation wp0(p0, orientation);
{
// way-point 0
const EcReal blendRadius = 0.02;
pMovePoseSeq->movePose(wp0, speedFactor, blendRadius);
}
{
// way-point 1
EcVector p1(0.1, 0.1, 0.25);
EcCoordinateSystemTransformation wp1(p1, orientation);
const EcReal blendRadius = 0.02;
pMovePoseSeq->movePose(wp1, speedFactor, blendRadius);
}
{
// way-point 2
EcVector p2(0.1, 0.1, 0.35);
EcCoordinateSystemTransformation wp2(p2, orientation);
const EcReal blendRadius = 0.02;
pMovePoseSeq->movePose(wp2, speedFactor, blendRadius);
}
{
// back to way-point 0
const EcReal tolerance = EcMILLI;
pMovePoseSeq->movePose(wp0, speedFactor, tolerance);
}
pMotionSeq->addMotion(pMovePoseSeq);
}
//-----------------------------------------
// main loop
//-----------------------------------------
std::cout << "Running motion script..." << std::endl;
EcMotionScriptObject::Status status = EcMotionScriptObject::IN_PROGRESS;
while (status == EcMotionScriptObject::IN_PROGRESS)
{
// run motion
status = pMotionSeq->run(controlExecModifier, state.time());
EcReal progress = pMotionSeq->progress();
printProgressBar(progress);
// calculate state
controlExecModifier.calculateStateTimeSteps(state);
}
std::cout << std::endl;
std::cout << "Motion script finished, time: " << state.time() << std::endl;
}
//------------------------------------------------------------------------------
void
(
)
{
//-----------------------------------------
// Configure system control executive
//-----------------------------------------
#if EC_HAS_boost_filesystem
// add the file location to the search path
EcString("/actinExamples"));
// declare the filename
EcString filename = "cytonRobotControlExec.xml";
// load the system control executive from an XML file
std::cout << "Loading system control executive from file: " << filename << "...";
EcBoolean result = EcXmlObjectReaderWriter::readFromFile(controlExec, filename);
if (!result)
{
std::cout << std::endl << "Could not load system control executive." << std::endl;
}
std::cout << "DONE" << std::endl;
#else
// load the system control executive from header
std::cout << "Loading system control executive from header...";
EcString stringBuffer(cytonRobotControlExec, cytonRobotControlExec + sizeof(cytonRobotControlExec));
EcBoolean result = EcXmlObjectReaderWriter::readFromBuffer(controlExec, stringBuffer);
if (!result)
{
std::cout << std::endl << "Could not load system control executive." << std::endl;
}
std::cout << "DONE" << std::endl;
#endif
// create the modifier
EcControlSystemExecutiveModifier controlExecModifier(controlExec);
// index of the relevant manipulator. cytonRobot simulation has only one manipulator
// 7dof cyton robot with a two finger gripper
const EcU32 manipIndex = 0;
// cytonRobot model has two motion constraint sets.
// Point EE set has point motion constraint attached to wrist-roll link.
// Frame EE Set has frame constraint attached to the wrist-roll link.
const EcU32 frameEeSetIndex = 1;
const EcU32 eeIndex = 0;
//-----------------------------------------
// Configure the group motion manager
//
// (motion_seq
// (move_joint 0 (-102.72deg 26.62deg 45.66deg -40.41deg -70.715deg -60.149deg 43.256deg) 1.0 1e-6)
// (interp_pose_seq 0 1 0
// (move_linear 0 1 0 ((0.1 0 0.35)(0 -90deg -90deg)) 1.0 0.02)
// (move_linear 0 1 0 ((0.1 0.1 0.25)(0 -90deg -90deg)) 1.0 0.02)
// (move_linear 0 1 0 ((0.1 0.1 0.35)(0 -90deg -90deg)) 1.0 0.02)
// (move_linear 0 1 0 ((0.1 0 0.35)(0 -90deg -90deg)) 1.0 0.02)
// )
// )
//
//-----------------------------------------
EcGroupMotionManager groupMotion;
groupMotion.setManipulatorIndex(manipIndex);
// add motions
EcInt32Vector vMotionId(5);
// move direct
EcRealVector desiredJoint = boost::assign::list_of(-102.72)(26.62)(45.66)(-40.41)(-70.715)(-60.149)(43.256);
for (EcSizeT ii = 0; ii < desiredJoint.size(); ++ii)
{
desiredJoint[ii] *= EcDEG2RAD;
}
vMotionId[0] = groupMotion.moveDirectAbsolute(desiredJoint, EcVelocityController::JOINT_CONTROL_INDEX, eeIndex,
1.0, 1.0, 1.0, 1.0, EcGroupMotionManager::BM_ABORTING, EcGroupMotionManager::TM_CORNER_DISTANCE, EcMICRO);
// move linear
EcOrientation orientation;
orientation.setFrom123Euler(0, -EcPi * 0.5, -EcPi * 0.5);
// way-point 1
EcVector p1(0.1, 0, 0.35);
EcCoordinateSystemTransformation wp1(p1, orientation);
vMotionId[1] = groupMotion.moveLinearAbsolute(wp1, frameEeSetIndex, eeIndex,
1.0, 1.0, 1.0, 1.0, EcGroupMotionManager::BM_BLENDING_NEXT, EcGroupMotionManager::TM_CORNER_DISTANCE, 0.02);
// way-point 2
EcVector p2(0.1, 0.1, 0.25);
EcCoordinateSystemTransformation wp2(p2, orientation);
vMotionId[2] = groupMotion.moveLinearAbsolute(wp2, frameEeSetIndex, eeIndex,
1.0, 1.0, 1.0, 1.0, EcGroupMotionManager::BM_BLENDING_NEXT, EcGroupMotionManager::TM_CORNER_DISTANCE, 0.02);
// way-point 3
EcVector p3(0.1, 0.1, 0.35);
EcCoordinateSystemTransformation wp3(p3, orientation);
vMotionId[3] = groupMotion.moveLinearAbsolute(wp3, frameEeSetIndex, eeIndex,
1.0, 1.0, 1.0, 1.0, EcGroupMotionManager::BM_BLENDING_NEXT, EcGroupMotionManager::TM_CORNER_DISTANCE, 0.02);
// back to way-point 1
vMotionId[4] = groupMotion.moveLinearAbsolute(wp1, frameEeSetIndex, eeIndex,
1.0, 1.0, 1.0, 1.0, EcGroupMotionManager::BM_BLENDING_NEXT, EcGroupMotionManager::TM_CORNER_DISTANCE, 0.02);
//-----------------------------------------
// main loop
//-----------------------------------------
std::cout << "Running group motion manager..." << std::endl;
EcGroupMotionManager::Status groupStatus = EcGroupMotionManager::GROUP_MOVING;
EcSizeT currentMotion = 0;
std::cout << "Running motion ID = " << vMotionId[currentMotion] << "..." << std::endl;
while (groupStatus != EcGroupMotionManager::GROUP_STANDBY)
{
// run the group motion manager
groupMotion.run(controlExecModifier, state.time());
groupStatus = groupMotion.groupReadStatus();
// calculate state
controlExecModifier.calculateStateTimeSteps(state);
// print progress of each motion
EcReal motionProgress;
groupMotion.readMotionStatus(vMotionId[currentMotion], motionStatus, &motionProgress);
printProgressBar(motionProgress);
if (motionStatus == EcMotionScriptObject::SUCCEEDED)
{
if (currentMotion + 1 < vMotionId.size())
{
++currentMotion;
}
std::cout << std::endl << "Running motion ID = " << vMotionId[currentMotion] << "..." << std::endl;
}
}
std::cout << std::endl;
std::cout << "Group motion finished, time: " << state.time() << std::endl;
}
//------------------------------------------------------------------------------
void
(
const EcReal progress
)
{
EcInt32 barWidth = 100;
EcInt32 pos = static_cast<EcInt32>(barWidth * progress);
std::cout << "[";
for (EcInt32 ii = 0; ii < barWidth; ++ii)
{
if (ii < pos)
{
std::cout << "=";
}
else if (ii == pos)
{
std::cout << ">";
}
else
{
std::cout << " ";
}
}
std::cout << "] " << static_cast<EcInt32>(progress * 100) << " %\r";
std::cout.flush();
}
//------------------------------------------------------------------------------
// Copyright (c) 2017-2018 Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// \brief main entry point
// \return application status value.
int main
(
int argc,
char* argv[]
)
{
// get Actin license handle
{
std::cout << "EcActinLicenseHandle: Failed to get a valid license for Actin." << std::endl;
return 1;
}
// create and run the example
example.runEcScript();
// run the group motion manager example
// shutdown license handle
return 0;
}
Created by Energid Technologies www.energid.com
Copyright © 2016 Energid. All trademarks mentioned in this document are property of their respective owners.