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

An example of loading a simulation file, Forward kinematics (given joint states, calculate EE (motion constraint) placements), and Inverse Kinematics (given desired EE (motion constraint) placements calculate position state (joints and base)
The header file for the EcQuickStartKinematicsExample class

#ifndef ecQuickStartKinematicsExample_H_
#define ecQuickStartKinematicsExample_H_
//------------------------------------------------------------------------------
// Copyright (c) 2016- Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
{
public:
virtual void run();
};
#endif // ecQuickStartKinematicsExample_H_


The implementation file for the EcQuickStartKinematicsExample class

//------------------------------------------------------------------------------
// Copyright (c) 2016- Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
// Functions: Big four and other header functions.
// Description:
// constructor
{
}
// destructor
{
}
// copy constructor
{
}
// assignment operator
{
return *this;
}
//End of header functions
// Functions: run()
// Description: Executes the quick-start code.
{
// -----------------------------------------------------------------
// Example section #1 - Loading simulation from a file.
// -----------------------------------------------------------------
// declare an error return code
EcBoolean success;
// add the file location to the search path
EcString("/actinExamples"));
// declare a filename
EcString filename="cytonRobot.xml";
// a variable holding the simulation
EcSystemSimulation simulation;
// load the system simulation from an XML file
success = EcXmlObjectReaderWriter::readFromFile(simulation, filename);
// make sure it loaded properly
if(!success)
{
EcPrint(Error)<<"Could not load system simulation."<<std::endl;
return;
}
// index of the relevant manipulator. cytonRobot simulation has only one manipulator - 7dof cyton robot with a two finger gripper
const EcU32 manipIndex = 0;
// the robot 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;
// -----------------------------------------------------------------
// End example section #1
// -----------------------------------------------------------------
// -----------------------------------------------------------------
// Example section #2 - Forward Kinematics.
// -----------------------------------------------------------------
std::cout << "\n*************** Forward Kinematics ***************" << std::endl;
// cache and create utility objects
const EcIndividualManipulator& manipulator = simulation.statedSystem().system().manipulators()[manipIndex];
const EcPositionController& posController = simulation.positionControlSystem().positionControllers()[manipIndex];
const EcEndEffectorSetVector& eeSets = posController.endEffectorSets();
// get frame motion constraint. Check vector indices (these should be correct for the model we load and the motion constraint (ee) indices we use)
if (frameEeSetIndex >= eeSets.size())
{
EcPrint(Error) << "Invalid EE Set Index: " << frameEeSetIndex << std::endl;
return;
}
if(eeIndex >= eeSets[frameEeSetIndex].endEffectors().size())
{
EcPrint(Error) << "Invalid EE Index: " << eeIndex << std::endl;
return;
}
const EcEndEffector& ee = eeSets[frameEeSetIndex].endEffectors()[eeIndex];
// Get current state of the manipulator
EcPositionState positionState = simulation.statedSystem().state().positionStates()[manipIndex];
// keep the base location same, update the joint state
EcXmlRealVector& jointPositions = positionState.jointPositions();
std::cout << "\n------ Current Joint States ------" << std::endl;
for (EcSizeT ii = 0; ii < jointPositions.size(); ++ii)
{
std::cout << "Joint " << ii << ": " << jointPositions[ii]*EcRAD2DEG<< " degree" << std::endl;
}
//for (EcSizeT jj = 0; jj < jointPositions.size(); ++jj)
//{
// jointPositions[jj] += 10 * EcDEG2RAD;
//}
// or
// update joint positions with absolute values. Insert values in radians.
jointPositions[0] = 10 * EcDEG2RAD;
jointPositions[1] = 20 * EcDEG2RAD;
jointPositions[2] = 30 * EcDEG2RAD;
jointPositions[3] = 40 * EcDEG2RAD;
jointPositions[4] = 50 * EcDEG2RAD;
jointPositions[5] = 60 * EcDEG2RAD;
jointPositions[6] = 70 * EcDEG2RAD;
std::cout << "\n------ New Joint States ------" << std::endl;
for (EcSizeT ii = 0; ii < jointPositions.size(); ++ii)
{
std::cout << "Joint " << ii << ": " << jointPositions[ii] * EcRAD2DEG << " degree" << std::endl;
}
// Get Actual Placements
activeState.setPositionState(positionState);
ee.calculatePlacementInRelative(manipulator, activeState, placement);
std::cout << "\n------- Calculated Frame EE location ------" << std::endl;
std::cout << "Translation: " << placement.coordSysXForm().translation() << std::endl;
EcReal roll, pitch, yaw;
placement.coordSysXForm().orientation().get123Euler(roll, pitch, yaw);
std::cout << "Orientation: " << roll*EcRAD2DEG << " (roll), " << pitch*EcRAD2DEG << " (pitch), " << yaw*EcRAD2DEG << " (yaw)" << std::endl;
// -----------------------------------------------------------------
// End example section #2
// -----------------------------------------------------------------
// -----------------------------------------------------------------
// Example section #3 - Inverse Kinematics. Inverse kinematics for control
// -----------------------------------------------------------------
//*** In this inverse kinematics example, we move the manipulator from its initial/current state
// to the desired placement in a straight line (EE space).
std::cout << "\n*************** Inverse Kinematics ***************" << std::endl;
// We can configure initial state as follows
state.setTime(0);
EcXmlRealVector initialJointPositions(7); // all joints 0
state.positionStates()[manipIndex].jointPositions() = initialJointPositions;
simulation.setState(state);
std::cout << "\n------ Initial Joint States ------" << std::endl;
for (EcSizeT ii = 0; ii < initialJointPositions.size(); ++ii)
{
std::cout << "Joint " << ii << ": " << initialJointPositions[ii] * EcRAD2DEG << " degree" << std::endl;
}
// set active motion constraint set (frame ee)
EcPositionControlSystem posControlSystem = simulation.positionControlSystem();
posControlSystem.setActiveEndEffectorSet(manipIndex, frameEeSetIndex);
// Set desired placements.
EcEndEffectorPlacement desiredPlacement;
// 6dof placement for frame ee in system coordinates(assuming the configured frame ee is not relative to any other link)
EcReal x, y, z, desiredRoll, desiredPitch, desiredYaw;
x = 0.100; // x coordinate in meters
y = -0.250; // y coordinate in meters
z = 0.350; // z coordinate in meters
desiredRoll = -160 * EcDEG2RAD; // rotation about x axis in radians
desiredPitch = -20 * EcDEG2RAD; // rotation about y axis in radians
desiredYaw = 45 * EcDEG2RAD; // rotation about z axis in radians
EcOrientation desiredOrien;
desiredOrien.setFrom123Euler(desiredRoll, desiredPitch, desiredYaw);
desiredPlacement.coordSysXForm().setTranslation(EcVector(x, y, z));
desiredPlacement.coordSysXForm().setOrientation(desiredOrien);
posControlSystem.setDesiredPlacement(manipIndex, eeIndex, desiredPlacement);
std::cout << "\n------- Desired Frame EE Placement ------" << std::endl;
std::cout << "Translation: " << x <<", " << y << ", " << z << std::endl;
std::cout << "Orientation: " << desiredRoll*EcRAD2DEG << " (roll), " << desiredPitch*EcRAD2DEG << " (pitch), " << desiredYaw*EcRAD2DEG << " (yaw)" << std::endl;
// The control system drives the robot to the desired placement in a straight line.
// We calculate state in discrete time steps up until max time is reached or the desired placement is reached.
// Going through control system ensures configured motion constraint settings and max joint speeds are satisfied.
EcReal tolerance = 1e-5;
EcReal maxTime = 10.0; // maximum 10 simulation seconds to reach the target
EcReal timeStep = 0.02; // calculate state every 20ms
EcU32 maxSteps = static_cast<EcU32>(maxTime / timeStep);
EcBoolean targetReached = EcFalse;
for (EcU32 ss = 0; ss < maxSteps; ++ss)
{
EcReal currentTime = ss*timeStep;
posControlSystem.calculateState(currentTime, newState);
// Check if we reached target (desired) position (within tolerance)
const EcEndEffectorPlacement& actualPlacement = posControlSystem.actualPlacement(manipIndex, eeIndex);
if (actualPlacement.approxEq(desiredPlacement, tolerance))
{
targetReached = EcTrue;
break;
}
// Check for any collision/singularities/joint limits during motion
const EcPropagationResultData& result = posControlSystem.propagationResultDataVector()[manipIndex];
if (result.exceptionOccurred())
{
if (result.hitJointLimit())
{
std::cout << "Joint Limit!" << std::endl;
// Use propgatation result data to know which joint(s) hit limits ...
}
if (result.collisionOccurred())
{
std::cout << "Collision!" << std::endl;
// use propagation result data to know which links collided ...
}
if (result.hitSingularity())
{
std::cout << "Singularity!" << std::endl;
}
break;
}
}
if (targetReached)
{
// Get joint state corresponding to desired (or converged actual) placements
const EcXmlRealVector& finalJointPositions = newState.positionStates()[manipIndex].jointPositions();
std::cout << "\n------ Final Joint States ------" << std::endl;
for (EcSizeT ii = 0; ii < finalJointPositions.size(); ++ii)
{
std::cout << "Joint " << ii << ": " << finalJointPositions[ii] * EcRAD2DEG << " degree" << std::endl;
}
}
else
{
EcPrint(Warning) << "*Cannot not converge to the desired placement in given time..." << std::endl;
}
// -----------------------------------------------------------------
// End example section #3
// -----------------------------------------------------------------
// -----------------------------------------------------------------
// Inverse Kinematics using RRT Path Planner
// -----------------------------------------------------------------
//*** RRT path planner tool can be used to find the joint configuration corresponding to
// the desired placement. Unlike Example #3, the path planner tool does not require a straight line motion from current placement
// to the desired placement. The path planner result gives a series of joint configurations required to reach the desired placement from the current state
// avoiding joint limits, collision and singularities.
// Hence RRT path planner can be used to go to the first point on the desired path from the current state, and from then onwards inverse
// kinematics as described in Example #3 or quickStartExample can be used to follow the continous path trajectory (discrete small straight line motions).
// finish
EcPRINT("\nSuccessfully completed example.\n");
}


main() for the quick start kinematics example

//------------------------------------------------------------------------------
// Copyright (c) 2016- Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
int main(int argc, char* argv[])
{
// set the MCML lanauge.
// Get Actin license handle
{
EcPrint(Error) << "Failed to get valid license for Actin" << std::endl;
return 0;
}
// create and run the example
example.run();
return 0;
}
Created by Energid Technologies www.energid.com
Copyright © 2016 Energid. All trademarks mentioned in this document are property of their respective owners.