19 #ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
20 # define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
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>
36 namespace interpolation {
37 using constraints::ComparisonTypes_t;
40 template<
class Helper_T>
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());
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());
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());
63 template<
class Reference>
88 const constraints::value_type& normalized_input, pinocchio::ConfigurationOut_t )
const
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;
95 eq->rightHandSide(
ref_->operator ()(unNormalized,success).head(
dim_));
96 assert(success &&
"path operator () did not succeed");
100 eq->rightHandSide(
ref_->operator ()(unNormalized,success).head(
dim_)) ;
101 assert(success &&
"path operator () did not succeed");
112 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t>
s_t;
116 template<
class Helper_T,
typename Reference>
117 void CreateComConstraint(Helper_T& helper,
const Reference &ref,
const fcl::Vec3f& initTarget=fcl::Vec3f())
119 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
121 core::ConfigProjectorPtr_t& proj = helper.proj_;
122 pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::
124 comComp->add (device->rootJoint());
127 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);
135 template<
class Helper_T,
typename Reference>
137 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
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());
145 for(
size_t i = 0 ; i < 3 ; i++){
149 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++)
174 for(size_type i = 3 ; i < 6 ; i++){
178 for(
size_t i = 3 ; i <= 9 ; i++){
189 for (size_type i = 0 ; i < weight.size() ; i++){
195 moy = moy/num_active;
196 for (size_type i = 0 ; i < weight.size() ; i++)
197 weight[i] = weight[i]/moy;
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);
207 inline constraints::PositionPtr_t
createPositionMethod(pinocchio::DevicePtr_t device,
const fcl::Vec3f& initTarget,
const pinocchio::Frame effectorFrame)
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,
217 effectorFrame.positionInParentFrame() * localFrame,
222 inline constraints::OrientationPtr_t
createOrientationMethod(pinocchio::DevicePtr_t device,
const fcl::Transform3f& initTarget,
const pinocchio::Frame effectorFrame)
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,
236 template<
class Helper_T,
typename Reference>
237 void CreateEffectorConstraint(Helper_T& helper,
const Reference &ref,
const pinocchio::Frame effectorFr,
const fcl::Vec3f& initTarget)
239 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
240 ComparisonTypes_t equals (3, constraints::Equality);
242 core::ConfigProjectorPtr_t& proj = helper.proj_;
244 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
245 constraints::ImplicitPtr_t effEq = constraints::Implicit::create (
248 proj->rightHandSide(effEq,initTarget);
249 helper.steeringMethod_->tds_.push_back(
261 template<
typename Reference,
typename fun>
271 const std::pair<core::value_type, core::value_type>
timeRange ()
273 return ref_->timeRange ();
288 const constraints::value_type& normalized_input, pinocchio::ConfigurationOut_t )
const
290 const std::pair<core::value_type, core::value_type>& tR (
ref_->timeRange());
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");
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());
311 constraints::OrientationPtr_t orConsRef =
createOrientationMethod(endEffectorDevice,initTarget, endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()->childJoint(0)->name()));
312 constraints::ImplicitPtr_t effEq = constraints::Implicit::create (orCons, equals);
315 boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv
317 helper.steeringMethod_->tds_.push_back(
322 template<
class Helper_T,
typename Reference>
323 void Create6DEffectorConstraint(Helper_T& helper,
const Reference &ref,
const pinocchio::Frame effectorJoint,
const fcl::Transform3f& initTarget)
326 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
329 if(ref->operator()(0,success).rows() < device->configSize())
331 device = device->clone();
332 device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension()-1);
334 ComparisonTypes_t equals; equals.push_back(constraints::Equality);
335 core::ConfigProjectorPtr_t& proj = helper.proj_;
336 pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
338 constraints::ImplicitPtr_t effEq = constraints::Implicit::create (orCons, equals);
341 boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv
343 helper.steeringMethod_->tds_.push_back(
350 template<
class Helper_T>
353 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
361 fcl::Vec3f
getNormal(
const std::string& effector,
const State &state,
bool& found);
364 const State &startState,
const State &nextState);
372 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH