Skip to content

GitLab

Unverified Commit 13561a10 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #18 from florent-lamiraux/devel 

Update to modifications in hpp-constraints
parents 687efefe b053f2c9
Hide whitespace changes
Inline Side-by-side
src/small-steps.cc
View file @ 13561a10
......@@ -114,8 +114,7 @@ namespace hpp {
// Position only
func = PointInJointFunction::create
("point-hand-walkgen", robot, PointInJoint::create (joint, vector3_t (0,0,0)));
eq = constraints::Implicit::create (func,
constraints::ComparisonTypes_t(3, constraints::Equality));
eq = constraints::Implicit::create (func, 3 * constraints::Equality);
TD = TimeDependant (eq, RightHandSideFunctorPtr_t
(new ReproducePath (func, path, param))
);
......@@ -429,8 +428,6 @@ namespace hpp {
assert (robot_);
const TimeToParameterMap_t& TTP = param.pairs_;
constraints::ComparisonTypes_t equals (3, constraints::Equality);
// Create the time varying equation for COM
pinocchio::CenterOfMassComputationPtr_t comComp = pinocchio::CenterOfMassComputation::
create (robot_);
......@@ -438,21 +435,22 @@ namespace hpp {
PointComFunctionPtr_t comFunc = PointComFunction::create ("COM-walkgen",
robot_, PointCom::create (comComp));
constraints::ImplicitPtr_t comEq = constraints::Implicit::create
(comFunc, equals);
(comFunc, 3 * constraints::Equality);
TimeDependant comEqTD (comEq, RightHandSideFunctorPtr_t (new CubicBSplineToCom (com, comHeight)));
// Create an time varying equation for each foot.
equals.resize (6, constraints::Equality);
JointFrameFunctionPtr_t leftFunc = JointFrameFunction::create ("left-foot-walkgen",
robot_, JointFrame::create (robot_->leftAnkle ()));
constraints::ImplicitPtr_t leftEq = constraints::Implicit::create (leftFunc, equals);
constraints::ImplicitPtr_t leftEq = constraints::Implicit::create
(leftFunc, 6 * constraints::Equality);
TimeDependant leftEqTD (leftEq, RightHandSideFunctorPtr_t
(new FootPathToFootPos (pg_->leftFoot (), pg_->leftFootTrajectory (), ankleShift))
);
JointFrameFunctionPtr_t rightFunc = JointFrameFunction::create ("right-foot-walkgen",
robot_, JointFrame::create (robot_->rightAnkle ()));
constraints::ImplicitPtr_t rightEq = constraints::Implicit::create (rightFunc, equals);
constraints::ImplicitPtr_t rightEq = constraints::Implicit::create
(rightFunc, 6 * constraints::Equality);
TimeDependant rightEqTD (rightEq, RightHandSideFunctorPtr_t
(new FootPathToFootPos (pg_->rightFoot (), pg_->rightFootTrajectory (), ankleShift))
);
......
src/static-stability-constraint.cc
View file @ 13561a10
......@@ -92,12 +92,14 @@ namespace hpp {
matrix3_t R1T (M1.rotation ().transpose());
vector3_t xloc = R1T * (x - M1.translation ());
result.push_back (Implicit::create (RelativeCom::create
(robot, comc, joint1, xloc)));
(robot, comc, joint1, xloc),
3 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// Relative orientation of the feet
matrix3_t reference = R1T * M2.rotation ();
result.push_back(Implicit::create (RelativeOrientation::create
("Feet relative orientation", robot, joint1, joint2, toSE3(reference))));
("Feet relative orientation", robot, joint1, joint2,
toSE3(reference)), 3 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// Relative position of the feet
vector3_t local1; local1.setZero ();
......@@ -107,17 +109,21 @@ namespace hpp {
matrix3_t R2T (M2.rotation ().transpose ());
vector3_t local2 = R2T * (global1 - M2.translation ());
result.push_back (Implicit::create (RelativePosition::create
("Feet relative position", robot, joint1, joint2, toSE3(local1), toSE3(local2))));
("Feet relative position", robot, joint1, joint2,
toSE3(local1), toSE3(local2)),
3 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// Orientation of the left foot
result.push_back (Implicit::create (Orientation::create
("Left foot rx/ry orientation", robot, joint1, MId,
list_of (true)(true)(false).convert_to_container<BoolVector_t>())));
list_of (true)(true)(false).convert_to_container<BoolVector_t>()),
2*constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// Position of the left foot
result.push_back (Implicit::create (Position::create
("Left foot z position", robot, joint1, MId, toSE3(M1.translation ()),
list_of (false)(false)(true).convert_to_container<BoolVector_t>())));
("Left foot z position", robot, joint1, MId, toSE3(M1.translation ()),
list_of (false)(false)(true).convert_to_container<BoolVector_t>()),
1 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
return result;
}
......@@ -136,13 +142,13 @@ namespace hpp {
result.push_back (Implicit::create (Orientation::create
("Left foot rz orientation", robot, joint1, MId,
list_of (false)(false)(true).convert_to_container<BoolVector_t>()),
ComparisonTypes_t(1, constraints::Equality)));
1 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// Position of the left foot
result.push_back (Implicit::create (Position::create
("Left foot xy position", robot, joint1, MId, toSE3(M1.translation()),
list_of (true)(true)(false).convert_to_container<BoolVector_t>()),
ComparisonTypes_t(2, constraints::Equality)));
2 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
return result;
}
......@@ -170,17 +176,20 @@ namespace hpp {
matrix3_t R1T (M1.rotation ().transpose ());
vector3_t xloc = R1T * (x - M1.translation ());
result.push_back (Implicit::create (RelativeCom::create
(robot, comc, joint1, xloc)));
(robot, comc, joint1, xloc),
3 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// pose of the left foot
result.push_back (Implicit::create
(constraints::Transformation::create
("Left foot pose", robot, joint1, M1)));
("Left foot pose", robot, joint1, M1),
6 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
// pose of the right foot
result.push_back (Implicit::create
(constraints::Transformation::create
("Right foot pose", robot, joint2, M2)));
("Right foot pose", robot, joint2, M2),
6 * constraints::EqualToZero));
result.back ()->function ().context (STABILITY_CONTEXT);
return result;
}
......@@ -205,9 +214,8 @@ namespace hpp {
// matrix3_t R1T (M1.rotation ().transpose ());
const vector3_t& x = comc->com ();
// position of center of mass in left ankle frame
ComparisonTypes_t comps = list_of
(constraints::EqualToZero) (constraints::EqualToZero)
(constraints::Superior) (constraints::Inferior);
ComparisonTypes_t comps(2 * constraints::EqualToZero
<< constraints::Superior << constraints::Inferior);
nm = Implicit::create (
ComBetweenFeet::create ("ComBetweenFeet", robot, comc,
joint1, joint2, zero, zero, robot->rootJoint (), x,
......@@ -223,13 +231,14 @@ namespace hpp {
// Pose of the right foot
nm = Implicit::create
(constraints::Transformation::create
("Right foot pose", robot, joint2, M2, mask));
("Right foot pose", robot, joint2, M2, mask),
3 * constraints::EqualToZero);
result.push_back(nm);
result.back ()->function ().context (STABILITY_CONTEXT);
// Pose of the left foot
nm = Implicit::create
(constraints::Transformation::create
("Right pose", robot, joint1, M1, mask));
("Right pose", robot, joint1, M1, mask), 3 * constraints::EqualToZero);
result.push_back(nm);
result.back ()->function ().context (STABILITY_CONTEXT);
return result;
......
tests/romeo-small.cc
View file @ 13561a10
......@@ -55,6 +55,7 @@ using hpp::core::ConfigProjector;
using hpp::core::ConfigProjectorPtr_t;
using hpp::constraints::Implicit;
using hpp::constraints::ImplicitPtr_t;
using hpp::constraints::Equality;
using hpp::constraints::solver::BySubstitution;
using hpp::constraints::solver::lineSearch::FixedSequence;
using hpp::constraints::solver::lineSearch::Constant;
......@@ -131,8 +132,7 @@ void constraints_check (LineSearchFactory factory)
for (std::size_t i = 0; i < 6; i++) mask[i] = false;
ImplicitPtr_t cc = Implicit::create
(hpp::constraints::ConfigurationConstraint::create
("Optimization constraint", hrp2, half_sitting, mask)
);
("Optimization constraint", hrp2, half_sitting, mask), 1 * Equality);
cc->function().context ("optimization");
ConfigProjectorPtr_t projOpt = HPP_STATIC_PTR_CAST (ConfigProjector,
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment