diff --git a/.gitignore b/.gitignore
index efe7e6e621bb44ff3b61c296ce841445f5353451..6bbc3ffa11b5ac3f4e9acca1b3eeeb70d886e60d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,17 +1,23 @@
-.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/
+
+
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 31911fccc1564278b7abec0dc0c5090970fd3554..644169dafb80c50029eaa149469249fb72381b47 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -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);