Commit 466969fd authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update to modification in hpp-wholebody-step-corba.

parent 0219699f
......@@ -33,47 +33,49 @@ module hpp {
module corbaserver {
module wholebody_step {
interface Problem {
/// Create static stability constraints and store into ProblemSolver
/// \param prefix prefix of the names of the constraint as stored in
/// core::ProblemSolver,
/// \param dofArray input configuration for computing constraint reference,
/// \param leftAnkle, rightAnkle: names of the ankle joints.
/// \param comName name of the PartialCOM in the problem solver map. Put "" for
/// a full COM computations.
///
/// Five constraints are created and store with the following keys by
/// calling method core::ProblemSolver::addNumericalConstraint:
/// \li prefix + "/relative-com": relative position of the center of mass in
/// the left ankle frame,
/// \li prefix + "/relative-orientation": relative orientation of the feet,
/// \li prefix + "/relative-position": relative position of the feet,
/// \li prefix + "/orientation-left-foot": orientation of the left foot,
/// \li prefix + "/position-left-foot": position of the left foot.
void addStaticStabilityConstraints (in string prefix, in dofSeq dofArray,
in string leftAnkle, in string rightAnkle,
in string comName)
raises (Error);
/// Create constraints that ensure that the COM is between the two ankles
/// and store into ProblemSolver
/// Type of static stability constraints.
enum StaticStabilityType {
/// Corresponds to a robot sliding on the floor, the relative position
/// ot the feet being constant. It contains five following constraints:
/// \li prefix + "/relative-com": relative position of the center of
/// mass in the left ankle frame,
/// \li prefix + "/relative-orientation": relative orientation of the
/// feet,
/// \li prefix + "/relative-position": relative position of the feet,
/// \li prefix + "/orientation-left-foot": orientation of the left foot,
/// \li prefix + "/position-left-foot": position of the left foot.
SLIDING,
/// Corresponds to a robot standing on two feets, the COM between the
/// the feet. The feet are constrained to stay on the floor, parallel to
/// it. It contains five following constraints:
/// \li prefix + "/com-between-feet": The COM projection along z lies
/// between the two ankles
/// \li prefix + "/orientation-right": orientation of the right foot,
/// \li prefix + "/position-right": position of the right foot,
/// \li prefix + "/orientation-left": orientation of the left foot,
/// \li prefix + "/position-left": position of the left foot.
ALIGNED_COM
};
/// Create static stability constraints and store into ProblemSolver
/// \param prefix prefix of the names of the constraint as stored in
/// core::ProblemSolver,
/// \param dofArray input configuration for computing constraint reference,
/// \param leftAnkle, rightAnkle: names of the ankle joints.
/// \param comName name of the PartialCOM in the problem solver map. Put "" for
/// a full COM computations.
/// \param type type of static stability constraints you wish to create.
///
/// Five constraints are created and store with the following keys by
/// calling method core::ProblemSolver::addNumericalConstraint:
/// \li prefix + "/com-between-feet": The COM projection along z lies between the
/// the two ankles
/// \li prefix + "/orientation-right": orientation of the right foot,
/// \li prefix + "/position-right": position of the right foot,
/// \li prefix + "/orientation-left": orientation of the left foot,
/// \li prefix + "/position-left": position of the left foot.
void addStabilityConstraints (in string prefix, in dofSeq dofArray,
in string leftAnkle, in string rightAnkle,
in string comName)
/// The constraints are stored in the core::ProblemSolver constraints map
/// and are accessible through the method
/// core::ProblemSolver::addNumericalConstraint:
void addStaticStabilityConstraints (in string prefix,
in dofSeq dofArray,
in string leftAnkle,
in string rightAnkle,
in string comName,
in StaticStabilityType type)
raises (Error);
/// Generate a goal configuration.
......
......@@ -29,10 +29,73 @@
namespace hpp {
namespace wholebodyStep {
using hpp::wholebodyStep::createSlidingStabilityConstraint;
using hpp::wholebodyStep::createStabilityConstraint;
using hpp::wholebodyStep::createAlignedCOMStabilityConstraint;
using hpp::core::ConstraintSetPtr_t;
using hpp::core::ConfigProjectorPtr_t;
using hpp::model::CenterOfMassComputation;
namespace impl {
namespace {
typedef std::pair <std::string, NumericalConstraintPtr_t>
NamedConstraint_t;
typedef std::list <NamedConstraint_t> NamedConstraints_t;
NamedConstraints_t createSliding (const DevicePtr_t robot,
const char* prefix, const ConfigurationPtr_t& config,
const JointPtr_t leftAnkle, const JointPtr_t rightAnkle,
const CenterOfMassComputationPtr_t comc)
{
NamedConstraints_t constraints;
std::vector <NumericalConstraintPtr_t> numericalConstraints =
createSlidingStabilityConstraint (robot, comc,
leftAnkle, rightAnkle, *config);
std::string p (prefix);
constraints.push_back (NamedConstraint_t
(p + std::string ("/relative-com"),
numericalConstraints [0]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/relative-orientation"),
numericalConstraints [1]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/relative-position"),
numericalConstraints [2]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/orientation-left-foot"),
numericalConstraints [3]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/position-left-foot"),
numericalConstraints [4]));
return constraints;
}
NamedConstraints_t createAlignedCOM (const DevicePtr_t robot,
const char* prefix, const ConfigurationPtr_t& config,
const JointPtr_t leftAnkle, const JointPtr_t rightAnkle,
const CenterOfMassComputationPtr_t comc)
{
NamedConstraints_t constraints;
std::vector <NumericalConstraintPtr_t> numericalConstraints =
createSlidingStabilityConstraint (robot, comc,
leftAnkle, rightAnkle, *config);
std::string p (prefix);
constraints.push_back (NamedConstraint_t
(p + std::string ("/com-between-feet"),
numericalConstraints [0]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/orientation-right"),
numericalConstraints [1]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/position-right"),
numericalConstraints [2]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/orientation-left"),
numericalConstraints [3]));
constraints.push_back (NamedConstraint_t
(p + std::string ("/position-left"),
numericalConstraints [4]));
return constraints;
}
}
static ConfigurationPtr_t dofSeqToConfig
(ProblemSolverPtr_t problemSolver, const hpp::dofSeq& dofArray)
......@@ -70,7 +133,8 @@ namespace hpp {
void Problem::addStaticStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
const char* leftAnkle, const char* rightAnkle, const char* comName)
const char* leftAnkle, const char* rightAnkle, const char* comName,
const StaticStabilityType type)
throw (hpp::Error)
{
using model::CenterOfMassComputationPtr_t;
......@@ -85,91 +149,36 @@ namespace hpp {
JointPtr_t la = robot->getJointByName (leftAnkle);
JointPtr_t ra = robot->getJointByName (rightAnkle);
std::string comN (comName);
std::vector <DifferentiableFunctionPtr_t> numericalConstraints;
if (comN.compare ("") == 0)
numericalConstraints =
createSlidingStabilityConstraint (robot, la, ra, *config);
else {
CenterOfMassComputationPtr_t comc =
problemSolver_->centerOfMassComputation (comN);
if (!comc)
throw Error ("This CenterOfMassComputation does not exist");
numericalConstraints = createSlidingStabilityConstraint (robot,
comc, la, ra, *config);
}
std::string p (prefix);
problemSolver_->addNumericalConstraint
(p + std::string ("/relative-com"), numericalConstraints [0]);
problemSolver_->addNumericalConstraint
(p + std::string ("/relative-orientation"),
numericalConstraints [1]);
problemSolver_->addNumericalConstraint
(p + std::string ("/relative-position"), numericalConstraints [2]);
problemSolver_->addNumericalConstraint
(p + std::string ("/orientation-left-foot"),
numericalConstraints [3]);
problemSolver_->addNumericalConstraint
(p + std::string ("/position-left-foot"),
numericalConstraints [4]);
} catch (const std::exception& exc) {
throw Error (exc.what ());
}
}
void Problem::addStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
const char* leftAnkle, const char* rightAnkle, const char* comName)
throw (hpp::Error)
{
using model::CenterOfMassComputationPtr_t;
using core::DifferentiableFunctionPtr_t;
try {
ConfigurationPtr_t config = dofSeqToConfig (problemSolver_, dofArray);
const DevicePtr_t& robot (problemSolver_->robot ());
if (!robot) {
throw Error ("You should set the robot before defining"
" constraints.");
}
JointPtr_t la = robot->getJointByName (leftAnkle);
JointPtr_t ra = robot->getJointByName (rightAnkle);
std::string comN (comName);
std::vector <core::NumericalConstraintPtr_t> numericalConstraints;
std::vector <DifferentiableFunctionPtr_t> numericalConstraints;
CenterOfMassComputationPtr_t comc;
if (comN.compare ("") == 0) {
model::CenterOfMassComputationPtr_t comc =
model::CenterOfMassComputation::create (robot);
comc = CenterOfMassComputation::create (robot);
comc->add (robot->rootJoint ());
comc->computeMass ();
numericalConstraints =
createStabilityConstraint (robot, comc, la, ra, *config);
} else {
CenterOfMassComputationPtr_t comc =
problemSolver_->centerOfMassComputation (comN);
comc = problemSolver_->centerOfMassComputation (comN);
if (!comc)
throw Error ("This CenterOfMassComputation does not exist");
numericalConstraints = createStabilityConstraint (robot,
comc, la, ra, *config);
}
std::string p (prefix);
problemSolver_->addNumericalConstraint
(p + std::string ("/com-between-feet"), numericalConstraints [0]->functionPtr ());
problemSolver_->comparisonType
(p + std::string ("/com-between-feet"), numericalConstraints [0]->comparisonType ());
problemSolver_->addNumericalConstraint
(p + std::string ("/orientation-right"), numericalConstraints [1]->functionPtr ());
problemSolver_->comparisonType
(p + std::string ("/orientation-right"), numericalConstraints [1]->comparisonType ());
problemSolver_->addNumericalConstraint
(p + std::string ("/orientation-left"), numericalConstraints [2]->functionPtr ());
problemSolver_->comparisonType
(p + std::string ("/orientation-left"), numericalConstraints [2]->comparisonType ());
problemSolver_->addNumericalConstraint
(p + std::string ("/position-right"), numericalConstraints [3]->functionPtr ());
problemSolver_->comparisonType
(p + std::string ("/position-right"), numericalConstraints [3]->comparisonType ());
problemSolver_->addNumericalConstraint
(p + std::string ("/position-left"), numericalConstraints [4]->functionPtr ());
problemSolver_->comparisonType
(p + std::string ("/position-left"), numericalConstraints [4]->comparisonType ());
NamedConstraints_t nc;
switch ( type ) {
case hpp::corbaserver::wholebody_step::Problem::SLIDING:
nc = createSliding (robot, prefix, config, la, ra, comc);
break;
case hpp::corbaserver::wholebody_step::Problem::ALIGNED_COM:
nc = createAlignedCOM (robot, prefix, config, la, ra, comc);
break;
default:
throw Error ("Unkown StatticStability type.");
break;
}
for (NamedConstraints_t::const_iterator it = nc.begin ();
it != nc.end (); ++it) {
problemSolver_->addNumericalConstraint
(it->first, it->second->functionPtr ());
problemSolver_->comparisonType
(it->first, it->second->comparisonType ());
}
} catch (const std::exception& exc) {
throw Error (exc.what ());
}
......
......@@ -32,20 +32,24 @@ namespace hpp {
POA_hpp::corbaserver::wholebody_step::Problem
{
public:
typedef hpp::corbaserver::wholebody_step::Problem::StaticStabilityType
StaticStabilityType;
Problem ();
void setProblemSolver (const ProblemSolverPtr_t& problemSolver);
virtual void addStaticStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
const char* leftAnkle, const char* rightAnkle, const char* comName)
throw (hpp::Error);
virtual void addStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
const char* leftAnkle, const char* rightAnkle, const char* comName)
const char* leftAnkle, const char* rightAnkle, const char* comName,
const StaticStabilityType type)
throw (hpp::Error);
virtual void
generateGoalConfig (CORBA::Double x, CORBA::Double y, CORBA::Double z,
CORBA::UShort nbConfig)
throw (hpp::Error);
private:
core::ProblemSolverPtr_t problemSolver_;
}; // class WholebodyStep
......
Supports Markdown
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