From 8a381c4a689f4ebf9fda7427759d94be6337e16f Mon Sep 17 00:00:00 2001
From: David Schiebener <david.schiebener@kit.edu>
Date: Mon, 5 Nov 2012 14:33:34 +0000
Subject: [PATCH] added state for IK following a trajectory of points, adapted
to diverse updates of Core
git-svn-id: https://svn.sfb588.uni-karlsruhe.de/svn/HumanoidRobotAPI@16 e7ad446d-f7d8-486a-9efb-df2f5bbea5c6
---
.../core/RobotStatechartContext.cpp | 4 +-
.../motioncontrol/MotionControl.cpp | 90 ++++++++++++++-----
.../motioncontrol/MotionControl.h | 9 +-
3 files changed, 79 insertions(+), 24 deletions(-)
diff --git a/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp b/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
index 55e685d53..98a186ff3 100644
--- a/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
+++ b/source/HumanoidRobotAPI/core/RobotStatechartContext.cpp
@@ -41,7 +41,7 @@ namespace armarx
void RobotStatechartContext::onInitStatechartContext()
{
// StatechartContext::onInitStatechart();
- ARMARX_LOG << eINFO << "Init Armar4Context" << flush;
+ ARMARX_LOG << eINFO << "Init RobotStatechartContext" << flush;
RobotStateObjectFactories::addFactories(getIceManager()->getCommunicator());
kinematicUnitObserverName = getProperty<std::string>("KinematicUnitObserverName").getValue();
@@ -55,7 +55,7 @@ namespace armarx
void RobotStatechartContext::onConnectStatechartContext()
{
// StatechartContext::onConnectStatechart();
- ARMARX_LOG << eINFO << "Starting Armar4Context" << flush;
+ ARMARX_LOG << eINFO << "Starting RobotStatechartContext" << flush;
// retrieve proxies
robotStateComponent = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
diff --git a/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp b/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
index b2d176276..8ce944e05 100644
--- a/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/HumanoidRobotAPI/motioncontrol/MotionControl.cpp
@@ -3,6 +3,7 @@
#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>
@@ -22,8 +23,9 @@ MotionControl::MotionControl()
void MotionControl::onInitRemoteStateHandler()
{
- addState<MoveJoints>("MoveJoints");
- addState<MotionControlTestState>("MotionControlTestState");
+ //addState<MoveJoints>("MoveJoints");
+ //addState<MotionControlTestState>("MotionControlTestState");
+ addState<MotionControlTestStateIK>("MotionControlTestStateIK");
}
@@ -34,12 +36,6 @@ void MotionControl::onConnectRemoteStateHandler()
-MoveJoints::MoveJoints()
-{
-}
-
-
-
void MoveJoints::defineParameters()
{
context = getContext<RobotStatechartContext>();
@@ -99,7 +95,7 @@ void MoveJoints::onEnter()
}
targetReachedCondition = installCondition<EvJointTargetReached>(cond);
- ARMARX_INFO << "condition installed" << std::endl;
+ ARMARX_INFO << "condition installed: EvJointTargetReached" << std::endl;
timeoutEvent = setTimeoutEvent(getInput<int>("timeoutInMs"), createEvent<EvTimeout>());
ARMARX_INFO << "timeout installed" << std::endl;
context->kinematicUnitPrx->setJointAngles(targetJointAngles);
@@ -166,7 +162,7 @@ void MoveJointsVelocityControl::defineParameters()
void MoveJointsVelocityControl::onEnter()
{
- float accelerationTime = getInput<float>("accelerationTime");
+ //float accelerationTime = getInput<float>("accelerationTime");
NameValueMap targetJointVelocities;
SingleTypeVariantListPtr jointNames = getInputList("jointNames");
SingleTypeVariantListPtr targetVelocitiesValues = getInputList("targetJointVelocities");
@@ -436,16 +432,30 @@ void CalculateJointAngleConfiguration::defineParameters()
void CalculateJointAngleConfiguration::run()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
- VirtualRobot::RobotPtr robot = (RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime"))).clone("IKCopy");
-
- VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName")) ));
-
- VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot,
- getInput<LinkedPosition>("targetTCPPosition"),
- getInput<LinkedOrientation>("targetTCPOrientation"));
-
- if(!ikSolver->solve(target.getInFrame(robot->getRootNode()), VirtualRobot::IKSolver::All, 50))
+ VirtualRobot::RobotPtr robotPtr(new RemoteRobot(context->robotStateComponent->getRobotSnapshot("CalculateTCPPoseTime")));
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot("/org/share/home/staff/schieben/armarx/data/ArmarIV/RobotModel/ArmarIV.xml");
+ //VirtualRobot::RobotConfigPtr configPtr(new VirtualRobot::RobotConfig(robotPtr, "blub"));
+ //robotPtr->getRobotNodeSet("Root")->getJointValues(configPtr);
+ //robot->setJointValues(robotPtr->getRobotNodeSet("Root"), robotPtr->getRobotNodeSet("Root")->getJointValues());
+
+ VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("KinematicChainName"))));
+ ikSolver->setVerbose(true);
+
+ VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<LinkedPosition>("targetTCPPosition"), getInput<LinkedOrientation>("targetTCPOrientation"));
+ Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
+
+ // test
+ VirtualRobot::RobotNodePtr rn = robot->getRobotNode("LeftTCP");
+ Eigen::Matrix4f goalTmpTCP = rn->getGlobalPose();
+ Eigen::Matrix4f goalGlobal = robot->getRootNode()->toGlobalCoordinateSystem(goalInRoot);
+ ARMARX_LOG << eINFO << "GOAL local :" << goalInRoot << endl;
+ ARMARX_LOG << eINFO << "GOAL global :" << goalGlobal << endl;
+ ARMARX_LOG << eINFO << "GOAL TCP:" << goalTmpTCP << endl;
+
+ ARMARX_LOG << eINFO << "Calculating IK" << flush;
+ if (!ikSolver->solve(goalInRoot, VirtualRobot::IKSolver::Position, 1))
{
+ ARMARX_WARNING << "IKSolver found no solution! " << flush;
sendEvent<EvFailure>();
}
else
@@ -538,8 +548,8 @@ void MotionControlTestState::defineParameters()
void MotionControlTestState::defineSubstates()
{
StatePtr moveJoints = addState<MoveJoints>("MoveJoints");
- StatePtr movingDone = addState<SuccessState>("movingDone") ;
- StatePtr movingFailed = addState<FailureState>("movingFailed") ;
+ StatePtr movingDone = addState<SuccessState>("movingDone");
+ StatePtr movingFailed = addState<FailureState>("movingFailed");
setInitState(moveJoints, PM::createMapping()->mapFromParent("*","*"));
@@ -549,6 +559,44 @@ void MotionControlTestState::defineSubstates()
+void MotionControlTestStateIK::defineParameters()
+{
+ std::string configFilepathAbsolute;
+ ArmarXDataPath::getAbsolutePath("HumanoidRobotAPIConfigs/stateconfigs/MotionControl.xml", configFilepathAbsolute);
+ setConfigFile(configFilepathAbsolute);
+
+ addToInput("KinematicChainName", VariantType::String, false);
+ addToInput("targetTCPPosition", VariantType::LinkedPosition, false);
+ addToInput("targetTCPOrientation", 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);
+}
+
+
+
+void MotionControlTestStateIK::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);
+ addTransition<EvTimeout>(moveTCPPoseIK, movingFailed);
+ addTransition<EvFailure>(moveTCPPoseIK, movingFailed);
+}
+
+
+
void MoveHelpers::StopRobot::onEnter()
{
RobotStatechartContext* context = getContext<RobotStatechartContext>();
diff --git a/source/HumanoidRobotAPI/motioncontrol/MotionControl.h b/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
index 9df74276b..eec7dda1c 100644
--- a/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
+++ b/source/HumanoidRobotAPI/motioncontrol/MotionControl.h
@@ -37,6 +37,14 @@ namespace MoveHelpers
};
+ struct MotionControlTestStateIK : StateTemplate<MotionControlTestStateIK>
+ {
+ void defineParameters();
+ void defineSubstates();
+ };
+
+
+
/**
* Move the joints of a kinematic chain to the desired values.
* Parameters?
@@ -46,7 +54,6 @@ namespace MoveHelpers
RobotStatechartContext* context;
ConditionIdentifier targetReachedCondition;
ActionEventIdentifier timeoutEvent;
- MoveJoints();
void defineParameters();
void onEnter();
void onBreak();
--
GitLab