19 #ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
20 #define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH
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>
37 namespace interpolation {
38 using constraints::ComparisonTypes_t;
41 template <
class Helper_T>
48 template <
class Helper_T,
typename Reference>
50 const pinocchio::Frame effectorJoint,
51 const fcl::Vec3f& initTarget = fcl::Vec3f());
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());
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());
73 template <
class Reference>
83 const bool times_ten =
false)
98 const constraints::value_type& normalized_input,
99 pinocchio::ConfigurationOut_t )
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;
106 ref_->operator()(unNormalized, success).head(
dim_));
107 assert(success &&
"path operator () did not succeed");
109 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
110 assert(success &&
"path operator () did not succeed");
121 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t,
122 PointCom::JacobianType_t>
127 template <
class Helper_T,
typename Reference>
129 const fcl::Vec3f& initTarget = fcl::Vec3f()) {
130 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
132 core::ConfigProjectorPtr_t& proj = helper.proj_;
133 pinocchio::CenterOfMassComputationPtr_t comComp =
134 pinocchio::CenterOfMassComputation::create(device);
135 comComp->add(device->rootJoint());
138 "COM-constraint", device, PointCom::create(comComp));
139 ComparisonTypes_t equals(3, constraints::Equality);
140 constraints::ImplicitPtr_t comEq =
141 constraints::Implicit::create(comFunc, equals);
143 proj->rightHandSide(comEq, initTarget);
144 helper.steeringMethod_->tds_.push_back(
149 template <
class Helper_T,
typename Reference>
151 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
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());
160 for (
size_t i = 0; i < 3; i++) {
164 for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
169 std::ostringstream oss;
170 for (std::size_t i = 0; i < mask.size(); ++i) {
171 oss << mask[i] <<
",";
173 hppDout(notice,
"mask = " << oss.str());
176 for (size_type i = 3; i < device->numberDof() - 7; ++i) {
184 for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
187 for (size_type i = 3; i < 6; i++) {
191 for (
size_t i = 3; i <= 9; i++) {
199 for (size_type i = 0; i < weight.size(); i++) {
205 moy = moy / num_active;
206 for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
208 constraints::ConfigurationConstraintPtr_t postFunc =
209 constraints::ConfigurationConstraint::create(
"Postural_Task", device, ref,
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);
220 pinocchio::DevicePtr_t device,
const fcl::Vec3f& initTarget,
221 const pinocchio::Frame effectorFrame) {
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);
239 pinocchio::DevicePtr_t device,
const fcl::Transform3f& initTarget,
240 const pinocchio::Frame effectorFrame) {
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,
255 template <
class Helper_T,
typename Reference>
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);
262 core::ConfigProjectorPtr_t& proj = helper.proj_;
264 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
265 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(
268 proj->rightHandSide(effEq, initTarget);
269 helper.steeringMethod_->tds_.push_back(
282 template <
typename Reference,
typename fun>
291 const std::pair<core::value_type, core::value_type>
timeRange() {
292 return ref_->timeRange();
307 const constraints::value_type& normalized_input,
308 pinocchio::ConfigurationOut_t )
const {
309 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
312 constraints::value_type unNormalized =
313 (tR.second - tR.first) * normalized_input + tR.first;
315 method_->operator()((
ref_->operator()(unNormalized, success)))
317 assert(success &&
"path operator () did not succeed");
325 template <
class Helper_T,
typename Reference>
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 =
338 endEffectorDevice, initTarget,
339 endEffectorDevice->getFrameByName(
340 endEffectorDevice->rootJoint()
345 constraints::ImplicitPtr_t effEq =
346 constraints::Implicit::create(orCons, equals);
349 shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
352 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
355 template <
class Helper_T,
typename Reference>
357 const pinocchio::Frame effectorJoint,
358 const fcl::Transform3f& initTarget) {
361 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
364 if (ref->operator()(0, success).rows() < device->configSize()) {
365 device = device->clone();
366 device->setDimensionExtraConfigSpace(
367 device->extraConfigSpace().dimension() - 1);
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 =
375 constraints::ImplicitPtr_t effEq =
376 constraints::Implicit::create(orCons, equals);
379 shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
381 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
385 pinocchio::DevicePtr_t device,
386 core::ConfigProjectorPtr_t projector,
388 const std::vector<std::string> active);
390 template <
class Helper_T>
393 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
404 const State& startState,
const State& nextState);
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