Commit b34b9a4b authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add builder for 'RbprmDynamicPathValidation' in python binding

parent 04a4a661
......@@ -2248,6 +2248,8 @@ assert(s2 == s1 +1);
boost::bind(&BindShooter::create, boost::ref(bindShooter_), _1));
problemSolver->add<core::PathValidationBuilder_t>("RbprmPathValidation",
boost::bind(&BindShooter::createPathValidation, boost::ref(bindShooter_), _1, _2));
problemSolver->add<core::PathValidationBuilder_t>("RbprmDynamicPathValidation",
boost::bind(&BindShooter::createDynamicPathValidation, boost::ref(bindShooter_), _1, _2));
problemSolver->add<core::PathPlannerBuilder_t>("DynamicPlanner",DynamicPlanner::createWithRoadmap);
problemSolver->add <core::SteeringMethodBuilder_t> ("RBPRMKinodynamic", SteeringMethodKinodynamic::create);
......
......@@ -33,6 +33,7 @@
# include <hpp/rbprm/rbprm-path-validation.hh>
# include <hpp/fcl/BVH/BVH_model.h>
# include <hpp/core/config-validations.hh>
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh>
namespace hpp {
namespace rbprm {
namespace impl {
......@@ -77,6 +78,41 @@ namespace hpp {
return collisionChecking;
}
hpp::core::PathValidationPtr_t createDynamicPathValidation (const hpp::model::DevicePtr_t& robot, const hpp::model::value_type& val)
{
hpp::model::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::model::RbPrmDevice>(robot);
affMap_ = problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap_.empty ()) {
throw hpp::Error ("No affordances found. Unable to create Path Validaton object.");
}
hpp::rbprm::RbPrmValidationPtr_t validation
(hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, affMap_));
hpp::rbprm::DynamicPathValidationPtr_t collisionChecking = hpp::rbprm::DynamicPathValidation::create(robot,val);
collisionChecking->add (validation);
problemSolver_->problem()->configValidation(core::ConfigValidations::create ());
problemSolver_->problem()->configValidations()->add(validation);
// build the dynamicValidation :
double sizeFootX,sizeFootY,mass,mu;
bool rectangularContact;
try {
sizeFootX = problemSolver_->problem()->get<double> (std::string("sizeFootX"))/2.;
sizeFootY = problemSolver_->problem()->get<double> (std::string("sizeFootY"))/2.;
rectangularContact = 1;
} catch (const std::exception& e) {
hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
sizeFootX =0;
sizeFootY =0;
rectangularContact = 0;
}
mass = robot->mass();
mu = 0.5; // FIXME : store it in problem ?
DynamicValidationPtr_t dynamicVal = DynamicValidation::create(rectangularContact,sizeFootX,sizeFootY,mass,mu);
collisionChecking->addDynamicValidator(dynamicVal);
return collisionChecking;
}
hpp::core::ProblemSolverPtr_t problemSolver_;
std::vector<std::string> romFilter_;
std::map<std::string, std::vector<std::string> > affFilter_;
......
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