diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 644d0dcc4445cb3c6b2899e6dab34d9e6190f1e4..dfb63eca2165533aaa75215bb2759f61e03a5e80 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -1,14 +1,18 @@
#include "MotionControl.h"
+
+// Core
+#include <Core/robotstate/remote/RemoteRobot.h>
+#include <Core/observers/variant/ChannelRef.h>
+#include <Core/observers/ConditionCheck.h>
+#include <Core/core/system/ArmarXDataPath.h>
+
+// Virtual Robot
+#include <VirtualRobot/IK/GazeIK.h>
#include <VirtualRobot/IK/GenericIKSolver.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <Core/robotstate/remote/RemoteRobot.h>
-#include <Core/observers/variant/ChannelRef.h>
-#include <Core/observers/ConditionCheck.h>
-
-#include <Core/core/system/ArmarXDataPath.h>
#include <Eigen/src/Geometry/Quaternion.h>
@@ -53,7 +57,6 @@ void MoveJoints::defineParameters()
addToInput("jointMaxSpeed", VariantType::List(VariantType::Float), true);
addToInput("jointTargetTolerance", VariantType::Float, false);
-
addToInput("timeoutInMs", VariantType::Int, false);
addToOutput("jointDistancesToTargets", VariantType::List(VariantType::Float), true);
@@ -82,8 +85,6 @@ void MoveJoints::onEnter()
// TODO: Set Max Velocities
// now install the condition
-
- //ExpressionPtr cond = Expression::create();
Term cond;
float tolerance = getInput<float>("jointTargetTolerance");
for (int i=0; i<jointNames->getSize(); i++)
@@ -511,7 +512,8 @@ void CalculateJointAngleConfiguration::run()
// ARMARX_INFO << "GOAL TCP:" << goalTmpTCP << endl;
ARMARX_INFO << "Calculating IK" << flush;
- if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
+ //if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
+ if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::All, 50))
{
ARMARX_WARNING << "IKSolver found no solution! " << flush;
sendEvent<EvFailure>();
@@ -696,3 +698,170 @@ void MotionControl::StopRobot::onEnter()
}
+
+
+void CloseHand::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+}
+
+
+void CloseHand::onEnter()
+{
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ bool useLeftHand = getInput<bool>("useLeftHand");
+ NameValueMap handConfig;
+ if (useLeftHand)
+ {
+ handConfig["left_hand_configuration_actual_float"] = 4;
+ }
+ else
+ {
+ handConfig["right_hand_configuration_actual_float"] = 4;
+ }
+
+ context->kinematicUnitPrx->setJointAngles(handConfig);
+
+ sendEvent<EvSuccess>();
+}
+
+
+
+
+void OpenHand::defineParameters()
+{
+ addToInput("useLeftHand", VariantType::Bool, false);
+}
+
+
+void OpenHand::onEnter()
+{
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ bool useLeftHand = getInput<bool>("useLeftHand");
+ NameValueMap handConfig;
+ if (useLeftHand)
+ {
+ handConfig["left_hand_configuration_actual_float"] = 1;
+ }
+ else
+ {
+ handConfig["right_hand_configuration_actual_float"] = 1;
+ }
+
+ context->kinematicUnitPrx->setJointAngles(handConfig);
+
+ sendEvent<EvSuccess>();
+}
+
+
+
+
+
+void HeadLookAtTarget::defineParameters()
+{
+ addToInput("target", VariantType::FramedPosition, false);
+ addToInput("headIKKinematicChainName", VariantType::String, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
+ addToInput("timeoutInMs", VariantType::Int, false);
+}
+
+
+void HeadLookAtTarget::defineSubstates()
+{
+ StatePtr stateCalculateHeadIK = addState<CalculateHeadIK>("CalculateHeadIKState");
+ StatePtr stateMoveHeadJoints = addState<MoveJoints>("MoveHeadJointsState");
+ StatePtr stateSuccess = addState<SuccessState>("Success");
+ StatePtr stateFailure = addState<FailureState>("Failure");
+
+ ParameterMappingPtr initialMapping = ParameterMapping::createMapping()->mapFromParent("*", "*");
+ ParameterMappingPtr transitionMapping = ParameterMapping::createMapping()->mapFromOutput("*", "*")->mapFromParent("*", "*");
+
+ setInitState(stateCalculateHeadIK, initialMapping);
+ addTransition<EvCalculationDone>(stateCalculateHeadIK, stateMoveHeadJoints, transitionMapping);
+ addTransition<EvJointTargetReached>(stateMoveHeadJoints, stateSuccess, transitionMapping);
+ addTransitionFromAllStates<EvFailure>(stateFailure);
+ addTransitionFromAllStates<EvTimeout>(stateFailure);
+}
+
+
+
+
+void CalculateHeadIK::defineParameters()
+{
+ addToInput("target", VariantType::FramedPosition, false);
+ addToInput("headIKKinematicChainName", VariantType::String, false);
+
+ addToOutput("jointNames", VariantType::List(VariantType::String), true);
+ addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
+}
+
+
+void CalculateHeadIK::onEnter()
+{
+ targetPosition = getInput<FramedPosition>("target");
+ kinematicChainName = getInput<std::string>("headIKKinematicChainName");
+}
+
+
+void CalculateHeadIK::run()
+{
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ VirtualRobot::RobotPtr robotSnapshot(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");
+ VirtualRobot::RobotConfigPtr robotSnapshotConfig = robotSnapshot->getConfig();
+ std::string robotModelFile;
+ ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
+ robot->setConfig(robotSnapshotConfig);
+
+ VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
+ VirtualRobot::RobotNodePrismaticPtr virtualJoint;
+ for (unsigned int i=0; i<kinematicChain->getSize(); i++)
+ {
+ if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
+ {
+ virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
+ }
+ }
+ VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
+ ikSolver.enableJointLimitAvoidance(true);
+
+ Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
+ FramedOrientationPtr targetOri = new FramedOrientation(m, targetPosition->frame);
+ VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, targetPosition, targetOri);
+ ARMARX_INFO << "Target: " << target.getPose() << armarx::flush;
+ Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
+ //Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
+ Eigen::Vector3f targetPos = goalInRoot.block<3,1>(0,3);
+
+ ARMARX_INFO << "Calculating IK for target position " << targetPos;
+ if (!ikSolver.solve(targetPos))
+ {
+ ARMARX_WARNING << "IKSolver found no solution! ";
+ sendEvent<EvFailure>();
+ }
+ else
+ {
+
+ SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
+ SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
+ for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
+ {
+ if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+ {
+ 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;
+ }
+ }
+ 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>();
+ }
+}
+
+
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index 47c97d2e378b7312c84461b9826a71c6d014bb93..8132c75e7549a5c8421eff5e0760415df8937ef2 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -225,6 +225,50 @@ namespace MotionControl
+
+ /**
+ * \ingroup MotionControl
+ * Closes the given hand of the robot
+ * \param useLeftHand True if left hand should be closed, false for the right hand
+ */
+ struct CloseHand : StateTemplate<CloseHand>
+ {
+ void defineParameters();
+ void onEnter();
+ };
+
+
+
+
+ /**
+ * \ingroup MotionControl
+ * Opens the given hand of the robot
+ * \param useLeftHand True if left hand should be opened, false for the right hand
+ */
+ struct OpenHand : StateTemplate<OpenHand>
+ {
+ void defineParameters();
+ void onEnter();
+ };
+
+
+
+
+ /**
+ * \ingroup MotionControl
+ * Moves the head so that it looks at the specified target position
+ * \param target Position at which the robot will try to look
+ * \param headIKKinematicChainName Name of the kinematic chain for the head that will be sued in the IK
+ */
+ struct HeadLookAtTarget : StateTemplate<HeadLookAtTarget>
+ {
+ void defineParameters();
+ void defineSubstates();
+ };
+
+
+
+
/////////////////////////////////////////////////////////////////////
//// intermediate states
/////////////////////////////////////////////////////////////////////
@@ -276,6 +320,18 @@ namespace MotionControl
void onEnter();
};
+
+
+
+ struct CalculateHeadIK : StateTemplate<CalculateHeadIK>
+ {
+ void defineParameters();
+ void onEnter();
+ void run();
+ FramedPositionPtr targetPosition;
+ std::string kinematicChainName;
+ };
+
} // namespace MotionControl
} // namespace armarx