Commit 4310e989 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add problem resolution for simple cases in unit-test.

parent b88fe107
......@@ -17,25 +17,25 @@
#define BOOST_TEST_MODULE problem
#include <boost/test/included/unit_test.hpp>
#include <hpp/core/problem.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/simple-device.hh>
#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/core/roadmap.hh>
using namespace hpp::core;
using namespace hpp::pinocchio;
DevicePtr_t createRobot ()
{
DevicePtr_t robot = unittest::makeDevice(unittest::ManipulatorArm2);
return robot;
}
BOOST_AUTO_TEST_CASE (memory_deallocation)
{
ProblemSolverPtr_t ps = ProblemSolver::create();
ps->robot(createRobot());
ps->robot(unittest::makeDevice(unittest::ManipulatorArm2));
ProblemPtr_t problem = ps->problem();
DeviceWkPtr_t dev = ps->robot();
SteeringMethodWkPtr_t sm = problem->steeringMethod();
......@@ -49,3 +49,106 @@ BOOST_AUTO_TEST_CASE (memory_deallocation)
BOOST_CHECK_MESSAGE( !distance.lock(), "Distance was not deleted");
BOOST_CHECK_MESSAGE( !roadmap .lock(), "Roadmap was not deleted");
}
void pointMassProblem (const char* steeringMethod,
const char* distance,
const char* pathValidation, value_type tolerance)
{
const char* urdfString =
"<robot name='foo'><link name='base_link'>"
"<collision><geometry><sphere radius='0.01'/></geometry></collision>"
"</link></robot>";
ProblemSolverPtr_t ps = ProblemSolver::create();
ps->maxIterPathPlanning (50);
DevicePtr_t robot = Device::create ("point");
urdf::loadModelFromString (robot, 0, "", "translation3d", urdfString, "");
BOOST_REQUIRE_EQUAL (robot->configSize(), 3);
robot->rootJoint()->lowerBound (0, -10);
robot->rootJoint()->lowerBound (1, -10);
robot->rootJoint()->lowerBound (2, -10);
robot->rootJoint()->upperBound (0, 10);
robot->rootJoint()->upperBound (1, 10);
robot->rootJoint()->upperBound (2, 10);
ps->robot(robot);
ps->pathValidationType (pathValidation, tolerance);
ps->steeringMethodType (steeringMethod);
ps->distanceType (distance);
FclCollisionObject box (
hpp::fcl::CollisionGeometryPtr_t (new hpp::fcl::Box (0.3, 0.3, 0.3)),
matrix3_t::Identity(), vector3_t (-2, 0, 0));
ps->addObstacle ("box", box, true, true);
ConfigurationPtr_t qinit (new Configuration_t (robot->neutralConfiguration()));
ConfigurationPtr_t qgoal (new Configuration_t (robot->neutralConfiguration()));
*qgoal << -4, 0, 0;
ps->initConfig (qinit);
ps->addGoalConfig (qgoal);
ps->solve ();
BOOST_CHECK (ps->roadmap()->nodes().size() > 2);
BOOST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
<< " nodes.");
}
void carLikeProblem (const char* steeringMethod,
const char* distance,
const char* pathValidation, value_type tolerance)
{
ProblemSolverPtr_t ps = ProblemSolver::create();
ps->maxIterPathPlanning (50);
DevicePtr_t robot = unittest::makeDevice(unittest::CarLike);
BOOST_REQUIRE_EQUAL (robot->configSize(), 6);
robot->rootJoint()->lowerBound (0, -10);
robot->rootJoint()->lowerBound (1, -10);
robot->rootJoint()->upperBound (0, 10);
robot->rootJoint()->upperBound (1, 10);
ps->robot(robot);
ps->pathValidationType (pathValidation, tolerance);
ps->steeringMethodType (steeringMethod);
ps->distanceType (distance);
FclCollisionObject box (
hpp::fcl::CollisionGeometryPtr_t (new hpp::fcl::Box (0.3, 0.3, 0.3)),
matrix3_t::Identity(), vector3_t (-2, 0, 0));
ps->addObstacle ("box", box, true, true);
ConfigurationPtr_t qinit (new Configuration_t (robot->neutralConfiguration()));
ConfigurationPtr_t qgoal (new Configuration_t (robot->neutralConfiguration()));
*qgoal << -4, 0, 0, 1, 0, 0;
ps->initConfig (qinit);
ps->addGoalConfig (qgoal);
ps->solve ();
BOOST_CHECK (ps->roadmap()->nodes().size() > 2);
BOOST_MESSAGE ("Solved the problem with " << ps->roadmap()->nodes().size()
<< " nodes.");
}
BOOST_AUTO_TEST_CASE (pointMass)
{
pointMassProblem ("Straight", "Weighed", "Discretized", 0.05);
pointMassProblem ("Straight", "Weighed", "Progressive", 0.05);
pointMassProblem ("Straight", "Weighed", "Dichotomy" , 0 );
}
BOOST_AUTO_TEST_CASE (carlike)
{
carLikeProblem ("Straight", "Weighed", "Discretized", 0.05);
carLikeProblem ("Straight", "Weighed", "Progressive", 0.05);
carLikeProblem ("Straight", "Weighed", "Dichotomy" , 0 );
carLikeProblem ("ReedsShepp", "ReedsShepp", "Discretized", 0.05);
// Not implemented
// carLikeProblem ("ReedsShepp", "ReedsShepp", "Progressive", 0.05);
// Not implemented
// carLikeProblem ("ReedsShepp", "ReedsShepp", "Dichotomy" , 0 );
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment