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>
48 const fcl::Vec3f& initTarget = fcl::Vec3f());
50 template <
class Helper_T,
typename Reference>
52 const fcl::Transform3f& initTarget = fcl::Transform3f());
54 template <
class Helper_T,
typename Reference>
56 const pinocchio::DevicePtr_t endEffectorDevice,
57 const fcl::Transform3f& initTarget = fcl::Transform3f());
67 template <
class Reference>
76 VecRightSide(
const Reference ref,
const int dim = 3,
const bool times_ten =
false)
90 virtual void operator()(constraints::ImplicitPtr_t eq,
const constraints::value_type& normalized_input,
91 pinocchio::ConfigurationOut_t )
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;
96 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
97 assert(success &&
"path operator () did not succeed");
99 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
100 assert(success &&
"path operator () did not succeed");
111 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t>
s_t;
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();
119 core::ConfigProjectorPtr_t& proj = helper.proj_;
120 pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::create(device);
121 comComp->add(device->rootJoint());
124 PointComFunction::create(
"COM-constraint", device, PointCom::create(comComp));
125 ComparisonTypes_t equals(3, constraints::Equality);
126 constraints::ImplicitPtr_t comEq = constraints::Implicit::create(comFunc, equals);
128 proj->rightHandSide(comEq, initTarget);
129 helper.steeringMethod_->tds_.push_back(
133 template <
class Helper_T,
typename Reference>
135 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
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());
143 for (
size_t i = 0; i < 3; i++) {
147 for (size_type i = device->numberDof() - 7; i < device->numberDof(); i++) {
152 std::ostringstream oss;
153 for (std::size_t i = 0; i < mask.size(); ++i) {
154 oss << mask[i] <<
",";
156 hppDout(notice,
"mask = " << oss.str());
159 for (size_type i = 3; i < device->numberDof() - 7; ++i) {
167 for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
170 for (size_type i = 3; i < 6; i++) {
174 for (
size_t i = 3; i <= 9; i++) {
182 for (size_type i = 0; i < weight.size(); i++) {
188 moy = moy / num_active;
189 for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
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);
200 inline constraints::PositionPtr_t
createPositionMethod(pinocchio::DevicePtr_t device,
const fcl::Vec3f& initTarget,
201 const pinocchio::Frame effectorFrame) {
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,
217 const fcl::Transform3f& initTarget,
218 const pinocchio::Frame effectorFrame) {
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);
230 template <
class Helper_T,
typename Reference>
232 const fcl::Vec3f& initTarget) {
233 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
234 ComparisonTypes_t equals(3, constraints::Equality);
236 core::ConfigProjectorPtr_t& proj = helper.proj_;
238 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
239 constraints::ImplicitPtr_t effEq =
242 proj->rightHandSide(effEq, initTarget);
243 helper.steeringMethod_->tds_.push_back(
255 template <
typename Reference,
typename fun>
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 )
const {
278 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
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");
291 template <
class Helper_T,
typename Reference>
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());
301 endEffectorDevice, initTarget,
302 endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()
306 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
309 boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
311 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
314 template <
class Helper_T,
typename Reference>
316 const fcl::Transform3f& initTarget) {
318 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
321 if (ref->operator()(0, success).rows() < device->configSize()) {
322 device = device->clone();
323 device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension() - 1);
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());
330 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
333 boost::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
335 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
339 core::ConfigProjectorPtr_t projector,
const State& state,
340 const std::vector<std::string> active);
342 template <
class Helper_T>
344 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
351 fcl::Vec3f
getNormal(
const std::string& effector,
const State& state,
bool& found);
360 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH