diff --git a/MotionPlanning/Planner/GraspIkRrt.cpp b/MotionPlanning/Planner/GraspIkRrt.cpp
index d959a49d4b01a1b06af0eb7a1035a0dc6919cf47..7d6c00e3f2ed2f4320584f12fde99665ad6a83c8 100644
--- a/MotionPlanning/Planner/GraspIkRrt.cpp
+++ b/MotionPlanning/Planner/GraspIkRrt.cpp
@@ -19,7 +19,7 @@ using namespace VirtualRobot;
namespace Saba
{
- GraspIkRrt::GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::IKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal)
+ GraspIkRrt::GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::AdvancedIKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal)
: BiRrt(cspace, Rrt::eConnect, Rrt::eConnect), object(object), ikSolver(ikSolver), graspSet(graspSet), sampleGoalProbab(probabSampleGoal)
{
THROW_VR_EXCEPTION_IF(!object, "NULL object");
diff --git a/MotionPlanning/Planner/GraspIkRrt.h b/MotionPlanning/Planner/GraspIkRrt.h
index ae0fa9e23e4b7bff91e667a78b450be3e93a3e31..f9f7fcbcca8c8b9faa10bbf1f0b72014078905ab 100644
--- a/MotionPlanning/Planner/GraspIkRrt.h
+++ b/MotionPlanning/Planner/GraspIkRrt.h
@@ -27,7 +27,7 @@
#include "../CSpace/CSpaceSampled.h"
#include "../CSpace/CSpacePath.h"
#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/IK/IKSolver.h>
+#include <VirtualRobot/IK/AdvancedIKSolver.h>
#include "BiRrt.h"
namespace Saba
@@ -51,7 +51,7 @@ namespace Saba
\param graspSet The grasps, defining potential goal configurations.
\param probabSampleGoal Probability with which a goal config is created during planning loop.
*/
- GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::IKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal = 0.1f);
+ GraspIkRrt(CSpaceSampledPtr cspace, VirtualRobot::ManipulationObjectPtr object, VirtualRobot::AdvancedIKSolverPtr ikSolver, VirtualRobot::GraspSetPtr graspSet, float probabSampleGoal = 0.1f);
virtual ~GraspIkRrt();
/*!
@@ -79,7 +79,7 @@ namespace Saba
float sampleGoalProbab;
VirtualRobot::ManipulationObjectPtr object;
- VirtualRobot::IKSolverPtr ikSolver;
+ VirtualRobot::AdvancedIKSolverPtr ikSolver;
VirtualRobot::GraspSetPtr graspSet;
VirtualRobot::RobotNodeSetPtr rns;
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 49082d29d0de1a36e311078cab523be2e85b01ce..9b337c91133a50d77d5096bc2e6e036279a13c37 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -65,6 +65,7 @@ XML/SceneIO.cpp
XML/ObjectIO.cpp
XML/FileIO.cpp
IK/IKSolver.cpp
+IK/AdvancedIKSolver.cpp
IK/DifferentialIK.cpp
IK/GenericIKSolver.cpp
IK/CoMIK.cpp
@@ -175,6 +176,7 @@ XML/SceneIO.h
XML/ObjectIO.h
XML/FileIO.h
IK/IKSolver.h
+IK/AdvancedIKSolver.h
IK/DifferentialIK.h
IK/GenericIKSolver.h
IK/CoMIK.h
diff --git a/VirtualRobot/IK/AdvancedIKSolver.cpp b/VirtualRobot/IK/AdvancedIKSolver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c26ec17a98553b7780c389c6f980c02b1395cf56
--- /dev/null
+++ b/VirtualRobot/IK/AdvancedIKSolver.cpp
@@ -0,0 +1,277 @@
+#include <Eigen/Geometry>
+#include "AdvancedIKSolver.h"
+#include "../Robot.h"
+#include "../VirtualRobotException.h"
+#include "../Nodes/RobotNodePrismatic.h"
+#include "../Nodes/RobotNodeRevolute.h"
+#include "../VirtualRobotException.h"
+#include "../Obstacle.h"
+#include "../ManipulationObject.h"
+#include "../Grasping/Grasp.h"
+#include "../Grasping/GraspSet.h"
+#include "../Workspace/Reachability.h"
+#include "../EndEffector/EndEffector.h"
+#include "../RobotConfig.h"
+#include "../CollisionDetection/CollisionChecker.h"
+#include "../CollisionDetection/CDManager.h"
+
+
+
+#include <algorithm>
+
+using namespace Eigen;
+
+namespace VirtualRobot
+{
+
+ AdvancedIKSolver::AdvancedIKSolver(RobotNodeSetPtr rns) :
+ rns(rns)
+ {
+ verbose = false;
+ THROW_VR_EXCEPTION_IF(!rns, "Null data");
+ tcp = rns->getTCP();
+ THROW_VR_EXCEPTION_IF(!tcp, "no tcp");
+ setMaximumError();
+ }
+
+ void AdvancedIKSolver::collisionDetection(SceneObjectPtr avoidCollisionsWith)
+ {
+ cdm.reset();
+
+ if (avoidCollisionsWith)
+ {
+ cdm.reset(new CDManager(avoidCollisionsWith->getCollisionChecker()));
+ cdm->addCollisionModel(avoidCollisionsWith);
+ cdm->addCollisionModel(rns);
+ }
+ }
+
+ void AdvancedIKSolver::collisionDetection(ObstaclePtr avoidCollisionsWith)
+ {
+ SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(avoidCollisionsWith);
+ collisionDetection(so);
+ }
+
+
+ void AdvancedIKSolver::collisionDetection(SceneObjectSetPtr avoidCollisionsWith)
+ {
+ cdm.reset();
+
+ if (avoidCollisionsWith)
+ {
+ cdm.reset(new CDManager(avoidCollisionsWith->getCollisionChecker()));
+ cdm->addCollisionModel(avoidCollisionsWith);
+ cdm->addCollisionModel(rns);
+ }
+ }
+
+
+ void AdvancedIKSolver::collisionDetection(CDManagerPtr avoidCollisions)
+ {
+ cdm = avoidCollisions;
+ }
+
+ std::vector<float> AdvancedIKSolver::solveNoRNSUpdate(const Eigen::Matrix4f& globalPose, CartesianSelection selection)
+ {
+ std::vector<float> result;
+ std::vector<float> v;
+ rns->getJointValues(v);
+
+ if (solve(globalPose, selection))
+ {
+ rns->getJointValues(result);
+ }
+
+ RobotPtr rob = rns->getRobot();
+ rob->setJointValues(rns, v);
+ return result;
+ }
+
+ bool AdvancedIKSolver::solve(const Eigen::Vector3f& globalPosition)
+ {
+ Eigen::Matrix4f t;
+ t.setIdentity();
+ t.block(0, 3, 3, 1) = globalPosition;
+ return solve(t, Position);
+ }
+
+
+
+ GraspPtr AdvancedIKSolver::solve(ManipulationObjectPtr object, CartesianSelection selection /*= All*/, int maxLoops)
+ {
+ THROW_VR_EXCEPTION_IF(!object, "NULL object");
+ // first get a compatible EEF
+ RobotPtr robot = rns->getRobot();
+ THROW_VR_EXCEPTION_IF(!robot, "NULL robot");
+ std::vector< EndEffectorPtr > eefs;
+ robot->getEndEffectors(eefs);
+ EndEffectorPtr eef;
+
+ for (size_t i = 0; i < eefs.size(); i++)
+ {
+ if (eefs[i]->getTcp() == rns->getTCP())
+ {
+ if (eef)
+ {
+ VR_ERROR << " Two end effectors with tcp " << rns->getTCP()->getName() << " defined in robot " << robot->getName() << ". taking the first one?!" << endl;
+ }
+ else
+ {
+ eef = eefs[i];
+ }
+ }
+ }
+
+ if (!eef)
+ {
+ VR_ERROR << " No end effector with tcp " << rns->getTCP()->getName() << " defined in robot " << robot->getName() << ". Aborting..." << endl;
+ return GraspPtr();
+ }
+
+ GraspSetPtr gs = object->getGraspSet(eef)->clone();
+
+ if (!gs || gs->getSize() == 0)
+ {
+ VR_ERROR << " No grasps defined for eef " << eef->getName() << " defined in object " << object->getName() << ". Aborting..." << endl;
+ return GraspPtr();
+ }
+
+ bool updateStatus = robot->getUpdateVisualizationStatus();
+ robot->setUpdateVisualization(false);
+
+ // check all grasps if there is an IK solution
+ while (gs->getSize() > 0)
+ {
+ GraspPtr g = sampleSolution(object, gs, selection, true, maxLoops);
+
+ if (g)
+ {
+ robot->setUpdateVisualization(updateStatus);
+ robot->applyJointValues();
+ return g;
+ }
+ }
+
+ robot->setUpdateVisualization(updateStatus);
+
+ // when here, no grasp was successful
+ return GraspPtr();
+ }
+
+ bool AdvancedIKSolver::solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection /*= All*/, int maxLoops)
+ {
+ THROW_VR_EXCEPTION_IF(!object, "NULL object");
+ THROW_VR_EXCEPTION_IF(!grasp, "NULL grasp");
+ RobotPtr robot = rns->getRobot();
+ THROW_VR_EXCEPTION_IF(!robot, "NULL robot");
+ bool updateStatus = robot->getUpdateVisualizationStatus();
+ robot->setUpdateVisualization(false);
+
+ std::vector<float> v;
+ rns->getJointValues(v);
+ Eigen::Matrix4f m = grasp->getTcpPoseGlobal(object->getGlobalPose());
+
+ if (_sampleSolution(m, selection, maxLoops))
+ {
+ robot->setUpdateVisualization(updateStatus);
+ robot->applyJointValues();
+ return true;
+ }
+
+ robot->setJointValues(rns, v);
+ robot->setUpdateVisualization(updateStatus);
+ return false;
+ }
+
+
+ GraspPtr AdvancedIKSolver::sampleSolution(ManipulationObjectPtr object, GraspSetPtr graspSet, CartesianSelection selection /*= All*/, bool removeGraspFromSet, int maxLoops)
+ {
+ THROW_VR_EXCEPTION_IF(!object, "NULL object");
+ //THROW_VR_EXCEPTION_IF(!eef,"NULL eef");
+ THROW_VR_EXCEPTION_IF(!graspSet, "NULL graspSet");
+
+ if (graspSet->getSize() == 0)
+ {
+ return GraspPtr();
+ }
+
+ std::vector<float> v;
+ rns->getJointValues(v);
+
+ int pos = rand() % graspSet->getSize();
+ GraspPtr g = graspSet->getGrasp(pos);
+ Eigen::Matrix4f m = g->getTcpPoseGlobal(object->getGlobalPose());
+
+#if 0
+
+ // assuming that reachability is checked in the solve() method, so we don't have ot check it here (->avoid double checks)
+ if (checkReachable(m))
+#endif
+ if (_sampleSolution(m, selection, maxLoops))
+ {
+ return g;
+ }
+
+ // did not succeed, reset joint values and remove grasp from temporary set
+ RobotPtr rob = rns->getRobot();
+ rob->setJointValues(rns, v);
+
+ if (removeGraspFromSet)
+ {
+ graspSet->removeGrasp(g);
+ }
+
+ return GraspPtr();
+ }
+
+ void AdvancedIKSolver::setMaximumError(float maxErrorPositionMM /*= 1.0f*/, float maxErrorOrientationRad /*= 0.02*/)
+ {
+ this->maxErrorPositionMM = maxErrorPositionMM;
+ this->maxErrorOrientationRad = maxErrorOrientationRad;
+ }
+
+ void AdvancedIKSolver::setReachabilityCheck(ReachabilityPtr reachabilitySpace)
+ {
+ this->reachabilitySpace = reachabilitySpace;
+
+ if (reachabilitySpace)
+ {
+ if (reachabilitySpace->getTCP() != tcp)
+ {
+ VR_ERROR << "Reachability representation has different tcp RobotNode (" << reachabilitySpace->getTCP()->getName() << ") than IK solver (" << tcp->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
+ }
+
+ if (reachabilitySpace->getNodeSet() != rns)
+ {
+ VR_ERROR << "Reachability representation is defined for a different RobotNodeSet (" << reachabilitySpace->getNodeSet()->getName() << ") than IK solver uses (" << rns->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
+ }
+ }
+ }
+
+ bool AdvancedIKSolver::checkReachable(const Eigen::Matrix4f& globalPose)
+ {
+ if (!reachabilitySpace)
+ {
+ return true;
+ }
+
+ return reachabilitySpace->isReachable(globalPose);
+ }
+
+ VirtualRobot::RobotNodePtr AdvancedIKSolver::getTcp()
+ {
+ return tcp;
+ }
+
+ VirtualRobot::RobotNodeSetPtr AdvancedIKSolver::getRobotNodeSet()
+ {
+ return rns;
+ }
+
+ void AdvancedIKSolver::setVerbose(bool enable)
+ {
+ verbose = enable;
+ }
+
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/AdvancedIKSolver.h b/VirtualRobot/IK/AdvancedIKSolver.h
new file mode 100644
index 0000000000000000000000000000000000000000..d586388299902fd8862b9302cf521c474c671f3e
--- /dev/null
+++ b/VirtualRobot/IK/AdvancedIKSolver.h
@@ -0,0 +1,171 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Nikolaus Vahrenkamp
+* @copyright 2011 Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#ifndef _VirtualRobot_AdvancedIKSolver_h_
+#define _VirtualRobot_AdvancedIKSolver_h_
+
+#include "../VirtualRobotImportExport.h"
+
+#include "../Nodes/RobotNode.h"
+#include "../RobotNodeSet.h"
+#include "IKSolver.h"
+
+#include <string>
+#include <vector>
+
+
+
+namespace VirtualRobot
+{
+
+ /*!
+ * An advanced IK solver:
+ * Can reject configurations that are in collision.
+ * Can consider reachability information.
+ * Can handle ManipulationObjects and associated grasping information.
+ */
+ class VIRTUAL_ROBOT_IMPORT_EXPORT AdvancedIKSolver : public IKSolver, public boost::enable_shared_from_this<AdvancedIKSolver>
+ {
+ public:
+
+ /*!
+ @brief Initialize a IK solver without collision detection.
+ \param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
+ */
+ AdvancedIKSolver(RobotNodeSetPtr rns);
+
+
+ /*!
+ Setup collision detection
+ \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
+ */
+ virtual void collisionDetection(SceneObjectPtr avoidCollisionsWith);
+ /*!
+ Setup collision detection
+ \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
+ */
+ virtual void collisionDetection(ObstaclePtr avoidCollisionsWith);
+ /*!
+ Setup collision detection
+ \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
+ */
+ virtual void collisionDetection(SceneObjectSetPtr avoidCollisionsWith);
+ /*!
+ Setup collision detection
+ \param avoidCollision The IK solver will consider collision checks, defined in this CDManager instance.
+ */
+ virtual void collisionDetection(CDManagerPtr avoidCollision);
+
+
+
+ /*!
+ Here, the default values of the maximum allowed error in translation and orientation can be changed.
+ \param maxErrorPositionMM The maximum position error that is allowed, given in millimeter.
+ \param maxErrorOrientationRad The maximum orientation error that is allowed, given in radian (0.02 is approx 1 degree).
+ */
+ virtual void setMaximumError(float maxErrorPositionMM = 1.0f, float maxErrorOrientationRad = 0.02);
+
+ /*!
+ When set, the reachability data is used to quickly decide if a given pose or grasp is reachable or not.
+ This option can be enabled by setting the reachability space and it can be disabled by setting an empty ReachabilityPtr.
+ */
+ virtual void setReachabilityCheck(ReachabilityPtr reachabilitySpace);
+
+ /*!
+ This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
+ \param globalPose The target pose given in global coordinate system.
+ \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
+ \param maxLoops An optional parameter, if multiple tries should be made
+ \return true on success
+ */
+ virtual bool solve(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All, int maxLoops = 1) = 0;
+
+
+
+ /*!
+ This method solves the IK up to the specified max error. The joints of the robot node set are not updated.
+ \param globalPose The target pose given in global coordinate system.
+ \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
+ \return The joint angles are returned as std::vector
+ */
+ virtual std::vector<float> solveNoRNSUpdate(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All);
+
+ /*!
+ Convenient method to solve IK queries without considering orientations.
+ */
+ virtual bool solve(const Eigen::Vector3f& globalPosition);
+
+ /*!
+ This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
+ \param object The object.
+ \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
+ \param maxLoops How many tries.
+ \return On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr
+ */
+ virtual GraspPtr solve(ManipulationObjectPtr object, CartesianSelection selection = All, int maxLoops = 1);
+ virtual bool solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection = All, int maxLoops = 1);
+
+
+ /*!
+ Try to find a solution for grasping the object with the given GraspSet.
+ \return On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr
+ */
+ virtual GraspPtr sampleSolution(ManipulationObjectPtr object, GraspSetPtr graspSet, CartesianSelection selection = All, bool removeGraspFromSet = false, int maxLoops = 1);
+
+ /*!
+ This method returns true, when no reachability data is specified for this IK solver.
+ If there is an reachability space defined, it is queried weather the pose is within the reachability or not and the result is returned.
+ */
+ virtual bool checkReachable(const Eigen::Matrix4f& globalPose);
+
+
+ RobotNodeSetPtr getRobotNodeSet();
+ RobotNodePtr getTcp();
+
+ void setVerbose(bool enable);
+
+ protected:
+
+
+ //! This method should deliver solution sample out of the set of possible solutions
+ virtual bool _sampleSolution(const Eigen::Matrix4f& globalPose, CartesianSelection selection, int maxLoops = 1)
+ {
+ return solve(globalPose, selection, maxLoops);
+ }
+
+ RobotNodeSetPtr rns;
+ RobotNodePtr tcp;
+
+ CDManagerPtr cdm;
+
+ float maxErrorPositionMM;
+ float maxErrorOrientationRad;
+
+ ReachabilityPtr reachabilitySpace;
+
+ bool verbose;
+ };
+
+ typedef boost::shared_ptr<AdvancedIKSolver> AdvancedIKSolverPtr;
+} // namespace VirtualRobot
+
+#endif // _VirtualRobot_IKSolver_h_
diff --git a/VirtualRobot/IK/GenericIKSolver.cpp b/VirtualRobot/IK/GenericIKSolver.cpp
index 4b32ca8d10dd1a3d7f3e59aea6f476e2df47cc64..c1708059f63d0c57057e2f7dcbd9cf3b76c2d310 100644
--- a/VirtualRobot/IK/GenericIKSolver.cpp
+++ b/VirtualRobot/IK/GenericIKSolver.cpp
@@ -19,7 +19,7 @@ namespace VirtualRobot
{
GenericIKSolver::GenericIKSolver(RobotNodeSetPtr rns, JacobiProvider::InverseJacobiMethod invJacMethod) :
- IKSolver(rns)
+ AdvancedIKSolver(rns)
{
this->invJacMethod = invJacMethod;
_init();
@@ -72,12 +72,12 @@ namespace VirtualRobot
VirtualRobot::GraspPtr GenericIKSolver::solve(ManipulationObjectPtr object, CartesianSelection selection /*= All*/, int maxLoops)
{
- return IKSolver::solve(object, selection, maxLoops);
+ return AdvancedIKSolver::solve(object, selection, maxLoops);
}
bool GenericIKSolver::solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection /*= All*/, int maxLoops)
{
- return IKSolver::solve(object, grasp, selection, maxLoops);
+ return AdvancedIKSolver::solve(object, grasp, selection, maxLoops);
}
void GenericIKSolver::setJointsRandom()
diff --git a/VirtualRobot/IK/GenericIKSolver.h b/VirtualRobot/IK/GenericIKSolver.h
index 1984c6099e33c86c415c4e4bb9f15e3ae2a090e7..bb18ca6a6994a9e188f156b7e3e7cccf8f3de403 100644
--- a/VirtualRobot/IK/GenericIKSolver.h
+++ b/VirtualRobot/IK/GenericIKSolver.h
@@ -24,7 +24,7 @@
#define _VirtualRobot_GenericIKSolver_h_
#include "../VirtualRobotImportExport.h"
-#include "IKSolver.h"
+#include "AdvancedIKSolver.h"
#include "DifferentialIK.h"
#include "../ManipulationObject.h"
@@ -32,7 +32,7 @@ namespace VirtualRobot
{
- class VIRTUAL_ROBOT_IMPORT_EXPORT GenericIKSolver : public IKSolver
+ class VIRTUAL_ROBOT_IMPORT_EXPORT GenericIKSolver : public AdvancedIKSolver
{
public:
diff --git a/VirtualRobot/IK/IKSolver.cpp b/VirtualRobot/IK/IKSolver.cpp
index f691b87265e2161df4efef485584d0812aae89a5..29359e68c19d27d27d87fd668bf6f2a5c06590b8 100644
--- a/VirtualRobot/IK/IKSolver.cpp
+++ b/VirtualRobot/IK/IKSolver.cpp
@@ -1,277 +1,9 @@
-#include <Eigen/Geometry>
#include "IKSolver.h"
-#include "../Robot.h"
-#include "../VirtualRobotException.h"
-#include "../Nodes/RobotNodePrismatic.h"
-#include "../Nodes/RobotNodeRevolute.h"
-#include "../VirtualRobotException.h"
-#include "../Obstacle.h"
-#include "../ManipulationObject.h"
-#include "../Grasping/Grasp.h"
-#include "../Grasping/GraspSet.h"
-#include "../Workspace/Reachability.h"
-#include "../EndEffector/EndEffector.h"
-#include "../RobotConfig.h"
-#include "../CollisionDetection/CollisionChecker.h"
-#include "../CollisionDetection/CDManager.h"
-
-
-
-#include <algorithm>
-
-using namespace Eigen;
namespace VirtualRobot
{
-
- IKSolver::IKSolver(RobotNodeSetPtr rns) :
- rns(rns)
- {
- verbose = false;
- THROW_VR_EXCEPTION_IF(!rns, "Null data");
- tcp = rns->getTCP();
- THROW_VR_EXCEPTION_IF(!tcp, "no tcp");
- setMaximumError();
- }
-
- void IKSolver::collisionDetection(SceneObjectPtr avoidCollisionsWith)
- {
- cdm.reset();
-
- if (avoidCollisionsWith)
- {
- cdm.reset(new CDManager(avoidCollisionsWith->getCollisionChecker()));
- cdm->addCollisionModel(avoidCollisionsWith);
- cdm->addCollisionModel(rns);
- }
- }
-
- void IKSolver::collisionDetection(ObstaclePtr avoidCollisionsWith)
- {
- SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(avoidCollisionsWith);
- collisionDetection(so);
- }
-
-
- void IKSolver::collisionDetection(SceneObjectSetPtr avoidCollisionsWith)
- {
- cdm.reset();
-
- if (avoidCollisionsWith)
- {
- cdm.reset(new CDManager(avoidCollisionsWith->getCollisionChecker()));
- cdm->addCollisionModel(avoidCollisionsWith);
- cdm->addCollisionModel(rns);
- }
- }
-
-
- void IKSolver::collisionDetection(CDManagerPtr avoidCollisions)
+ IKSolver::IKSolver()
{
- cdm = avoidCollisions;
}
- std::vector<float> IKSolver::solveNoRNSUpdate(const Eigen::Matrix4f& globalPose, CartesianSelection selection)
- {
- std::vector<float> result;
- std::vector<float> v;
- rns->getJointValues(v);
-
- if (solve(globalPose, selection))
- {
- rns->getJointValues(result);
- }
-
- RobotPtr rob = rns->getRobot();
- rob->setJointValues(rns, v);
- return result;
- }
-
- bool IKSolver::solve(const Eigen::Vector3f& globalPosition)
- {
- Eigen::Matrix4f t;
- t.setIdentity();
- t.block(0, 3, 3, 1) = globalPosition;
- return solve(t, Position);
- }
-
-
-
- GraspPtr IKSolver::solve(ManipulationObjectPtr object, CartesianSelection selection /*= All*/, int maxLoops)
- {
- THROW_VR_EXCEPTION_IF(!object, "NULL object");
- // first get a compatible EEF
- RobotPtr robot = rns->getRobot();
- THROW_VR_EXCEPTION_IF(!robot, "NULL robot");
- std::vector< EndEffectorPtr > eefs;
- robot->getEndEffectors(eefs);
- EndEffectorPtr eef;
-
- for (size_t i = 0; i < eefs.size(); i++)
- {
- if (eefs[i]->getTcp() == rns->getTCP())
- {
- if (eef)
- {
- VR_ERROR << " Two end effectors with tcp " << rns->getTCP()->getName() << " defined in robot " << robot->getName() << ". taking the first one?!" << endl;
- }
- else
- {
- eef = eefs[i];
- }
- }
- }
-
- if (!eef)
- {
- VR_ERROR << " No end effector with tcp " << rns->getTCP()->getName() << " defined in robot " << robot->getName() << ". Aborting..." << endl;
- return GraspPtr();
- }
-
- GraspSetPtr gs = object->getGraspSet(eef)->clone();
-
- if (!gs || gs->getSize() == 0)
- {
- VR_ERROR << " No grasps defined for eef " << eef->getName() << " defined in object " << object->getName() << ". Aborting..." << endl;
- return GraspPtr();
- }
-
- bool updateStatus = robot->getUpdateVisualizationStatus();
- robot->setUpdateVisualization(false);
-
- // check all grasps if there is an IK solution
- while (gs->getSize() > 0)
- {
- GraspPtr g = sampleSolution(object, gs, selection, true, maxLoops);
-
- if (g)
- {
- robot->setUpdateVisualization(updateStatus);
- robot->applyJointValues();
- return g;
- }
- }
-
- robot->setUpdateVisualization(updateStatus);
-
- // when here, no grasp was successful
- return GraspPtr();
- }
-
- bool IKSolver::solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection /*= All*/, int maxLoops)
- {
- THROW_VR_EXCEPTION_IF(!object, "NULL object");
- THROW_VR_EXCEPTION_IF(!grasp, "NULL grasp");
- RobotPtr robot = rns->getRobot();
- THROW_VR_EXCEPTION_IF(!robot, "NULL robot");
- bool updateStatus = robot->getUpdateVisualizationStatus();
- robot->setUpdateVisualization(false);
-
- std::vector<float> v;
- rns->getJointValues(v);
- Eigen::Matrix4f m = grasp->getTcpPoseGlobal(object->getGlobalPose());
-
- if (_sampleSolution(m, selection, maxLoops))
- {
- robot->setUpdateVisualization(updateStatus);
- robot->applyJointValues();
- return true;
- }
-
- robot->setJointValues(rns, v);
- robot->setUpdateVisualization(updateStatus);
- return false;
- }
-
-
- GraspPtr IKSolver::sampleSolution(ManipulationObjectPtr object, GraspSetPtr graspSet, CartesianSelection selection /*= All*/, bool removeGraspFromSet, int maxLoops)
- {
- THROW_VR_EXCEPTION_IF(!object, "NULL object");
- //THROW_VR_EXCEPTION_IF(!eef,"NULL eef");
- THROW_VR_EXCEPTION_IF(!graspSet, "NULL graspSet");
-
- if (graspSet->getSize() == 0)
- {
- return GraspPtr();
- }
-
- std::vector<float> v;
- rns->getJointValues(v);
-
- int pos = rand() % graspSet->getSize();
- GraspPtr g = graspSet->getGrasp(pos);
- Eigen::Matrix4f m = g->getTcpPoseGlobal(object->getGlobalPose());
-
-#if 0
-
- // assuming that reachability is checked in the solve() method, so we don't have ot check it here (->avoid double checks)
- if (checkReachable(m))
-#endif
- if (_sampleSolution(m, selection, maxLoops))
- {
- return g;
- }
-
- // did not succeed, reset joint values and remove grasp from temporary set
- RobotPtr rob = rns->getRobot();
- rob->setJointValues(rns, v);
-
- if (removeGraspFromSet)
- {
- graspSet->removeGrasp(g);
- }
-
- return GraspPtr();
- }
-
- void IKSolver::setMaximumError(float maxErrorPositionMM /*= 1.0f*/, float maxErrorOrientationRad /*= 0.02*/)
- {
- this->maxErrorPositionMM = maxErrorPositionMM;
- this->maxErrorOrientationRad = maxErrorOrientationRad;
- }
-
- void IKSolver::setReachabilityCheck(ReachabilityPtr reachabilitySpace)
- {
- this->reachabilitySpace = reachabilitySpace;
-
- if (reachabilitySpace)
- {
- if (reachabilitySpace->getTCP() != tcp)
- {
- VR_ERROR << "Reachability representation has different tcp RobotNode (" << reachabilitySpace->getTCP()->getName() << ") than IK solver (" << tcp->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
- }
-
- if (reachabilitySpace->getNodeSet() != rns)
- {
- VR_ERROR << "Reachability representation is defined for a different RobotNodeSet (" << reachabilitySpace->getNodeSet()->getName() << ") than IK solver uses (" << rns->getName() << ") ?! " << endl << "Reachability results may not be valid!" << endl;
- }
- }
- }
-
- bool IKSolver::checkReachable(const Eigen::Matrix4f& globalPose)
- {
- if (!reachabilitySpace)
- {
- return true;
- }
-
- return reachabilitySpace->isReachable(globalPose);
- }
-
- VirtualRobot::RobotNodePtr IKSolver::getTcp()
- {
- return tcp;
- }
-
- VirtualRobot::RobotNodeSetPtr IKSolver::getRobotNodeSet()
- {
- return rns;
- }
-
- void IKSolver::setVerbose(bool enable)
- {
- verbose = enable;
- }
-
-
} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/IKSolver.h b/VirtualRobot/IK/IKSolver.h
index 92a2dd9fd9696b28e79e676baf7a66e1d2457c3d..14373d3283f7ee912db68eff6b70b446748fcf7c 100644
--- a/VirtualRobot/IK/IKSolver.h
+++ b/VirtualRobot/IK/IKSolver.h
@@ -35,15 +35,16 @@
namespace VirtualRobot
{
-
-
+ /*!
+ Abstract IK solver interface.
+ */
class VIRTUAL_ROBOT_IMPORT_EXPORT IKSolver : public boost::enable_shared_from_this<IKSolver>
{
public:
/*!
- @brief Flags for the selection of the target components.
- @details The flags can be combined with the +-operator.
+ @brief Flags for the selection of the target components.
+ @details The flags can be combined with the +-operator.
*/
enum CartesianSelection
{
@@ -55,125 +56,11 @@ namespace VirtualRobot
All = 15
};
- /*!
- @brief Initialize a IK solver without collision detection.
- \param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
- */
- IKSolver(RobotNodeSetPtr rns);
-
-
- /*!
- Setup collision detection
- \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
- */
- virtual void collisionDetection(SceneObjectPtr avoidCollisionsWith);
- /*!
- Setup collision detection
- \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
- */
- virtual void collisionDetection(ObstaclePtr avoidCollisionsWith);
- /*!
- Setup collision detection
- \param avoidCollisionsWith The IK solver will consider collision checks between rns and avoidCollisionsWith
- */
- virtual void collisionDetection(SceneObjectSetPtr avoidCollisionsWith);
- /*!
- Setup collision detection
- \param avoidCollision The IK solver will consider collision checks, defined in this CDManager instance.
- */
- virtual void collisionDetection(CDManagerPtr avoidCollision);
-
-
-
- /*!
- Here, the default values of the maximum allowed error in translation and orientation can be changed.
- \param maxErrorPositionMM The maximum position error that is allowed, given in millimeter.
- \param maxErrorOrientationRad The maximum orientation error that is allowed, given in radian (0.02 is approx 1 degree).
- */
- virtual void setMaximumError(float maxErrorPositionMM = 1.0f, float maxErrorOrientationRad = 0.02);
-
- /*!
- When set, the reachability data is used to quickly decide if a given pose or grasp is reachable or not.
- This option can be enabled by setting the reachability space and it can be disabled by setting an empty ReachabilityPtr.
- */
- virtual void setReachabilityCheck(ReachabilityPtr reachabilitySpace);
-
- /*!
- This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
- \param globalPose The target pose given in global coordinate system.
- \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
- \param maxLoops An optional parameter, if multiple tries should be made
- \return true on success
- */
- virtual bool solve(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All, int maxLoops = 1) = 0;
-
-
-
- /*!
- This method solves the IK up to the specified max error. The joints of the robot node set are not updated.
- \param globalPose The target pose given in global coordinate system.
- \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
- \return The joint angles are returned as std::vector
- */
- virtual std::vector<float> solveNoRNSUpdate(const Eigen::Matrix4f& globalPose, CartesianSelection selection = All);
-
- /*!
- Convenient method to solve IK queries without considering orientations.
- */
- virtual bool solve(const Eigen::Vector3f& globalPosition);
-
- /*!
- This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.
- \param object The object.
- \param selection Select the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
- \param maxLoops How many tries.
- \return On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr
- */
- virtual GraspPtr solve(ManipulationObjectPtr object, CartesianSelection selection = All, int maxLoops = 1);
- virtual bool solve(ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection = All, int maxLoops = 1);
-
-
- /*!
- Try to find a solution for grasping the object with the given GraspSet.
- \return On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr
- */
- virtual GraspPtr sampleSolution(ManipulationObjectPtr object, GraspSetPtr graspSet, CartesianSelection selection = All, bool removeGraspFromSet = false, int maxLoops = 1);
-
- /*!
- This method returns true, when no reachability data is specified for this IK solver.
- If there is an reachability space defined, it is queried weather the pose is within the reachability or not and the result is returned.
- */
- virtual bool checkReachable(const Eigen::Matrix4f& globalPose);
-
-
- RobotNodeSetPtr getRobotNodeSet();
- RobotNodePtr getTcp();
-
- void setVerbose(bool enable);
-
- protected:
-
-
- //! This method should deliver solution sample out of the set of possible solutions
- virtual bool _sampleSolution(const Eigen::Matrix4f& globalPose, CartesianSelection selection, int maxLoops = 1)
- {
- return solve(globalPose, selection, maxLoops);
- }
-
- RobotNodeSetPtr rns;
- RobotNodePtr tcp;
-
- CDManagerPtr cdm;
-
- float maxErrorPositionMM;
- float maxErrorOrientationRad;
-
- ReachabilityPtr reachabilitySpace;
-
- bool verbose;
+ IKSolver();
};
typedef boost::shared_ptr<IKSolver> IKSolverPtr;
+
} // namespace VirtualRobot
#endif // _VirtualRobot_IKSolver_h_