hpp-rbprm  4.10.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 
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, const fcl::Vec3f& initTarget=fcl::Vec3f());
48 
49  template<class Helper_T, typename Reference>
50  void Create6DEffectorConstraint(Helper_T& helper, const Reference& ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f& initTarget=fcl::Transform3f());
51 
52  template<class Helper_T, typename Reference>
53  void CreateOrientationConstraint(Helper_T& helper, const Reference &ref, const pinocchio::Frame effector, const pinocchio::DevicePtr_t endEffectorDevice, const fcl::Transform3f& initTarget=fcl::Transform3f());
54 
55  // Implementation
56 
63  template<class Reference>
65  {
73  VecRightSide (const Reference ref, const int dim = 3, const bool times_ten = false) : ref_(ref), dim_(dim), times_ten_(times_ten)
74  {}
87  virtual void operator() (constraints::ImplicitPtr_t eq,
88  const constraints::value_type& normalized_input, pinocchio::ConfigurationOut_t /*conf*/) const
89  {
90  const std::pair<core::value_type, core::value_type>& tR (ref_->timeRange());
91  constraints::value_type unNormalized = (tR.second-tR.first)* normalized_input + tR.first;
92  bool success;
93  if(times_ten_)
94  {
95  eq->rightHandSide(ref_->operator ()(unNormalized,success).head(dim_)); // * (10000) ;
96  assert(success && "path operator () did not succeed");
97  }
98  else
99  {
100  eq->rightHandSide(ref_->operator ()(unNormalized,success).head(dim_)) ;
101  assert(success && "path operator () did not succeed");
102  }
103  }
105  const Reference ref_;
107  const int dim_;
108  const bool times_ten_;
109  };
110 
112  typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t> s_t;
113  typedef constraints::SymbolicFunction<s_t> PointComFunction;
114  typedef constraints::SymbolicFunction<s_t>::Ptr_t PointComFunctionPtr_t;
115 
116  template<class Helper_T, typename Reference>
117  void CreateComConstraint(Helper_T& helper, const Reference &ref, const fcl::Vec3f& initTarget=fcl::Vec3f())
118  {
119  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
120  //constraints::ComparisonType equals = constraints::Equality;
121  core::ConfigProjectorPtr_t& proj = helper.proj_;
122  pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::
123  create (device);
124  comComp->add (device->rootJoint());
125  comComp->compute ();
126  PointComFunctionPtr_t comFunc = PointComFunction::create ("COM-constraint",
127  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(TimeDependant(comEq, boost::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference> (ref, 3, true))));
133  }
134 
135  template<class Helper_T, typename Reference>
136  void CreatePosturalTaskConstraint(Helper_T& helper, const Reference &ref){
137  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
138  //core::ComparisonTypePtr_t equals = core::Equality::create ();
139  core::ConfigProjectorPtr_t& proj = helper.proj_;
140  hppDout(notice,"create postural task, ref config = "<<pinocchio::displayConfig(ref));
141  std::vector <bool> mask (device->numberDof(),false);
142  Configuration_t weight(device->numberDof());
143 
144  // mask : 0 for the freeflyer and the extraDoFs :
145  for(size_t i = 0 ; i < 3 ; i++){
146  mask[i]=false;
147  weight[i] = 0.;
148  }
149  for(size_type i = device->numberDof()-7 ; i < device->numberDof() ; i++){
150  mask[i]=false;
151  weight[i] = 0.;
152  }
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++)
171  weight[i] = 1.;
172 
173  // root's orientation
174  for(size_type i = 3 ; i < 6 ; i++){
175  weight[i] = 50.;
176  }
177 
178  for(size_t i = 3 ; i <= 9 ; i++){
179  mask[i] = true;
180  }
181  // mask[5] = false; // z root rotation ????
182 
183 
184 
185 
186  // normalize weight array :
187  double moy =0;
188  int num_active = 0;
189  for (size_type i = 0 ; i < weight.size() ; i++){
190  if(mask[i]){
191  moy += weight[i];
192  num_active++;
193  }
194  }
195  moy = moy/num_active;
196  for (size_type i = 0 ; i < weight.size() ; i++)
197  weight[i] = weight[i]/moy;
198 
199 
200  constraints::ConfigurationConstraintPtr_t postFunc = constraints::ConfigurationConstraint::create("Postural_Task",device,ref,weight);
201  ComparisonTypes_t comps; comps.push_back(constraints::Equality);
202  const constraints::ImplicitPtr_t posturalTask = constraints::Implicit::create (postFunc, comps);
203  proj->add(posturalTask,segments_t(0),1);
204  //proj->updateRightHandSide();
205  }
206 
207  inline constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f& initTarget, const pinocchio::Frame effectorFrame)
208  {
209  //std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
210  std::vector<bool> mask; mask.push_back(true); mask.push_back(true); mask.push_back(true);
211  pinocchio::Transform3f localFrame, globalFrame;
212  localFrame = localFrame.Identity(); globalFrame = globalFrame.Identity();
213  globalFrame.translation(initTarget);
214  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
215  return constraints::Position::create("",device,
216  effectorJoint,
217  effectorFrame.positionInParentFrame() * localFrame,
218  globalFrame,
219  mask);
220  }
221 
222  inline constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f& initTarget, const pinocchio::Frame effectorFrame)
223  {
224  //std::vector<bool> mask; mask.push_back(false); mask.push_back(false); mask.push_back(true);
225  std::vector<bool> mask; mask.push_back(true); mask.push_back(true); mask.push_back(true);
226  pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
227  pinocchio::Transform3f rotation(1);
228  rotation.rotation(effectorFrame.positionInParentFrame().rotation()*initTarget.getRotation());
229  return constraints::Orientation::create("", device,
230  effectorJoint,
231  rotation,
232  mask);
233  }
234 
235 
236  template<class Helper_T, typename Reference>
237  void CreateEffectorConstraint(Helper_T& helper, const Reference &ref, const pinocchio::Frame effectorFr, const fcl::Vec3f& initTarget)
238  {
239  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
240  ComparisonTypes_t equals (3, constraints::Equality);
241 
242  core::ConfigProjectorPtr_t& proj = helper.proj_;
243 
244  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
245  constraints::ImplicitPtr_t effEq = constraints::Implicit::create (
246  createPositionMethod(device,initTarget, effectorFrame), equals);
247  proj->add(effEq);
248  proj->rightHandSide(effEq,initTarget);
249  helper.steeringMethod_->tds_.push_back(
250  TimeDependant(effEq, boost::shared_ptr<VecRightSide<Reference> >(new VecRightSide<Reference>(ref, 3))));
251  }
252 
261  template<typename Reference, typename fun>
263  {
268  funEvaluator(const Reference& ref, const fun& method) : ref_(ref), method_(method),
269  dim_(method_->inputSize ()){}
271  const std::pair<core::value_type, core::value_type> timeRange ()
272  {
273  return ref_->timeRange ();
274  }
287  void operator ()(constraints::ImplicitPtr_t eq,
288  const constraints::value_type& normalized_input, pinocchio::ConfigurationOut_t /*conf*/) const
289  {
290  const std::pair<core::value_type, core::value_type>& tR (ref_->timeRange());
291  bool success;
292  // maps from interval [0,1] to definition interval.
293  constraints::value_type unNormalized = (tR.second-tR.first)* normalized_input + tR.first;
294  eq->rightHandSide( method_->operator ()((ref_->operator ()(unNormalized,success))).vector());
295  assert(success && "path operator () did not succeed");
296  }
297 
298  const Reference ref_;
299  const fun method_;
300  const std::size_t dim_;
301  };
302 
303 
304  template<class Helper_T, typename Reference>
305  void CreateOrientationConstraint(Helper_T& helper, const Reference &ref, const pinocchio::Frame effectorFr,const pinocchio::DevicePtr_t endEffectorDevice, const fcl::Transform3f& initTarget){
306  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
307  ComparisonTypes_t equals; equals.push_back(constraints::Equality);
308  core::ConfigProjectorPtr_t& proj = helper.proj_;
309  pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
310  constraints::OrientationPtr_t orCons = createOrientationMethod(device,initTarget, effectorFrame);
311  constraints::OrientationPtr_t orConsRef = createOrientationMethod(endEffectorDevice,initTarget, endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()->childJoint(0)->name())); // same orientation constraint but for a freeflyer device that represent the end effector (same dim as the ref path)
312  constraints::ImplicitPtr_t effEq = constraints::Implicit::create (orCons, equals);
313  proj->add(effEq);
314  //proj->updateRightHandSide();
315  boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv
317  helper.steeringMethod_->tds_.push_back(
318  TimeDependant(effEq, orEv));
319  }
320 
321 
322  template<class Helper_T, typename Reference>
323  void Create6DEffectorConstraint(Helper_T& helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f& initTarget)
324  {
325  //CreateEffectorConstraint(helper, ref, effectorJoint, initTarget.getTranslation());
326  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
327  // reduce dof if reference path is of lower dimension
328  bool success;
329  if(ref->operator()(0,success).rows() < device->configSize())
330  {
331  device = device->clone();
332  device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension()-1);
333  }
334  ComparisonTypes_t equals; equals.push_back(constraints::Equality);
335  core::ConfigProjectorPtr_t& proj = helper.proj_;
336  pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
337  constraints::OrientationPtr_t orCons = createOrientationMethod(device,initTarget, effector);
338  constraints::ImplicitPtr_t effEq = constraints::Implicit::create (orCons, equals);
339  proj->add(effEq);
340  //proj->updateRightHandSide();
341  boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv
343  helper.steeringMethod_->tds_.push_back(
344  TimeDependant(effEq, orEv));
345  }
346 
347 
348  void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State& state, const std::vector<std::string> active);
349 
350  template<class Helper_T>
351  void CreateContactConstraints(Helper_T& helper, const State& from, const State& to)
352  {
353  pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
354  std::vector<std::string> fixed = to.fixedContacts(from);
355  addContactConstraints(helper.fullbody_, device, helper.proj_,from,fixed);
356  }
357 
358 
359  std::string getEffectorLimb(const State &startState, const State &nextState);
360 
361  fcl::Vec3f getNormal(const std::string& effector, const State &state, bool& found);
362 
363  pinocchio::Frame getEffector(RbPrmFullBodyPtr_t fullbody,
364  const State &startState, const State &nextState);
365 
366  DevicePtr_t createFreeFlyerDevice();
367 
368 
369  } // namespace interpolation
370  } // namespace rbprm
371 } // namespace hpp
372 #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:107
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:323
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:271
hpp::rbprm::interpolation::createPositionMethod
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:207
hpp::rbprm::interpolation::createOrientationMethod
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:222
hpp::rbprm::interpolation::VecRightSide::times_ten_
const bool times_ten_
Definition: interpolation-constraints.hh:108
hpp::rbprm::interpolation::CreateComConstraint
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:117
hpp::rbprm::interpolation::TimeDependant
Definition: time-dependant.hh:50
hpp::rbprm::interpolation::funEvaluator::dim_
const std::size_t dim_
Definition: interpolation-constraints.hh:300
hpp::rbprm::interpolation::s_t
constraints::CalculusBaseAbstract< PointCom::ValueType_t, PointCom::JacobianType_t > s_t
Definition: interpolation-constraints.hh:112
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:113
hpp::rbprm::interpolation::funEvaluator::method_
const fun method_
Definition: interpolation-constraints.hh:299
hpp::rbprm::interpolation::getNormal
fcl::Vec3f getNormal(const std::string &effector, const State &state, bool &found)
hpp::rbprm::interpolation::VecRightSide
Definition: interpolation-constraints.hh:64
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:105
hpp::rbprm::interpolation::funEvaluator::funEvaluator
funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:268
hpp::rbprm::interpolation::PointComFunctionPtr_t
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:114
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:87
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:305
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:136
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:73
hpp::rbprm::interpolation::funEvaluator::ref_
const Reference ref_
Definition: interpolation-constraints.hh:298
hpp::rbprm::interpolation::VecRightSide::~VecRightSide
~VecRightSide()
Definition: interpolation-constraints.hh:75
hpp::rbprm::interpolation::PointCom
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:111
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:237
hpp::rbprm::interpolation::funEvaluator
Definition: interpolation-constraints.hh:262
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:287