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());
123 PointComFunctionPtr_t comFunc =
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>
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 )
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 funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:261
virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:90
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:216
Definition: algorithm.hh:27
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
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:116
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:113
Definition: interpolation-constraints.hh:256
const Reference ref_
Definition: interpolation-constraints.hh:286
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:200
void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:276
const bool times_ten_
Definition: interpolation-constraints.hh:107
void CreateContactConstraints(const State &from, const State &to)
Definition: time-dependant.hh:50
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
void Create6DEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:315
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:111
Definition: interpolation-constraints.hh:68
const std::pair< core::value_type, core::value_type > timeRange()
Time range of reference path of right hand side.
Definition: interpolation-constraints.hh:263
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:110
const Reference ref_
Reference path of the right hand side of the constraint.
Definition: interpolation-constraints.hh:104
fcl::Vec3f getNormal(const std::string &effector, const State &state, bool &found)
std::string getEffectorLimb(const State &startState, const State &nextState)
const fun method_
Definition: interpolation-constraints.hh:287
DevicePtr_t createFreeFlyerDevice()
void CreatePosturalTaskConstraint(Helper_T &helper, const Reference &ref)
Definition: interpolation-constraints.hh:134
~VecRightSide()
Definition: interpolation-constraints.hh:78
const std::size_t dim_
Definition: interpolation-constraints.hh:288
const int dim_
Dimension of the right hand side of the constraint.
Definition: interpolation-constraints.hh:106
void addContactConstraints(rbprm::RbPrmFullBodyPtr_t fullBody, pinocchio::DevicePtr_t device, core::ConfigProjectorPtr_t projector, const State &state, const std::vector< std::string > active)
std::vector< std::string > fixedContacts(const State &previous) const
Definition: rbprm-state.hh:40
Time varying right hand side of constraint.
Definition: time-dependant.hh:31
void CreateEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:231
VecRightSide(const Reference ref, const int dim=3, const bool times_ten=false)
Definition: interpolation-constraints.hh:76
constraints::SymbolicFunction< s_t > PointComFunction
Definition: interpolation-constraints.hh:112