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

update to changes in hpp-core

parent a97a5976
...@@ -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);
......
...@@ -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 =
std::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,8 +62,8 @@ struct BindShooter { ...@@ -62,8 +62,8 @@ 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;
} }
......
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