19 #ifndef HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH 20 #define HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH 22 #include <pinocchio/fwd.hpp> 23 #include <pinocchio/multibody/frame.hpp> 27 #include <hpp/core/path.hh> 28 #include <hpp/core/problem.hh> 29 #include <hpp/core/config-projector.hh> 30 #include <hpp/constraints/relative-com.hh> 31 #include <hpp/constraints/symbolic-calculus.hh> 32 #include <hpp/constraints/symbolic-function.hh> 33 #include <hpp/constraints/configuration-constraint.hh> 34 #include <hpp/pinocchio/configuration.hh> 35 #include <hpp/pinocchio/frame.hh> 38 namespace interpolation {
39 using constraints::ComparisonTypes_t;
42 template <
class Helper_T>
48 template <
class Helper_T,
typename Reference>
50 const fcl::Vec3f& initTarget = fcl::Vec3f());
52 template <
class Helper_T,
typename Reference>
54 const fcl::Transform3f& initTarget = fcl::Transform3f());
56 template <
class Helper_T,
typename Reference>
58 const pinocchio::DevicePtr_t endEffectorDevice,
59 const fcl::Transform3f& initTarget = fcl::Transform3f());
69 template <
class Reference>
78 VecRightSide(
const Reference ref,
const int dim = 3,
const bool times_ten =
false)
92 virtual void operator()(constraints::ImplicitPtr_t eq,
const constraints::value_type& normalized_input,
93 pinocchio::ConfigurationOut_t )
const {
94 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
95 constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
98 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
99 assert(success &&
"path operator () did not succeed");
101 eq->rightHandSide(
ref_->operator()(unNormalized, success).head(
dim_));
102 assert(success &&
"path operator () did not succeed");
113 typedef constraints::CalculusBaseAbstract<PointCom::ValueType_t, PointCom::JacobianType_t>
s_t;
117 template <
class Helper_T,
typename Reference>
118 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::create(device);
123 comComp->add(device->rootJoint());
125 PointComFunctionPtr_t comFunc =
126 PointComFunction::create(
"COM-constraint", device, PointCom::create(comComp));
127 ComparisonTypes_t equals(3, constraints::Equality);
128 constraints::ImplicitPtr_t comEq = constraints::Implicit::create(comFunc, equals);
130 proj->rightHandSide(comEq, initTarget);
131 helper.steeringMethod_->tds_.push_back(
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++) {
154 std::ostringstream oss;
155 for (std::size_t i = 0; i < mask.size(); ++i) {
156 oss << mask[i] <<
",";
158 hppDout(notice,
"mask = " << oss.str());
161 for (size_type i = 3; i < device->numberDof() - 7; ++i) {
169 for (size_type i = 7; i <= 9; i++) weight[i] = 1.;
172 for (size_type i = 3; i < 6; i++) {
176 for (
size_t i = 3; i <= 9; i++) {
184 for (size_type i = 0; i < weight.size(); i++) {
190 moy = moy / num_active;
191 for (size_type i = 0; i < weight.size(); i++) weight[i] = weight[i] / moy;
193 constraints::ConfigurationConstraintPtr_t postFunc =
194 constraints::ConfigurationConstraint::create(
"Postural_Task", device, ref, weight);
195 ComparisonTypes_t comps;
196 comps.push_back(constraints::Equality);
197 const constraints::ImplicitPtr_t posturalTask = constraints::Implicit::create(postFunc, comps);
198 proj->add(posturalTask, 1);
202 inline constraints::PositionPtr_t
createPositionMethod(pinocchio::DevicePtr_t device,
const fcl::Vec3f& initTarget,
203 const pinocchio::Frame effectorFrame) {
205 std::vector<bool> mask;
206 mask.push_back(
true);
207 mask.push_back(
true);
208 mask.push_back(
true);
209 pinocchio::Transform3f localFrame, globalFrame;
210 localFrame = localFrame.Identity();
211 globalFrame = globalFrame.Identity();
212 globalFrame.translation(initTarget);
213 pinocchio::JointPtr_t effectorJoint = effectorFrame.joint();
214 return constraints::Position::create(
"", device, effectorJoint, effectorFrame.positionInParentFrame() * localFrame,
219 const fcl::Transform3f& initTarget,
220 const pinocchio::Frame effectorFrame) {
222 std::vector<bool> mask;
223 mask.push_back(
true);
224 mask.push_back(
true);
225 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, effectorJoint, rotation, mask);
232 template <
class Helper_T,
typename Reference>
234 const fcl::Vec3f& initTarget) {
235 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
236 ComparisonTypes_t equals(3, constraints::Equality);
238 core::ConfigProjectorPtr_t& proj = helper.proj_;
240 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
241 constraints::ImplicitPtr_t effEq =
244 proj->rightHandSide(effEq, initTarget);
245 helper.steeringMethod_->tds_.push_back(
257 template <
typename Reference,
typename fun>
263 funEvaluator(
const Reference& ref,
const fun& method) :
ref_(ref), method_(method),
dim_(method_->inputSize()) {}
265 const std::pair<core::value_type, core::value_type>
timeRange() {
return ref_->timeRange(); }
278 void operator()(constraints::ImplicitPtr_t eq,
const constraints::value_type& normalized_input,
279 pinocchio::ConfigurationOut_t )
const {
280 const std::pair<core::value_type, core::value_type>& tR(
ref_->timeRange());
283 constraints::value_type unNormalized = (tR.second - tR.first) * normalized_input + tR.first;
284 eq->rightHandSide(method_->operator()((
ref_->operator()(unNormalized, success))).vector());
285 assert(success &&
"path operator () did not succeed");
293 template <
class Helper_T,
typename Reference>
295 const pinocchio::DevicePtr_t endEffectorDevice,
const fcl::Transform3f& initTarget) {
296 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
297 ComparisonTypes_t equals;
298 equals.push_back(constraints::Equality);
299 core::ConfigProjectorPtr_t& proj = helper.proj_;
300 pinocchio::Frame effectorFrame = device->getFrameByName(effectorFr.name());
303 endEffectorDevice, initTarget,
304 endEffectorDevice->getFrameByName(endEffectorDevice->rootJoint()
308 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
311 std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
313 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
316 template <
class Helper_T,
typename Reference>
318 const fcl::Transform3f& initTarget) {
320 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
323 if (ref->operator()(0, success).rows() < device->configSize()) {
324 device = device->clone();
325 device->setDimensionExtraConfigSpace(device->extraConfigSpace().dimension() - 1);
327 ComparisonTypes_t equals;
328 equals.push_back(constraints::Equality);
329 core::ConfigProjectorPtr_t& proj = helper.proj_;
330 pinocchio::Frame effector = device->getFrameByName(effectorJoint.name());
332 constraints::ImplicitPtr_t effEq = constraints::Implicit::create(orCons, equals);
335 std::shared_ptr<funEvaluator<Reference, constraints::OrientationPtr_t> > orEv(
337 helper.steeringMethod_->tds_.push_back(
TimeDependant(effEq, orEv));
341 core::ConfigProjectorPtr_t projector,
const State& state,
342 const std::vector<std::string> active);
344 template <
class Helper_T>
346 pinocchio::DevicePtr_t device = helper.rootProblem_->robot();
353 fcl::Vec3f
getNormal(
const std::string& effector,
const State& state,
bool& found);
362 #endif // HPP_RBPRM_INTERPOLATION_CONSTRAINTS_HH funEvaluator(const Reference &ref, const fun &method)
Definition: interpolation-constraints.hh:263
std::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
virtual void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:92
constraints::OrientationPtr_t createOrientationMethod(pinocchio::DevicePtr_t device, const fcl::Transform3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:218
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:294
void CreateComConstraint(Helper_T &helper, const Reference &ref, const fcl::Vec3f &initTarget=fcl::Vec3f())
Definition: interpolation-constraints.hh:118
constraints::SymbolicFunction< s_t >::Ptr_t PointComFunctionPtr_t
Definition: interpolation-constraints.hh:115
Definition: interpolation-constraints.hh:258
const Reference ref_
Definition: interpolation-constraints.hh:288
constraints::PositionPtr_t createPositionMethod(pinocchio::DevicePtr_t device, const fcl::Vec3f &initTarget, const pinocchio::Frame effectorFrame)
Definition: interpolation-constraints.hh:202
void operator()(constraints::ImplicitPtr_t eq, const constraints::value_type &normalized_input, pinocchio::ConfigurationOut_t) const
Definition: interpolation-constraints.hh:278
const bool times_ten_
Definition: interpolation-constraints.hh:109
void CreateContactConstraints(const State &from, const State &to)
Definition: time-dependant.hh:50
void Create6DEffectorConstraint(Helper_T &helper, const Reference &ref, const pinocchio::Frame effectorJoint, const fcl::Transform3f &initTarget=fcl::Transform3f())
Definition: interpolation-constraints.hh:317
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:113
Definition: interpolation-constraints.hh:70
const std::pair< core::value_type, core::value_type > timeRange()
Time range of reference path of right hand side.
Definition: interpolation-constraints.hh:265
constraints::PointCom PointCom
Definition: interpolation-constraints.hh:112
const Reference ref_
Reference path of the right hand side of the constraint.
Definition: interpolation-constraints.hh:106
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:289
DevicePtr_t createFreeFlyerDevice()
void CreatePosturalTaskConstraint(Helper_T &helper, const Reference &ref)
Definition: interpolation-constraints.hh:136
~VecRightSide()
Definition: interpolation-constraints.hh:80
const std::size_t dim_
Definition: interpolation-constraints.hh:290
const int dim_
Dimension of the right hand side of the constraint.
Definition: interpolation-constraints.hh:108
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:233
VecRightSide(const Reference ref, const int dim=3, const bool times_ten=false)
Definition: interpolation-constraints.hh:78
constraints::SymbolicFunction< s_t > PointComFunction
Definition: interpolation-constraints.hh:114