hpp-rbprm  4.10.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 
24 #include <hpp/core/path.hh>
25 #include <hpp/core/problem.hh>
26 #include <hpp/core/config-projector.hh>
27 #include <hpp/constraints/relative-com.hh>
28 #include <hpp/constraints/symbolic-calculus.hh>
29 #include <hpp/constraints/symbolic-function.hh>
30 #include <hpp/constraints/configuration-constraint.hh>
31 #include <hpp/pinocchio/configuration.hh>
32 #include <hpp/pinocchio/frame.hh>
33 #include <pinocchio/multibody/frame.hpp>
34 namespace hpp {
35 namespace rbprm {
36 namespace interpolation {
37 using constraints::ComparisonTypes_t;
38 // declaration of available constraints
39 
40 template <class Helper_T>
41 void CreateContactConstraints(const State& from, const State& to);
42 
43 // template<class Helper_T, typename Reference>
44 // void CreateComConstraint(Helper_T& helper, const Reference& ref, const fcl::Vec3f& initTarget=fcl::Vec3f());
45 
46 template <class Helper_T, typename Reference>
47 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
48  const fcl::Vec3f& initTarget = fcl::Vec3f());
49 
50 template <class Helper_T, typename Reference>
51 void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
52  const fcl::Transform3f& initTarget = fcl::Transform3f());
53 
54 template <class Helper_T, typename Reference>
55 void CreateOrientationConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effector,
56  const pinocchio::DevicePtr_t endEffectorDevice,
57  const fcl::Transform3f& initTarget = fcl::Transform3f());
58 
59 // Implementation
60 
67 template <class Reference>
76  VecRightSide(const Reference ref, const int dim = 3, const bool times_ten = false)
77  : ref_(ref), dim_(dim), times_ten_(times_ten) {}
90  virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type& normalized_input,
91  pinocchio::ConfigurationOut_t /*conf*/) const {
92  const std::pair<core::value_type, core::value_type>& tR(ref_->timeRange());
93  constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
94  bool success;
95  if (times_ten_) {
96  eq->rightHandSide(ref_->operator()(unNormalized, success).head(dim_)); // * (10000) ;
97  assert(success && "path operator () did not succeed");
98  } else {
99  eq->rightHandSide(ref_->operator()(unNormalized, success).head(dim_));
100  assert(success && "path operator () did not succeed");
101  }
102  }
104  const Reference ref_;
106  const int dim_;
107  const bool times_ten_;
108 };
109 
111 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t> s_t;
112 typedef constraints::SymbolicFunction<s_t> PointComFunction;
113 typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t;
114 
115 template <class Helper_T, typename Reference>
116 void CreateComConstraint(Helper_T& helper, const Reference& ref, const fcl::Vec3f& initTarget = fcl::Vec3f()) {
117  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
118  // constraints::ComparisonType equals = constraints::Equality;
119  core::ConfigProjectorPtr_t& proj = helper.proj_;
120  pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::create(device);
121  comComp->add(device->rootJoint());
122  comComp->compute();
123  PointComFunctionPtr_t comFunc =
124  PointComFunction::create("COM-constraint", device, /*10000 **/ PointCom::create(comComp));
125  ComparisonTypes_t equals(3, constraints::Equality);
126  constraints::ImplicitPtr_t comEq = constraints::Implicit::create(comFunc, equals);
127  proj->add(comEq);
128  proj->rightHandSide(comEq, initTarget);
129  helper.steeringMethod_->tds_.push_back(
130  TimeDependant(comEq, boost::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference>(ref, 3, true))));
131 }
132 
133 template <class Helper_T, typename Reference>
134 void CreatePosturalTaskConstraint(Helper_T& helper, const Reference& ref) {
135  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
136  // core::ComparisonTypePtr_t equals = core::Equality::create ();
137  core::ConfigProjectorPtr_t& proj = helper.proj_;
138  hppDout(notice, "create postural task, ref config = " << pinocchio::displayConfig(ref));
139  std::vector<bool> mask(device->numberDof(), false);
140  Configuration_t weight(device->numberDof());
141 
142  // mask : 0 for the freeflyer and the extraDoFs :
143  for (size_t i = 0; i < 3; i++) {
144  mask[i] = false;
145  weight[i] = 0.;
146  }
147  for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
148  mask[i] = false;
149  weight[i] = 0.;
150  }
151 
152  std::ostringstream oss;
153  for (std::size_t i = 0; i < mask.size(); ++i) {
154  oss << mask[i] << ",";
155  }
156  hppDout(notice, "mask = " << oss.str());
157 
158  // create a weight vector
159  for (size_type i = 3; i < device->numberDof() - 7; ++i) {
160  weight[i] = 1.;
161  }
162  // TODO : retrieve it from somewhere, store it in fullbody ?
163  // value here for hrp2, from Justin
164  // : chest
165  weight[6] = 10.;
166  // head :
167  for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
168 
169  // root's orientation
170  for (size_type i = 3; i < 6; i++) {
171  weight[i] = 50.;
172  }
173 
174  for (size_t i = 3; i <= 9; i++) {
175  mask[i] = true;
176  }
177  // mask[5] = false; // z root rotation ????
178 
179  // normalize weight array :
180  double moy = 0;
181  int num_active = 0;
182  for (size_type i = 0; i < weight.size(); i++) {
183  if (mask[i]) {
184  moy += weight[i];
185  num_active++;
186  }
187  }
188  moy = moy / num_active;
189  for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
190 
191  constraints::ConfigurationConstraintPtr_t postFunc =
192  constraints::ConfigurationConstraint::create("Postural_Task", device, ref, weight);
193  ComparisonTypes_t comps;
194  comps.push_back(constraints::Equality);
195  const constraints::ImplicitPtr_t posturalTask = constraints::Implicit::create(postFunc, comps);
196  proj->add(posturalTask, segments_t(0), 1);
197  // proj->updateRightHandSide();
198 }
199 
200 inline constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f& initTarget,
201  const pinocchio::Frame effectorFrame) {
202  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
203  std::vector<bool> mask;
204  mask.push_back(true);
205  mask.push_back(true);
206  mask.push_back(true);
207  pinocchio::Transform3f localFrame, globalFrame;
208  localFrame = localFrame.Identity();
209  globalFrame = globalFrame.Identity();
210  globalFrame.translation(initTarget);
211  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
212  return constraints::Position::create("", device, effectorJoint, effectorFrame.positionInParentFrame() * localFrame,
213  globalFrame, mask);
214 }
215 
216 inline constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device,
217  const fcl::Transform3f& initTarget,
218  const pinocchio::Frame effectorFrame) {
219  // std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
220  std::vector<bool> mask;
221  mask.push_back(true);
222  mask.push_back(true);
223  mask.push_back(true);
224  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
225  pinocchio::Transform3f rotation(1);
226  rotation.rotation(effectorFrame.positionInParentFrame().rotation() * initTarget.getRotation());
227  return constraints::Orientation::create("", device, effectorJoint, rotation, mask);
228 }
229 
230 template <class Helper_T, typename Reference>
231 void CreateEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorFr,
232  const fcl::Vec3f& initTarget) {
233  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
234  ComparisonTypes_t equals(3, constraints::Equality);
235 
236  core::ConfigProjectorPtr_t& proj = helper.proj_;
237 
238  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
239  constraints::ImplicitPtr_t effEq =
240  constraints::Implicit::create(createPositionMethod(device, initTarget, effectorFrame), equals);
241  proj->add(effEq);
242  proj->rightHandSide(effEq, initTarget);
243  helper.steeringMethod_->tds_.push_back(
244  TimeDependant(effEq, boost::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference>(ref, 3))));
245 }
246 
255 template <typename Reference, typename fun>
261  funEvaluator(const Reference& ref, const fun& method) : ref_(ref), method_(method), dim_(method_->inputSize()) {}
263  const std::pair<core::value_type, core::value_type> timeRange() { return ref_->timeRange(); }
276  void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type& normalized_input,
277  pinocchio::ConfigurationOut_t /*conf*/) const {
278  const std::pair<core::value_type, core::value_type>& tR(ref_->timeRange());
279  bool success;
280  // maps from interval [0,1] to definition interval.
281  constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
282  eq->rightHandSide(method_->operator()((ref_->operator()(unNormalized, success))).vector());
283  assert(success && "path operator () did not succeed");
284  }
285 
286  const Reference ref_;
287  const fun method_;
288  const std::size_t dim_;
289 };
290 
291 template <class Helper_T, typename Reference>
292 void CreateOrientationConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorFr,
293  const pinocchio::DevicePtr_t endEffectorDevice, const fcl::Transform3f& initTarget) {
294  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
295  ComparisonTypes_t equals;
296  equals.push_back(constraints::Equality);
297  core::ConfigProjectorPtr_t& proj = helper.proj_;
298  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
299  constraints::OrientationPtr_t orCons = createOrientationMethod(device, initTarget, effectorFrame);
300  constraints::OrientationPtr_t orConsRef = createOrientationMethod(
301  endEffectorDevice, initTarget,
302  endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()
303  ->childJoint(0)
304  ->name())); // same orientation constraint but for a freeflyer device that
305  // represent the end effector (same dim as the ref path)
306  constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
307  proj->add(effEq);
308  // proj->updateRightHandSide();
309  boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
311  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
312 }
313 
314 template <class Helper_T, typename Reference>
315 void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint,
316  const fcl::Transform3f& initTarget) {
317  // CreateEffectorConstraint(helper, ref, effectorJoint, initTarget.getTranslation());
318  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
319  // reduce dof if reference path is of lower dimension
320  bool success;
321  if (ref->operator()(0, success).rows() < device->configSize()) {
322  device = device->clone();
323  device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension() - 1);
324  }
325  ComparisonTypes_t equals;
326  equals.push_back(constraints::Equality);
327  core::ConfigProjectorPtr_t& proj = helper.proj_;
328  pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
329  constraints::OrientationPtr_t orCons = createOrientationMethod(device, initTarget, effector);
330  constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
331  proj->add(effEq);
332  // proj->updateRightHandSide();
333  boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
335  helper.steeringMethod_->tds_.push_back(TimeDependant(effEq, orEv));
336 }
337 
338 void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device,
339  core::ConfigProjectorPtr_t projector, const State& state,
340  const std::vector<std::string> active);
341 
342 template <class Helper_T>
343 void CreateContactConstraints(Helper_T& helper, const State& from, const State& to) {
344  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
345  std::vector<std::string> fixed = to.fixedContacts(from);
346  addContactConstraints(helper.fullbody_, device, helper.proj_, from, fixed);
347 }
348 
349 std::string getEffectorLimb(const State& startState, const State& nextState);
350 
351 fcl::Vec3f getNormal(const std::string& effector, const State& state, bool& found);
352 
353 pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody, const State& startState, const State& nextState);
354 
355 DevicePtr_t createFreeFlyerDevice();
356 
357 } // namespace interpolation
358 } // namespace rbprm
359 } // namespace hpp
360 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
hpp::rbprm::interpolation::VecRightSide::dim_
const int dim_
Dimension of the right hand side of the constraint.
Definition: interpolation-constraints.hh:106
hpp::rbprm::interpolation::Create6DEffectorConstraint
void Create6DEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:315
hpp::rbprm::interpolation::funEvaluator::timeRange
const std::pair< core::value_type, core::value_type > timeRange()
Time range of reference path of right hand side.
Definition: interpolation-constraints.hh:263
hpp::rbprm::interpolation::createPositionMethod
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:200
hpp::rbprm::interpolation::createOrientationMethod
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:216
hpp::rbprm::interpolation::VecRightSide::times_ten_
const bool times_ten_
Definition: interpolation-constraints.hh:107
hpp::rbprm::interpolation::CreateComConstraint
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:116
hpp::rbprm::interpolation::TimeDependant
Definition: time-dependant.hh:50
hpp::rbprm::interpolation::funEvaluator::dim_
const std::size_t dim_
Definition: interpolation-constraints.hh:288
hpp::rbprm::interpolation::s_t
constraints::CalculusBaseAbstract< PointCom::ValueType_t, PointCom::JacobianType_t > s_t
Definition: interpolation-constraints.hh:111
hpp::rbprm::interpolation::CreateContactConstraints
void CreateContactConstraints(const State &from, const State &to)
hpp::rbprm::interpolation::PointComFunction
constraints::SymbolicFunction< s_t > PointComFunction
Definition: interpolation-constraints.hh:112
hpp::rbprm::interpolation::funEvaluator::method_
const fun method_
Definition: interpolation-constraints.hh:287
hpp::rbprm::interpolation::getNormal
fcl::Vec3f getNormal(const std::string &effector, const State &state, bool &found)
hpp::rbprm::interpolation::VecRightSide
Definition: interpolation-constraints.hh:68
hpp::rbprm::interpolation::RightHandSideFunctor
Time varying right hand side of constraint.
Definition: time-dependant.hh:31
time-dependant.hh
hpp::rbprm::interpolation::VecRightSide::ref_
const Reference ref_
Reference path of the right hand side of the constraint.
Definition: interpolation-constraints.hh:104
hpp::rbprm::interpolation::funEvaluator::funEvaluator
funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:261
hpp::rbprm::interpolation::PointComFunctionPtr_t
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:113
hpp::rbprm::interpolation::VecRightSide::operator()
virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:90
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::CreateOrientationConstraint
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:292
hpp::rbprm::interpolation::getEffector
pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody, const State &startState, const State &nextState)
time-constraint-utils.hh
hpp::rbprm::interpolation::addContactConstraints
void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State &state, const std::vector< std::string > active)
hpp::rbprm::interpolation::CreatePosturalTaskConstraint
void CreatePosturalTaskConstraint(Helper_T &helper, const Reference &ref)
Definition: interpolation-constraints.hh:134
hpp::rbprm::interpolation::createFreeFlyerDevice
DevicePtr_t createFreeFlyerDevice()
hpp::rbprm::State
Definition: rbprm-state.hh:40
hpp::rbprm::RbPrmFullBodyPtr_t
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
hpp::rbprm::State::fixedContacts
std::vector< std::string > fixedContacts(const State &previous) const
hpp::rbprm::interpolation::getEffectorLimb
std::string getEffectorLimb(const State &startState, const State &nextState)
hpp::rbprm::interpolation::VecRightSide::VecRightSide
VecRightSide(const Reference ref, const int dim=3, const bool times_ten=false)
Definition: interpolation-constraints.hh:76
hpp::rbprm::interpolation::funEvaluator::ref_
const Reference ref_
Definition: interpolation-constraints.hh:286
hpp::rbprm::interpolation::VecRightSide::~VecRightSide
~VecRightSide()
Definition: interpolation-constraints.hh:78
hpp::rbprm::interpolation::PointCom
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:110
hpp::rbprm::interpolation::CreateEffectorConstraint
void CreateEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:231
hpp::rbprm::interpolation::funEvaluator
Definition: interpolation-constraints.hh:256
hpp::rbprm::interpolation::funEvaluator::operator()
void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:276