Actin  Version 5.5.3
Software for Robotics Simulation and Control
ecCirclePathExampleDirection.cpp

An example that shows how to create a manipulation direction as extension
The header file for the EcCirclePathExampleDirection class

#ifndef ecCirclePathExampleDirection_H_
#define ecCirclePathExampleDirection_H_
//------------------------------------------------------------------------------
// Copyright (c) 2016 Energid Technologies. All rights reserved.
//
//
//
//------------------------------------------------------------------------------
{
public:
enum
{
X_AXIS,
Y_AXIS,
Z_AXIS
};
virtual void registerComponents
(
virtual EcU32 axis
(
) const;
virtual void setAxis
(
EcU32 axis
);
virtual const EcString& originPose
(
) const;
virtual void setOriginPose
(
const EcString& pose
);
virtual EcReal radius
(
) const;
virtual void setRadius
(
EcReal radius
);
virtual EcReal tolerance
(
) const;
virtual void setTolerance
(
EcReal tolerance
);
virtual const EcString descriptor
(
) const EC_OVERRIDE;
virtual EcBoolean init
(
const EcManipulationDirector& director,
const EcString& role,
) const EC_OVERRIDE;
(
EcReal time,
const EcManipulationDirector& director,
const EcString& role,
) const EC_OVERRIDE;
virtual void convert
(
) const EC_OVERRIDE;
(
) const EC_OVERRIDE;
virtual EcReal progress
(
) const EC_OVERRIDE;
(
const EcString& separator
) const EC_OVERRIDE;
protected:
EcXmlString m_OriginPose;
EcXmlEnumU32 m_Axis;
EcXmlReal m_Radius;
EcXmlReal m_Tolerance;
};
#endif // ecCirclePathExampleDirection_H_


The implementation file for the EcCirclePathExampleDirection class

//------------------------------------------------------------------------------
// Copyright (c) 2016 Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
#include "ecManipDirectorExtensionsTokens.h"
#include <ecboost/format.hpp>
EC_XMLOBJECT_PLUGIN_DEFINE(EcCirclePathExampleDirection, EcManipDirectorExtensionsTokens::EcCirclePathExampleDirectionToken)
//---
// EcCirclePathExampleDirection big four
//---
//---------------------------------------------------------------------------
(
) :
m_OriginPose(),
m_Axis(X_AXIS),
m_Radius(0.0),
m_Tolerance(0.001)
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection::~EcCirclePathExampleDirection
(
)
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection::EcCirclePathExampleDirection
(
) :
m_Axis(orig.m_Axis),
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection& EcCirclePathExampleDirection::operator=
(
)
{
// self assignment.
if (this==&orig)
{
return *this;
}
// call parent
// copy data
m_Axis = orig.m_Axis;
return *this;
}
//---------------------------------------------------------------------------
EcBoolean EcCirclePathExampleDirection::operator==
(
) const
{
return
(
EcManipulationDirection::operator==(orig) &&
(m_Axis == orig.m_Axis) &&
(m_Radius == orig.m_Radius) &&
);
}
//---
// EcCirclePathExampleDirection public methods
//---
//---------------------------------------------------------------------------
(
)
{
m_Axis.setEnumString(X_AXIS, EcManipDirectorExtensionsTokens::EcXAxisToken);
m_Axis.setEnumString(Y_AXIS, EcManipDirectorExtensionsTokens::EcYAxisToken);
m_Axis.setEnumString(Z_AXIS, EcManipDirectorExtensionsTokens::EcZAxisToken);
registerComponent(EcManipDirectorExtensionsTokens::EcOriginPoseToken, &m_OriginPose);
registerComponent(EcManipDirectorExtensionsTokens::EcAxisToken, &m_Axis);
registerComponent(EcManipDirectorExtensionsTokens::EcRadiusToken, &m_Radius);
registerComponent(EcManipDirectorExtensionsTokens::EcToleranceToken, &m_Tolerance);
}
//---------------------------------------------------------------------------
(
) const
{
return m_Axis.value();
}
//---------------------------------------------------------------------------
(
)
{
}
//---------------------------------------------------------------------------
(
) const
{
return m_OriginPose.value();
}
//---------------------------------------------------------------------------
(
const EcString& pose
)
{
}
//---------------------------------------------------------------------------
(
) const
{
return m_Radius.value();
}
//---------------------------------------------------------------------------
(
)
{
m_Radius.setValue(radius);
}
//---------------------------------------------------------------------------
(
) const
{
return m_Tolerance.value();
}
//---------------------------------------------------------------------------
(
)
{
m_Tolerance.setValue(tolerance);
}
//---------------------------------------------------------------------------
(
) const
{
if (!m_Tag.value().empty())
{
return m_Tag.value();
}
return str(ecboost::format("Circle Path Around %1%") % originPose());
}
//---------------------------------------------------------------------------
(
const EcManipulationDirector& director,
const EcString& role,
) const
{
dynamic_cast<EcCirclePathExampleDirectionState*>(dstateContainer.element());
EcString name;
if (!director.nameFromRole(role, name))
{
ret = EcFalse;
}
//TODO: map
//pMyState->setManipIndex(manipIndex);
// here all possible error conditions need to be evaluated
// return EcFalse if the condition has to be met to continue further
// otherwise, add error string pMyState->addErrorString("radius <= 0.0"); set ret as EcFalse and continue
if (radius() <= 0.0)
{
pMyState->addErrorString("radius <= 0.0");
ret = EcFalse;
}
if (tolerance() <= 0.0)
{
pMyState->addErrorString("tolerance <= 0.0");
ret = EcFalse;
}
const EcString& poseName = originPose();
if (poseName.empty())
{
pMyState->addErrorString("origin pose must be defined");
ret = EcFalse;
}
else
{
const EcDirectorObject* pOriginPoseObject = director.objectDirectory().object(poseName);
if (!pOriginPoseObject)
{
pMyState->addErrorString(str(ecboost::format("Origin pose %1% not found") % poseName));
ret = EcFalse;
}
else
{
const EcPose* pOriginPose = pOriginPoseObject->pose();
if (!pOriginPose)
{
pMyState->addErrorString(str(ecboost::format("Origin pose %1% not valid") % poseName));
ret = EcFalse;
}
else
{
pMyState->setOriginPose(pOriginPose);
}
}
}
if (ret)
{
const EcReal r = radius();
const EcVector t = (axis() == Z_AXIS) ? EcVector::xVector(r) : EcVector::zVector(r);
const EcReal da = 2.0 * asin(tolerance() / (2.0 * r));
for (EcReal angle = 0.0; angle < Ec2Pi; angle += da)
{
const EcOrientation o = (axis() == X_AXIS) ? EcOrientation::xRotation(angle) : (axis() == Y_AXIS) ? EcOrientation::yRotation(angle) : EcOrientation::zRotation(angle);
if ((angle + da) >= Ec2Pi)
{
}
}
pMyState->setPathXforms(pathXforms);
}
return ret;
}
//---------------------------------------------------------------------------
(
EcReal time,
const EcManipulationDirector& director,
const EcString& role,
) const
{
// call to compute elapsed time
EcManipulationDirection::update(time, director, params, role, dstate, sdstate);
dynamic_cast<EcCirclePathExampleDirectionState&>(sdstate);
const EcCoordinateSystemTransformationVector& pathXforms = myState.pathXforms();
const EcU32 pathIndex = myState.pathIndex();
const EcSizeT pathSize = pathXforms.size();
// Any error conditions can be displayed by adding error string as pMyState->addErrorString("radius <= 0.0");
if (!pathSize || (pathIndex >= pathSize))
{
return EcManipulationStatus::FAILED;
}
// Get the current transformation of the origin
if (!myState.originPose()->offsetInSystem(params.statedSystem(), target))
{
return EcManipulationStatus::FAILED;
}
// Set the target to the appropriate point on the circle path
const EcCoordinateSystemTransformation& pathXform = pathXforms[pathIndex];
target.setTranslation(target * pathXform.orientation() * pathXform.translation());
const EcU32 manipIndex = myState.manipIndex();
const EcPositionControlSystem& posContSystem = params.manipPositionControlSystem();
const EcPositionController& posController = posContSystem.positionControllers()[manipIndex];
// Get the actual placement
const EcManipulatorEndEffectorPlacementVector& manipPlacementVector = params.actualPlacements();
const EcManipulatorEndEffectorPlacement& actualPlacement = manipPlacementVector[manipIndex];
const EcEndEffectorPlacementVector& actualTransformations = actualPlacement.offsetTransformations();
// Set the desired placement
EcManipulatorEndEffectorPlacement desiredPlacement = actualPlacement;
EcEndEffectorPlacementVector desiredTransformations = desiredPlacement.offsetTransformations();
EcEndEffectorPlacement& desiredTransformation = desiredTransformations[0];
desiredTransformation.setCoordSysXForm(target);
desiredPlacement.setOffsetTransformations(desiredTransformations);
activeState.setEndEffectorPlacement(desiredPlacement);
// Determine the current end effector delta (compare actual to desired)
const EcU32 eeSetIndex = posController.activeEndEffectorSetIndex();
const EcEndEffectorVector& eeVector = (eeSetIndex == EcVelocityController::JOINT_CONTROL_INDEX) ? posController.jointControlEndEffectorSet().endEffectors() : posController.endEffectorSets()[eeSetIndex].endEffectors();
const EcReal delta = eeVector[0].difference(actualTransformations[0], desiredTransformation);
// Move to the next point in the circle if the end effector delta is below the tolerance
if (delta < tolerance())
{
if ((pathIndex + 1) >= pathSize)
{
myState.setProgress(1.0);
return EcManipulationStatus::SUCCESS;
}
myState.setPathIndex(pathIndex + 1);
}
// Set the progress
if (myState.pathIndex() > 0)
{
myState.setProgress(myState.pathIndex() / EcReal(pathSize));
}
return EcManipulationStatus::IN_PROGRESS;
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<EcBasicDirectionSimpleState*>(sdstateContainer.element());
if (!pBasicState)
{
pBasicState = dynamic_cast<EcBasicDirectionSimpleState*>(sdstateContainer.element());
}
const EcManipulationSceneDirectionState* pState = dstateContainer.element();
pBasicState->setProgress((pState) ? progress(*pState) : 0.0);
pBasicState->setElapsedTime((pState) ? elapsedTime(*pState) : 0.0);
if (pState)
{
pBasicState->setDirectorIssues(pState->directorIssues()); // To display the issues over the block
}
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<const EcCirclePathExampleDirectionState&>(dstate);
return &activeState;
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<const EcCirclePathExampleDirectionState&>(dstate);
return myState.progress();
}
//---------------------------------------------------------------------------
(
const EcString& separator
) const
{
return descriptor();
}
//------------------------------------------------------------------------------
// Copyright (c) 2016 Energid Technologies. All rights reserved.
//
//
//------------------------------------------------------------------------------
#include "ecManipDirectorExtensionsTokens.h"
#include <ecboost/format.hpp>
EC_XMLOBJECT_PLUGIN_DEFINE(EcCirclePathExampleDirection, EcManipDirectorExtensionsTokens::EcCirclePathExampleDirectionToken)
//---
// EcCirclePathExampleDirection big four
//---
//---------------------------------------------------------------------------
(
) :
m_OriginPose(),
m_Axis(X_AXIS),
m_Radius(0.0),
m_Tolerance(0.001)
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection::~EcCirclePathExampleDirection
(
)
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection::EcCirclePathExampleDirection
(
) :
m_Axis(orig.m_Axis),
{
}
//---------------------------------------------------------------------------
EcCirclePathExampleDirection& EcCirclePathExampleDirection::operator=
(
)
{
// self assignment.
if (this==&orig)
{
return *this;
}
// call parent
// copy data
m_Axis = orig.m_Axis;
return *this;
}
//---------------------------------------------------------------------------
EcBoolean EcCirclePathExampleDirection::operator==
(
) const
{
return
(
EcManipulationDirection::operator==(orig) &&
(m_Axis == orig.m_Axis) &&
(m_Radius == orig.m_Radius) &&
);
}
//---
// EcCirclePathExampleDirection public methods
//---
//---------------------------------------------------------------------------
(
)
{
m_Axis.setEnumString(X_AXIS, EcManipDirectorExtensionsTokens::EcXAxisToken);
m_Axis.setEnumString(Y_AXIS, EcManipDirectorExtensionsTokens::EcYAxisToken);
m_Axis.setEnumString(Z_AXIS, EcManipDirectorExtensionsTokens::EcZAxisToken);
registerComponent(EcManipDirectorExtensionsTokens::EcOriginPoseToken, &m_OriginPose);
registerComponent(EcManipDirectorExtensionsTokens::EcAxisToken, &m_Axis);
registerComponent(EcManipDirectorExtensionsTokens::EcRadiusToken, &m_Radius);
registerComponent(EcManipDirectorExtensionsTokens::EcToleranceToken, &m_Tolerance);
}
//---------------------------------------------------------------------------
(
) const
{
return m_Axis.value();
}
//---------------------------------------------------------------------------
(
)
{
}
//---------------------------------------------------------------------------
(
) const
{
return m_OriginPose.value();
}
//---------------------------------------------------------------------------
(
const EcString& pose
)
{
}
//---------------------------------------------------------------------------
(
) const
{
return m_Radius.value();
}
//---------------------------------------------------------------------------
(
)
{
m_Radius.setValue(radius);
}
//---------------------------------------------------------------------------
(
) const
{
return m_Tolerance.value();
}
//---------------------------------------------------------------------------
(
)
{
m_Tolerance.setValue(tolerance);
}
//---------------------------------------------------------------------------
(
) const
{
if (!m_Tag.value().empty())
{
return m_Tag.value();
}
return str(ecboost::format("Circle Path Around %1%") % originPose());
}
//---------------------------------------------------------------------------
(
const EcManipulationDirector& director,
const EcString& role,
) const
{
dynamic_cast<EcCirclePathExampleDirectionState*>(dstateContainer.element());
EcString name;
if (!director.nameFromRole(role, name))
{
ret = EcFalse;
}
//TODO: map
//pMyState->setManipIndex(manipIndex);
// here all possible error conditions need to be evaluated
// return EcFalse if the condition has to be met to continue further
// otherwise, add error string pMyState->addErrorString("radius <= 0.0"); set ret as EcFalse and continue
if (radius() <= 0.0)
{
pMyState->addErrorString("radius <= 0.0");
ret = EcFalse;
}
if (tolerance() <= 0.0)
{
pMyState->addErrorString("tolerance <= 0.0");
ret = EcFalse;
}
const EcString& poseName = originPose();
if (poseName.empty())
{
pMyState->addErrorString("origin pose must be defined");
ret = EcFalse;
}
else
{
const EcDirectorObject* pOriginPoseObject = director.objectDirectory().object(poseName);
if (!pOriginPoseObject)
{
pMyState->addErrorString(str(ecboost::format("Origin pose %1% not found") % poseName));
ret = EcFalse;
}
else
{
const EcPose* pOriginPose = pOriginPoseObject->pose();
if (!pOriginPose)
{
pMyState->addErrorString(str(ecboost::format("Origin pose %1% not valid") % poseName));
ret = EcFalse;
}
else
{
pMyState->setOriginPose(pOriginPose);
}
}
}
if (ret)
{
const EcReal r = radius();
const EcVector t = (axis() == Z_AXIS) ? EcVector::xVector(r) : EcVector::zVector(r);
const EcReal da = 2.0 * asin(tolerance() / (2.0 * r));
for (EcReal angle = 0.0; angle < Ec2Pi; angle += da)
{
const EcOrientation o = (axis() == X_AXIS) ? EcOrientation::xRotation(angle) : (axis() == Y_AXIS) ? EcOrientation::yRotation(angle) : EcOrientation::zRotation(angle);
if ((angle + da) >= Ec2Pi)
{
}
}
pMyState->setPathXforms(pathXforms);
}
return ret;
}
//---------------------------------------------------------------------------
(
EcReal time,
const EcManipulationDirector& director,
const EcString& role,
) const
{
// call to compute elapsed time
EcManipulationDirection::update(time, director, params, role, dstate, sdstate);
dynamic_cast<EcCirclePathExampleDirectionState&>(sdstate);
const EcCoordinateSystemTransformationVector& pathXforms = myState.pathXforms();
const EcU32 pathIndex = myState.pathIndex();
const EcSizeT pathSize = pathXforms.size();
// Any error conditions can be displayed by adding error string as pMyState->addErrorString("radius <= 0.0");
if (!pathSize || (pathIndex >= pathSize))
{
return EcManipulationStatus::FAILED;
}
// Get the current transformation of the origin
if (!myState.originPose()->offsetInSystem(params.statedSystem(), target))
{
return EcManipulationStatus::FAILED;
}
// Set the target to the appropriate point on the circle path
const EcCoordinateSystemTransformation& pathXform = pathXforms[pathIndex];
target.setTranslation(target * pathXform.orientation() * pathXform.translation());
const EcU32 manipIndex = myState.manipIndex();
const EcPositionControlSystem& posContSystem = params.manipPositionControlSystem();
const EcPositionController& posController = posContSystem.positionControllers()[manipIndex];
// Get the actual placement
const EcManipulatorEndEffectorPlacementVector& manipPlacementVector = params.actualPlacements();
const EcManipulatorEndEffectorPlacement& actualPlacement = manipPlacementVector[manipIndex];
const EcEndEffectorPlacementVector& actualTransformations = actualPlacement.offsetTransformations();
// Set the desired placement
EcManipulatorEndEffectorPlacement desiredPlacement = actualPlacement;
EcEndEffectorPlacementVector desiredTransformations = desiredPlacement.offsetTransformations();
EcEndEffectorPlacement& desiredTransformation = desiredTransformations[0];
desiredTransformation.setCoordSysXForm(target);
desiredPlacement.setOffsetTransformations(desiredTransformations);
activeState.setEndEffectorPlacement(desiredPlacement);
// Determine the current end effector delta (compare actual to desired)
const EcU32 eeSetIndex = posController.activeEndEffectorSetIndex();
const EcEndEffectorVector& eeVector = (eeSetIndex == EcVelocityController::JOINT_CONTROL_INDEX) ? posController.jointControlEndEffectorSet().endEffectors() : posController.endEffectorSets()[eeSetIndex].endEffectors();
const EcReal delta = eeVector[0].difference(actualTransformations[0], desiredTransformation);
// Move to the next point in the circle if the end effector delta is below the tolerance
if (delta < tolerance())
{
if ((pathIndex + 1) >= pathSize)
{
myState.setProgress(1.0);
return EcManipulationStatus::SUCCESS;
}
myState.setPathIndex(pathIndex + 1);
}
// Set the progress
if (myState.pathIndex() > 0)
{
myState.setProgress(myState.pathIndex() / EcReal(pathSize));
}
return EcManipulationStatus::IN_PROGRESS;
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<EcBasicDirectionSimpleState*>(sdstateContainer.element());
if (!pBasicState)
{
pBasicState = dynamic_cast<EcBasicDirectionSimpleState*>(sdstateContainer.element());
}
const EcManipulationSceneDirectionState* pState = dstateContainer.element();
pBasicState->setProgress((pState) ? progress(*pState) : 0.0);
pBasicState->setElapsedTime((pState) ? elapsedTime(*pState) : 0.0);
if (pState)
{
pBasicState->setDirectorIssues(pState->directorIssues()); // To display the issues over the block
}
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<const EcCirclePathExampleDirectionState&>(dstate);
return &activeState;
}
//---------------------------------------------------------------------------
(
) const
{
dynamic_cast<const EcCirclePathExampleDirectionState&>(dstate);
return myState.progress();
}
//---------------------------------------------------------------------------
(
const EcString& separator
) const
{
return descriptor();
}