Skip to content
Snippets Groups Projects
Commit ae8c266c authored by Nikolaus Vahrenkamp's avatar Nikolaus Vahrenkamp
Browse files

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
parent 0b274f0c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/motioncontrol/MotionControl.cpp
+ 84
− 20
View file @ ae8c266c
...@@ -244,6 +244,8 @@ void MoveTCPPoseIK::defineParameters() ...@@ -244,6 +244,8 @@ void MoveTCPPoseIK::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false); addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("timeoutInMs", VariantType::Int, false); addToInput("timeoutInMs", VariantType::Int, false);
addToInput("jointTargetTolerance", VariantType::Float, false); addToInput("jointTargetTolerance", VariantType::Float, false);
...@@ -300,8 +302,11 @@ void MoveTCPTrajectory::defineParameters() ...@@ -300,8 +302,11 @@ void MoveTCPTrajectory::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false); addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("timeoutInMs", VariantType::Int, false); addToInput("timeoutInMs", VariantType::Int, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true); addToOutput("TCPDistanceToTarget", VariantType::Float, true);
addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true); addToOutput("TCPOrientationDistanceToTarget", VariantType::Float, true);
...@@ -424,11 +429,14 @@ void MoveTCPTrajectoryNextPoint::defineParameters() ...@@ -424,11 +429,14 @@ void MoveTCPTrajectoryNextPoint::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false); addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("timeoutInMs", VariantType::Int, false); addToInput("timeoutInMs", VariantType::Int, false);
addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false); addToInput("trajectoryPointCounterID", VariantType::ChannelRef, false);
addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false); addToOutput("trajectoryPointCounterID", VariantType::ChannelRef, false);
addToInput("jointTargetTolerance", VariantType::Float, false);
addToLocal("targetTCPPosition", VariantType::FramedPosition); addToLocal("targetTCPPosition", VariantType::FramedPosition);
addToLocal("targetTCPOrientation", VariantType::FramedOrientation); addToLocal("targetTCPOrientation", VariantType::FramedOrientation);
...@@ -476,6 +484,8 @@ void CalculateJointAngleConfiguration::defineParameters() ...@@ -476,6 +484,8 @@ void CalculateJointAngleConfiguration::defineParameters()
addToInput("targetTCPPosition", VariantType::FramedPosition, false); addToInput("targetTCPPosition", VariantType::FramedPosition, false);
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false); addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToOutput("jointNames", VariantType::List(VariantType::String), true); addToOutput("jointNames", VariantType::List(VariantType::String), true);
addToOutput("targetJointValues", VariantType::List(VariantType::Float), true); addToOutput("targetJointValues", VariantType::List(VariantType::Float), true);
...@@ -490,18 +500,52 @@ void CalculateJointAngleConfiguration::run() ...@@ -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!!! // 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::RobotPtr robot = robotPtr->clone("CalculateTCPPoseClone");
std::string robotModelFile; //std::string robotModelFile;
//ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile); //ArmarXDataPath::getAbsolutePath("Armar4/data/Armar4/ArmarIV.xml", robotModelFile);
ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile); //ArmarXDataPath::getAbsolutePath("Armar3/data/robotmodel/ArmarIII.xml", robotModelFile);
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str()); //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->setVerbose(true);
ikSolver->setMaximumError(getInput<float>("targetPositionDistanceTolerance")); ikSolver->setMaximumError(maxError,maxErrorRot);
ikSolver->setupJacobian(0.1, 40); ikSolver->setupJacobian(0.6f, 10);
VirtualRobot::LinkedCoordinate target = ArmarPose::createLinkedCoordinate(robot, getInput<FramedPosition>("targetTCPPosition"), getInput<FramedOrientation>("targetTCPOrientation")); 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()); Eigen::Matrix4f goalInRoot = target.getInFrame(robot->getRootNode());
// // test // // test
...@@ -514,7 +558,7 @@ void CalculateJointAngleConfiguration::run() ...@@ -514,7 +558,7 @@ void CalculateJointAngleConfiguration::run()
ARMARX_INFO << "Calculating IK" << flush; 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)) if (!ikSolver->solve(goalGlobal, ikType, 5))
{ {
ARMARX_WARNING << "IKSolver found no solution! " << flush; ARMARX_WARNING << "IKSolver found no solution! " << flush;
sendEvent<EvFailure>(); sendEvent<EvFailure>();
...@@ -524,15 +568,12 @@ void CalculateJointAngleConfiguration::run() ...@@ -524,15 +568,12 @@ void CalculateJointAngleConfiguration::run()
SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String); SingleTypeVariantList jointNames = SingleTypeVariantList(VariantType::String);
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float); 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++) 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()));
jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName())); ARMARX_LOG << " joint: " << kinematicChain->getNode(i)->getName() << " value: " << kinematicChain->getNode(i)->getJointValue() << flush;
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 << "number of joints to be set: " << jointNames.getSize() << flush;
ARMARX_LOG << "setting output: jointNames" << flush; ARMARX_LOG << "setting output: jointNames" << flush;
...@@ -551,6 +592,7 @@ void ValidateJointAngleConfiguration::defineParameters() ...@@ -551,6 +592,7 @@ void ValidateJointAngleConfiguration::defineParameters()
addToInput("targetTCPOrientation", VariantType::FramedOrientation, false); addToInput("targetTCPOrientation", VariantType::FramedOrientation, false);
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false); addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("kinematicChainName", VariantType::String, false); addToInput("kinematicChainName", VariantType::String, false);
addToOutput("TCPDistanceToTarget", VariantType::Float, true); addToOutput("TCPDistanceToTarget", VariantType::Float, true);
...@@ -582,7 +624,8 @@ void ValidateJointAngleConfiguration::onEnter() ...@@ -582,7 +624,8 @@ void ValidateJointAngleConfiguration::onEnter()
setOutput("TCPDistanceToTarget", cartesianDistance); setOutput("TCPDistanceToTarget", cartesianDistance);
setOutput("TCPOrientationDistanceToTarget", orientationDistance); 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>(); sendEvent<EvSuccess>();
} }
...@@ -643,6 +686,8 @@ void MotionControlTestStateIK::defineParameters() ...@@ -643,6 +686,8 @@ void MotionControlTestStateIK::defineParameters()
addToInput("targetPositionDistanceTolerance", VariantType::Float, false); addToInput("targetPositionDistanceTolerance", VariantType::Float, false);
addToInput("targetOrientationDistanceTolerance", VariantType::Float, false); addToInput("targetOrientationDistanceTolerance", VariantType::Float, false);
addToInput("ikWithOrientation", VariantType::Bool, false);
addToInput("timeoutInMs", VariantType::Int, false); addToInput("timeoutInMs", VariantType::Int, false);
...@@ -993,6 +1038,7 @@ void CalculateHeadIK::onEnter() ...@@ -993,6 +1038,7 @@ void CalculateHeadIK::onEnter()
{ {
targetPosition = getInput<FramedPosition>("target"); targetPosition = getInput<FramedPosition>("target");
kinematicChainName = getInput<std::string>("headIKKinematicChainName"); kinematicChainName = getInput<std::string>("headIKKinematicChainName");
virtualPrismaticJointName = getInput<std::string>("headIKVirtualPrismaticJointName");
} }
...@@ -1008,15 +1054,33 @@ void CalculateHeadIK::run() ...@@ -1008,15 +1054,33 @@ void CalculateHeadIK::run()
VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure); VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotModelFile.c_str(), VirtualRobot::RobotIO::eStructure);
robot->setConfig(robotSnapshotConfig); robot->setConfig(robotSnapshotConfig);
if (!robot)
{
ARMARX_WARNING << "No robot!" << flush;
sendEvent<EvFailure>();
}
VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName); VirtualRobot::RobotNodeSetPtr kinematicChain = robot->getRobotNodeSet(kinematicChainName);
VirtualRobot::RobotNodePrismaticPtr virtualJoint; if (!kinematicChain)
for (unsigned int i=0; i<kinematicChain->getSize(); i++) {
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) if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
{ {
virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i)); virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
} }
} }*/
VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint); VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
ikSolver.enableJointLimitAvoidance(true); ikSolver.enableJointLimitAvoidance(true);
...@@ -1041,7 +1105,7 @@ void CalculateHeadIK::run() ...@@ -1041,7 +1105,7 @@ void CalculateHeadIK::run()
SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float); SingleTypeVariantList targetJointValues = SingleTypeVariantList(VariantType::Float);
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++) 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())); jointNames.addVariant(Variant(kinematicChain->getNode(i)->getName()));
targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue())); targetJointValues.addVariant(Variant(kinematicChain->getNode(i)->getJointValue()));
......
source/RobotAPI/motioncontrol/MotionControl.h
+ 5
− 0
View file @ ae8c266c
...@@ -59,6 +59,7 @@ namespace MotionControl ...@@ -59,6 +59,7 @@ namespace MotionControl
- targetTCPOrientation: the target orientation - targetTCPOrientation: the target orientation
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull - targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation 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 - timeoutInMs: a timeout after which the motion is aborted
Output parameters: Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position - TCPDistanceToTarget: kartesian distance between TCP and target position
...@@ -72,6 +73,7 @@ namespace MotionControl ...@@ -72,6 +73,7 @@ namespace MotionControl
- targetTCPOrientations: the list of target orientations - targetTCPOrientations: the list of target orientations
- targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull - targetPositionDistanceTolerance: tolerance for the position to decide if the motion was successfull
- targetOrientationDistanceTolerance: tolerance for the orientation 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 - timeoutInMs: a timeout after which the motion is aborted
Output parameters: Output parameters:
- TCPDistanceToTarget: kartesian distance between TCP and target position - TCPDistanceToTarget: kartesian distance between TCP and target position
...@@ -169,6 +171,7 @@ namespace MotionControl ...@@ -169,6 +171,7 @@ namespace MotionControl
* \param targetTCPPosition the target position for the TCP * \param targetTCPPosition the target position for the TCP
* \param targetTCPOrientation the target orientation * \param targetTCPOrientation the target orientation
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull * \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 targetOrientationDistanceTolerance tolerance for the orientation to decide if the motion was successfull
* \param timeoutInMs a timeout after which the motion is aborted * \param timeoutInMs a timeout after which the motion is aborted
*/ */
...@@ -195,6 +198,7 @@ namespace MotionControl ...@@ -195,6 +198,7 @@ namespace MotionControl
* \param targetTCPOrientations the list of target orientations * \param targetTCPOrientations the list of target orientations
* \param targetPositionDistanceTolerance tolerance for the position to decide if the motion was successfull * \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 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 * \param timeoutInMs a timeout after which the motion is aborted
*/ */
struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory> struct MoveTCPTrajectory : StateTemplate<MoveTCPTrajectory>
...@@ -377,6 +381,7 @@ namespace MotionControl ...@@ -377,6 +381,7 @@ namespace MotionControl
void run(); void run();
FramedPositionPtr targetPosition; FramedPositionPtr targetPosition;
std::string kinematicChainName; std::string kinematicChainName;
std::string virtualPrismaticJointName;
}; };
} // namespace MotionControl } // namespace MotionControl
......
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