Commit 16344234 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'master' into devel

parents 6aed5bec f928014c
Pipeline #14466 failed with stage
in 1 minute and 51 seconds
...@@ -78,3 +78,5 @@ ADD_SUBDIRECTORY(tests) ...@@ -78,3 +78,5 @@ ADD_SUBDIRECTORY(tests)
CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh) CONFIG_FILES (include/${CUSTOM_HEADER_DIR}/doc.hh)
PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME}) PKG_CONFIG_APPEND_LIBS(${PROJECT_NAME})
INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
Subproject commit 32015cb28d977b592227675665d17d11531ef418 Subproject commit 3a52692a40839b10f38352c1b06ccfebc0b53f36
<?xml version='1.0'?>
<package format='2'>
<name>hpp-rbprm-corba</name>
<version>4.11.0</version>
<description>RB-PRM planner for HPP</description>
<maintainer email='guilhem.saurel@laas.fr'>Guilhem Saurel</maintainer>
<license>BSD</license>
<author>Pierre Fernbach</author>
<author>Steve Tonneau</author>
<buildtool_depend>catkin</buildtool_depend>
</package>
...@@ -50,6 +50,8 @@ class FullBody(Robot): ...@@ -50,6 +50,8 @@ class FullBody(Robot):
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots # "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
def loadFullBodyModel(self, robotName, rootJointType): def loadFullBodyModel(self, robotName, rootJointType):
urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
if hasattr(self, 'urdfDir'):
urdfFilename = urdfFilename.replace('/urdf/', '/%s/' % self.urdfDir)
print("selected problem: ",self.client.problem.getSelected("problem")[0]) print("selected problem: ",self.client.problem.getSelected("problem")[0])
self.clientRbprm.rbprm.loadFullBodyRobot(robotName, rootJointType, self.clientRbprm.rbprm.loadFullBodyRobot(robotName, rootJointType,
urdfFilename, srdfFilename, urdfFilename, srdfFilename,
......
...@@ -529,7 +529,7 @@ void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw( ...@@ -529,7 +529,7 @@ void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw(
void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq& ref) throw(hpp::Error) { void RbprmBuilder::setReferenceEndEffector(const char* romName, const hpp::floatSeq& ref) throw(hpp::Error) {
std::string name(romName); std::string name(romName);
hpp::pinocchio::RbPrmDevicePtr_t device = hpp::pinocchio::RbPrmDevicePtr_t device =
boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot()); std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot());
if (!device) throw Error("No rbprmDevice in problemSolver"); if (!device) throw Error("No rbprmDevice in problemSolver");
if (device->robotRoms_.find(name) == device->robotRoms_.end()) throw Error("Device do not contain this rom "); if (device->robotRoms_.find(name) == device->robotRoms_.end()) throw Error("Device do not contain this rom ");
Configuration_t config(dofArrayToConfig(3, ref)); Configuration_t config(dofArrayToConfig(3, ref));
...@@ -832,9 +832,11 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi ...@@ -832,9 +832,11 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
rotConstraints.push_back(true); rotConstraints.push_back(true);
const pinocchio::Frame effectorFrame = fullBody()->device_->getFrameByName(limb->effector_.name()); const pinocchio::Frame effectorFrame = fullBody()->device_->getFrameByName(limb->effector_.name());
pinocchio::JointPtr_t effectorJoint = limb->effector_.joint(); pinocchio::JointPtr_t effectorJoint = limb->effector_.joint();
proj->add(core::NumericalConstraint::create(constraints::Position::create( const constraints::DifferentiableFunctionPtr_t& function = constraints::Position::create(
"", fullBody()->device_, effectorJoint, effectorFrame.pinocchio().placement * globalFrame, localFrame, "", fullBody()->device_, effectorJoint, effectorFrame.pinocchio().placement * globalFrame, localFrame,
posConstraints))); posConstraints);
constraints::ComparisonTypes_t comp (function->outputDerivativeSize(), constraints::EqualToZero);
proj->add(constraints::Implicit::create(function, comp));
if (limb->contactType_ == hpp::rbprm::_6_DOF) { if (limb->contactType_ == hpp::rbprm::_6_DOF) {
// rotation matrix around z // rotation matrix around z
value_type theta = 2 * (value_type(rand()) / value_type(RAND_MAX) - 0.5) * M_PI; value_type theta = 2 * (value_type(rand()) / value_type(RAND_MAX) - 0.5) * M_PI;
...@@ -843,9 +845,10 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi ...@@ -843,9 +845,10 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
fcl::Matrix3f rotation = fcl::Matrix3f rotation =
r * effectorFrame.pinocchio().placement.rotation().transpose(); // * limb->effector_->initialPosition r * effectorFrame.pinocchio().placement.rotation().transpose(); // * limb->effector_->initialPosition
// ().getRotation(); // ().getRotation();
proj->add(core::NumericalConstraint::create( const constraints::DifferentiableFunctionPtr_t& orientation_function = constraints::Orientation::create("", fullBody()->device_, effectorJoint,
constraints::Orientation::create("", fullBody()->device_, effectorJoint, pinocchio::Transform3f(rotation, fcl::Vec3f::Zero()), rotConstraints);
pinocchio::Transform3f(rotation, fcl::Vec3f::Zero()), rotConstraints))); constraints::ComparisonTypes_t orientation_comp(orientation_function->outputDerivativeSize(), constraints::EqualToZero);
proj->add(constraints::Implicit::create(orientation_function, orientation_comp));
} }
} }
shooter->shoot(config); shooter->shoot(config);
...@@ -1337,24 +1340,24 @@ Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& confi ...@@ -1337,24 +1340,24 @@ Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& confi
std::vector<std::string> res; std::vector<std::string> res;
std::string name(limbName); std::string name(limbName);
// hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = // hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice =
// boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ()); // std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name]; const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name];
pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration); pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration);
romDevice->currentConfiguration(q); romDevice->currentConfiguration(q);
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(romDevice); hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(romDevice);
RbPrmPathValidationPtr_t rbprmPathValidation_( RbPrmPathValidationPtr_t rbprmPathValidation_(
boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation())); std::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
rbprmPathValidation_->getValidator()->computeAllContacts(true); rbprmPathValidation_->getValidator()->computeAllContacts(true);
core::ValidationReportPtr_t report; core::ValidationReportPtr_t report;
problemSolver()->problem()->configValidations()->validate(q, report); problemSolver()->problem()->configValidations()->validate(q, report);
core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport>(report); core::RbprmValidationReportPtr_t rbReport = std::dynamic_pointer_cast<hpp::core::RbprmValidationReport>(report);
for (std::map<std::string, core::CollisionValidationReportPtr_t>::const_iterator it = rbReport->ROMReports.begin(); for (std::map<std::string, core::CollisionValidationReportPtr_t>::const_iterator it = rbReport->ROMReports.begin();
it != rbReport->ROMReports.end(); ++it) { it != rbReport->ROMReports.end(); ++it) {
if (name == it->first) if (name == it->first)
// if (true) // if (true)
{ {
core::AllCollisionsValidationReportPtr_t romReports = core::AllCollisionsValidationReportPtr_t romReports =
boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(it->second); std::dynamic_pointer_cast<core::AllCollisionsValidationReport>(it->second);
if (!romReports) { if (!romReports) {
hppDout(warning, "For rom : " << it->first hppDout(warning, "For rom : " << it->first
<< " unable to cast in a AllCollisionsValidationReport, did you correctly " << " unable to cast in a AllCollisionsValidationReport, did you correctly "
...@@ -1389,25 +1392,25 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con ...@@ -1389,25 +1392,25 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con
hppDout(notice, "begin getContactSurfacesAtConfig"); hppDout(notice, "begin getContactSurfacesAtConfig");
std::string name(limbName); std::string name(limbName);
// hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = // hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice =
// boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ()); // std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name]; const hpp::pinocchio::DevicePtr_t romDevice = romDevices_[name];
pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration); pinocchio::Configuration_t q = dofArrayToConfig(romDevice, configuration);
romDevice->currentConfiguration(q); romDevice->currentConfiguration(q);
RbPrmPathValidationPtr_t rbprmPathValidation_( RbPrmPathValidationPtr_t rbprmPathValidation_(
boost::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation())); std::dynamic_pointer_cast<hpp::rbprm::RbPrmPathValidation>(problemSolver()->problem()->pathValidation()));
rbprmPathValidation_->getValidator()->computeAllContacts(true); rbprmPathValidation_->getValidator()->computeAllContacts(true);
core::ValidationReportPtr_t report; core::ValidationReportPtr_t report;
hppDout(notice, "begin collision check"); hppDout(notice, "begin collision check");
problemSolver()->problem()->configValidations()->validate(q, report); problemSolver()->problem()->configValidations()->validate(q, report);
hppDout(notice, "done."); hppDout(notice, "done.");
core::RbprmValidationReportPtr_t rbReport = boost::dynamic_pointer_cast<hpp::core::RbprmValidationReport>(report); core::RbprmValidationReportPtr_t rbReport = std::dynamic_pointer_cast<hpp::core::RbprmValidationReport>(report);
hppDout(notice, "try to find rom name"); hppDout(notice, "try to find rom name");
if (rbReport->ROMReports.find(name) == rbReport->ROMReports.end()) { if (rbReport->ROMReports.find(name) == rbReport->ROMReports.end()) {
throw std::runtime_error("The given ROM name is not in collision in the given configuration."); throw std::runtime_error("The given ROM name is not in collision in the given configuration.");
} }
hppDout(notice, "try to cast report"); hppDout(notice, "try to cast report");
core::AllCollisionsValidationReportPtr_t romReports = core::AllCollisionsValidationReportPtr_t romReports =
boost::dynamic_pointer_cast<core::AllCollisionsValidationReport>(rbReport->ROMReports.at(name)); std::dynamic_pointer_cast<core::AllCollisionsValidationReport>(rbReport->ROMReports.at(name));
if (!romReports) { if (!romReports) {
throw std::runtime_error("Error while retrieving collision reports."); throw std::runtime_error("Error while retrieving collision reports.");
} }
...@@ -1416,7 +1419,7 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con ...@@ -1416,7 +1419,7 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con
hppDout(notice, "done."); hppDout(notice, "done.");
// Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface: // Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface:
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice =
boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot()); std::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot());
fcl::Vec3f reference = rbprmDevice->getEffectorReference(name); fcl::Vec3f reference = rbprmDevice->getEffectorReference(name);
hppDout(notice, "Reference position for rom" << name << " = " << reference); hppDout(notice, "Reference position for rom" << name << " = " << reference);
// apply transform from currernt config : // apply transform from currernt config :
...@@ -1702,7 +1705,7 @@ hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) throw(hpp ...@@ -1702,7 +1705,7 @@ hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) throw(hpp
PathVectorPtr_t pathVector = problemSolver()->paths()[pathId]; PathVectorPtr_t pathVector = problemSolver()->paths()[pathId];
PathPtr_t path = pathVector->pathAtRank(0); PathPtr_t path = pathVector->pathAtRank(0);
// try to cast path as BezierPath : // try to cast path as BezierPath :
BezierPathPtr_t bezierPath = boost::dynamic_pointer_cast<BezierPath>(path); BezierPathPtr_t bezierPath = std::dynamic_pointer_cast<BezierPath>(path);
if (!bezierPath) if (!bezierPath)
throw std::runtime_error("Not a bezier path at index " + boost::lexical_cast<std::string>(pathId)); throw std::runtime_error("Not a bezier path at index " + boost::lexical_cast<std::string>(pathId));
......
...@@ -51,10 +51,10 @@ struct BindShooter { ...@@ -51,10 +51,10 @@ struct BindShooter {
: shootLimit_(shootLimit), displacementLimit_(displacementLimit) {} : shootLimit_(shootLimit), displacementLimit_(displacementLimit) {}
hpp::rbprm::RbPrmShooterPtr_t create( hpp::rbprm::RbPrmShooterPtr_t create(
/*const hpp::pinocchio::DevicePtr_t& robot,*/ const hpp::core::Problem& problem) { /*const hpp::pinocchio::DevicePtr_t& robot,*/ const hpp::core::ProblemConstPtr_t& problem) {
affMap_ = problemSolver_->affordanceObjects; affMap_ = problemSolver_->affordanceObjects;
hpp::pinocchio::RbPrmDevicePtr_t robotcast = hpp::pinocchio::RbPrmDevicePtr_t robotcast =
boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(problem.robot()); std::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(problem->robot());
if (affMap_.map.empty()) { if (affMap_.map.empty()) {
throw hpp::Error("No affordances found. Unable to create shooter object."); throw hpp::Error("No affordances found. Unable to create shooter object.");
} }
...@@ -62,14 +62,14 @@ struct BindShooter { ...@@ -62,14 +62,14 @@ struct BindShooter {
hpp::rbprm::RbPrmShooter::create(robotcast, problemSolver_->problem()->collisionObstacles(), affMap_, hpp::rbprm::RbPrmShooter::create(robotcast, problemSolver_->problem()->collisionObstacles(), affMap_,
romFilter_, affFilter_, shootLimit_, displacementLimit_); romFilter_, affFilter_, shootLimit_, displacementLimit_);
if (!so3Bounds_.empty()) shooter->BoundSO3(so3Bounds_); if (!so3Bounds_.empty()) shooter->BoundSO3(so3Bounds_);
shooter->sampleExtraDOF(problem.getParameter("ConfigurationShooter/sampleExtraDOF").boolValue()); shooter->sampleExtraDOF(problem->getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
shooter->ratioWeighted(problem.getParameter("RbprmShooter/ratioWeighted").floatValue()); shooter->ratioWeighted(problem->getParameter("RbprmShooter/ratioWeighted").floatValue());
return shooter; return shooter;
} }
hpp::core::PathValidationPtr_t createPathValidation(const hpp::pinocchio::DevicePtr_t& robot, hpp::core::PathValidationPtr_t createPathValidation(const hpp::pinocchio::DevicePtr_t& robot,
const hpp::pinocchio::value_type& val) { const hpp::pinocchio::value_type& val) {
hpp::pinocchio::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot); hpp::pinocchio::RbPrmDevicePtr_t robotcast = std::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot);
affMap_ = problemSolver_->affordanceObjects; affMap_ = problemSolver_->affordanceObjects;
if (affMap_.map.empty()) { if (affMap_.map.empty()) {
throw hpp::Error("No affordances found. Unable to create Path Validaton object."); throw hpp::Error("No affordances found. Unable to create Path Validaton object.");
...@@ -85,7 +85,7 @@ struct BindShooter { ...@@ -85,7 +85,7 @@ struct BindShooter {
hpp::core::PathValidationPtr_t createDynamicPathValidation(const hpp::pinocchio::DevicePtr_t& robot, hpp::core::PathValidationPtr_t createDynamicPathValidation(const hpp::pinocchio::DevicePtr_t& robot,
const hpp::pinocchio::value_type& val) { const hpp::pinocchio::value_type& val) {
hpp::pinocchio::RbPrmDevicePtr_t robotcast = boost::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot); hpp::pinocchio::RbPrmDevicePtr_t robotcast = std::static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot);
affMap_ = problemSolver_->affordanceObjects; affMap_ = problemSolver_->affordanceObjects;
if (affMap_.map.empty()) { if (affMap_.map.empty()) {
throw hpp::Error("No affordances found. Unable to create Path Validaton object."); throw hpp::Error("No affordances found. Unable to create Path Validaton object.");
......
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