Commit 99479824 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Update unit-tests to API changes.

parent de650c3d
......@@ -34,19 +34,21 @@ namespace hpp {
namespace core {
namespace {
inline std::size_t collide (const CollisionPairs_t::const_iterator& _colPair,
const fcl::CollisionRequest& req, fcl::CollisionResult& res) {
const fcl::CollisionRequest& req, fcl::CollisionResult& res,
pinocchio::DeviceData& data) {
res.clear();
return fcl::collide (
_colPair->first ->fcl (),
_colPair->second->fcl (),
_colPair->first ->fcl (data),
_colPair->second->fcl (data),
req, res);
}
inline bool collide (const CollisionPairs_t& pairs,
const fcl::CollisionRequest& req, fcl::CollisionResult& res,
CollisionPairs_t::const_iterator& _col) {
CollisionPairs_t::const_iterator& _col,
pinocchio::DeviceData& data) {
for (_col = pairs.begin (); _col != pairs.end (); ++_col)
if (collide (_col, req, res) != 0)
if (collide (_col, req, res, data) != 0)
return true;
return false;
}
......@@ -69,16 +71,17 @@ namespace hpp {
bool CollisionValidation::validate (const Configuration_t& config,
ValidationReportPtr_t& validationReport)
{
robot_->currentConfiguration (config);
robot_->computeForwardKinematics ();
robot_->updateGeometryPlacements ();
pinocchio::DeviceSync device (robot_);
device.currentConfiguration (config);
device.computeForwardKinematics ();
device.updateGeometryPlacements ();
fcl::CollisionResult collisionResult;
CollisionPairs_t::const_iterator _col;
if (collide (collisionPairs_, collisionRequest_, collisionResult, _col)
if (collide (collisionPairs_, collisionRequest_, collisionResult, _col, device.d())
||
( checkParameterized_ &&
collide (parameterizedPairs_, collisionRequest_, collisionResult, _col)
collide (parameterizedPairs_, collisionRequest_, collisionResult, _col, device.d())
)) {
CollisionValidationReportPtr_t report (new CollisionValidationReport);
report->object1 = _col->first;
......@@ -168,7 +171,8 @@ namespace hpp {
hppDout(info, "Disabling collision between "
<< _colPair->first ->name() << " and "
<< _colPair->second->name());
if (collide (_colPair, collisionRequest_, unused) != 0) {
if (fcl::collide (_colPair->first ->fcl (), _colPair->second->fcl (),
collisionRequest_, unused) != 0) {
hppDout(warning, "Disabling collision detection between two "
"body in collision.");
}
......
......@@ -54,7 +54,7 @@ DevicePtr_t createRobot ()
//DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidRomeo, "romeo");
DevicePtr_t robot (Device::create ("2-objects"));
urdf::loadModel (robot, 0, "obj1/", "freeflyer", "file://" DATA_DIR "/empty.urdf", "");
robot->controlComputation((Device::Computation_t) (Device::JOINT_POSITION | Device::JACOBIAN));
robot->controlComputation((Computation_t) (JOINT_POSITION | JACOBIAN));
robot->rootJoint()->lowerBound (0, -10);
robot->rootJoint()->lowerBound (1, -10);
robot->rootJoint()->lowerBound (2, -10);
......
......@@ -77,8 +77,8 @@ DevicePtr_t createRobot ()
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
......
......@@ -57,8 +57,8 @@ DevicePtr_t createRobot ()
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
......@@ -102,8 +102,8 @@ DevicePtr_t createRobot2 ()
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
......
......@@ -79,8 +79,8 @@ DevicePtr_t createRobot ()
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
......
......@@ -93,8 +93,8 @@ BOOST_AUTO_TEST_CASE (Roadmap1) {
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
......@@ -347,8 +347,8 @@ BOOST_AUTO_TEST_CASE (nearestNeighbor) {
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
JointModelPX::TangentVector_t max_effort = JointModelPX::TangentVector_t::Constant(JointModelPX::NV,std::numeric_limits<double>::max());
......
......@@ -40,7 +40,7 @@ using namespace hpp::pinocchio;
DevicePtr_t createRobot ()
{
DevicePtr_t robot = unittest::makeDevice(unittest::HumanoidRomeo);
robot->controlComputation((Device::Computation_t) (Device::JOINT_POSITION | Device::JACOBIAN));
robot->controlComputation((Computation_t) (JOINT_POSITION | JACOBIAN));
robot->rootJoint()->lowerBound (0, -1);
robot->rootJoint()->lowerBound (1, -1);
robot->rootJoint()->lowerBound (2, -1);
......
......@@ -79,8 +79,8 @@ DevicePtr_t createRobot ()
const std::string& name = robot->name ();
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
Transform3f mat; mat.setIdentity ();
std::string jointName = name + "_x";
......
......@@ -81,8 +81,8 @@ BOOST_AUTO_TEST_CASE (kdTree) {
DevicePtr_t robot = Device::create("robot");
ModelPtr_t m = ModelPtr_t(new ::se3::Model());
GeomModelPtr_t gm = GeomModelPtr_t(new ::se3::GeometryModel());
robot->model(m);
robot->geomModel(gm);
robot->setModel(m);
robot->setGeomModel(gm);
const std::string& name = robot->name ();
Transform3f mat; mat.setIdentity ();
......
Supports Markdown
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