Skip to content
Snippets Groups Projects
Commit bf4c22ed authored by Mirko Wächter's avatar Mirko Wächter
Browse files

Merge branch 'master' of https://i61wiki.itec.uka.de/git/robot-api 

Conflicts:
	source/RobotAPI/motioncontrol/MotionControl.cpp
parents 75d41418 7a056cf2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
.gitignore
+ 25
− 4
View file @ bf4c22ed
.keep_in_git
/build/*
!/build/.gitkeep
!.gitkeep
!.gitignore
/build*
source/RobotAPI/Test.h
source/*/Test.h
*.bak
*#
.#*
*~
*.swp
.*.kate-swp
.*.swo
*.pyc
.DS_Store
CMakeFiles
CMakeCache.txt
CMakeLists.txt.user
*.o
*.os
......@@ -22,3 +28,18 @@ CMakeCache.txt
*.dylib
moc_*
# eclipse stuff
.project
.pydevproject
.settings
.metadata
.cproject
.project
# MemoryX
.cache
mongod.log*
data/db/
data/dbdump/
source/RobotAPI/motioncontrol/MotionControl.cpp
+ 53
− 44
View file @ bf4c22ed
......@@ -15,24 +15,22 @@
using namespace armarx;
using namespace armarx::MotionControl;
MotionControlOfferer::MotionControlOfferer()
MotionControlHandler::MotionControlHandler()
{
}
void MotionControlOfferer::onInitRemoteStateOfferer()
void MotionControlHandler::onInitRemoteStateOfferer()
{
addState<MoveJoints>("MoveJoints");
addState<MoveJointsVelocityControl>("MoveJointsVelocityControl");
addState<MoveTCPPoseIK>("MoveTCPPoseIK");
//addState<MoveJoints>("MoveJoints");
//addState<MotionControlTestState>("MotionControlTestState");
// addState<MotionControlTestStateIK>("MotionControlTestStateIK");
addState<MotionControlTestStateIK>("MotionControlTestStateIK");
}
void MotionControlOfferer::onConnectRemoteStateOfferer()
void MotionControlHandler::onConnectRemoteStateOfferer()
{
}
......@@ -42,16 +40,16 @@ void MoveJoints::defineParameters()
{
context = getContext<RobotStatechartContext>();
setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
addToInput("targetJointValues", VariantType::Float, false);
addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
// TODO: add when we have decided how to do it
addToInput("jointMaxSpeeds", VariantType::List(VariantType::Float), true);
addToInput("jointMaxSpeed", VariantType::List(VariantType::Float), true);
addToInput("targetTolerance", VariantType::Float, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
......@@ -71,7 +69,9 @@ void MoveJoints::onEnter()
throw LocalException("Sizes of joint name list and joint value list do not match!");
}
for(int i=0; i<jointNames->getSize(); i++ )
ARMARX_LOG << "number of joints that will be set: " << jointNames->getSize() << flush;
for (int i=0; i<jointNames->getSize(); i++)
{
targetJointAngles[jointNames->getVariant(i)->getString()] = targetJointValues->getVariant(i)->getFloat();
ARMARX_VERBOSE << "setting joint angle for joint '" << jointNames->getVariant(i)->getString() << "' to " << targetJointValues->getVariant(i)->getFloat() << std::endl;
......@@ -82,14 +82,15 @@ void MoveJoints::onEnter()
//ExpressionPtr cond = Expression::create();
Term cond;
float tolerance = getInput<float>("targetTolerance");
for(int i=0; i<jointNames->getSize(); i++ )
float tolerance = getInput<float>("jointTargetTolerance");
for (int i=0; i<jointNames->getSize(); i++)
{
//ARMARX_VERBOSE << "creating condition (" << i << " of " << jointNames->getSize() << ")" << flush;
ParameterList inrangeCheck;
inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()-tolerance));
inrangeCheck.push_back(new Variant(targetJointValues->getVariant(i)->getFloat()+tolerance));
Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(),"jointangles", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
Literal inrangeLiteral(DataFieldIdentifier(context->getKinematicUnitObserverName(), "jointangles", jointNames->getVariant(i)->getString()), "inrange", inrangeCheck);
cond = cond && inrangeLiteral;
}
......@@ -137,7 +138,7 @@ void MoveJointsVelocityControl::defineParameters()
{
context = getContext<RobotStatechartContext>();
setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
addToInput("targetJointVelocities", VariantType::List(VariantType::Float), false);
......@@ -146,7 +147,7 @@ void MoveJointsVelocityControl::defineParameters()
addToInput("jointMaxSpeeds", VariantType::List(VariantType::Float), true);
addToInput("jointMaxSpeed", VariantType::Float, true);
addToInput("accelerationTime", VariantType::Float, true);
addToInput("accelerationTime", VariantType::Float, false);
addToInput("targetJointVelocityTolerance", VariantType::Float, false);
......@@ -227,7 +228,7 @@ void MoveTCPPoseIK::defineParameters()
{
context = getContext<RobotStatechartContext>();
setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPosition", VariantType::FramedPosition, false);
......@@ -239,6 +240,7 @@ void MoveTCPPoseIK::defineParameters()
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
......@@ -262,12 +264,13 @@ void MoveTCPPoseIK::onExit()
void MoveTCPPoseIK::defineSubstates()
{
StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
StatePtr calculateJointAngleConfiguration = addState<CalculateJointAngleConfiguration>("CalculateJointAngleConfiguration");
StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
StatePtr validateJointAngleConfiguration = addState<ValidateJointAngleConfiguration>("ValidateJointAngleConfiguration");
StatePtr movingDone = addState<SuccessState>("movingDone") ;
StatePtr movingFailed = addState<FailureState>("movingFailed") ;
setInitState(calculateJointAngleConfiguration, PM::createMapping()->mapFromParent("*","*"));
setInitState(calculateJointAngleConfiguration, createMapping()->mapFromParent("*","*")); // PM::createMapping()->mapFromParent("*","*") ??
addTransition<EvCalculationDone>(calculateJointAngleConfiguration, moveJoints, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvJointTargetReached>(moveJoints, validateJointAngleConfiguration, createMapping()->mapFromParent("*","*")->mapFromOutput("*","*"));
addTransition<EvSuccess>(validateJointAngleConfiguration, movingDone, createMapping()->mapFromOutput("*","*"));
......@@ -281,7 +284,7 @@ void MoveTCPTrajectory::defineParameters()
{
context = getContext<RobotStatechartContext>();
setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPI/data/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPositions", VariantType::List(VariantType::FramedPosition), false);
......@@ -322,7 +325,7 @@ void MoveTCPTrajectory::onEnter()
void MoveTCPTrajectory::onExit()
{
removeTimeoutEvent(timeoutEvent);
ChannelRefPtr r = getLocal<ChannelRefPtr>("trajectoryPointCounterID");
ChannelRefPtr r = getLocal<ChannelRef>("trajectoryPointCounterID");
getContext()->systemObserverPrx->removeCounter(r);
//removeCounterEvent(trajectoryPointCounterID);
}
......@@ -463,12 +466,13 @@ void MoveTCPTrajectoryNextPoint::defineSubstates()
void CalculateJointAngleConfiguration::defineParameters()
{
addToInput("KinematicChainName", VariantType::String, false);
addToInput("kinematicChainName", VariantType::String, false);
addToInput("targetTCPPosition", VariantType::FramedPosition, false);
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToOutput("jointNames", VariantType::String, true);
addToOutput("targetJointValues", VariantType::Float, true);
addToOutput("jointNames", VariantType::List(VariantType::String), true);
addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
}
......@@ -476,17 +480,18 @@ void CalculateJointAngleConfiguration::defineParameters()
void CalculateJointAngleConfiguration::run()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
VirtualRobot::RobotPtr robotPtr(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
// TODO: with the following line, the IK doesn't find a solution, so this terrible hack must be used. Fix it!!!
//VirtualRobot::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
std::string robotModelFile;
// ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
// VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
//VirtualRobot::RobotConfigPtr configPtr(new VirtualRobot::RobotConfig(robotPtr, "blub"));
//robotPtr->getRobotNodeSet("Root")->getJointValues(configPtr);
//robot->setJointValues(robotPtr->getRobotNodeSet("Root"), robotPtr->getRobotNodeSet("Root")->getJointValues());
VirtualRobot::RobotNodeSetPtr robotNodeset = robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"));
VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robotNodeset));
//ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))));
ikSolver->setVerbose(true);
ikSolver->setMaximumError(getInput<float>("targetPositionDistanceTolerance"));
VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
......@@ -500,7 +505,7 @@ void CalculateJointAngleConfiguration::run()
// ARMARX_INFO << "GOAL TCP:" << goalTmpTCP << endl;
ARMARX_INFO << "Calculating IK" << flush;
if (!ikSolver->solve(goalInRoot, VirtualRobot::IKSolver::All, 1))
if (!ikSolver->solve(goalInRoot, VirtualRobot::IKSolver::Position, 1))
{
ARMARX_WARNING << "IKSolver found no solution! " << flush;
sendEvent<EvFailure>();
......@@ -510,14 +515,18 @@ void CalculateJointAngleConfiguration::run()
SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
// VirtualRobot::RobotNodeSetPtr robotNodeset = robot->getRobotNodeSet(getInput<std::string>("KinematicChainName") );
for (int i = 0; i < (signed int)robotNodeset->getSize(); i++)
VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName") );
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
jointNames.addVariant(Variant(robotNodeset->getNode(i)->getName()));
targetJointValues.addVariant(Variant(robotNodeset->getNode(i)->getJointValue()));
jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
ARMARX_LOG << " joint: " << kinematicChain->getNode(i)->getName() << " value: " << kinematicChain->getNode(i)->getJointValue() << flush;
}
setOutput("targetJointValues",targetJointValues);
setOutput("jointNames",jointNames);
ARMARX_LOG << "number of joints to be set: " << jointNames.getSize() << flush;
ARMARX_LOG << "setting output: jointNames" << flush;
setOutput("jointNames", jointNames);
ARMARX_LOG << "setting output: targetJointValues" << flush;
setOutput("targetJointValues", targetJointValues);
sendEvent<EvCalculationDone>();
}
}
......@@ -545,7 +554,7 @@ void ValidateJointAngleConfiguration::onEnter()
RobotStatechartContext* context = getContext<RobotStatechartContext>();
VirtualRobot::RobotPtr robot(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("ValidateTCPPoseTime")));
VirtualRobot::LinkedCoordinate actualPose(robot);
actualPose.set(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"))->getTCP()->getName(), Vector3f(0,0,0));
actualPose.set(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))->getTCP()->getName(), Vector3f(0,0,0));
actualPose.changeFrame(robot->getRootNode());
Vector3f actualPosition = actualPose.getPosition();
Quaternionf actualOrientation(actualPose.getPose().block<3,3>(0,0));
......@@ -578,14 +587,14 @@ void ValidateJointAngleConfiguration::onEnter()
void MotionControlTestState::defineParameters()
{
setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
addToInput("jointNames", VariantType::List(VariantType::String), false);
addToInput("targetJointValues", VariantType::List(VariantType::Float), false);
addToInput("jointMaxSpeed", VariantType::Float, true);
addToInput("targetTolerance", VariantType::Float, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
addToInput("timeoutInMs", VariantType::Int, false);
......@@ -610,7 +619,7 @@ void MotionControlTestState::defineSubstates()
void MotionControlTestStateIK::defineParameters()
{
setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
//setConfigFile("RobotAPIConfigs/stateconfigs/MotionControl.xml");
addToInput("kinematicChainName", VariantType::String, false);
//addToInput("targetTCPPosition", VariantType::FramedPosition, false);
......
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