hpp-rbprm  4.11.0
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 
23 #include <pinocchio/fwd.hpp>
24 #include <pinocchio/multibody/frame.hpp>
25 
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>
37 namespace hpp {
38 namespace rbprm {
39 namespace interpolation {
40 using constraints::ComparisonTypes_t;
41 // declaration of available constraints
42 
43 template <class Helper_T>
44 void CreateContactConstraints(const State& from, const State& to);
45 
46 // template<class Helper_T, typename Reference>
47 // void CreateComConstraint(Helper_T& helper, const Reference& ref, const fcl::Vec3f& initTarget=fcl::Vec3f());
48 
49 template <class Helper_T, typename Reference>
50 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
51  const fcl::Vec3f& initTarget = fcl::Vec3f());
52 
53 template <class Helper_T, typename Reference>
54 void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
55  const fcl::Transform3f& initTarget = fcl::Transform3f());
56 
57 template <class Helper_T, typename Reference>
58 void CreateOrientationConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effector,
59  const pinocchio::DevicePtr_t endEffectorDevice,
60  const fcl::Transform3f& initTarget = fcl::Transform3f());
61 
62 // Implementation
63 
70 template <class Reference>
79  VecRightSide(const Reference ref, const int dim = 3, const bool times_ten = false)
80  : ref_(ref), dim_(dim), times_ten_(times_ten) {}
93  virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type& normalized_input,
94  pinocchio::ConfigurationOut_t /*conf*/) 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;
97  bool success;
98  if (times_ten_) {
99  eq->rightHandSide(ref_->operator()(unNormalized, success).head(dim_)); // * (10000) ;
100  assert(success && "path operator () did not succeed");
101  } else {
102  eq->rightHandSide(ref_->operator()(unNormalized, success).head(dim_));
103  assert(success && "path operator () did not succeed");
104  }
105  }
107  const Reference ref_;
109  const int dim_;
110  const bool times_ten_;
111 };
112 
114 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t> s_t;
115 typedef constraints::SymbolicFunction<s_t> PointComFunction;
116 typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t;
117 
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();
121  // constraints::ComparisonType equals = constraints::Equality;
122  core::ConfigProjectorPtr_t& proj = helper.proj_;
123  pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::create(device);
124  comComp->add(device->rootJoint());
125  comComp->compute();
126  PointComFunctionPtr_t comFunc =
127  PointComFunction::create("COM-constraint", device, /*10000 **/ PointCom::create(comComp));
128  ComparisonTypes_t equals(3, constraints::Equality);
129  constraints::ImplicitPtr_t comEq = constraints::Implicit::create(comFunc, equals);
130  proj->add(comEq);
131  proj->rightHandSide(comEq, initTarget);
132  helper.steeringMethod_->tds_.push_back(
133  TimeDependant(comEq, std::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference>(ref, 3, true))));
134 }
135 
136 template <class Helper_T, typename Reference>
137 void CreatePosturalTaskConstraint(Helper_T& helper, const Reference& ref) {
138  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
139  // core::ComparisonTypePtr_t equals = core::Equality::create ();
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());
144 
145  // mask : 0 for the freeflyer and the extraDoFs :
146  for (size_t i = 0; i < 3; i++) {
147  mask[i] = false;
148  weight[i] = 0.;
149  }
150  for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
151  mask[i] = false;
152  weight[i] = 0.;
153  }
154 
155  std::ostringstream oss;
156  for (std::size_t i = 0; i < mask.size(); ++i) {
157  oss << mask[i] << ",";
158  }
159  hppDout(notice, "mask = " << oss.str());
160 
161  // create a weight vector
162  for (size_type i = 3; i < device->numberDof() - 7; ++i) {
163  weight[i] = 1.;
164  }
165  // TODO : retrieve it from somewhere, store it in fullbody ?
166  // value here for hrp2, from Justin
167  // : chest
168  weight[6] = 10.;
169  // head :
170  for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
171 
172  // root's orientation
173  for (size_type i = 3; i < 6; i++) {
174  weight[i] = 50.;
175  }
176 
177  for (size_t i = 3; i <= 9; i++) {
178  mask[i] = true;
179  }
180  // mask[5] = false; // z root rotation ????
181 
182  // normalize weight array :
183  double moy = 0;
184  int num_active = 0;
185  for (size_type i = 0; i < weight.size(); i++) {
186  if (mask[i]) {
187  moy += weight[i];
188  num_active++;
189  }
190  }
191  moy = moy / num_active;
192  for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
193 
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);
200  // proj->updateRightHandSide();
201 }
202 
203 inline constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f& initTarget,
204  const pinocchio::Frame effectorFrame) {
205  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
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,
216  globalFrame, mask);
217 }
218 
219 inline constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device,
220  const fcl::Transform3f& initTarget,
221  const pinocchio::Frame effectorFrame) {
222  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
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);
231 }
232 
233 template <class Helper_T, typename Reference>
234 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorFr,
235  const fcl::Vec3f& initTarget) {
236  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
237  ComparisonTypes_t equals(3, constraints::Equality);
238 
239  core::ConfigProjectorPtr_t& proj = helper.proj_;
240 
241  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
242  constraints::ImplicitPtr_t effEq =
243  constraints::Implicit::create(createPositionMethod(device, initTarget, effectorFrame), equals);
244  proj->add(effEq);
245  proj->rightHandSide(effEq, initTarget);
246  helper.steeringMethod_->tds_.push_back(
247  TimeDependant(effEq, std::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference>(ref, 3))));
248 }
249 
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 /*conf*/) const {
281  const std::pair<core::value_type, core::value_type>& tR(ref_->timeRange());
282  bool success;
283  // maps from interval [0,1] to definition interval.
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");
287  }
288 
289  const Reference ref_;
290  const fun method_;
291  const std::size_t dim_;
292 };
293 
294 template <class Helper_T, typename Reference>
295 void CreateOrientationConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorFr,
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());
302  constraints::OrientationPtr_t orCons = createOrientationMethod(device, initTarget, effectorFrame);
303  constraints::OrientationPtr_t orConsRef = createOrientationMethod(
304  endEffectorDevice, initTarget,
305  endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()
306  ->childJoint(0)
307  ->name())); // same orientation constraint but for a freeflyer device that
308  // represent the end effector (same dim as the ref path)
309  constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
310  proj->add(effEq);
311  // proj->updateRightHandSide();
312  std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
314  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
315 }
316 
317 template <class Helper_T, typename Reference>
318 void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
319  const fcl::Transform3f& initTarget) {
320  // CreateEffectorConstraint(helper, ref, effectorJoint, initTarget.getTranslation());
321  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
322  // reduce dof if reference path is of lower dimension
323  bool success;
324  if (ref->operator()(0, success).rows() < device->configSize()) {
325  device = device->clone();
326  device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension() - 1);
327  }
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());
332  constraints::OrientationPtr_t orCons = createOrientationMethod(device, initTarget, effector);
333  constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
334  proj->add(effEq);
335  // proj->updateRightHandSide();
336  std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
338  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
339 }
340 
341 void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device,
342  core::ConfigProjectorPtr_t projector, const State& state,
343  const std::vector<std::string> active);
344 
345 template <class Helper_T>
346 void CreateContactConstraints(Helper_T& helper, const State& from, const State& to) {
347  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
348  std::vector<std::string> fixed = to.fixedContacts(from);
349  addContactConstraints(helper.fullbody_, device, helper.proj_, from, fixed);
350 }
351 
352 std::string getEffectorLimb(const State& startState, const State& nextState);
353 
354 fcl::Vec3f getNormal(const std::string& effector, const State& state, bool& found);
355 
356 pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody, const State& startState, const State& nextState);
357 
358 DevicePtr_t createFreeFlyerDevice();
359 
360 } // namespace interpolation
361 } // namespace rbprm
362 } // namespace hpp
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