diff --git a/CMakeLists.txt b/CMakeLists.txt
index 29306c552d3ae134f3d0a3862de83409f0a495c7..80195df552a26b3e359a87678a1c03f23ba9e611 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -62,7 +62,7 @@ IF (HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
ADD_REQUIRED_DEPENDENCY("hpp-wholebody-step >= 4")
ENDIF ()
IF (TEST_UR5)
- ADD_REQUIRED_DEPENDENCY(hpp-model-urdf >= 3.0.0)
+ ADD_REQUIRED_DEPENDENCY("hpp_universal_robot")
ENDIF ()
CONFIG_FILES (doc/main.hh
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index f13c6d6fd918d881d39925e22ecfb70156507242..5a419baf6749912bedada57e6df641575561ba02 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -3,17 +3,17 @@
# Author: Mathieu Geisert
#
# This file is part of hpp-core
-# hpp-model-urdf is free software: you can redistribute it and/or modify
+# hpp-manipulation 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 3 of the License, or
# (at your option) any later version.
#
-# hpp-core is distributed in the hope that it will be useful,
+# hpp-manipulation 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 General Lesser Public License for more details.
# You should have received a copy of the GNU Lesser General Public License
-# along with hpp-core If not, see <http://www.gnu.org/licenses/>.
+# along with hpp-manipulation If not, see <http://www.gnu.org/licenses/>.
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
@@ -36,13 +36,11 @@ MACRO(ADD_TESTCASE NAME GENERATED)
ENDIF()
ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
- PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-core)
- PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-constraints)
-
+ PKG_CONFIG_USE_DEPENDENCY (${NAME} hpp-constraints)
# Link against Boost and project library.
TARGET_LINK_LIBRARIES(${NAME}
- ${Boost_LIBRARIES}
${PROJECT_NAME}
+ ${Boost_LIBRARIES}
)
ENDMACRO(ADD_TESTCASE)
@@ -50,6 +48,5 @@ ENDMACRO(ADD_TESTCASE)
IF (TEST_UR5)
ADD_TESTCASE (test-constraintgraph FALSE)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DTEST_UR5")
- PKG_CONFIG_USE_DEPENDENCY(test-constraintgraph hpp-model-urdf)
ENDIF ()
# ADD_TESTCASE (path-projection FALSE) # moved in hpp-core
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index 53084deb97d369ab5d10b44db8bc8e9adb139f80..eb51e13e8c8bf1766c58b1a59f7b895e8338ed38 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -15,13 +15,16 @@
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/util/pointer.hh>
-#include <hpp/model/urdf/util.hh>
+#include <hpp/pinocchio/urdf/util.hh>
+#include <hpp/pinocchio/liegroup-element.hh>
-#include <hpp/core/steering-method-straight.hh>
+#include <hpp/core/steering-method/straight.hh>
+#include <hpp/core/path-validation-report.hh>
-#include <hpp/constraints/position.hh>
+#include <hpp/constraints/generic-transformation.hh>
#include <hpp/constraints/relative-com.hh>
+#include <hpp/manipulation/constraint-set.hh>
#include "hpp/manipulation/graph/state.hh"
#include "hpp/manipulation/graph/state-selector.hh"
#include "hpp/manipulation/graph/graph.hh"
@@ -29,61 +32,78 @@
#include "hpp/manipulation/device.hh"
#include "hpp/manipulation/problem.hh"
#include "hpp/manipulation/graph-path-validation.hh"
+#include <hpp/manipulation/steering-method/graph.hh>
#include <boost/test/unit_test.hpp>
-using namespace ::hpp::manipulation;
using hpp::core::SteeringMethodStraight;
using hpp::core::SteeringMethodPtr_t;
-typedef std::vector <graph::GraphComponentPtr_t> GraphComponents;
+typedef std::vector <hpp::manipulation::graph::GraphComponentPtr_t>
+GraphComponents_t;
namespace hpp_test {
- DevicePtr_t robot;
-
+ using hpp::core::Configuration_t;
+ using hpp::manipulation::graph::GraphPtr_t;
+ using hpp::manipulation::graph::StateSelectorPtr_t;
+ using hpp::manipulation::graph::StateSelectorPtr_t;
+ using hpp::manipulation::graph::StatePtr_t;
+ using hpp::manipulation::graph::EdgePtr_t;
+ using hpp::manipulation::graph::Graph;
+ using hpp::manipulation::graph::GraphComponent;
+ using hpp::manipulation::graph::EdgePtr_t;
+ using hpp::manipulation::graph::Edges_t;
+
+ hpp::manipulation::DevicePtr_t robot;
Configuration_t q1, q2;
- GraphComponents components;
- graph::GraphPtr_t graph_;
- graph::NodeSelectorPtr_t ns;
- graph::NodePtr_t n1;
- graph::NodePtr_t n2;
- graph::EdgePtr_t e11;
- graph::EdgePtr_t e21;
- graph::EdgePtr_t e12;
- graph::EdgePtr_t e22;
+ GraphComponents_t components;
+ GraphPtr_t graph_;
+ StateSelectorPtr_t ns;
+ StatePtr_t n1;
+ StatePtr_t n2;
+ EdgePtr_t e11;
+ EdgePtr_t e21;
+ EdgePtr_t e12;
+ EdgePtr_t e22;
void initialize (bool ur5)
{
+ robot = hpp::manipulation::Device::create ("test-robot");
+ hpp::manipulation::ProblemPtr_t problem
+ (new hpp::manipulation::Problem (robot));
if (ur5) {
#ifdef TEST_UR5
- robot = Device::create ("test-robot");
- hpp::model::urdf::loadUrdfModel (robot, "anchor", "ur_description", "ur5_robot");
+ hpp::pinocchio::urdf::loadUrdfModel (robot, "anchor", "ur_description",
+ "ur5_joint_limited_robot");
#else // TEST_UR5
BOOST_ERROR ("Set TEST_UR5 in cmake to activate this.");
#endif // TEST_UR5
- } else {
- robot = Device::create ("test-robot");
}
- SteeringMethodPtr_t sm (SteeringMethodStraight::create (robot));
- graph_ = graph::Graph::create ("manpulation-graph", robot, sm);
+ SteeringMethodPtr_t sm
+ (hpp::manipulation::steeringMethod::Graph::create (*problem));
+ hpp::core::ProblemPtr_t pb (problem);
+ pb->steeringMethod (sm);
+
+ graph_ = Graph::create ("manipulation-graph", robot, problem);
components.push_back(graph_);
graph_->maxIterations (20);
graph_->errorThreshold (1e-4);
- ns = graph_->createNodeSelector("node-selector"); components.push_back(ns);
- n1 = ns->createNode ("node 1"); components.push_back(n1);
- n2 = ns->createNode ("node 2"); components.push_back(n2);
+ ns = graph_->createStateSelector("node-selector"); components.push_back(ns);
+ n1 = ns->createState ("node 1"); components.push_back(n1);
+ n2 = ns->createState ("node 2"); components.push_back(n2);
e11 = n1->linkTo ("edge 11", n1); components.push_back(e11);
e21 = n2->linkTo ("edge 21", n1); components.push_back(e21);
e12 = n1->linkTo ("edge 12", n2); components.push_back(e12);
e22 = n2->linkTo ("edge 22", n2); components.push_back(e22);
+ graph_->initialize ();
q1 = Configuration_t::Zero(6);
q2 = Configuration_t::Zero(6);
q1 << 1,1,1,0,2.5,-1.9;
q2 << 2,0,1,0,2.5,-1.9;
}
-}
+} // namespace hpp_test
BOOST_AUTO_TEST_CASE (GraphStructure)
{
@@ -93,16 +113,16 @@ BOOST_AUTO_TEST_CASE (GraphStructure)
// Check that GraphComponent keeps track of all object properly.
size_t index = 0;
- for (GraphComponents::iterator it = components.begin();
+ for (GraphComponents_t::iterator it = components.begin();
it != components.end(); ++it) {
- BOOST_CHECK_MESSAGE (*it == graph::GraphComponent::get (index).lock(),
- "GraphComponent class do not track properly GraphComponents inherited objects");
+ BOOST_CHECK_MESSAGE (*it == graph_->get (index).lock(),
+ "GraphComponent class do not track properly GraphComponents_t inherited objects");
index++;
}
// Test function Graph::getEdge
- graph::NodePtr_t from(n1), to(n2);
- graph::Edges_t checkPossibleEdges,
+ StatePtr_t from(n1), to(n2);
+ Edges_t checkPossibleEdges,
possibleEdges = graph_->getEdges (from, to);
checkPossibleEdges.push_back (e12);
for (size_t j = 0; j < possibleEdges.size(); j++)
@@ -110,67 +130,7 @@ BOOST_AUTO_TEST_CASE (GraphStructure)
"Possible edge j = " << j);
Configuration_t cfg;
- graph::NodePtr_t node = graph_->getNode (cfg);
+ StatePtr_t node = graph_->getState (cfg);
BOOST_CHECK (node == n1);
}
-#ifdef TEST_UR5
-BOOST_AUTO_TEST_CASE (ConstraintSets)
-{
- using namespace hpp_test;
- using namespace hpp::constraints;
-
- vector_t res, expectedRes;
-
- initialize (true);
- JointPtr_t ee = robot->getJointByBodyName ("ee_link");
-
- robot->currentConfiguration (q2);
- robot->computeForwardKinematics ();
- RelativeComPtr_t com = RelativeCom::create (robot, robot->rootJoint(), robot->positionCenterOfMass());
- res.resize (com->outputSize()); expectedRes = vector_t::Zero(res.size());
- (*com) (res,q2);
- BOOST_CHECK (res == expectedRes);
-
- robot->currentConfiguration (q1);
- robot->computeForwardKinematics ();
- fcl::Vec3f position = ee->currentTransformation ().getTranslation ();
- PositionPtr_t pos = Position::create (robot, ee, vector3_t(0,0,0),
- vector3_t(position[0],position[1],position[2]));
- res.resize (pos->outputSize()); expectedRes = vector_t::Zero(res.size());
- (*pos) (res,q1);
- BOOST_CHECK (res == expectedRes);
-
- //graph_->addNumericalConstraint (com);
- n1->addNumericalConstraint (pos, Equality::create ());
-}
-
-BOOST_AUTO_TEST_CASE (PathValidationTest)
-{
- using namespace hpp_test;
-
- ProblemPtr_t pb = new Problem (robot);
- BOOST_CHECK(robot->configSize() == 6);
- pb->constraintGraph (graph_);
- ConstraintSetPtr_t constraintn1 = graph_->configConstraint (n1);
- ConstraintSetPtr_t constraintn2 = graph_->configConstraint (n2);
- BOOST_CHECK ( constraintn1->isSatisfied (q1));
- BOOST_CHECK (!constraintn1->isSatisfied (q2));
- BOOST_CHECK ( constraintn2->isSatisfied (q2));
- PathPtr_t p = (*pb->steeringMethod ())(ConfigurationIn_t(q1),ConfigurationIn_t(q2)),
- validp;
- graph::NodePtr_t nq1 = graph_->getNode (q1);
- graph::NodePtr_t nq2 = graph_->getNode (q2);
- BOOST_CHECK (nq1 == n1);
- BOOST_CHECK (nq2 == n2);
- GraphPathValidationPtr_t pv = pb->pathValidation ();
- BOOST_CHECK (pv);
- if (!pv->validate(p, false, validp)) {
- BOOST_CHECK_MESSAGE (false,
- "Valid part has length " << validp->length() << " and p " << p->length());
- pv->innerValidation ()->validate (p, false, validp);
- BOOST_CHECK_MESSAGE (false,
- "Inner validation returned: Valid part has length " << validp->length() << " and p " << p->length());
- }
-}
-#endif // TEST_UR5