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
rotConstraints.push_back(true);
const pinocchio::Frame effectorFrame = fullBody()->device_->getFrameByName(limb->effector_.name());
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,
posConstraints)));
posConstraints);
constraints::ComparisonTypes_t comp (function->outputDerivativeSize(), constraints::EqualToZero);
proj->add(constraints::Implicit::create(function, comp));
if (limb->contactType_ == hpp::rbprm::_6_DOF) {
// rotation matrix around z
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
fcl::Matrix3f rotation =
r * effectorFrame.pinocchio().placement.rotation().transpose(); // * limb->effector_->initialPosition
// ().getRotation();
proj->add(core::NumericalConstraint::create(
constraints::Orientation::create("", fullBody()->device_, effectorJoint,
pinocchio::Transform3f(rotation, fcl::Vec3f::Zero()), rotConstraints)));
const constraints::DifferentiableFunctionPtr_t& orientation_function = constraints::Orientation::create("", fullBody()->device_, effectorJoint,
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);
......
......@@ -51,10 +51,10 @@ struct BindShooter {
: shootLimit_(shootLimit), displacementLimit_(displacementLimit) {}
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;
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()) {
throw hpp::Error("No affordances found. Unable to create shooter object.");
}
......@@ -62,8 +62,8 @@ struct BindShooter {
hpp::rbprm::RbPrmShooter::create(robotcast, problemSolver_->problem()->collisionObstacles(), affMap_,
romFilter_, affFilter_, shootLimit_, displacementLimit_);
if (!so3Bounds_.empty()) shooter->BoundSO3(so3Bounds_);
shooter->sampleExtraDOF(problem.getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
shooter->ratioWeighted(problem.getParameter("RbprmShooter/ratioWeighted").floatValue());
shooter->sampleExtraDOF(problem->getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
shooter->ratioWeighted(problem->getParameter("RbprmShooter/ratioWeighted").floatValue());
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