From c54db1535b50b3111300bc7328373fee8b2bd3cf Mon Sep 17 00:00:00 2001
From: Joshua Alexander Haustein <joshua.haustein@gmail.com>
Date: Sat, 23 May 2015 17:30:28 +0200
Subject: [PATCH] Load test and unit tests for RobotIK successful
---
.../RobotAPI/components/RobotIK/RobotIK.cpp | 111 ++++-
source/RobotAPI/components/RobotIK/RobotIK.h | 190 +++++++-
.../components/RobotIK/test/RobotIKTest.cpp | 458 +++++++++++++-----
.../RobotIK/test/RobotIKTestEnvironment.h | 10 +-
source/RobotAPI/interface/core/RobotIK.ice | 20 +-
source/RobotAPI/libraries/core/Pose.h | 1 +
6 files changed, 645 insertions(+), 145 deletions(-)
diff --git a/source/RobotAPI/components/RobotIK/RobotIK.cpp b/source/RobotAPI/components/RobotIK/RobotIK.cpp
index 705fb081d..759ed4f87 100644
--- a/source/RobotAPI/components/RobotIK/RobotIK.cpp
+++ b/source/RobotAPI/components/RobotIK/RobotIK.cpp
@@ -33,7 +33,7 @@
#include <boost/foreach.hpp>
#include <boost/format.hpp>
-#include <boost/foreach.hpp>
+#include <boost/filesystem.hpp>
#include <Core/core/system/ArmarXDataPath.h>
#include <Core/core/ArmarXManager.h>
@@ -82,6 +82,33 @@ namespace armarx
<< " This component, however, uses " << _robotFile << " Both models must be identical!";
}
_synchronizedRobot = _robotStateComponentPrx->getSynchronizedRobot();
+
+ // Get number of ik trials
+ _numIKTrials = getProperty<int>("NumIKTrials").getValue();
+
+ // Load reachability spaces if a path is specified
+ if (getProperty<std::string>("ReachabilitySpacesFolder").isSet())
+ {
+ std::string spacesPath = getProperty<std::string>("ReachabilitySpacesFolder").getValue();
+ filesystem::path spacesFolder(spacesPath);
+ if (filesystem::is_directory(spacesFolder))
+ {
+ filesystem::directory_iterator end_itr;
+ for (filesystem::directory_iterator iter(spacesFolder); iter != end_itr; iter++)
+ {
+ if (!filesystem::is_directory(iter->status()))
+ {
+ std::string stringPath = iter->path().string();
+ loadReachabilitySpace(stringPath);
+ }
+ }
+ }
+ else
+ {
+ ARMARX_WARNING << "The provided path for reachability spaces is not a directory.";
+ }
+ }
+
}
void RobotIK::onDisconnectComponent()
@@ -114,8 +141,16 @@ namespace armarx
NameValueMap RobotIK::computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current&) const
{
// Make sure we have valid input parameters
- if (!_robotModel->hasRobotNodeSet(robotNodeSetJoints)) return NameValueMap();
- if (!_robotModel->hasRobotNodeSet(desc.robotNodeSetBodies)) return NameValueMap();
+ if (!_robotModel->hasRobotNodeSet(robotNodeSetJoints))
+ {
+ return NameValueMap();
+ }
+
+ if (!_robotModel->hasRobotNodeSet(desc.robotNodeSetBodies))
+ {
+ return NameValueMap();
+ }
+
RobotNodePtr coordSystem = RobotNodePtr();
if (desc.coordSysName.size() > 0 && _robotModel->hasRobotNode(desc.coordSysName))
{
@@ -124,9 +159,10 @@ namespace armarx
// Create and initialize ik solver
RobotNodeSetPtr joints = _robotModel->getRobotNodeSet(robotNodeSetJoints);
- CoMIK comIkSolver(joints, _robotModel->getRobotNodeSet(desc.robotNodeSetBodies));
- Eigen::VectorXf goal;
- goal << desc.goal->x, desc.goal->y;
+ CoMIK comIkSolver(joints, _robotModel->getRobotNodeSet(desc.robotNodeSetBodies), coordSystem);
+ Eigen::VectorXf goal(2);
+ goal(0) = desc.gx;
+ goal(1) = desc.gy;
comIkSolver.setGoal(goal, desc.tolerance);
// Solve
@@ -157,6 +193,8 @@ namespace armarx
throw UserException("Unknown robot node set " + robotNodeSetName);
}
+ synchRobot();
+
RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
// First add all ik tasks
BOOST_FOREACH(DifferentialIKDescriptor ikTask, iktasks)
@@ -183,7 +221,7 @@ namespace armarx
// Now add the center of mass task
if (enableCenterOfMass)
{
- if (!_robotModel->hasRobotNodeSet(comIK.robotNodeSetBodies))
+ if (!_robotModel->hasRobotNodeSet(comIK.robotNodeSetBodies))
{
throw UserException("Unknown robot node set for bodies: " + comIK.robotNodeSetBodies);
}
@@ -195,8 +233,9 @@ namespace armarx
}
CoMIKPtr comIkSolver(new CoMIK(rns, _robotModel->getRobotNodeSet(comIK.robotNodeSetBodies)));
- Eigen::VectorXf goal;
- goal << comIK.goal->x, comIK.goal->y;
+ Eigen::VectorXf goal(2);
+ goal(0) = comIK.gx;
+ goal(1) = comIK.gy;
comIkSolver->setGoal(goal, comIK.tolerance);
JacobiProviderPtr jacoProv = comIkSolver;
jacobiProviders.insert(PriorityJacobiProviderPair(comIK.priority, jacoProv));
@@ -214,7 +253,7 @@ namespace armarx
jacobies.push_back(avoidanceJacobi);
}
- // std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
+ std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
HierarchicalIK hik(rns);
Eigen::VectorXf delta = hik.computeStep(jacobies, stepSize);
NameValueMap result;
@@ -229,10 +268,10 @@ namespace armarx
return result;
}
- bool RobotIK::createReachabilitySpace(const std::string& chainName, float stepTranslation, float stepRotation,
- const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current&)
+ bool RobotIK::createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation,
+ float stepRotation, const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current&)
{
- std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
+ std::lock_guard<std::recursive_mutex> cacheLock(_accessReachabilityCacheMutex);
if (_reachabilities.count(chainName) == 0)
{
if (!_robotModel->hasRobotNodeSet(chainName))
@@ -242,8 +281,24 @@ namespace armarx
ReachabilityPtr reachability(new Reachability(_robotModel));
float minBoundsArray[] = {minBounds.x, minBounds.y, minBounds.z, minBounds.ro, minBounds.pi, minBounds.ya};
float maxBoundsArray[] = {maxBounds.x, maxBounds.y, maxBounds.z, maxBounds.ro, maxBounds.pi, maxBounds.ya};
- // TODO add collision checks etc.
- reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray);
+
+ std::lock_guard<std::recursive_mutex> robotLock(_modifyRobotModelMutex);
+ // TODO add collision checks
+ if (coordinateSystem.size() > 0)
+ {
+ if (!_robotModel->hasRobotNode(coordinateSystem))
+ {
+ ARMARX_ERROR << "Unknown coordinate system " << coordinateSystem;
+ return false;
+ }
+ reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray,
+ VirtualRobot::SceneObjectSetPtr(), VirtualRobot::SceneObjectSetPtr(), _robotModel->getRobotNode(coordinateSystem));
+ }
+ else
+ {
+ reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray);
+ ARMARX_WARNING << "Using global coordinate system to create reachability space.";
+ }
reachability->addRandomTCPPoses(numSamples);
_reachabilities.insert(ReachabilityCacheType::value_type(chainName, reachability));
}
@@ -255,7 +310,6 @@ namespace armarx
{
auto stringsCompareEqual = [] (const std::string& a, const std::string& b) { return a.compare(b) == 0; };
auto stringsCompareSmaller = [] (const std::string& a, const std::string& b) { return a.compare(b) <= 0; };
-
// First check if there is already a set with the given name
// We need to lock here, to make sure we are not adding similar named sets at the same time.
std::lock_guard<std::recursive_mutex> lock(_editRobotNodeSetsMutex);
@@ -283,7 +337,7 @@ namespace armarx
std::pair< std::vector<std::string>::iterator, std::vector<std::string>::iterator > mismatch;
mismatch = std::mismatch(nodeNames.begin(), nodeNames.end(), inputNodeNames.begin(), stringsCompareEqual);
setsIdentical &= mismatch.first == nodeNames.end() && mismatch.second == inputNodeNames.end();
-
+
return setsIdentical;
}
// Else we can add the new robot node set
@@ -343,6 +397,8 @@ namespace armarx
ReachabilityCacheType::const_iterator it = _reachabilities.find(robotNodeSet);
try
{
+ filesystem::path savePath(filename);
+ filesystem::create_directories(savePath.parent_path());
it->second->save(filename);
success = true;
} catch (Exception e)
@@ -367,12 +423,16 @@ namespace armarx
void RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
armarx::CartesianSelection cs, NameValueMap& ikSolution) const
{
+
ikSolution.clear();
- GenericIKSolver ikSolver(nodeSet, JacobiProvider::eSVDDamped);
// For the rest of this function we need to lock access to the robot,
// because we need to make sure we read the correct result from the robot node set.
std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
- bool success = ikSolver.solve(tcpPose, convertCartesianSelection(cs));
+ // Synch the internal robot state with that of the robot state component
+ synchRobot();
+
+ GenericIKSolver ikSolver(nodeSet, JacobiProvider::eSVDDamped);
+ bool success = ikSolver.solve(tcpPose, convertCartesianSelection(cs), _numIKTrials);
// Read solution from node set
if (success)
{
@@ -436,6 +496,19 @@ namespace armarx
}
return VirtualRobot::JacobiProvider::InverseJacobiMethod::eSVD;
}
+
+ void RobotIK::synchRobot() const
+ {
+ std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
+ VirtualRobot::RobotConfigPtr robotConfig = _robotModel->getConfig();
+ BOOST_FOREACH(auto roboNode, robotConfig->getNodes())
+ {
+ SharedRobotNodeInterfacePrx sharedRoboNode = _synchronizedRobot->getRobotNode(roboNode->getName());
+ float val = sharedRoboNode->getJointValue();
+ _robotModel->getRobotNode(roboNode->getName())->setJointValue(val);
+ }
+ }
+
}
diff --git a/source/RobotAPI/components/RobotIK/RobotIK.h b/source/RobotAPI/components/RobotIK/RobotIK.h
index 3a6f2c2be..a84c1e174 100644
--- a/source/RobotAPI/components/RobotIK/RobotIK.h
+++ b/source/RobotAPI/components/RobotIK/RobotIK.h
@@ -49,8 +49,9 @@ namespace armarx
{
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
defineRequiredProperty<std::string>("RobotStateComponentName", "Name of the robot state component that should be used");
- //TODO: define optional properties
+ //Define optional properties
defineOptionalProperty<std::string>("ReachabilitySpacesFolder", "Path to a folder containing reachability spaces");
+ defineOptionalProperty<int>("NumIKTrials", 10, "Number of trials to find an ik solution");
}
};
@@ -61,13 +62,47 @@ namespace armarx
* The RobotIK component is a wrapper for several IK-Solver from
* VirtualRobot (see [Simox](http://simox.sourceforge.net/)).
* It provides a set of functions that allow computing IKs for any kinematic chain
- * in a robot. The VirtualRobot robot model must be specified within the component property.
+ * in a robot. This component requires the following properties:
+ * - RobotFileName: The VirtualRobot robot model to be used.
+ * - RobotStateComponentName: The name of a robot state component.
+ *
+ * Furthermore, there are the following optional properties:
+ * - ReachabilitySpacesFolder: A Path to a folder containing reachability spaces.
+ * - NumIKTrials: The number of trials the underlying ik solver tries to find a solution before giving up (default: 10).
+ *
+ * The robot model is required to solve the IK. Some of the provided functionalities require the current robot state,
+ * which is retrieved from the robot state component specified in the properties. These functions will only work properly,
+ * if the robot model used by the robot state component is identical to the robot model of this component.
+ *
+ * The functionalities include:
+ * - computation of an IK solution for a kinematic chain given a goal TCP pose.
+ * - computation of an IK solution for a set of robot joints such that
+ * the projection of the center of mass to the support surface lies within a goal region.
+ * - computation of a joint value gradient to minimize the workspace error for several tasks simultaneously.
+ * - creation and query of reachability spaces for kinematic chains.
+ *
+ * A reachability space for a kinematic chain allows a fast check whether
+ * a TCP pose is reachable. Creating a reachability space, however, is a computational intensive process
+ * that should be done offline. You may create a reachability space by calling the respective function
+ * or by specifying a path to a folder, ReachabilitySpacesFolder, that contains a set of reachability spaces.
+ * If ReachabilitySpacesFolder is specified, each file in the folder is attempted to be loaded as reachability space
+ * during initialization.
*
* While this implementation allows parallel access to its interface functions,
* it is not optimized towards maximizing parallelism.
* If you need to access this component from a large number of threads/processes
* and you care about performance, you should consider creating multiple instances
- * of this component.
+ * of this component or using local instances of the IK solver from Simox instead.
+ *
+ * Furthermore, at the beginning of an operation this component synchronizes its
+ * internal robot state with that provided by the robot state component.
+ * This can lead to invalid IK solutions if the robot state changes while computation is still in progress.
+ * For example, imagine you compute an IK solution just
+ * for the forearm and the wrist given a global eef pose. If during the computation the configuration
+ * of the elbow changes, the computed ik solution will no longer achieve the desired eef pose.
+ *
+ * Also see [Simox-Tutorial] (http://sourceforge.net/p/simox/wiki/VirtualRobot/) for more information
+ * of how to use Simox for IK solving.
*/
class ARMARXCOMPONENT_IMPORT_EXPORT RobotIK :
virtual public Component,
@@ -91,34 +126,174 @@ namespace armarx
}
/**
- * Compute IK for node set.
- */
+ * @brief Computes a single IK solution for the given robot node set and desired TCP pose.
+ * @details The TCP pose can be defined in any frame and is converted internally to a global pose
+ * according to the current robot state. The CartesianSelection
+ * parameter defines which part of the target pose should be reached.
+ *
+ * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
+ * within the robot model or has been defined via \ref defineRobotNodeSet.
+ * @param tcpPose The framed target pose for the TCP.
+ * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
+ * the orientation only or all.
+ * @return A map that maps each joint name to its value in the found IK solution.
+ */
virtual NameValueMap computeIKFramedPose(const std::string& robotNodeSetName,
const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
+ * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
+ * parameter defines which part of the target pose should be reached.
+ *
+ * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
+ * within the robot model or has been defined via \ref defineRobotNodeSet.
+ * @param tcpPose The global target pose for the TCP.
+ * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
+ * the orientation only or all.
+ * @return A map that maps each joint name to its value in the found IK solution.
+ */
virtual NameValueMap computeIKGlobalPose(const std::string& robotNodeSetName,
const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
+ * given point.
+ * @details
+ *
+ * @param robotNodeSetJoints Name of the robot node set that contains the joints you wish to compute the IK for.
+ * @param comIK A center of mass description. Note that the priority field is relevant for this function as there
+ * is only a single CoM of descriptor.
+ *
+ * @return The ik-solution. Returns an empty vector if there is no solution.
+ */
virtual NameValueMap computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
+ * @details This function allows you to use the HierarchicalIK solver provided by Simox.
+ * It computes a configuration gradient for the given robot node set that minimizes the workspace errors
+ * for multiple tasks simultaneously. You can specify IK tasks, a center of mass task and a joint limit avoidance task.
+ * For details for the different type of tasks, see the parameter description. You must define a priority for each task.
+ * The task with maximal priority is the first task to be solved. Each subsequent task is then solved
+ * within the null space of the previous task. <b> Note that this function returns a gradient and NOT
+ * an absolute IK solution. The gradient is computed on the current configuration of the robot. <\b>.
+ * The gradient is computed from the current robot configuration.
+ *
+ * See @url http://simox.sourceforge.net/documentation/class_virtual_robot_1_1_hierarchical_i_k.html
+ * and @url http://sourceforge.net/p/simox/wiki/VirtualRobot/#hierarchical-ik-solving for more information.
+ *
+ * @param robotNodeSet The robot node set (e.g. kinematic tree) you wish to compute the gradient for.
+ * @param diffIKs A list of IK tasks. Each IK task specifies a TCP and a desired pose for this TCP.
+ * @param comIK A center of mass tasks. Defines where the center should be and its priority. Is only used if the
+ CoM-task is enabled.
+ * @param stepSize
+ * @param avoidJointLimits Set whether or not to avoid joint limits.
+ * @param enableCenterOfMass Set whether or not to adjust the center of mass.
+ * @return A configuration gradient...
+ */
virtual NameValueMap computeHierarchicalDeltaIK(const std::string& robotNodeSet,
const IKTasks& iktasks, const CoMIKDescriptor& comIK,
float stepSize, bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current& = Ice::Current()) const;
- virtual bool createReachabilitySpace(const std::string& chainName, float stepTranslation, float stepRotation,
+ /**
+ * @brief Creates a new reachability space for the given robot node set.
+ * @details If there is no reachability space for the given robot node set yet, a new one is created. This may take
+ * some time. The function returns true iff a reachability space for the given robot node set exists after
+ * execution of this function.
+ *
+ * @param chainName The name of a defined robot node set. This can be either a robot node set that is defined in the
+ * robot model or a robot node set that has been manually defined calling \ref defineRobotNodeSet.
+ * @param coordinateSystem The name of the robot node in whose coordinate system the reachability space shall be defined
+ * in. If you wish to choose the global coordinate system, pass an empty string.
+ * Note, however, that any reachability space defined in the
+ * global coordinate system gets invalidated once the robot moves.
+ * @param stepTranslation The extend of a voxel dimension in translational dimensions (x,y,z) [mm]
+ * @param stepRotation The extend of a voxel dimension in rotational dimensions (roll,pitch,yaw) [rad]
+ * @param minBounds The minimum workspace poses (x,y,z,ro,pi,ya) given in the base node's coordinate system [mm and rad]
+ * @param maxBounds The maximum workspace poses (x,y,z,ro,pi,ya) given in base node's coordinate system [mm and rad]
+ * @param numSamples The number of tcp samples to take to create the reachability space (e.g 1000000)
+ * @return True iff the a reachability space for the given robot node set is available after execution of this function.
+ * False in case of a failure, e.g. there is no chain with the given name.
+ */
+ virtual bool createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation, float stepRotation,
const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current& = Ice::Current());
+ /**
+ * @brief Defines a new robot node set.
+ * @details Define a new robot node set with the given name that consists out of the given list of nodes with given
+ * TCP and root frame. Iff the chain is successfully added or already exists, <it>true</it> is returned.
+ * @param name The name of the robot node set.
+ * @param nodes The list of robot nodes that make up the robot node set.
+ * @param tcpName The name of the TCP node.
+ * @param rootNode The name of the kinematic root.
+ *
+ * @return True, iff chain was added or already exists. False, iff a different chain with similar name already exists.
+ */
virtual bool defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
const std::string& tcpName, const std::string& rootNode, const Ice::Current& = Ice::Current());
+ /**
+ * @brief Returns whether this component has a reachability space for the given robot node set.
+ * @details True if there is a reachability space available, else false.
+ *
+ * @param chainName Name of the robot node set.
+ * @return True if there is a reachability space available, else false.
+ */
virtual bool hasReachabilitySpace(const std::string& chainName, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Returns whether a given framed pose is currently reachable by the TCP of the given robot node set.
+ * @details To determine whether a pose is reachable a reachability space for the given robot node set is
+ * required. If no such space exists, the function returns <it>false<\it>.
+ * Call \ref createReachabilitySpace first to ensure there is such a space.
+ *
+ * @param chainName A name of a robot node set either defined in the robot model or previously defined via
+ * \ref defineRobotNodeSet.
+ * @param framedPose A framed pose to check for reachability. The pose is transformed into a global pose using the
+ * current robot state.
+ *
+ * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
+ * there is no reachability space for the given chain available.
+ */
virtual bool isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
+ * @details To determine whether a pose is reachable a reachability space for the given robot node set is
+ * required. If no such space exists, the function returns <it>false<\it>.
+ * Call \ref createReachabilitySpace first to ensure there is such a space.
+ *
+ * @param chainName A name of a robot node set either defined in the robot model or previously defined via
+ * \ref defineRobotNodeSet.
+ * @param pose A global pose to check for reachability.
+ *
+ * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
+ * there is no reachability space for the given chain available.
+ */
virtual bool isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current& = Ice::Current()) const;
+ /**
+ * @brief Loads the reachability space from the given file.
+ * @details If loading the reachability space succeeds, the reachability space is ready to be used after this function
+ * terminates. A reachability space can only be loaded if there is no reachability space for the respective
+ * robot node set yet.
+ *
+ * @param filename Binary file containing a reachability space. Ensure that the path is valid and accessible from
+ * where this component is running.
+ * @return True iff loading the reachability space was successful.
+ */
virtual bool loadReachabilitySpace(const std::string& filename, const Ice::Current& = Ice::Current());
+ /**
+ * @brief Saves a previously created reachability space of the given robot node set.
+ * @details A reachability space for a robot node set can only be saved, if actually is one.
+ * You can check whether a reachability space is available by calling \ref hasReachabilitySpace(robotNodeSet).
+ *
+ * @param robotNodeSet The robot node set for which you wish to save the reachability space.
+ * @param filename The filename if the file(must be an accessible path for this component) you wish to save the space in.
+ * @return True iff saving was successful.
+ */
virtual bool saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current& = Ice::Current()) const;
protected:
@@ -145,6 +320,8 @@ namespace armarx
VirtualRobot::JacobiProvider::InverseJacobiMethod convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const;
+ void synchRobot() const;
+
// Lock this mutex if you plan to modify the robot model
// Exception: When adding node sets we don't need to.
// Invariant: We never delete node sets nor nodes!
@@ -162,6 +339,7 @@ namespace armarx
// Reachability cache: not thread safe, always lock!
typedef std::map<std::string, VirtualRobot::ReachabilityPtr> ReachabilityCacheType;
ReachabilityCacheType _reachabilities;
+ int _numIKTrials;
};
}
diff --git a/source/RobotAPI/components/RobotIK/test/RobotIKTest.cpp b/source/RobotAPI/components/RobotIK/test/RobotIKTest.cpp
index 49d4bc23e..5e59c4762 100644
--- a/source/RobotAPI/components/RobotIK/test/RobotIKTest.cpp
+++ b/source/RobotAPI/components/RobotIK/test/RobotIKTest.cpp
@@ -37,6 +37,10 @@
#include <iostream>
#include <thread>
+#include <chrono>
+#include <ctime>
+#include <ratio>
+#include <random>
bool comparePosition(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
{
@@ -45,6 +49,11 @@ bool comparePosition(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
{
equal &= (m1(i, 3) - m2(i, 3)) * (m1(i, 3) - m2(i, 3)) <= EPSILON_POS;
}
+
+ if (!equal)
+ {
+ BOOST_TEST_MESSAGE("Positions not equal for poses: " << m1 << ", " << m2);
+ }
return equal;
}
@@ -58,6 +67,11 @@ bool compareOrientation(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2)
equal &= (m1(i, j) - m2(i, j)) * (m1(i, j) - m2(i, j)) <= EPSILON_ROT;
}
}
+
+ if (!equal)
+ {
+ BOOST_TEST_MESSAGE("Poses not equal: " << m1 << ", " << m2);
+ }
return equal;
}
@@ -79,38 +93,62 @@ bool compareResults(const Eigen::Matrix4f& m1, const Eigen::Matrix4f& m2, Cartes
return success;
}
-void testIKGlobalPose(const std::string& setName, PosePtr pose, bool& success, CartesianSelection csel, RobotIKTestEnvironment& testEnv)
+void synchRobot(VirtualRobot::RobotPtr localRobot, RobotStateComponentInterfacePrx robotState)
+{
+ VirtualRobot::RobotConfigPtr robotConfig = localRobot->getConfig();
+ SharedRobotInterfacePrx sharedRobot = robotState->getSynchronizedRobot();
+ BOOST_FOREACH(auto roboNode, robotConfig->getNodes())
+ {
+ SharedRobotNodeInterfacePrx sharedRoboNode = sharedRobot->getRobotNode(roboNode->getName());
+ float val = sharedRoboNode->getJointValue();
+ localRobot->getRobotNode(roboNode->getName())->setJointValue(val);
+ }
+}
+
+void testIKGlobalPose(const std::string& setName, PosePtr pose, bool& success, CartesianSelection csel,
+ VirtualRobot::RobotPtr localRobot, RobotIKInterfacePrx robotIK, RobotStateComponentInterfacePrx sharedRobot)
{
// Test component
- NameValueMap iksolution = testEnv._robotIK->computeIKGlobalPose(setName, pose, csel);
+ synchRobot(localRobot, sharedRobot);
+ NameValueMap iksolution = robotIK->computeIKGlobalPose(setName, pose, csel);
// Check validity of output
if (iksolution.size() > 0)
{
+ BOOST_TEST_MESSAGE("Pose " << pose);
BOOST_TEST_MESSAGE("Validating ik solution" << iksolution);
- testEnv._robotModel->setJointValues(iksolution);
- VirtualRobot::RobotNodeSetPtr nodeset = testEnv._robotModel->getRobotNodeSet(setName);
+ localRobot->setJointValues(iksolution);
+ BOOST_TEST_MESSAGE("configuration after setting:");
+ BOOST_FOREACH(auto ikEntry, iksolution)
+ {
+ float val = localRobot->getRobotNode(ikEntry.first)->getJointValue();
+ BOOST_TEST_MESSAGE(ikEntry.first << ": " << val);
+ }
+ VirtualRobot::RobotNodeSetPtr nodeset = localRobot->getRobotNodeSet(setName);
Eigen::Matrix4f tcpPose = nodeset->getTCP()->getGlobalPose();
- BOOST_TEST_MESSAGE("Comparing tcp pose");
+ // BOOST_TEST_MESSAGE("Comparing tcp pose");
success = compareResults(tcpPose, pose->toEigen(), csel);
} else {
+ BOOST_TEST_MESSAGE("Empty solution");
success = false;
}
}
-void testIKFramedPose(const std::string& setName, FramedPosePtr fpose, bool& success, CartesianSelection csel, RobotIKTestEnvironment& testEnv)
+void testIKFramedPose(const std::string& setName, FramedPosePtr fpose, bool& success, CartesianSelection csel,
+ VirtualRobot::RobotPtr localRobot, RobotIKInterfacePrx robotIK, RobotStateComponentInterfacePrx roboState)
{
- FramedPosePtr globalPose = fpose->toGlobal(testEnv._robotStateComponent->getSynchronizedRobot());
+ FramedPosePtr globalPose = fpose->toGlobal(roboState->getSynchronizedRobot());
Eigen::Matrix4f poseGlobal = globalPose->toEigen();
// Test component
- NameValueMap iksolution = testEnv._robotIK->computeIKFramedPose(setName, fpose, csel);
+ synchRobot(localRobot, roboState);
+ NameValueMap iksolution = robotIK->computeIKFramedPose(setName, fpose, csel);
// Check validity of output
if (iksolution.size() > 0)
{
- VirtualRobot::RobotNodeSetPtr nodeset = testEnv._robotModel->getRobotNodeSet(setName);
- testEnv._robotModel->setJointValues(iksolution);
+ VirtualRobot::RobotNodeSetPtr nodeset = localRobot->getRobotNodeSet(setName);
+ localRobot->setJointValues(iksolution);
Eigen::Matrix4f tcpPose = nodeset->getTCP()->getGlobalPose();
success = compareResults(tcpPose, poseGlobal, csel);
@@ -119,11 +157,8 @@ void testIKFramedPose(const std::string& setName, FramedPosePtr fpose, bool& suc
}
}
-BOOST_AUTO_TEST_CASE(RobotIKGlobalPoseTest)
+void doGlobalPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
- // Create test environment first
- RobotIKTestEnvironment testEnv("RobotIKGlobalPoseTest");
-
// Create test input
std::string nodeSetName1("TorsoLeftArm");
Eigen::Matrix4f poseMatrix1;
@@ -153,12 +188,15 @@ BOOST_AUTO_TEST_CASE(RobotIKGlobalPoseTest)
std::vector<std::thread> threads;
for (unsigned int tid = 0; tid < NUMBER_THREADS; ++tid)
{
+ VirtualRobot::RobotPtr localRobot1 = testEnv._robotModel->clone("LocalRobot1");
threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName1), pose1,
- std::ref(testSuccesses[3 * tid]), CartesianSelection::ePosition, std::ref(testEnv)));
+ std::ref(testSuccesses[3 * tid]), CartesianSelection::ePosition, localRobot1, testEnv._robotIK, testEnv._robotStateComponent));
+ VirtualRobot::RobotPtr localRobot2 = testEnv._robotModel->clone("LocalRobot2");
threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName2), pose2,
- std::ref(testSuccesses[3 * tid + 1]), CartesianSelection::eAll, std::ref(testEnv)));
+ std::ref(testSuccesses[3 * tid + 1]), CartesianSelection::eAll, localRobot2, testEnv._robotIK, testEnv._robotStateComponent));
+ VirtualRobot::RobotPtr localRobot3 = testEnv._robotModel->clone("LocalRobot3");
threads.push_back(std::thread(testIKGlobalPose, std::ref(nodeSetName2), unreachablePose,
- std::ref(testSuccesses[3 * tid + 2]), CartesianSelection::eAll, std::ref(testEnv)));
+ std::ref(testSuccesses[3 * tid + 2]), CartesianSelection::eAll, localRobot3, testEnv._robotIK, testEnv._robotStateComponent));
}
for (size_t tid = 0; tid < threads.size(); ++tid)
@@ -172,13 +210,11 @@ BOOST_AUTO_TEST_CASE(RobotIKGlobalPoseTest)
BOOST_CHECK(testSuccesses[3 * tid + 1]);
BOOST_CHECK(!testSuccesses[3 * tid + 2]);
}
+ finished = true;
}
-BOOST_AUTO_TEST_CASE(RobotIKFramedPoseTest)
+void doFramedPoseTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
- // Create test environment first
- RobotIKTestEnvironment testEnv("RobotIKFramedPoseTest");
-
// Create test input
std::string nodeSetName("TorsoLeftArm");
std::string frame("TCP L");
@@ -193,8 +229,9 @@ BOOST_AUTO_TEST_CASE(RobotIKFramedPoseTest)
std::vector<std::thread> threads;
for (unsigned int tid = 0; tid < NUMBER_THREADS; ++tid)
{
+ VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("LocalRobot");
threads.push_back(std::thread(testIKFramedPose, std::ref(nodeSetName), pose,
- std::ref(testSuccesses[tid]), CartesianSelection::eAll, std::ref(testEnv)));
+ std::ref(testSuccesses[tid]), CartesianSelection::eAll, localRobot, testEnv._robotIK, testEnv._robotStateComponent));
}
for (size_t tid = 0; tid < threads.size(); ++tid)
@@ -205,13 +242,14 @@ BOOST_AUTO_TEST_CASE(RobotIKFramedPoseTest)
for (size_t tid = 0; tid < NUMBER_THREADS; ++tid) {
BOOST_CHECK(testSuccesses[tid]);
}
+ finished = true;
}
-BOOST_AUTO_TEST_CASE(RobotIKReachabilityTest)
+void doReachabilityTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
- RobotIKTestEnvironment testEnv("RobotIKReachabilityTest");
std::string nodeSetName("TorsoLeftArm");
std::string frame("TCP L");
+ std::string baseFrame("Armar3_Base");
// Define two global poses
// First, a reachable pose
@@ -268,7 +306,7 @@ BOOST_AUTO_TEST_CASE(RobotIKReachabilityTest)
// BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, reachableFramedPose));
BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, unreachableFramedPose));
- bool createdReachabilitySpace = testEnv._robotIK->createReachabilitySpace(nodeSetName, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
+ bool createdReachabilitySpace = testEnv._robotIK->createReachabilitySpace(nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
// This should work
BOOST_CHECK(createdReachabilitySpace);
@@ -280,38 +318,101 @@ BOOST_AUTO_TEST_CASE(RobotIKReachabilityTest)
BOOST_CHECK(!testEnv._robotIK->isPoseReachable(nodeSetName, unreachablePose));
// BOOST_CHECK(testEnv._robotIK->isFramedPoseReachable(nodeSetName, reachableFramedPose));
BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, unreachableFramedPose));
+
+ std::string savepath(testEnv._spaceSavePath + "testReachabilitySpace.bin");
+ BOOST_TEST_MESSAGE(savepath);
+ BOOST_CHECK(testEnv._robotIK->saveReachabilitySpace(nodeSetName, savepath));
+ finished = true;
}
-BOOST_AUTO_TEST_CASE(RobotIKDefineRobotSetTest)
+void doLoadReachabilityTest(const RobotIKTestEnvironment& testEnv, bool& finished)
+{
+ std::string nodeSetName("TorsoLeftArm");
+ std::string frame("TCP L");
+ std::string baseFrame("Armar3_Base");
+
+ // Define two global poses
+ // First, a reachable pose
+ Eigen::Matrix4f matrix;
+ matrix << 0.275582f, 0.950628f, 0.142693f, -363.636f,
+ 0.892777f, -0.198076f, -0.404616f, 181.818f,
+ -0.356376f, 0.238898f, -0.903285f, 727.273f,
+ 1.0f, 1.0f, 1.0f, 1.0f;
+ PosePtr reachablePose = new Pose(matrix);
+ // Second, a unreachable pose
+ matrix << 1.0f, 0.0f, 0.0f, 1000.0f,
+ 0.0f, 1.0f, 0.0f, 1000.0f,
+ 0.0f, 0.0f, 1.0f, 1300.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+ PosePtr unreachablePose = new Pose(matrix);
+
+ matrix << 0.86603f, -0.5f, 0.0f, 1000.0f,
+ 0.5f, 0.86603f, 0.0f, 600.0f,
+ 0.0f, 0.0f, 1.0f, 400.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+ FramedPosePtr unreachableFramedPose = new FramedPose(matrix, frame, testEnv._robotStateComponent->getRobotName());
+
+ // To create a reachability space we need workspace bounds.
+ WorkspaceBounds minBounds;
+ minBounds.x = -1000.0f;
+ minBounds.y = -1000.0f;
+ minBounds.z = -1000.0f;
+ minBounds.ro = -3.15f;
+ minBounds.pi = -3.15f;
+ minBounds.ya = -3.15f;
+
+ WorkspaceBounds maxBounds;
+ maxBounds.x = 1000.0f;
+ maxBounds.y = 1000.0f;
+ maxBounds.z = 1000.0f;
+ maxBounds.ro = 3.15f;
+ maxBounds.pi = 3.15f;
+ maxBounds.ya = 3.15f;
+
+ // Now start testing
+ testEnv._robotIK->createReachabilitySpace(nodeSetName, baseFrame, 200.0f, 0.3f, minBounds, maxBounds, 1000000);
+
+ // Now we should have a reachability space:
+ BOOST_CHECK(testEnv._robotIK->hasReachabilitySpace(nodeSetName));
+
+ // Finally test the poses:
+ BOOST_CHECK(testEnv._robotIK->isPoseReachable(nodeSetName, reachablePose));
+ BOOST_CHECK(!testEnv._robotIK->isPoseReachable(nodeSetName, unreachablePose));
+ // BOOST_CHECK(testEnv._robotIK->isFramedPoseReachable(nodeSetName, reachableFramedPose));
+ BOOST_CHECK(!testEnv._robotIK->isFramedPoseReachable(nodeSetName, unreachableFramedPose));
+
+ finished = true;
+}
+
+void doDefineNodeSetTests(const RobotIKTestEnvironment& testEnv, bool& finished)
{
- RobotIKTestEnvironment testEnv("RobotIKReachabilityTest");
std::string setName1("TestSet1");
std::string tcpName("TCP L");
- std::string rootNode("Armar3_Base");
+ std::string rootNode("Platform");
NodeNameList jointList;
jointList.push_back("Shoulder 1 L");
jointList.push_back("Shoulder 2 L");
jointList.push_back("Upperarm L");
jointList.push_back("Elbow L");
jointList.push_back("Underarm L");
- jointList.push_back("TCP L");
+
+ VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("MyLocalRobot");
bool defineSuccessful = testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName, rootNode);
BOOST_CHECK(defineSuccessful);
- BOOST_TEST_MESSAGE(testEnv._robotModel->getRobotNode(tcpName)->getGlobalPose());
-
- VirtualRobot::RobotNodeSet::createRobotNodeSet(testEnv._robotModel, setName1, jointList, rootNode, tcpName, true);
+ VirtualRobot::RobotNodeSet::createRobotNodeSet(localRobot, setName1, jointList, rootNode, tcpName, true);
Eigen::Matrix4f poseMatrix1;
poseMatrix1 << -0.258819f, -0.965926f, 0.0f, -315.0f,
0.0f, 0.0f, 1.0f, 550.0f,
-0.965926f, 0.258819f, 0.0f, 1077.5f,
0.0f, 0.0f, 0.0f, 1.0f;
+
PosePtr pose1 = new Pose(poseMatrix1);
bool ikSuccess = false;
- testIKGlobalPose(setName1, pose1, ikSuccess, CartesianSelection::ePosition, testEnv);
+ testIKGlobalPose(setName1, pose1, ikSuccess, CartesianSelection::ePosition, localRobot, testEnv._robotIK, testEnv._robotStateComponent);
BOOST_CHECK(ikSuccess);
// Redefine should always return true.
@@ -325,86 +426,221 @@ BOOST_AUTO_TEST_CASE(RobotIKDefineRobotSetTest)
BOOST_CHECK(!testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName2, rootNode));
std::string rootNode2("Shoulder 1 L");
BOOST_CHECK(!testEnv._robotIK->defineRobotNodeSet(setName1, jointList, tcpName, rootNode2));
+ finished = true;
}
-// BOOST_AUTO_TEST_CASE(RobotIKCustomNodeSetGlobalPoseTest)
-// {
-// // Create test environment first
-// RobotIKTestEnvironment testEnv("RobotIKCustomNodeSetGlobalPoseTest");
-
-// // Create test input
-// JointList jointList;
-// jointList.push_back("Shoulder 1 L");
-// jointList.push_back("Shoulder 2 L");
-// jointList.push_back("Upperarm L");
-// jointList.push_back("Elbow L");
-// jointList.push_back("Underarm L");
-// jointList.push_back("TCP L");
-
-// Eigen::Matrix4f poseMatrix;
-// poseMatrix << 1.0f, 0.0f, 0.0f, -290.0f,
-// 0.0f, 1.0f, 0.0f, 500.0f,
-// 0.0f, 0.0f, 1.0f, 1077.0f,
-// 0.0f, 0.0f, 0.0f, 1.0f;
-// PosePtr pose = new Pose(poseMatrix);
-
-// // Test component
-// NameValueMap iksolution = testEnv._robotIK->computeCustomNodeSetIKGlobalPose(jointList, pose, CartesianSelection::ePosition);
+void doRobotIKComputeCoMIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+{
+ std::string completeRobotSetname("Robot");
+ CoMIKDescriptor comdesc;
+ comdesc.priority = 0;
+ comdesc.gx = 4.0;
+ comdesc.gy = 45.0;
+ comdesc.robotNodeSetBodies = completeRobotSetname;
+ comdesc.tolerance = 2.0f;
+ comdesc.coordSysName = "Armar3_Base";
+ std::string jointsSetName("TorsoBothArms");
+ VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("MyLocalRobot");
-// // Check validity of output
-// testEnv._robotModel->setJointValues(iksolution);
-// VirtualRobot::RobotNodePtr robotNode = testEnv._robotModel->getRobotNode("TCP L");
-// Eigen::Matrix4f nodePose = robotNode->getGlobalPose();
-// BOOST_CHECK(comparePosition(nodePose, poseMatrix));
-
-// // Create test input 2
-// poseMatrix << 0.0f, -1.0f, 0.0f, -315.0f,
-// 0.0f, 0.0f, 1.0f, 480.0f,
-// -1.0f, 0.0f, 0.0f, 1080.0f,
-// 0.0f, 0.0f, 0.0f, 1.0f;
-// pose = new Pose(poseMatrix);
-
-// // Test component
-// iksolution = testEnv._robotIK->computeCustomNodeSetIKGlobalPose(jointList, pose, CartesianSelection::eAll);
+ NameValueMap ikSolution = testEnv._robotIK->computeCoMIK(jointsSetName, comdesc);
+ VirtualRobot::RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(completeRobotSetname);
-// // Check validity of output
-// testEnv._robotModel->setJointValues(iksolution);
-// robotNode = testEnv._robotModel->getRobotNode("TCP L");
-// nodePose = robotNode->getGlobalPose();
-// BOOST_CHECK(comparePosition(nodePose, poseMatrix) && compareOrientation(nodePose, poseMatrix));
-// }
-
-// BOOST_AUTO_TEST_CASE(RobotIKCustomNodeSetFramedPoseTest)
-// {
-// // Create test environment first
-// RobotIKTestEnvironment testEnv("RobotIKCustomNodeSetFramedPoseTest");
-
-// // Create test input
-// JointList jointList;
-// jointList.push_back("Shoulder 1 L");
-// jointList.push_back("Shoulder 2 L");
-// jointList.push_back("Upperarm L");
-// jointList.push_back("Elbow L");
-// jointList.push_back("Underarm L");
-// jointList.push_back("Wrist 1 L");
-// jointList.push_back("Wrist 2 L");
-// jointList.push_back("TCP L");
-// std::string frame("TCP L");
-// Eigen::Matrix4f poseMatrix;
-// poseMatrix << 0.86603f, -0.5f, 0.0f, 50.0f,
-// 0.5f, 0.86603f, 0.0f, 60.0f,
-// 0.0f, 0.0f, 1.0f, 40.0f,
-// 0.0f, 0.0f, 0.0f, 1.0f;
-// FramedPosePtr pose = new FramedPose(poseMatrix, frame, testEnv._robotStateComponent->getRobotName());
-// FramedPosePtr globalPose = pose->toGlobal(testEnv._robotStateComponent->getSynchronizedRobot());
-// Eigen::Matrix4f poseGlobal = globalPose->toEigen();
-
-// // Test component
-// NameValueMap iksolution = testEnv._robotIK->computeCustomNodeSetIKFramedPose(jointList, pose, CartesianSelection::eAll);
+ localRobot->setJointValues(ikSolution);
+ Eigen::Vector3f resultCoM = robotNodeSet->getCoM();
+
+ // BOOST_TEST_MESSAGE("desired com " << comdesc.gx << " " << comdesc.gy);
+ // BOOST_TEST_MESSAGE("is com " << resultCoM[0] << " " << resultCoM[1]);
+ BOOST_CHECK((comdesc.gx - resultCoM[0])*(comdesc.gx - resultCoM[0]) <= comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gy - resultCoM[1])*(comdesc.gy - resultCoM[1]) <= comdesc.tolerance * comdesc.tolerance);
+ finished = true;
+}
+
+void doRobotIKComputeHierarchicalDeltaIKTests(const RobotIKTestEnvironment& testEnv, bool& finished)
+{
+ std::string jointsSetName("TorsoBothArms");
+ IKTasks ikTasks;
+ // Set up a task for the left arm first
+ DifferentialIKDescriptor leftArmGoal;
+ leftArmGoal.priority = 2;
+ leftArmGoal.tcpName = "TCP L";
+ leftArmGoal.csel = CartesianSelection::ePosition;
+ leftArmGoal.ijm = InverseJacobiMethod::eSVD;
+ Eigen::Matrix4f poseMatrix1;
+ poseMatrix1 << 1.0f, 0.0f, 0.0f, -285.0f,
+ 0.0f, 1.0f, 0.0f, 540.0f,
+ 0.0f, 0.0f, 1.0f, 1125.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+ PosePtr pose1 = new Pose(poseMatrix1);
+ leftArmGoal.tcpGoal = pose1;
+ ikTasks.push_back(leftArmGoal);
+ VirtualRobot::RobotPtr localRobot = testEnv._robotModel->clone("MyLocalRobot");
+
+ // now let's set up a task for the right arm
+ DifferentialIKDescriptor rightArmGoal;
+ rightArmGoal.priority = 1;
+ rightArmGoal.tcpName = "TCP R";
+ rightArmGoal.csel = CartesianSelection::ePosition;
+ rightArmGoal.ijm = InverseJacobiMethod::eSVD;
+ Eigen::Matrix4f poseMatrix2;
+ poseMatrix2 << 1.0f, 0.0f, 0.0f, 400.0f,
+ 0.0f, 1.0f, 0.0f, 100.0f,
+ 0.0f, 0.0f, 1.0f, 900.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+ PosePtr pose2 = new Pose(poseMatrix2);
+ rightArmGoal.tcpGoal = pose2;
+ ikTasks.push_back(rightArmGoal);
+
+ // finally let's add a center of mass task
+ std::string completeRobotSetname("Robot");
+ CoMIKDescriptor comdesc;
+ comdesc.priority = 0;
+ comdesc.gx = 4.0;
+ comdesc.gy = 45.0;
+ comdesc.robotNodeSetBodies = completeRobotSetname;
+ comdesc.tolerance = 2.0f;
+ comdesc.coordSysName = "Armar3_Base";
+
+ float stepSize = 0.1;
+ NameValueMap ikGradient;
+ BOOST_TEST_MESSAGE(poseMatrix1);
+ while (!compareResults(poseMatrix1, localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(), CartesianSelection::ePosition)
+ || !compareResults(poseMatrix2, localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(), CartesianSelection::ePosition))
+ {
+ ikGradient = testEnv._robotIK->computeHierarchicalDeltaIK(jointsSetName, ikTasks, comdesc, stepSize, true, true);
+ BOOST_CHECK(ikGradient.size() > 0);
+
+ VirtualRobot::RobotNodeSetPtr rns = localRobot->getRobotNodeSet(jointsSetName);
+ const std::vector<VirtualRobot::RobotNodePtr> robotNodes = rns->getAllRobotNodes();
+
+ NameValueMap newConfig;
+ BOOST_FOREACH(auto node, robotNodes)
+ {
+ float val = node->getJointValue();
+ float delta = ikGradient.find(node->getName())->second;
+ newConfig.insert(NameValueMap::value_type(node->getName(), val + delta));
+ }
+ localRobot->setJointValues(newConfig);
+ testEnv._robotStateComponent->reportJointAngles(newConfig, true);
+ }
+
+ BOOST_CHECK(compareResults(poseMatrix1, localRobot->getRobotNodeSet("TorsoLeftArm")->getTCP()->getGlobalPose(), CartesianSelection::ePosition));
+ BOOST_CHECK(compareResults(poseMatrix2, localRobot->getRobotNodeSet("TorsoRightArm")->getTCP()->getGlobalPose(), CartesianSelection::ePosition));
+
+ Eigen::Vector3f resultCoM = localRobot->getRobotNodeSet(completeRobotSetname)->getCoM();
+ BOOST_TEST_MESSAGE("desired com " << comdesc.gx << " " << comdesc.gy);
+ BOOST_TEST_MESSAGE("is com " << resultCoM[0] << " " << resultCoM[1]);
-// // Check validity of output
-// VirtualRobot::RobotNodePtr robotNode = testEnv._robotModel->getRobotNode("TCP L");
-// testEnv._robotModel->setJointValues(iksolution);
-// Eigen::Matrix4f tcpPose = robotNode->getGlobalPose();
-// BOOST_CHECK(comparePosition(tcpPose, poseGlobal) && compareOrientation(tcpPose, poseGlobal));
-// }
+ BOOST_CHECK((comdesc.gx - resultCoM[0])*(comdesc.gx - resultCoM[0]) <= comdesc.tolerance * comdesc.tolerance);
+ BOOST_CHECK((comdesc.gy - resultCoM[1])*(comdesc.gy - resultCoM[1]) <= comdesc.tolerance * comdesc.tolerance);
+ finished = true;
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKGlobalPoseTest)
+{
+ // Create test environment first
+ RobotIKTestEnvironment testEnv("RobotIKGlobalPoseTest");
+ bool finished = false;
+ doGlobalPoseTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKFramedPoseTest)
+{
+ // Create test environment first
+ RobotIKTestEnvironment testEnv("RobotIKFramedPoseTest");
+ bool finished = false;
+ doFramedPoseTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKReachabilityTest)
+{
+ RobotIKTestEnvironment testEnv("RobotIKReachabilityTest");
+ bool finished = false;
+ doReachabilityTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKDefineRobotSetTest)
+{
+ RobotIKTestEnvironment testEnv("RobotIKDefineNodeTest");
+ bool finished = false;
+ doDefineNodeSetTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKComputeCoMIKTest)
+{
+ RobotIKTestEnvironment testEnv("RobotIKComputeCoMIKTest");
+ bool finished = false;
+ doRobotIKComputeCoMIKTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKComputeHierarchicalDeltaIK)
+{
+ RobotIKTestEnvironment testEnv("RobotIKComputeHierarchicalDeltaIK");
+ bool finished = false;
+ doRobotIKComputeHierarchicalDeltaIKTests(testEnv, finished);
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKLoadReachabilitySpaceTest)
+{
+ RobotIKTestEnvironment testEnv("RobotIKLoadReachabilitySpaceTest", true);
+ std::string nodeSetName("TorsoLeftArm");
+ BOOST_CHECK(testEnv._robotIK->hasReachabilitySpace(nodeSetName));
+}
+
+BOOST_AUTO_TEST_CASE(RobotIKLoadTest)
+{
+ RobotIKTestEnvironment testEnv("RobotIKLoadTest");
+ // set up time stuff
+ std::chrono::steady_clock::time_point startPoint = std::chrono::steady_clock::now();
+ std::chrono::duration<double> time_span = std::chrono::duration_cast< std::chrono::duration<double> >(std::chrono::steady_clock::now() - startPoint);
+ // set up random generators
+ std::default_random_engine generator; //1234567890
+ // std::uniform_int_distribution<int> uniDistribution(0, 5);
+ std::uniform_int_distribution<int> uniDistribution(0, 5);
+ // some bookkeeping data structures
+ std::vector<std::thread> threads;
+ bool hikStarted = false;
+ bool hikFinished = false;
+ bool dummyFinished = false;
+
+ while (time_span.count() < 120.0)
+ {
+ int testNumber = uniDistribution(generator);
+ switch (testNumber)
+ {
+ case 0:
+ threads.push_back(std::thread(doGlobalPoseTests, std::ref(testEnv), std::ref(dummyFinished)));
+ break;
+ case 1:
+ threads.push_back(std::thread(doFramedPoseTests, std::ref(testEnv), std::ref(dummyFinished)));
+ break;
+ case 2:
+ threads.push_back(std::thread(doLoadReachabilityTest, std::ref(testEnv), std::ref(dummyFinished)));
+ break;
+ case 3:
+ threads.push_back(std::thread(doDefineNodeSetTests, std::ref(testEnv), std::ref(dummyFinished)));
+ break;
+ case 4:
+ threads.push_back(std::thread(doRobotIKComputeCoMIKTests, std::ref(testEnv), std::ref(dummyFinished)));
+ break;
+ case 5:
+ if (!hikStarted)
+ {
+ hikStarted = true;
+ // threads.push_back(std::thread(doRobotIKComputeHierarchicalDeltaIKTests, std::ref(testEnv), std::ref(hikFinished)));
+ }
+ break;
+ }
+ if (hikFinished)
+ {
+ hikStarted = false;
+ hikFinished = false;
+ }
+ std::this_thread::sleep_for(std::chrono::seconds(uniDistribution(generator)));
+ time_span = std::chrono::duration_cast< std::chrono::duration<double> >(std::chrono::steady_clock::now() - startPoint);
+ }
+
+ for (size_t i = 0; i < threads.size(); ++i)
+ {
+ threads[i].join();
+ }
+}
\ No newline at end of file
diff --git a/source/RobotAPI/components/RobotIK/test/RobotIKTestEnvironment.h b/source/RobotAPI/components/RobotIK/test/RobotIKTestEnvironment.h
index 3f137aca3..114b097d4 100644
--- a/source/RobotAPI/components/RobotIK/test/RobotIKTestEnvironment.h
+++ b/source/RobotAPI/components/RobotIK/test/RobotIKTestEnvironment.h
@@ -37,7 +37,7 @@ using namespace armarx;
class RobotIKTestEnvironment
{
public:
- RobotIKTestEnvironment(const std::string& testName, int registryPort = 11220, bool addObjects = true)
+ RobotIKTestEnvironment(const std::string& testName, bool loadSpaces = false, int registryPort = 11220, bool addObjects = true)
{
Ice::PropertiesPtr properties = Ice::createProperties();
armarx::Application::LoadDefaultConfig(properties);
@@ -48,6 +48,13 @@ public:
properties->setProperty("ArmarX.RobotStateComponent.RobotNodeSetName", "Robot");
properties->setProperty("ArmarX.RobotIK.RobotFileName", robotFilename);
properties->setProperty("ArmarX.RobotIK.RobotStateComponentName", "RobotStateComponent");
+
+ _spaceSavePath = test::getCmakeValue("PROJECT_BINARY_DIR") + "/Testing/Temporary/spaces/";
+
+ if (loadSpaces)
+ {
+ properties->setProperty("ArmarX.RobotIK.ReachabilitySpacesFolder", _spaceSavePath);
+ }
_iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
_iceTestHelper->startEnvironment();
@@ -73,6 +80,7 @@ public:
TestArmarXManagerPtr _manager;
IceTestHelperPtr _iceTestHelper;
VirtualRobot::RobotPtr _robotModel;
+ std::string _spaceSavePath;
};
typedef boost::shared_ptr<RobotIKTestEnvironment> RobotIKTestEnvironmentPtr;
diff --git a/source/RobotAPI/interface/core/RobotIK.ice b/source/RobotAPI/interface/core/RobotIK.ice
index 4a0436488..7a155177d 100644
--- a/source/RobotAPI/interface/core/RobotIK.ice
+++ b/source/RobotAPI/interface/core/RobotIK.ice
@@ -41,12 +41,13 @@ module armarx
struct CoMIKDescriptor
{
// REQUIRED:
- int priority;
- Vector2Base goal;
- string robotNodeSetBodies;
- float tolerance;
+ int priority; // priority for hierarchical ik
+ float gx; // desired center of mass x
+ float gy; // desired center of mass y
+ string robotNodeSetBodies; // body set for which the center of mass is computed
+ float tolerance; // error tolerance
// OPTIONAL:
- string coordSysName;
+ string coordSysName; // coordinate system name in which the center of mass is defined; empty string for global system
};
struct DifferentialIKDescriptor
@@ -107,7 +108,8 @@ module armarx
* @details
*
* @param robotNodeSetJoints Name of the robot node set that contains the joints you wish to compute the IK for.
- * @param comIK A center of mass description.
+ * @param comIK A center of mass description. Note that the priority field is relevant for this function as there
+ * is only a single CoM of descriptor.
*
* @return The ik-solution. Returns an empty vector if there is no solution.
*/
@@ -122,7 +124,8 @@ module armarx
* For details for the different type of tasks, see the parameter description. You must define a priority for each task.
* The task with maximal priority is the first task to be solved. Each subsequent task is then solved
* within the null space of the previous task. <b> Note that this function returns a gradient and NOT
- * an absolute IK solution <\b>. The gradient is computed from the current robot configuration.
+ * an absolute IK solution. The gradient is computed on the current configuration of the robot. <\b>.
+ * The gradient is computed from the current robot configuration.
*
* See @url http://simox.sourceforge.net/documentation/class_virtual_robot_1_1_hierarchical_i_k.html
* and @url http://sourceforge.net/p/simox/wiki/VirtualRobot/#hierarchical-ik-solving for more information.
@@ -163,6 +166,7 @@ module armarx
*
* @param chainName The name of a defined robot node set. This can be either a robot node set that is defined in the
* robot model or a robot node set that has been manually defined calling \ref defineRobotNodeSet.
+ * @param coordinateSystem The name of the robot node in whose coordinate system the reachability space shall be defined in. If you wish to choose the global coordinate system, pass an empty string. Note, however, that any reachability space defined in the global coordinate system gets invalidated once the robot moves.
* @param stepTranslation The extend of a voxel dimension in translational dimensions (x,y,z) [mm]
* @param stepRotation The extend of a voxel dimension in rotational dimensions (roll,pitch,yaw) [rad]
* @param minBounds The minimum workspace poses (x,y,z,ro,pi,ya) given in the base node's coordinate system [mm and rad]
@@ -171,7 +175,7 @@ module armarx
* @return True iff the a reachability space for the given robot node set is available after execution of this function.
* False in case of a failure, e.g. there is no chain with the given name.
*/
- bool createReachabilitySpace(string chainName, float stepTranslation, float stepRotation,
+ bool createReachabilitySpace(string chainName, string coordinateSystem, float stepTranslation, float stepRotation,
WorkspaceBounds minBounds, WorkspaceBounds maxBounds, int numSamples);
/**
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 6de7bcad9..8cbafbcf3 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -48,6 +48,7 @@ namespace armarx
class Vector2 : virtual public Vector2Base
{
+ public:
Vector2();
Vector2(const Eigen::Vector2f &);
Vector2(::Ice::Float x, ::Ice::Float y);
--
GitLab