19 #ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH 20 #define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH 23 #include <pinocchio/fwd.hpp> 24 #include <pinocchio/multibody/frame.hpp> 28 #include <hpp/core/path.hh> 29 #include <hpp/core/problem.hh> 30 #include <hpp/core/config-projector.hh> 31 #include <hpp/constraints/relative-com.hh> 32 #include <hpp/constraints/symbolic-calculus.hh> 33 #include <hpp/constraints/symbolic-function.hh> 34 #include <hpp/constraints/configuration-constraint.hh> 35 #include <hpp/pinocchio/configuration.hh> 36 #include <hpp/pinocchio/frame.hh> 39 namespace interpolation {
40 using constraints::ComparisonTypes_t;
43 template <
class Helper_T>
49 template <
class Helper_T,
typename Reference>
51 const fcl::Vec3f& initTarget = fcl::Vec3f());
53 template <
class Helper_T,
typename Reference>
55 const fcl::Transform3f& initTarget = fcl::Transform3f());
57 template <
class Helper_T,
typename Reference>
59 const pinocchio::DevicePtr_t endEffectorDevice,
60 const fcl::Transform3f& initTarget = fcl::Transform3f());
70 template <
class Reference>
79 VecRightSide(
const Reference ref,
const int dim = 3,
const bool times_ten =
false)
93 virtual void operator()(constraints::ImplicitPtr_t eq,
const constraints::value_type& normalized_input,
94 pinocchio::ConfigurationOut_t )
const {
95 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
96 constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
99 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
100 assert(success &&
"path operator () did not succeed");
102 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
103 assert(success &&
"path operator () did not succeed");
114 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t>
s_t;
118 template <
class Helper_T,
typename Reference>
119 void CreateComConstraint(Helper_T& helper,
const Reference& ref,
const fcl::Vec3f& initTarget = fcl::Vec3f()) {
120 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
122 core::ConfigProjectorPtr_t& proj = helper.proj_;
123 pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::create(device);
124 comComp->add(device->rootJoint());
126 PointComFunctionPtr_t comFunc =
127 PointComFunction::create(
"COM-constraint", device, PointCom::create(comComp));
128 ComparisonTypes_t equals(3, constraints::Equality);
129 constraints::ImplicitPtr_t comEq = constraints::Implicit::create(comFunc, equals);
131 proj->rightHandSide(comEq, initTarget);
132 helper.steeringMethod_->tds_.push_back(
136 template <
class Helper_T,
typename Reference>
138 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
140 core::ConfigProjectorPtr_t& proj = helper.proj_;
141 hppDout(notice,
"create postural task, ref config = " << pinocchio::displayConfig(ref));
142 std::vector<bool> mask(device->numberDof(),
false);
143 Configuration_t weight(device->numberDof());
146 for (
size_t i = 0; i < 3; i++) {
150 for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
155 std::ostringstream oss;
156 for (std::size_t i = 0; i < mask.size(); ++i) {
157 oss << mask[i] <<
",";
159 hppDout(notice,
"mask = " << oss.str());
162 for (size_type i = 3; i < device->numberDof() - 7; ++i) {
170 for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
173 for (size_type i = 3; i < 6; i++) {
177 for (
size_t i = 3; i <= 9; i++) {
185 for (size_type i = 0; i < weight.size(); i++) {
191 moy = moy / num_active;
192 for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
194 constraints::ConfigurationConstraintPtr_t postFunc =
195 constraints::ConfigurationConstraint::create(
"Postural_Task", device, ref, weight);
196 ComparisonTypes_t comps;
197 comps.push_back(constraints::Equality);
198 const constraints::ImplicitPtr_t posturalTask = constraints::Implicit::create(postFunc, comps);
199 proj->add(posturalTask, 1);
203 inline constraints::PositionPtr_t
createPositionMethod(pinocchio::DevicePtr_t device,
const fcl::Vec3f& initTarget,
204 const pinocchio::Frame effectorFrame) {
206 std::vector<bool> mask;
207 mask.push_back(
true);
208 mask.push_back(
true);
209 mask.push_back(
true);
210 pinocchio::Transform3f localFrame, globalFrame;
211 localFrame = localFrame.Identity();
212 globalFrame = globalFrame.Identity();
213 globalFrame.translation(initTarget);
214 pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
215 return constraints::Position::create(
"", device, effectorJoint, effectorFrame.positionInParentFrame() * localFrame,
220 const fcl::Transform3f& initTarget,
221 const pinocchio::Frame effectorFrame) {
223 std::vector<bool> mask;
224 mask.push_back(
true);
225 mask.push_back(
true);
226 mask.push_back(
true);
227 pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
228 pinocchio::Transform3f rotation(1);
229 rotation.rotation(effectorFrame.positionInParentFrame().rotation() * initTarget.getRotation());
230 return constraints::Orientation::create(
"", device, effectorJoint, rotation, mask);
233 template <
class Helper_T,
typename Reference>
235 const fcl::Vec3f& initTarget) {
236 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
237 ComparisonTypes_t equals(3, constraints::Equality);
239 core::ConfigProjectorPtr_t& proj = helper.proj_;
241 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
242 constraints::ImplicitPtr_t effEq =
245 proj->rightHandSide(effEq, initTarget);
246 helper.steeringMethod_->tds_.push_back(
258 template <
typename Reference,
typename fun>
264 funEvaluator(
const Reference& ref,
const fun& method) :
ref_(ref), method_(method),
dim_(method_->inputSize()) {}
266 const std::pair<core::value_type, core::value_type>
timeRange() {
return ref_->timeRange(); }
279 void operator()(constraints::ImplicitPtr_t eq,
const constraints::value_type& normalized_input,
280 pinocchio::ConfigurationOut_t )
const {
281 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
284 constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
285 eq->rightHandSide(method_->operator()((
ref_->operator()(unNormalized, success))).vector());
286 assert(success &&
"path operator () did not succeed");
294 template <
class Helper_T,
typename Reference>
296 const pinocchio::DevicePtr_t endEffectorDevice,
const fcl::Transform3f& initTarget) {
297 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
298 ComparisonTypes_t equals;
299 equals.push_back(constraints::Equality);
300 core::ConfigProjectorPtr_t& proj = helper.proj_;
301 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
304 endEffectorDevice, initTarget,
305 endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()
309 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
312 std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
314 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
317 template <
class Helper_T,
typename Reference>
319 const fcl::Transform3f& initTarget) {
321 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
324 if (ref->operator()(0, success).rows() < device->configSize()) {
325 device = device->clone();
326 device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension() - 1);
328 ComparisonTypes_t equals;
329 equals.push_back(constraints::Equality);
330 core::ConfigProjectorPtr_t& proj = helper.proj_;
331 pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
333 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
336 std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
338 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
342 core::ConfigProjectorPtr_t projector,
const State& state,
343 const std::vector<std::string> active);
345 template <
class Helper_T>
347 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
354 fcl::Vec3f
getNormal(
const std::string& effector,
const State& state,
bool& found);
363 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:264
std::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:93
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:219
Definition: algorithm.hh:27
void CreateOrientationConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effector, const pinocchio::DevicePtr_t endEffectorDevice, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:295
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:119
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:116
Definition: interpolation-constraints.hh:259
const Reference ref_
Definition: interpolation-constraints.hh:289
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:203
void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:279
const bool times_ten_
Definition: interpolation-constraints.hh:110
void CreateContactConstraints(const State &from, const State &to)
Definition: time-dependant.hh:50
void Create6DEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:318
pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody, const State &startState, const State &nextState)
constraints::CalculusBaseAbstract< PointCom::ValueType_t, PointCom::JacobianType_t > s_t
Definition: interpolation-constraints.hh:114
Definition: interpolation-constraints.hh:71
const std::pair< core::value_type, core::value_type > timeRange()
Time range of reference path of right hand side.
Definition: interpolation-constraints.hh:266
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:113
const Reference ref_
Reference path of the right hand side of the constraint.
Definition: interpolation-constraints.hh:107
fcl::Vec3f getNormal(const std::string &effector, const State &state, bool &found)
std::string getEffectorLimb(const State &startState, const State &nextState)
const fun method_
Definition: interpolation-constraints.hh:290
DevicePtr_t createFreeFlyerDevice()
void CreatePosturalTaskConstraint(Helper_T &helper, const Reference &ref)
Definition: interpolation-constraints.hh:137
~VecRightSide()
Definition: interpolation-constraints.hh:81
const std::size_t dim_
Definition: interpolation-constraints.hh:291
const int dim_
Dimension of the right hand side of the constraint.
Definition: interpolation-constraints.hh:109
void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State &state, const std::vector< std::string > active)
std::vector< std::string > fixedContacts(const State &previous) const
Definition: rbprm-state.hh:40
Time varying right hand side of constraint.
Definition: time-dependant.hh:31
void CreateEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:234
VecRightSide(const Reference ref, const int dim=3, const bool times_ten=false)
Definition: interpolation-constraints.hh:79
constraints::SymbolicFunction< s_t > PointComFunction
Definition: interpolation-constraints.hh:115