Skip to content
Snippets Groups Projects
Commit 66742d4c authored by David Schiebener's avatar David Schiebener
Browse files

added MoveTCPTrajectory functionality 

git-svn-id: https://svn.sfb588.uni-karlsruhe.de/svn/HumanoidRobotAPI@10 e7ad446d-f7d8-486a-9efb-df2f5bbea5c6
parent 43380679
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
motioncontrol/MotionControl.cpp
+ 184
− 31
View file @ 66742d4c
......@@ -18,24 +18,30 @@ MotionControl::MotionControl()
{
}
void MotionControl::onInitRemoteStateHandler()
{
addState<MoveJoints>("MoveJoints");
addState<MotionControlTestState>("MotionControlTestState");
}
void MotionControl::onConnectRemoteStateHandler()
{
}
MoveJoints::MoveJoints()
{
}
void MoveJoints::defineParameters()
{
context = getContext<RobotStatechartContext>();
std::string configFilepathAbsolute;
......@@ -53,16 +59,13 @@ void MoveJoints::defineParameters()
addToInput("timeoutInMs", VariantType::Int, false);
addListToOutput("jointDistancesToTargets", VariantType::Float, true);
}
}
void MoveJoints::onEnter()
{
NameValueMap targetJointAngles;
SingleTypeVariantListPtr jointNames = getInputList("jointNames");
SingleTypeVariantListPtr targetJointValues = getInputList("targetJointValues");
......@@ -81,7 +84,7 @@ void MoveJoints::onEnter()
// now install the condition
ConditionPtr cond = Condition::create();
ExpressionPtr cond = Expression::create();
float tolerance = getInput<float>("targetTolerance");
for(int i=0; i<jointNames->getSize(); i++ )
{
......@@ -99,21 +102,22 @@ void MoveJoints::onEnter()
context->kinematicUnitPrx->setJointAngles(targetJointAngles);
}
void MoveJoints::onBreak()
{
removeCondition(targetReachedCondition);
removeTimeoutEvent(timeoutEvent);
}
void MoveJoints::onExit()
{
SingleTypeVariantListPtr jointNames = getInputList("jointNames");
SingleTypeVariantListPtr targetJointValues = getInputList("targetJointValues");
SingleTypeVariantList resultList(VariantType::Float);
for(int i=0; i<jointNames->getSize(); i++ )
{
DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getElement(i)->getString());
......@@ -129,9 +133,10 @@ void MoveJoints::onExit()
removeTimeoutEvent(timeoutEvent);
}
void MoveJointsVelocityControl::defineParameters()
{
context = getContext<RobotStatechartContext>();
std::string configFilepathAbsolute;
......@@ -152,13 +157,12 @@ void MoveJointsVelocityControl::defineParameters()
addToInput("timeoutInMs", VariantType::Int, false);
addListToOutput("jointVelocitiesDistancesToTargets", VariantType::Float, true);
}
}
void MoveJointsVelocityControl::onEnter()
{
float accelerationTime = getInput<float>("accelerationTime");
NameValueMap targetJointVelocities;
SingleTypeVariantListPtr jointNames = getInputList("jointNames");
......@@ -169,7 +173,7 @@ void MoveJointsVelocityControl::onEnter()
throw LocalException("Sizes of joint name list and joint velocities list do not match!");
}
for(int i=0; i<jointNames->getSize(); i++ )
for(int i=0; i<jointNames->getSize(); i++)
{
targetJointVelocities[jointNames->getElement(i)->getString()] = targetVelocitiesValues->getElement(i)->getFloat();
ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getElement(i)->getString() << "' to " << targetVelocitiesValues->getElement(i)->getFloat() << std::endl;
......@@ -178,9 +182,9 @@ void MoveJointsVelocityControl::onEnter()
// now install the condition
ConditionPtr cond = Condition::create();
ExpressionPtr cond = Expression::create();
float tolerance = getInput<float>("targetJointVelocityTolerance");
for(int i=0; i<jointNames->getSize(); i++ )
for(int i=0; i<jointNames->getSize(); i++)
{
ParameterList inrangeCheck;
inrangeCheck.push_back(new Variant(targetVelocitiesValues->getElement(i)->getFloat()-tolerance));
......@@ -197,16 +201,15 @@ void MoveJointsVelocityControl::onEnter()
context->kinematicUnitPrx->setJointVelocities(targetJointVelocities);
}
void MoveJointsVelocityControl::onExit()
{
SingleTypeVariantListPtr jointNames = getInputList("jointNames");
SingleTypeVariantListPtr targetJointVelocities = getInputList("targetJointVelocities");
SingleTypeVariantList resultList(VariantType::Float);
for(int i=0; i<jointNames->getSize(); i++ )
for(int i=0; i<jointNames->getSize(); i++)
{
DataFieldIdentifierPtr datafield = new DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointvelocities", jointNames->getElement(i)->getString());
float jointVelocityActual = context->kinematicUnitObserverPrx->getDataField(datafield)->getFloat();
......@@ -219,12 +222,12 @@ void MoveJointsVelocityControl::onExit()
removeCondition(targetVelocitiesReachedCondition);
removeTimeoutEvent(timeoutEvent);
}
void MoveTCPPoseIK::defineParameters()
{
context = getContext<RobotStatechartContext>();
std::string configFilepathAbsolute;
......@@ -244,21 +247,24 @@ void MoveTCPPoseIK::defineParameters()
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
}
}
void MoveTCPPoseIK::onEnter()
{
timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
}
void MoveTCPPoseIK::onExit()
{
removeTimeoutEvent(timeoutEvent);
}
void MoveTCPPoseIK::defineSubstates()
{
StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
......@@ -266,17 +272,151 @@ void MoveTCPPoseIK::defineSubstates()
StatePtr validateJointAngleConfiguration = addState<ValidateJointAngleConfiguration>("ValidateJointAngleConfiguration");
StatePtr movingDone = addState<SuccessState>("movingDone") ;
StatePtr movingFailed = addState<FailureState>("movingFailed") ;
setInitState(calculateJointAngleConfiguration,
PM::createMapping()->mapFromParent("*","*"));
setInitState(calculateJointAngleConfiguration, PM::createMapping()->mapFromParent("*","*"));
addTransition<EvCalculationDone>(calculateJointAngleConfiguration, moveJoints, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvJointTargetReached>(moveJoints, validateJointAngleConfiguration, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvSuccess>(validateJointAngleConfiguration, movingDone);
addTransitionFromAllStates<EvTimeout>(movingFailed);
addTransitionFromAllStates<EvFailure>(movingFailed);
}
void MoveTCPTrajectory::defineParameters()
{
context = getContext<RobotStatechartContext>();
std::string configFilepathAbsolute;
ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
setConfigFile(configFilepathAbsolute);
addToInput("KinematicChainName", VariantType::String, false);
addListToInput("targetTCPPositions", VariantType::LinkedPosition, false);
addListToInput("targetTCPOrientations", VariantType::LinkedOrientation, false);
addToInput("tcpMaxSpeed", VariantType::Float, true);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
addToLocal("trajectoryPointCounterID", VariantType::String);
}
void MoveTCPTrajectory::onEnter()
{
timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
int numberOfPoints = getInputList("targetTCPPositions")->getSize();
if (numberOfPoints == 0)
{
sendEvent<EvSuccess>();
}
else if (numberOfPoints != getInputList("targetTCPOrientations")->getSize())
{
throw armarx::exceptions::local::eLogicError("the number of targetTCPPositions does not equal the number of targetTCPOrientations");
sendEvent<EvFailure>();
}
// create a counter to keep track of the poses that were already reached. As we dont use the event, set the max to n+1 so it will never be sent
trajectoryPointCounterID = setCounterEvent(numberOfPoints, createEvent<EvLastPointReached>());
setLocal("trajectoryPointCounterID", trajectoryPointCounterID.actionId->channelName);
}
void MoveTCPTrajectory::onExit()
{
removeTimeoutEvent(timeoutEvent);
removeCounterEvent(trajectoryPointCounterID);
}
void MoveTCPTrajectory::defineSubstates()
{
StatePtr moveTCPTrajectoryNextPoint = addState<MoveTCPTrajectoryNextPoint>("MoveTCPTrajectoryNextPoint");
StatePtr movingDone = addState<SuccessState>("movingDone");
StatePtr movingFailed = addState<FailureState>("movingFailed");
setInitState(moveTCPTrajectoryNextPoint, PM::createMapping()->mapFromParent("*","*"));
addTransition<EvSuccess>(moveTCPTrajectoryNextPoint, moveTCPTrajectoryNextPoint, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvLastPointReached>(moveTCPTrajectoryNextPoint, movingDone, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransitionFromAllStates<EvTimeout>(movingFailed);
addTransitionFromAllStates<EvFailure>(movingFailed);
}
void MoveTCPTrajectoryNextPoint::defineParameters()
{
addToInput("KinematicChainName", VariantType::String, false);
addListToInput("targetTCPPositions", VariantType::LinkedPosition, false);
addListToInput("targetTCPOrientations", VariantType::LinkedOrientation, false);
addToInput("tcpMaxSpeed", VariantType::Float, true);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
addToInput("trajectoryPointCounterID", VariantType::String, false);
addToLocal("targetTCPPosition", VariantType::LinkedPosition);
addToLocal("targetTCPOrientation", VariantType::LinkedOrientation);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
}
void MoveTCPTrajectoryNextPoint::onEnter()
{
context = getContext<RobotStatechartContext>();
std::string trajectoryPointCounterID = getInput<std::string>("trajectoryPointCounterID");
int counterValue = context->getDataFromObserver(new DataFieldIdentifier("SystemObserver", trajectoryPointCounterID, "value"))->getInt();
if (counterValue < getInputList("targetTCPPositions")->getSize())
{
setLocal("targetTCPPosition", *VariantPtr::dynamicCast(getInputList("targetTCPPositions")->getElement(counterValue)));
setLocal("targetTCPOrientation", *VariantPtr::dynamicCast(getInputList("targetTCPOrientations")->getElement(counterValue)));
context->systemObserverPrx->incrementCounter(trajectoryPointCounterID);
}
else
{
sendEvent<EvLastPointReached>();
}
}
void MoveTCPTrajectoryNextPoint::onExit()
{
}
void MoveTCPTrajectoryNextPoint::defineSubstates()
{
StatePtr moveTCPPoseIK = addState<MoveTCPPoseIK>("MoveTCPPoseIK");
StatePtr movingDone = addState<SuccessState>("movingDone") ;
StatePtr movingFailed = addState<FailureState>("movingFailed") ;
setInitState(moveTCPPoseIK, PM::createMapping()->mapFromParent("*","*"));
addTransition<EvSuccess>(moveTCPPoseIK, movingDone);
addTransitionFromAllStates<EvTimeout>(movingFailed);
addTransitionFromAllStates<EvFailure>(movingFailed);
}
void CalculateJointAngleConfiguration::defineParameters()
{
addToInput("KinematicChainName", VariantType::String, false);
......@@ -287,6 +427,8 @@ void CalculateJointAngleConfiguration::defineParameters()
addListToOutput("targetJointValues", VariantType::Float, true);
}
void CalculateJointAngleConfiguration::run()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
......@@ -299,7 +441,9 @@ void CalculateJointAngleConfiguration::run()
getInput<LinkedOrientation>("targetTCPOrientation"));
if(!ikSolver->solve(target.getInFrame(robot->getRootNode()), VirtualRobot::IKSolver::All, 50))
{
sendEvent<EvFailure>();
}
else
{
......@@ -317,6 +461,8 @@ void CalculateJointAngleConfiguration::run()
}
}
void ValidateJointAngleConfiguration::defineParameters()
{
addToInput("targetTCPPosition", VariantType::LinkedPosition, false);
......@@ -329,11 +475,13 @@ void ValidateJointAngleConfiguration::defineParameters()
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
}
void ValidateJointAngleConfiguration::onEnter()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
VirtualRobot::RobotPtr robot = (RemoteRobot(context->robotStateComponent->getRobotSnapshot("ValidateTCPPoseTime"))).clone("IKCopy");
using namespace Eigen;
using namespace Eigen;
VirtualRobot::LinkedCoordinate actualPose(robot);
actualPose.set(getInput<std::string>("KinematicChainName"), Eigen::Vector3f(0,0,0));
actualPose.changeFrame(robot->getRootNode());
......@@ -352,11 +500,17 @@ using namespace Eigen;
setOutput("TCPDistanceToTarget", cartesianDistance);
setOutput("TCPOrientationDistanceToTarget", orientationDistance);
if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && orientationDistance <= getInput<float>("targetOrientationDistanceTolerance"))
{
sendEvent<EvSuccess>();
}
else
{
sendEvent<EvFailure>();
}
}
void MotionControlTestState::defineParameters()
{
std::string configFilepathAbsolute;
......@@ -373,24 +527,23 @@ void MotionControlTestState::defineParameters()
addToInput("timeoutInMs", VariantType::Int, false);
addListToOutput("jointDistancesToTargets", VariantType::Float, true);
}
void MotionControlTestState::defineSubstates()
{
StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
StatePtr movingDone = addState<SuccessState>("movingDone") ;
StatePtr movingFailed = addState<FailureState>("movingFailed") ;
setInitState(moveJoints,
PM::createMapping()->mapFromParent("*","*"));
setInitState(moveJoints, PM::createMapping()->mapFromParent("*","*"));
addTransition<EvJointTargetReached>(moveJoints, movingDone);
addTransition<EvTimeout>(moveJoints, movingFailed);
}
}
void MoveHelpers::StopRobot::onEnter()
{
......
motioncontrol/MotionControl.h
+ 82
− 18
View file @ 66742d4c
......@@ -9,6 +9,11 @@ namespace armarx
namespace MoveHelpers
{
/**
* MotionControl contains convenience functions that allow to move parts of the robot,
* i.e. direct and inverse kinematics for arbitrary kinematic chains and predefined
* parts like the arms.
*/
class MotionControl : public RemoteStateHandler<RobotStatechartContext>
{
public:
......@@ -18,17 +23,24 @@ namespace MoveHelpers
std::string getDefaultName() const { return "MotionControl"; }
};
DEFINEEVENT(EvJointTargetReached)
DEFINEEVENT(EvJointVelocitiesTargetReached)
DEFINEEVENT(EvCalculationDone)
DEFINEEVENT(EvTimeout)
struct MotionControlTestState : StateTemplate<MotionControlTestState>
{
void defineParameters();
void defineSubstates();
};
/**
* Move the joints of a kinematic chain to the desired values.
* Parameters?
*/
struct MoveJoints : StateTemplate<MoveJoints>
{
RobotStatechartContext* context;
......@@ -41,16 +53,26 @@ namespace MoveHelpers
void onExit();
};
/**
* Move the joints of a kinematic chain to the desired values using velocity control.
* Parameters?
*/
struct MoveJointsVelocityControl : StateTemplate<MoveJointsVelocityControl>
{
RobotStatechartContext* context;
ConditionIdentifier targetVelocitiesReachedCondition;
ActionEventIdentifier timeoutEvent;
void defineParameters();
void onEnter();
void onExit();
};
{
RobotStatechartContext* context;
ConditionIdentifier targetVelocitiesReachedCondition;
ActionEventIdentifier timeoutEvent;
void defineParameters();
void onEnter();
void onExit();
};
/**
* Move the TCP to a desired pose.
* Parameters?
*/
struct MoveTCPPoseIK : StateTemplate<MoveTCPPoseIK>
{
RobotStatechartContext* context;
......@@ -62,6 +84,56 @@ namespace MoveHelpers
void onExit();
};
DEFINEEVENT(EvLastPointReached)
/**
* Move the TCP along a trajectory of poses.
* Parameters?
*/
struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
{
RobotStatechartContext* context;
ConditionIdentifier lastTargetReachedCondition;
ActionEventIdentifier timeoutEvent;
ActionEventIdentifier trajectoryPointCounterID;
void defineParameters();
void defineSubstates();
void onEnter();
void onExit();
};
struct MoveTCPTrajectoryNextPoint : StateTemplate<MoveTCPTrajectoryNextPoint>
{
RobotStatechartContext* context;
void defineParameters();
void defineSubstates();
void onEnter();
void onExit();
};
/**
* Stop all motion of the robot.
* Parameters?
*/
struct StopRobot : StateTemplate<StopRobot>
{
RobotStatechartContext* context;
void onEnter();
};
/////////////////////////////////////////////////////////////////////
//// intermediate states
/////////////////////////////////////////////////////////////////////
struct CalculateJointAngleConfiguration : StateTemplate<CalculateJointAngleConfiguration>
{
......@@ -79,16 +151,8 @@ namespace MoveHelpers
void onEnter();
};
/////////////////////////////////////////////////////////////////////
//// Stopping robot
/////////////////////////////////////////////////////////////////////
struct StopRobot : StateTemplate<StopRobot>
{
RobotStatechartContext* context;
} // namespace MoveHelpers
void onEnter();
};
}
} // namespace armarx
}
#endif // MOTIONCONTROL_H
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment