From ae8c266c3f54218308d5159085e1f2707bec91a2 Mon Sep 17 00:00:00 2001
From: Nikolaus Vahrenkamp <vahrenkamp@kit.edu>
Date: Thu, 31 Jul 2014 11:58:54 +0200
Subject: [PATCH] Added ikWithOrientation input to allow position IK requests
(without orientation) Improved target reached checks by considering
orientation Optimized IK calls in order to get better and faster results
Removed robot loading code in IK state, switched to
RemoteRobot::createLocalClone which is much faster removed hard coded check
for VirtualCentralGaze RobotNode from ik skill
---
.../RobotAPI/motioncontrol/MotionControl.cpp | 104 ++++++++++++++----
source/RobotAPI/motioncontrol/MotionControl.h | 5 +
2 files changed, 89 insertions(+), 20 deletions(-)
diff --git a/source/RobotAPI/motioncontrol/MotionControl.cpp b/source/RobotAPI/motioncontrol/MotionControl.cpp
index 14d506848..b984a8545 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.cpp
+++ b/source/RobotAPI/motioncontrol/MotionControl.cpp
@@ -244,6 +244,8 @@ void MoveTCPPoseIK::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
+
addToInput("timeoutInMs", VariantType::Int, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
@@ -300,8 +302,11 @@ void MoveTCPTrajectory::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
+
addToInput("timeoutInMs", VariantType::Int, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
@@ -424,11 +429,14 @@ void MoveTCPTrajectoryNextPoint::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
+
addToInput("timeoutInMs", VariantType::Int, false);
addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
+ addToInput("jointTargetTolerance", VariantType::Float, false);
addToLocal("targetTCPPosition", VariantType::FramedPosition);
addToLocal("targetTCPOrientation", VariantType::FramedOrientation);
@@ -476,6 +484,8 @@ void CalculateJointAngleConfiguration::defineParameters()
addToInput("targetTCPPosition", VariantType::FramedPosition, false);
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
+ addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
addToOutput("jointNames", VariantType::List(VariantType::String), true);
addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
@@ -490,18 +500,52 @@ void CalculateJointAngleConfiguration::run()
// 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;
+ //std::string robotModelFile;
//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());
+ //ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
+ //VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str());
+
+ std::string kinChainName = getInput<std::string>("kinematicChainName");
+ float maxError = getInput<float>("targetPositionDistanceTolerance");
+ float maxErrorRot = getInput<float>("targetOrientationDistanceTolerance");
+ bool withOrientation = getInput<bool>("ikWithOrientation");
+ VirtualRobot::IKSolver::CartesianSelection ikType = VirtualRobot::IKSolver::All;
+ if (!withOrientation)
+ ikType = VirtualRobot::IKSolver::Position;
+
+
+ RobotStatechartContext* context = getContext<RobotStatechartContext>();
+ if (!context)
+ {
+ ARMARX_WARNING << "Need a RobotStatechartContext" << flush;
+ sendEvent<EvFailure>();
+ }
- VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(getInput<std::string>("kinematicChainName"))));
+ if (!context->robotStateComponent)
+ {
+ ARMARX_WARNING << "No RobotStatechartContext->robotStateComponent" << flush;
+ sendEvent<EvFailure>();
+ }
+
+ VirtualRobot::RobotPtr robot = RemoteRobot::createLocalClone(context->robotStateComponent);
+ if (!robot)
+ {
+ ARMARX_WARNING << "Could not create a local clone of RemoteRobot" << flush;
+ sendEvent<EvFailure>();
+ }
+ if (!robot->hasRobotNodeSet(kinChainName))
+ {
+ ARMARX_WARNING << "Robot does not know kinematic chain with name " << kinChainName << flush;
+ sendEvent<EvFailure>();
+ }
+ ARMARX_INFO << "Setting up ik solver for kin chain: " << kinChainName << ", max position error:" << maxError << ", max orientation erorr " << maxErrorRot << ", useOrientation:" << withOrientation << armarx::flush;
+ VirtualRobot::GenericIKSolverPtr ikSolver(new VirtualRobot::GenericIKSolver(robot->getRobotNodeSet(kinChainName), VirtualRobot::JacobiProvider::eSVDDamped));
ikSolver->setVerbose(true);
- ikSolver->setMaximumError(getInput<float>("targetPositionDistanceTolerance"));
- ikSolver->setupJacobian(0.1, 40);
+ ikSolver->setMaximumError(maxError,maxErrorRot);
+ ikSolver->setupJacobian(0.6f, 10);
VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation"));
- ARMARX_INFO << "target: " << target.getPose() << armarx::flush;
+ ARMARX_INFO << "IK target: " << target.getPose() << armarx::flush;
Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
// // test
@@ -514,7 +558,7 @@ void CalculateJointAngleConfiguration::run()
ARMARX_INFO << "Calculating IK" << flush;
//if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::Position, 50))
- if (!ikSolver->solve(goalGlobal, VirtualRobot::IKSolver::All, 50))
+ if (!ikSolver->solve(goalGlobal, ikType, 5))
{
ARMARX_WARNING << "IKSolver found no solution! " << flush;
sendEvent<EvFailure>();
@@ -524,15 +568,12 @@ void CalculateJointAngleConfiguration::run()
SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
- VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(getInput<std::string>("kinematicChainName") );
+ VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinChainName);
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;
- }
+ 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;
@@ -551,6 +592,7 @@ void ValidateJointAngleConfiguration::defineParameters()
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("kinematicChainName", VariantType::String, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true);
@@ -582,7 +624,8 @@ void ValidateJointAngleConfiguration::onEnter()
setOutput("TCPDistanceToTarget", cartesianDistance);
setOutput("TCPOrientationDistanceToTarget", orientationDistance);
- if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && orientationDistance <= getInput<float>("targetOrientationDistanceTolerance"))
+ bool withOri = getInput<bool>("ikWithOrientation");
+ if (cartesianDistance <= getInput<float>("targetPositionDistanceTolerance") && (!withOri || orientationDistance <= getInput<float>("targetOrientationDistanceTolerance")))
{
sendEvent<EvSuccess>();
}
@@ -643,6 +686,8 @@ void MotionControlTestStateIK::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
+ addToInput("ikWithOrientation", VariantType::Bool, false);
+
addToInput("timeoutInMs", VariantType::Int, false);
@@ -993,6 +1038,7 @@ void CalculateHeadIK::onEnter()
{
targetPosition = getInput<FramedPosition>("target");
kinematicChainName = getInput<std::string>("headIKKinematicChainName");
+ virtualPrismaticJointName = getInput<std::string>("headIKVirtualPrismaticJointName");
}
@@ -1008,15 +1054,33 @@ void CalculateHeadIK::run()
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
robot->setConfig(robotSnapshotConfig);
+ if (!robot)
+ {
+ ARMARX_WARNING << "No robot!" << flush;
+ sendEvent<EvFailure>();
+ }
+
VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
- VirtualRobot::RobotNodePrismaticPtr virtualJoint;
- for (unsigned int i=0; i<kinematicChain->getSize(); i++)
+ if (!kinematicChain)
+ {
+ ARMARX_WARNING << "No kinematicChain with name " << kinematicChainName << flush;
+ sendEvent<EvFailure>();
+ }
+
+ VirtualRobot::RobotNodePrismaticPtr virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(robot->getRobotNode(virtualPrismaticJointName));
+ if (!virtualJoint)
+ {
+ ARMARX_WARNING << "No virtualJoint with name " << virtualPrismaticJointName << flush;
+ sendEvent<EvFailure>();
+ }
+
+ /*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);
@@ -1041,7 +1105,7 @@ void CalculateHeadIK::run()
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+ if (kinematicChain->getNode(i)->getName().compare(virtualPrismaticJointName) != 0)
{
jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
diff --git a/source/RobotAPI/motioncontrol/MotionControl.h b/source/RobotAPI/motioncontrol/MotionControl.h
index c629105c0..8aff5a0c3 100644
--- a/source/RobotAPI/motioncontrol/MotionControl.h
+++ b/source/RobotAPI/motioncontrol/MotionControl.h
@@ -59,6 +59,7 @@ namespace MotionControl
- targetTCPOrientation: the target orientation
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
+ - ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
- timeoutInMs: a timeout after which the motion is aborted
Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position
@@ -72,6 +73,7 @@ namespace MotionControl
- targetTCPOrientations: the list of target orientations
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation to decide if the motion was successfull
+ - ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
- timeoutInMs: a timeout after which the motion is aborted
Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position
@@ -169,6 +171,7 @@ namespace MotionControl
* \param targetTCPPosition the target position for the TCP
* \param targetTCPOrientation the target orientation
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
+ * \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
* \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
* \param timeoutInMs a timeout after which the motion is aborted
*/
@@ -195,6 +198,7 @@ namespace MotionControl
* \param targetTCPOrientations the list of target orientations
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull
* \param targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
+ * \param ikWithOrientation: Consider orientation for IK computation. if false, the rotation of the TCP is not considered.
* \param timeoutInMs a timeout after which the motion is aborted
*/
struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
@@ -377,6 +381,7 @@ namespace MotionControl
void run();
FramedPositionPtr targetPosition;
std::string kinematicChainName;
+ std::string virtualPrismaticJointName;
};
} // namespace MotionControl
--
GitLab