Unverified Commit 10061c3c authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #12 from pFernbach/topic/kino

Topic/kino
parents f7d8e9aa 50388a4a
......@@ -23,6 +23,10 @@
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
# include <hpp/rbprm/rbprm-validation-report.hh>
namespace hpp {
namespace pinocchio {
class RbPrmDevice; //fwd declaration of rbprmDevice class
typedef boost::shared_ptr <RbPrmDevice> RbPrmDevicePtr_t;
}
namespace rbprm {
......@@ -52,7 +56,7 @@ namespace hpp {
class DynamicValidation : public core::ConfigValidation
{
public:
static DynamicValidationPtr_t create (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu);
static DynamicValidationPtr_t create (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu,core::DevicePtr_t robot);
/// Compute whether the configuration is valid
......@@ -69,13 +73,14 @@ namespace hpp {
protected:
DynamicValidation (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu);
DynamicValidation (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu,core::DevicePtr_t robot);
private:
bool rectangularContact_;
double sizeFootX_;
double sizeFootY_;
double mass_;
double mu_;
pinocchio::RbPrmDevicePtr_t robot_;
centroidal_dynamics::Equilibrium* sEq_;
core::RbprmValidationReportPtr_t lastReport_;
bool initContacts_;
......
......@@ -7,6 +7,10 @@
namespace hpp {
namespace pinocchio {
class RbPrmDevice; //fwd declaration of rbprmDevice class
typedef boost::shared_ptr <RbPrmDevice> RbPrmDevicePtr_t;
}
namespace core {
HPP_PREDEF_CLASS (RbprmNode);
......@@ -82,9 +86,9 @@ namespace hpp {
size_type getNumberOfContacts(){return numberOfContacts_;}
void fillNodeMatrices(ValidationReportPtr_t report,bool rectangularContact, double sizeFootx, double sizeFooty, double m,double mu);
void fillNodeMatrices(ValidationReportPtr_t report,bool rectangularContact, double sizeFootx, double sizeFooty, double m,double mu,pinocchio::RbPrmDevicePtr_t device);
void chooseBestContactSurface(ValidationReportPtr_t report,std::map<std::string,fcl::Vec3f> rom_ref_endEffector );
void chooseBestContactSurface(ValidationReportPtr_t report, hpp::pinocchio::RbPrmDevicePtr_t device );
Eigen::Quaterniond getQuaternion();
......
......@@ -70,6 +70,21 @@ namespace hpp {
virtual void setDimensionExtraConfigSpace (const size_type& dimension);
///
/// \brief setEffectorReference set a 3D position reference for the end effector of the given ROM
/// \param romName
/// \param ref
///
virtual void setEffectorReference(std::string romName, vector3_t ref);
///
/// \brief getEffectorReference get the reference position of the given ROM, return (0,0,0) if the reference was never set
/// \param romName
/// \return
///
virtual vector3_t getEffectorReference(std::string romName);
public:
/// Range Of Motion of the robot
const T_Rom robotRoms_;
......@@ -83,6 +98,7 @@ namespace hpp {
void init (const RbPrmDeviceWkPtr_t& weakPtr);
private:
std::map<std::string, vector3_t> effectorsReferences_;
RbPrmDeviceWkPtr_t weakPtr_;
}; // class RbPrmDevice
} // namespace rbprm
......
......@@ -80,6 +80,9 @@ namespace hpp {
/// [z_inf, z_sup, y_inf, y_sup, x_inf, x_sup]
void BoundSO3(const std::vector<double>& limitszyx);
void sampleExtraDOF(bool sampleExtraDOF);
public:
typedef std::pair<fcl::Vec3f, TrianglePoints> T_TriangleNormal;
......@@ -105,12 +108,15 @@ namespace hpp {
void InitWeightedTriangles (const core::ObjectStdVector_t &geometries);
const T_TriangleNormal& RandomPointIntriangle () const;
const T_TriangleNormal& WeightedTriangle () const;
void randConfigAtPos(const pinocchio::RbPrmDevicePtr_t robot, const std::vector<double>& eulerSo3, core::ConfigurationPtr_t config, const fcl::Vec3f p) const;
private:
std::vector<double> weights_;
std::vector<T_TriangleNormal> triangles_;
const pinocchio::RbPrmDevicePtr_t robot_;
rbprm::RbPrmValidationPtr_t validator_;
core::configurationShooter::UniformPtr_t uniformShooter_;
RbPrmShooterWkPtr_t weak_;
//pinocchio::DevicePtr_t eulerSo3_;
std::vector<double> eulerSo3_;
......
......@@ -152,8 +152,9 @@ namespace hpp {
/// \brief set if the collision validation should compute all the possible
/// contacts or stop after the first pairs in collision
/// This method set the parameter for all the romValidations_ objects (but not the trunk)
///
// void computeAllContacts(bool computeAllContacts);
void computeAllContacts(bool computeAllContacts);
public:
......
......@@ -23,7 +23,6 @@
# include <hpp/pinocchio/joint.hh>
# include <hpp/rbprm/config.hh>
# include <hpp/pinocchio/collision-object.hh>
# include <hpp/pinocchio/device-object-vector.hh>
# include <hpp/pinocchio/frame.hh>
# include <Eigen/Core>
......
......@@ -47,7 +47,7 @@ namespace geom
typedef fcl::BVHModel<fcl::OBBRSS> BVHModelOB;
typedef boost::shared_ptr<const BVHModelOB> BVHModelOBConst_Ptr_t;
BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object);
BVHModelOBConst_Ptr_t GetModel(const hpp::pinocchio::CollisionObjectConstPtr_t object, hpp::pinocchio::DeviceData &deviceData);
void projectZ(IT_Point pointsBegin, IT_Point pointsEnd);
......@@ -64,25 +64,19 @@ namespace geom
/// Test whether a 2d point belongs to a 2d convex hull
/// source http://softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm#wn_PinPolygon()
///
/// \param pointsBegin, pointsEnd iterators to first and last points
/// of the convex hull.ATTENTION: first point is included twice in
/// representation (it is also the last point).
/// \param pointsBegin, list of points of the convex hull.
/// ATTENTION: first point is included twice in representation (it is also the last point).
/// \param aPoint The point for which to test belonging the the convex hull
/// \return whether aPoint belongs to the convex polygon determined as the convex hull of the rectangle indicated
/* template<int Dim=3, typename Numeric=double, typename Point=Eigen::Matrix<Numeric, Dim, 1>,
typename Point2=Eigen::Matrix<Numeric, 2, 1>,
typename CPointRef= const Eigen::Ref<const Point>&, typename In>
bool containsHull(In pointsBegin, In pointsEnd, CPointRef aPoint, const Numeric Epsilon = 10e-6);
*/
bool containsHull(T_Point hull, CPointRef aPoint, const double epsilon = 10e-6);
/// Test whether a 2d point belongs to a 2d convex hull determined by a list of unordered points
///
/// \param pointsBegin, pointsEnd iterators to first and last points for which to consider the convex hull
/// \param points list of points
/// \param aPoint The point for which to test belonging the the convex hull
/// \return whether aPoint belongs to the convex polygon determined as the convex hull of the rectangle indicated
/* template<typename T, int Dim=3, typename Numeric=double, typename Point=Eigen::Matrix<Numeric, Dim, 1>,
typename CPointRef= const Eigen::Ref<const Point>&, typename In>
bool contains(In pointsBegin, In pointsEnd, const CPointRef& aPoint);
*/
bool contains(T_Point points, CPointRef aPoint, const double epsilon = 10e-6);
/// Computes whether two convex polygons intersect
///
/// \param aPointsBegin, aPointsEnd iterators to first and last points of the first polygon
......@@ -157,7 +151,37 @@ namespace geom
* @return distance
*/
double distanceToPlane(const fcl::Vec3f& n, double t, const fcl::Vec3f& v);
/**
* @brief distanceToPlane compute the min distance from a point to an (infinite) plan
* @param point the point
* @param Pn normal of the plan
* @param P0 a point in the plan
* @return
*/
double distanceToPlane(CPointRef point,CPointRef Pn,CPointRef P0);
/**
* @brief projectPointOnPlane othrogonal projection of a given point on the plan
* @param point point to project
* @param Pn normal of the plan
* @param P0 a point in the plan
* @return the orthogonal projection of the point in the plane
*/
Point projectPointOnPlane(CPointRef point,CPointRef Pn,CPointRef P0);
/**
* @brief projectPointInsidePlan project a point inside on the given plan, inside the given convex hull
* @param plan convex hull of points
* @param point original point
* @param Pn normal of the plan
* @param P0 a point in the plan
* @param res the resulting projection
* @return the distance between the original point and the projection
*/
double projectPointInsidePlan(T_Point plan, CPointRef point, CPointRef Pn, CPointRef P0,Eigen::Ref<Point> res);
/**
* @brief computeTrianglePlaneDistance compute distance between each vertice of the triangle and a plane
* @param tri_point
......@@ -219,15 +243,23 @@ namespace geom
T_Point intersectSegmentPlane(Point s0, Point s1, Eigen::Vector3d pn, Point p0 );
/**
* @brief intersectPolygonePlane compute the intersection between a polygone and a plane
* @brief intersectPolygonePlane compute the intersection between a polygone and an (infinite) plane
* @param polygone
* @param plane
* @param plane a model which first triangle will define the infinite plan used
* @param pn : output, normal of the plan
* @return an ordoned list of point (clockwise), which belong to both the polygone and the plane
*/
T_Point intersectPolygonePlane(BVHModelOBConst_Ptr_t polygone, BVHModelOBConst_Ptr_t plane, Eigen::Ref<Point> Pn);
T_Point convertBVH(BVHModelOBConst_Ptr_t obj);
/**
* @brief computePlanEquation compute a plan normal and a point in the plan from the first triangle of a BVHModel
* @param plane
* @param Pn the normal of the plan
* @param P0 a point in the plan
*/
void computePlanEquation( BVHModelOBConst_Ptr_t plane,Eigen::Ref<Point> Pn,Eigen::Ref<Point> P0);
} //namespace geom
......
......@@ -518,7 +518,10 @@ Result isReachableDynamic(const RbPrmFullBodyPtr_t& fullbody, State &previous, S
pData.contacts_.push_back(midData);
}
pData.contacts_.push_back(nextData);
//pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC | bezier_com_traj::END_VEL | bezier_com_traj::END_POS;
pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::INIT_ACC | bezier_com_traj::END_ACC | bezier_com_traj::END_VEL | bezier_com_traj::END_POS;
pData.constraints_.constraintAcceleration_=true;
pData.constraints_.maxAcceleration_=10.;
pData.constraints_.reduce_h_ = 1e-3;
// set init/goal values :
pData.c0_ = com_previous;
pData.c1_ = com_next;
......
......@@ -21,14 +21,14 @@
# include <hpp/rbprm/planner/rbprm-node.hh>
#include <hpp/util/timer.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/rbprm/rbprm-device.hh>
namespace hpp {
namespace rbprm {
DynamicValidationPtr_t DynamicValidation::create
(bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu)
(bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu, core::DevicePtr_t robot)
{
DynamicValidation* ptr = new DynamicValidation (rectangularContact,sizeFootX,sizeFootY,mass,mu);
DynamicValidation* ptr = new DynamicValidation (rectangularContact,sizeFootX,sizeFootY,mass,mu,robot);
return DynamicValidationPtr_t (ptr);
}
......@@ -97,7 +97,7 @@ namespace hpp {
lastReport_=rbReport;
core::ConfigurationPtr_t q = core::ConfigurationPtr_t (new core::Configuration_t(config));
core::RbprmNode node(q);
node.fillNodeMatrices(rbReport,rectangularContact_,sizeFootX_,sizeFootY_,mass_,mu_);
node.fillNodeMatrices(rbReport,rectangularContact_,sizeFootX_,sizeFootY_,mass_,mu_,robot_);
sEq_->setG(node.getG());
h_=node.geth();
H_=node.getH();
......@@ -128,10 +128,11 @@ namespace hpp {
DynamicValidation::DynamicValidation (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu) :
rectangularContact_(rectangularContact),sizeFootX_(sizeFootX),sizeFootY_(sizeFootY),mass_(mass),mu_(mu),
DynamicValidation::DynamicValidation (bool rectangularContact, double sizeFootX, double sizeFootY, double mass, double mu,core::DevicePtr_t robot) :
rectangularContact_(rectangularContact),sizeFootX_(sizeFootX),sizeFootY_(sizeFootY),mass_(mass),mu_(mu),robot_(boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(robot)),
sEq_(new centroidal_dynamics::Equilibrium("dynamic_val", mass,4,centroidal_dynamics::SOLVER_LP_QPOASES,true,10,false))
{
assert(robot_ && "Error in dynamic cast of problem device to rbprmDevice");
hppDout(info,"Dynamic validation created with attribut : rectangular contact = "<<rectangularContact<<" size foot : "<<sizeFootX);
hppDout(info,"mass = "<<mass<<" mu = "<<mu);
}
......
......@@ -93,32 +93,20 @@ namespace hpp {
{
assert(sm_ && "steering method should be a kinodynamic steering method for this solver");
assert(rbprmPathValidation_ && "Path validation should be a RbPrmPathValidation class for this solver");
assert(problem.robot()->mass() > 0. && "When using dynamic planner, the robot mass should be correctly defined.");
hppDout(notice,"number of affordances objects : "<<problem.collisionObstacles().size());
try {
sizeFootX_ = problem.getParameter ("sizeFootX").floatValue()/2.;
sizeFootY_ = problem.getParameter ("sizeFootY").floatValue()/2.;
sizeFootX_ = problem.getParameter (std::string("DynamicPlanner/sizeFootX")).floatValue()/2.;
sizeFootY_ = problem.getParameter (std::string("DynamicPlanner/sizeFootY")).floatValue()/2.;
if(sizeFootX_ > 0. && sizeFootY_ > 0.)
rectangularContact_ = 1;
} catch (const std::exception& e) {
hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
sizeFootX_ =0;
sizeFootY_ =0;
else
rectangularContact_ = 0;
}
try {
tryJump_ = problem.getParameter ("tryJump").boolValue();
} catch (const std::exception& e) {
tryJump_=false;
}
hppDout(notice,"tryJump in steering method = "<<tryJump_);
try {
mu_ = problem.getParameter ("friction").floatValue();
hppDout(notice,"mu define in python : "<<mu_);
} catch (const std::exception& e) {
mu_= 0.5;
hppDout(notice,"mu not defined, take : "<<mu_<<" as default.");
}
tryJump_ = problem.getParameter (std::string("DynamicPlanner/tryJump")).boolValue();
hppDout(notice,"tryJump in dynamic planner = "<<tryJump_);
mu_ = problem.getParameter (std::string("DynamicPlanner/friction")).floatValue();
hppDout(notice,"mu define in python : "<<mu_);
// create the map of end effector reference position in root frame
pinocchio::RbPrmDevicePtr_t rbDevice = boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem.robot());
......@@ -150,32 +138,21 @@ namespace hpp {
{
assert(sm_ && "steering method should be a kinodynamic steering method for this solver");
assert(rbprmPathValidation_ && "Path validation should be a RbPrmPathValidation class for this solver");
assert(problem.robot()->mass() > 0. && "When using dynamic planner, the robot mass should be correctly defined.");
hppDout(notice,"number of affordances objects : "<<problem.collisionObstacles().size());
try {
sizeFootX_ = problem.getParameter ("sizeFootX").floatValue()/2.;
sizeFootY_ = problem.getParameter ("sizeFootY").floatValue()/2.;
sizeFootX_ = problem.getParameter (std::string("DynamicPlanner/sizeFootX")).floatValue()/2.;
sizeFootY_ = problem.getParameter (std::string("DynamicPlanner/sizeFootY")).floatValue()/2.;
if(sizeFootX_ > 0. && sizeFootY_ > 0.)
rectangularContact_ = 1;
} catch (const std::exception& e) {
hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
sizeFootX_ =0;
sizeFootY_ =0;
else
rectangularContact_ = 0;
}
try {
tryJump_ = problem.getParameter ("tryJump").boolValue();
} catch (const std::exception& e) {
tryJump_=false;
}
hppDout(notice,"tryJump in steering method = "<<tryJump_);
try {
mu_ = problem.getParameter ("friction").floatValue();
hppDout(notice,"mu define in python : "<<mu_);
} catch (const std::exception& e) {
mu_= 0.5;
hppDout(notice,"mu not defined, take : "<<mu_<<" as default.");
}
tryJump_ = problem.getParameter (std::string("DynamicPlanner/tryJump")).boolValue();
hppDout(notice,"tryJump in dynamic planner = "<<tryJump_);
mu_ = problem.getParameter (std::string("DynamicPlanner/friction")).floatValue();
hppDout(notice,"mu define in python : "<<mu_);
// create the map of end effector reference position in root frame
pinocchio::RbPrmDevicePtr_t rbDevice = boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem.robot());
......@@ -454,11 +431,12 @@ namespace hpp {
// (because only the first one in collision is considered by fcl and put in the report)
rbprmPathValidation_->getValidator()->randomnizeCollisionPairs(); // FIXME : remove if we compute all collision pairs
rbprmPathValidation_->getValidator()->computeAllContacts(true);
hppDout(notice,"Compute GIWC, call validate for configuration : "<<pinocchio::displayConfig(*(x->configuration())));
problem().configValidations()->validate(*(x->configuration()),report);
rbprmPathValidation_->getValidator()->computeAllContacts(false);
if(use_bestReport){
core::RbprmNodePtr_t node = static_cast<core::RbprmNodePtr_t>(x);
node->chooseBestContactSurface(report,rom_ref_endEffector_);
node->chooseBestContactSurface(report,boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem().robot()));
}
computeGIWC(x,report);
}
......@@ -477,10 +455,7 @@ namespace hpp {
}
hppDout(info,"~~ q = "<<displayConfig(*q));
node->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_);
node->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_,boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem().robot()));
}// computeGIWC
......@@ -563,11 +538,11 @@ namespace hpp {
core::PathVectorPtr_t DynamicPlanner::finishSolve (const core::PathVectorPtr_t& path){
std::cout<<"total_path_computed = "<<sm_->totalTimeComputed_<<std::endl;
/*std::cout<<"total_path_computed = "<<sm_->totalTimeComputed_<<std::endl;
std::cout<<"total_path_validated = "<<sm_->totalTimeValidated_<<std::endl;
std::cout<<"percentage validated path ="<<((sm_->totalTimeValidated_)/(sm_->totalTimeComputed_))*100.<<std::endl;
std::cout<<"rejected_paths = "<<sm_->rejectedPath_<<std::endl;
*/
/*
std::ofstream myfile;
myfile.open ("/local/dev_hpp/benchs/benchHyq_darpa.txt", std::ios::out | std::ios::app );
......@@ -585,6 +560,25 @@ namespace hpp {
return path;
}
} // namespace core
HPP_START_PARAMETER_DECLARATION(Kinodynamic)
Problem::declareParameter(core::ParameterDescription (core::Parameter::FLOAT,
"DynamicPlanner/sizeFootX",
"The lenght of the feet along X axis (assuming rectangular feet).",
core::Parameter(0.)));
Problem::declareParameter(core::ParameterDescription (core::Parameter::FLOAT,
"DynamicPlanner/sizeFootY",
"The lenght of the feet along Y axis (assuming rectangular feet).",
core::Parameter(0.)));
Problem::declareParameter(core::ParameterDescription (core::Parameter::FLOAT,
"DynamicPlanner/friction",
"Value of the friction coefficient between the feet of the robot and the ground.",
core::Parameter(0.5)));
Problem::declareParameter(core::ParameterDescription (core::Parameter::BOOL,
"DynamicPlanner/tryJump",
"If True, when a trajectory is invalid because all the rom leave the contact, a ballistic motion is tried to connect both states",
core::Parameter(false)));
HPP_END_PARAMETER_DECLARATION(Kinodynamic)
} // namespace rbprm
} // namespace hpp
......@@ -64,30 +64,17 @@ namespace hpp{
assert(rbprmPathValidation_ && "Path validation should be a RbPrmPathValidation class for this solver");
// retrieve parameters from problem :
try {
sizeFootX_ = problem.getParameter ("sizeFootX").floatValue()/2.;
sizeFootY_ = problem.getParameter ("sizeFootY").floatValue()/2.;
sizeFootX_ = problem.getParameter (std::string("DynamicPlanner/sizeFootX")).floatValue()/2.;
sizeFootY_ = problem.getParameter (std::string("DynamicPlanner/sizeFootY")).floatValue()/2.;
if(sizeFootX_ > 0. && sizeFootY_ > 0.)
rectangularContact_ = 1;
} catch (const std::exception& e) {
hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
sizeFootX_ =0;
sizeFootY_ =0;
else
rectangularContact_ = 0;
}
try {
tryJump_ = problem.getParameter ("tryJump").boolValue();
} catch (const std::exception& e) {
tryJump_=false;
}
tryJump_ = problem.getParameter (std::string("DynamicPlanner/tryJump")).boolValue();
hppDout(notice,"tryJump in steering method = "<<tryJump_);
mu_ = problem.getParameter (std::string("DynamicPlanner/friction")).floatValue();
hppDout(notice,"mu define in python : "<<mu_);
try {
mu_ = problem.getParameter ("friction").floatValue();
hppDout(notice,"mu define in python : "<<mu_);
} catch (const std::exception& e) {
mu_= 0.5;
hppDout(notice,"mu not defined, take : "<<mu_<<" as default.");
}
}
......@@ -232,7 +219,7 @@ namespace hpp{
problem().configValidations()->validate(q1,report);
rbprmPathValidation_->getValidator()->computeAllContacts(false);
hppDout(notice,"Random shortucut, fillNodeMatrices : ");
x1->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_);
x1->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_,boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem().robot()));
// call steering method kinodynamic with the newly created node
hppDout(notice,"Random shortucut, steering method : ");
......
......@@ -66,30 +66,16 @@ namespace hpp{
assert(rbprmPathValidation_ && "Path validation should be a RbPrmPathValidation class for this solver");
// retrieve parameters from problem :
try {
sizeFootX_ = problem.getParameter ("sizeFootX").floatValue()/2.;
sizeFootY_ = problem.getParameter ("sizeFootY").floatValue()/2.;
rectangularContact_ = 1;
} catch (const std::exception& e) {
hppDout(warning,"Warning : size of foot not definied, use 0 (contact point)");
sizeFootX_ =0;
sizeFootY_ =0;
sizeFootX_ = problem.getParameter (std::string("DynamicPlanner/sizeFootX")).floatValue()/2.;
sizeFootY_ = problem.getParameter (std::string("DynamicPlanner/sizeFootY")).floatValue()/2.;
if(sizeFootX_ > 0. && sizeFootY_ > 0.)
rectangularContact_ = 1;
else
rectangularContact_ = 0;
}
try {
tryJump_ = problem.getParameter ("tryJump").boolValue();
} catch (const std::exception& e) {
tryJump_=false;
}
hppDout(notice,"tryJump in random shortcut = "<<tryJump_);
try {
mu_ = problem.getParameter ("friction").floatValue();
hppDout(notice,"mu define in python : "<<mu_);
} catch (const std::exception& e) {
mu_= 0.5;
hppDout(notice,"mu not defined, take : "<<mu_<<" as default.");
}
tryJump_ = problem.getParameter (std::string("DynamicPlanner/tryJump")).boolValue();
hppDout(notice,"tryJump in steering method = "<<tryJump_);
mu_ = problem.getParameter (std::string("DynamicPlanner/friction")).floatValue();
hppDout(notice,"mu define in python : "<<mu_);
}
......@@ -128,9 +114,7 @@ namespace hpp{
PathVectorPtr_t tmpPath = path;
// Maximal number of iterations without improvements
std::size_t n = problem().getParameter("PathOptimizersNumberOfLoops").intValue();
n = 100;
std::cout<<" number of loops : "<<n<<std::endl;
std::size_t n = problem().getParameter("PathOptimization/RandomShortcut/NumberOfLoops").intValue();
std::size_t projectionError = n;
std::deque <value_type> length (n-1,
numeric_limits <value_type>::infinity ());
......@@ -201,12 +185,19 @@ namespace hpp{
continue;
}
}
length.push_back (PathLength<>::run (result, problem ().distance ()));
length.pop_front ();
finished = (length [0] - length [n-1]) <= 1e-4 * length[n-1];
hppDout (info, "length = " << length [n-1]);
tmpPath = result;
core::value_type newLength = PathLength<>::run (result, problem ().distance ());
if ((length[n-1] - newLength) <= 10.*std::numeric_limits<core::value_type>::epsilon()) {
hppDout (info, "the length would increase:" << length[n-1] << " " << newLength);
result = tmpPath;
projectionError--;
} else {
length.push_back (newLength);
length.pop_front ();
finished = (length [0] - length [n-1]) <= 1e-4 * length[n-1];
hppDout (info, "length = " << length [n-1]);
tmpPath = result;
projectionError = n;
}
}
for (std::size_t i = 0; i < result->numberPaths (); ++i) {
if (result->pathAtRank(i)->constraints())
......@@ -232,7 +223,7 @@ namespace hpp{
problem().configValidations()->validate(q1,report);
rbprmPathValidation_->getValidator()->computeAllContacts(false);
hppDout(notice,"Random shortucut, fillNodeMatrices : ");
x1->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_);
x1->fillNodeMatrices(report,rectangularContact_,sizeFootX_,sizeFootY_,problem().robot()->mass(),mu_,boost::dynamic_pointer_cast<pinocchio::RbPrmDevice>(problem().robot()));
// call steering method kinodynamic with the newly created node
hppDout(notice,"Random shortucut, steering method : ");
PathPtr_t dp = (*sm_)(x1,q2);
......
......@@ -24,7 +24,7 @@
#include <hpp/util/timer.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/core/collision-validation-report.hh>
#include <hpp/rbprm/rbprm-device.hh>
namespace hpp{