hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
interpolation-constraints.hh
Go to the documentation of this file.
1 // This file is part of hpp-wholebody-step.
6 // hpp-wholebody-step-planner is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-wholebody-step-planner is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-wholebody-step-planner. If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
20 #define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
21 
22 #include <hpp/constraints/configuration-constraint.hh>
23 #include <hpp/constraints/relative-com.hh>
24 #include <hpp/constraints/symbolic-calculus.hh>
25 #include <hpp/constraints/symbolic-function.hh>
26 #include <hpp/core/config-projector.hh>
27 #include <hpp/core/path.hh>
28 #include <hpp/core/problem.hh>
29 #include <hpp/pinocchio/configuration.hh>
30 #include <hpp/pinocchio/frame.hh>
33 #include <pinocchio/fwd.hpp>
34 #include <pinocchio/multibody/frame.hpp>
35 namespace hpp {
36 namespace rbprm {
37 namespace interpolation {
38 using constraints::ComparisonTypes_t;
39 // declaration of available constraints
40 
41 template <class Helper_T>
42 void CreateContactConstraints(const State& from, const State& to);
43 
44 // template<class Helper_T, typename Reference>
45 // void CreateComConstraint(Helper_T& helper, const Reference& ref, const
46 // fcl::Vec3f& initTarget=fcl::Vec3f());
47 
48 template <class Helper_T, typename Reference>
49 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref,
50  const pinocchio::Frame effectorJoint,
51  const fcl::Vec3f& initTarget = fcl::Vec3f());
52 
53 template <class Helper_T, typename Reference>
55  Helper_T& helper, const Reference& ref,
56  const pinocchio::Frame effectorJoint,
57  const fcl::Transform3f& initTarget = fcl::Transform3f());
58 
59 template <class Helper_T, typename Reference>
61  Helper_T& helper, const Reference& ref, const pinocchio::Frame effector,
62  const pinocchio::DevicePtr_t endEffectorDevice,
63  const fcl::Transform3f& initTarget = fcl::Transform3f());
64 
65 // Implementation
66 
73 template <class Reference>
82  VecRightSide(const Reference ref, const int dim = 3,
83  const bool times_ten = false)
84  : ref_(ref), dim_(dim), times_ten_(times_ten) {}
97  virtual void operator()(constraints::ImplicitPtr_t eq,
98  const constraints::value_type& normalized_input,
99  pinocchio::ConfigurationOut_t /*conf*/) const {
100  const std::pair<core::value_type, core::value_type>& tR(ref_->timeRange());
101  constraints::value_type unNormalized =
102  (tR.second - tR.first) * normalized_input + tR.first;
103  bool success;
104  if (times_ten_) {
105  eq->rightHandSide(
106  ref_->operator()(unNormalized, success).head(dim_)); // * (10000) ;
107  assert(success && "path operator () did not succeed");
108  } else {
109  eq->rightHandSide(ref_->operator()(unNormalized, success).head(dim_));
110  assert(success && "path operator () did not succeed");
111  }
112  }
114  const Reference ref_;
116  const int dim_;
117  const bool times_ten_;
118 };
119 
121 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t,
122  PointCom::JacobianType_t>
124 typedef constraints::SymbolicFunction<s_t> PointComFunction;
125 typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t;
126 
127 template <class Helper_T, typename Reference>
128 void CreateComConstraint(Helper_T& helper, const Reference& ref,
129  const fcl::Vec3f& initTarget = fcl::Vec3f()) {
130  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
131  // constraints::ComparisonType equals = constraints::Equality;
132  core::ConfigProjectorPtr_t& proj = helper.proj_;
133  pinocchio::CenterOfMassComputationPtr_t comComp =
134  pinocchio::CenterOfMassComputation::create(device);
135  comComp->add(device->rootJoint());
136  comComp->compute();
137  PointComFunctionPtr_t comFunc = PointComFunction::create(
138  "COM-constraint", device, /*10000 **/ PointCom::create(comComp));
139  ComparisonTypes_t equals(3, constraints::Equality);
140  constraints::ImplicitPtr_t comEq =
141  constraints::Implicit::create(comFunc, equals);
142  proj->add(comEq);
143  proj->rightHandSide(comEq, initTarget);
144  helper.steeringMethod_->tds_.push_back(
145  TimeDependant(comEq, shared_ptr<VecRightSide<Reference> >(
146  new VecRightSide<Reference>(ref, 3, true))));
147 }
148 
149 template <class Helper_T, typename Reference>
150 void CreatePosturalTaskConstraint(Helper_T& helper, const Reference& ref) {
151  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
152  // core::ComparisonTypePtr_t equals = core::Equality::create ();
153  core::ConfigProjectorPtr_t& proj = helper.proj_;
154  hppDout(notice, "create postural task, ref config = "
155  << pinocchio::displayConfig(ref));
156  std::vector<bool> mask(device->numberDof(), false);
157  Configuration_t weight(device->numberDof());
158 
159  // mask : 0 for the freeflyer and the extraDoFs :
160  for (size_t i = 0; i < 3; i++) {
161  mask[i] = false;
162  weight[i] = 0.;
163  }
164  for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
165  mask[i] = false;
166  weight[i] = 0.;
167  }
168 
169  std::ostringstream oss;
170  for (std::size_t i = 0; i < mask.size(); ++i) {
171  oss << mask[i] << ",";
172  }
173  hppDout(notice, "mask = " << oss.str());
174 
175  // create a weight vector
176  for (size_type i = 3; i < device->numberDof() - 7; ++i) {
177  weight[i] = 1.;
178  }
179  // TODO : retrieve it from somewhere, store it in fullbody ?
180  // value here for hrp2, from Justin
181  // : chest
182  weight[6] = 10.;
183  // head :
184  for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
185 
186  // root's orientation
187  for (size_type i = 3; i < 6; i++) {
188  weight[i] = 50.;
189  }
190 
191  for (size_t i = 3; i <= 9; i++) {
192  mask[i] = true;
193  }
194  // mask[5] = false; // z root rotation ????
195 
196  // normalize weight array :
197  double moy = 0;
198  int num_active = 0;
199  for (size_type i = 0; i < weight.size(); i++) {
200  if (mask[i]) {
201  moy += weight[i];
202  num_active++;
203  }
204  }
205  moy = moy / num_active;
206  for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
207 
208  constraints::ConfigurationConstraintPtr_t postFunc =
209  constraints::ConfigurationConstraint::create("Postural_Task", device, ref,
210  weight);
211  ComparisonTypes_t comps;
212  comps.push_back(constraints::Equality);
213  const constraints::ImplicitPtr_t posturalTask =
214  constraints::Implicit::create(postFunc, comps);
215  proj->add(posturalTask, 1);
216  // proj->updateRightHandSide();
217 }
218 
219 inline constraints::PositionPtr_t createPositionMethod(
220  pinocchio::DevicePtr_t device, const fcl::Vec3f& initTarget,
221  const pinocchio::Frame effectorFrame) {
222  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false);
223  // mask.push_back(true);
224  std::vector<bool> mask;
225  mask.push_back(true);
226  mask.push_back(true);
227  mask.push_back(true);
228  pinocchio::Transform3f localFrame, globalFrame;
229  localFrame = localFrame.Identity();
230  globalFrame = globalFrame.Identity();
231  globalFrame.translation(initTarget);
232  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
233  return constraints::Position::create(
234  "", device, effectorJoint,
235  effectorFrame.positionInParentFrame() * localFrame, globalFrame, mask);
236 }
237 
238 inline constraints::OrientationPtr_t createOrientationMethod(
239  pinocchio::DevicePtr_t device, const fcl::Transform3f& initTarget,
240  const pinocchio::Frame effectorFrame) {
241  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false);
242  // mask.push_back(true);
243  std::vector<bool> mask;
244  mask.push_back(true);
245  mask.push_back(true);
246  mask.push_back(true);
247  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
248  pinocchio::Transform3f rotation(1);
249  rotation.rotation(effectorFrame.positionInParentFrame().rotation() *
250  initTarget.getRotation());
251  return constraints::Orientation::create("", device, effectorJoint, rotation,
252  mask);
253 }
254 
255 template <class Helper_T, typename Reference>
256 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref,
257  const pinocchio::Frame effectorFr,
258  const fcl::Vec3f& initTarget) {
259  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
260  ComparisonTypes_t equals(3, constraints::Equality);
261 
262  core::ConfigProjectorPtr_t& proj = helper.proj_;
263 
264  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
265  constraints::ImplicitPtr_t effEq = constraints::Implicit::create(
266  createPositionMethod(device, initTarget, effectorFrame), equals);
267  proj->add(effEq);
268  proj->rightHandSide(effEq, initTarget);
269  helper.steeringMethod_->tds_.push_back(
270  TimeDependant(effEq, shared_ptr<VecRightSide<Reference> >(
271  new VecRightSide<Reference>(ref, 3))));
272 }
273 
282 template <typename Reference, typename fun>
288  funEvaluator(const Reference& ref, const fun& method)
289  : ref_(ref), method_(method), dim_(method_->inputSize()) {}
291  const std::pair<core::value_type, core::value_type> timeRange() {
292  return ref_->timeRange();
293  }
306  void operator()(constraints::ImplicitPtr_t eq,
307  const constraints::value_type& normalized_input,
308  pinocchio::ConfigurationOut_t /*conf*/) const {
309  const std::pair<core::value_type, core::value_type>& tR(ref_->timeRange());
310  bool success;
311  // maps from interval [0,1] to definition interval.
312  constraints::value_type unNormalized =
313  (tR.second - tR.first) * normalized_input + tR.first;
314  eq->rightHandSide(
315  method_->operator()((ref_->operator()(unNormalized, success)))
316  .vector());
317  assert(success && "path operator () did not succeed");
318  }
319 
320  const Reference ref_;
321  const fun method_;
322  const std::size_t dim_;
323 };
324 
325 template <class Helper_T, typename Reference>
326 void CreateOrientationConstraint(Helper_T& helper, const Reference& ref,
327  const pinocchio::Frame effectorFr,
328  const pinocchio::DevicePtr_t endEffectorDevice,
329  const fcl::Transform3f& initTarget) {
330  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
331  ComparisonTypes_t equals;
332  equals.push_back(constraints::Equality);
333  core::ConfigProjectorPtr_t& proj = helper.proj_;
334  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
335  constraints::OrientationPtr_t orCons =
336  createOrientationMethod(device, initTarget, effectorFrame);
337  constraints::OrientationPtr_t orConsRef = createOrientationMethod(
338  endEffectorDevice, initTarget,
339  endEffectorDevice->getFrameByName(
340  endEffectorDevice->rootJoint()
341  ->childJoint(0)
342  ->name())); // same orientation constraint but for a freeflyer
343  // device that represent the end effector (same dim
344  // as the ref path)
345  constraints::ImplicitPtr_t effEq =
346  constraints::Implicit::create(orCons, equals);
347  proj->add(effEq);
348  // proj->updateRightHandSide();
349  shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
351  orConsRef));
352  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
353 }
354 
355 template <class Helper_T, typename Reference>
356 void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref,
357  const pinocchio::Frame effectorJoint,
358  const fcl::Transform3f& initTarget) {
359  // CreateEffectorConstraint(helper, ref, effectorJoint,
360  // initTarget.getTranslation());
361  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
362  // reduce dof if reference path is of lower dimension
363  bool success;
364  if (ref->operator()(0, success).rows() < device->configSize()) {
365  device = device->clone();
366  device->setDimensionExtraConfigSpace(
367  device->extraConfigSpace().dimension() - 1);
368  }
369  ComparisonTypes_t equals;
370  equals.push_back(constraints::Equality);
371  core::ConfigProjectorPtr_t& proj = helper.proj_;
372  pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
373  constraints::OrientationPtr_t orCons =
374  createOrientationMethod(device, initTarget, effector);
375  constraints::ImplicitPtr_t effEq =
376  constraints::Implicit::create(orCons, equals);
377  proj->add(effEq);
378  // proj->updateRightHandSide();
379  shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
381  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
382 }
383 
385  pinocchio::DevicePtr_t device,
386  core::ConfigProjectorPtr_t projector,
387  const State& state,
388  const std::vector<std::string> active);
389 
390 template <class Helper_T>
391 void CreateContactConstraints(Helper_T& helper, const State& from,
392  const State& to) {
393  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
394  std::vector<std::string> fixed = to.fixedContacts(from);
395  addContactConstraints(helper.fullbody_, device, helper.proj_, from, fixed);
396 }
397 
398 std::string getEffectorLimb(const State& startState, const State& nextState);
399 
400 fcl::Vec3f getNormal(const std::string& effector, const State& state,
401  bool& found);
402 
403 pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody,
404  const State& startState, const State& nextState);
405 
406 DevicePtr_t createFreeFlyerDevice();
407 
408 } // namespace interpolation
409 } // namespace rbprm
410 } // namespace hpp
411 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:238
void CreateEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:256
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:128
std::string getEffectorLimb(const State &startState, const State &nextState)
fcl::Vec3f getNormal(const std::string &effector, const State &state, bool &found)
DevicePtr_t createFreeFlyerDevice()
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:123
constraints::SymbolicFunction< s_t > PointComFunction
Definition: interpolation-constraints.hh:124
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:326
void CreatePosturalTaskConstraint(Helper_T &helper, const Reference &ref)
Definition: interpolation-constraints.hh:150
void CreateContactConstraints(const State &from, const State &to)
void Create6DEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:356
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:219
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:125
void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State &state, const std::vector< std::string > active)
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:120
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40
std::vector< std::string > fixedContacts(const State &previous) const
Time varying right hand side of constraint.
Definition: time-dependant.hh:31
Definition: time-dependant.hh:51
Definition: interpolation-constraints.hh:74
virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:97
VecRightSide(const Reference ref, const int dim=3, const bool times_ten=false)
Definition: interpolation-constraints.hh:82
const bool times_ten_
Definition: interpolation-constraints.hh:117
const Reference ref_
Reference path of the right hand side of the constraint.
Definition: interpolation-constraints.hh:114
~VecRightSide()
Definition: interpolation-constraints.hh:85
const int dim_
Dimension of the right hand side of the constraint.
Definition: interpolation-constraints.hh:116
Definition: interpolation-constraints.hh:283
void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:306
const std::pair< core::value_type, core::value_type > timeRange()
Time range of reference path of right hand side.
Definition: interpolation-constraints.hh:291
const Reference ref_
Definition: interpolation-constraints.hh:320
const fun method_
Definition: interpolation-constraints.hh:321
const std::size_t dim_
Definition: interpolation-constraints.hh:322
funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:288