30#ifndef HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
31#define HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
41namespace pathOptimization {
42class CollisionConstraintsResult;
58 typedef Eigen::JacobiSVD<matrix_t> Jacobi_t;
69 for (std::size_t i = 0; i < path->numberPaths() - 1; ++i) {
70 const PathPtr_t& localPath = path->pathAtRank(i);
72 (*localPath)(x.segment(index, configSize_), t1);
75 assert(index == x.size());
82 while (index < x.size()) {
83 q1 = x.segment(index, configSize_);
85 result->appendPath(p);
90 PathPtr_t p = (*steeringMethod_)(q0, q1);
91 result->appendPath(p);
94 void initializeProblemConstraints();
102 void updateProblemConstraints(vectorIn_t x);
115 bool constraintsSatisfied(
116 vectorOut_t& x, PathVectorPtr_t& path,
117 CollisionConstraintsResults_t& collisionConstraints);
129 bool solveConstraints(vectorOut_t x, PathVectorPtr_t& path,
130 CollisionConstraintsResults_t& collisionConstraints);
132 void integrate(vectorIn_t x0, vectorIn_t step, vectorOut_t x1)
const;
135 vector_t computeIterate(vectorIn_t x)
const;
138 void displayPath(vectorIn_t x, std::string
143 hppDout(info, prefix);
144 for (
int i = 0; i < x.size(); i++) hppDout(info, x[i]);
145 hppDout(info,
"finish path parsing");
155 void addCollisionConstraint(
const CollisionConstraintsResult& ccr,
156 const PathVectorPtr_t& path)
const;
165 void updateRightHandSide(
166 const CollisionConstraintsResults_t& collisionConstraints,
167 const PathVectorPtr_t& path)
const;
170 bool getProblemConstraints();
182 steeringMethod::StraightPtr_t steeringMethod_;
190 mutable bool fullRank_;
198 std::size_t iterMax_;
Definition: path-optimizer.hh:44
Definition: gradient-based.hh:45
#define HPP_CORE_DLLAPI
Definition: config.hh:88
virtual PathVectorPtr_t optimize(const PathVectorPtr_t &path)
Optimize path.
std::vector< CollisionConstraintsResult > CollisionConstraintsResults_t
Definition: gradient-based.hh:43
static GradientBasedPtr_t create(const ProblemConstPtr_t &problem)
GradientBased(const ProblemConstPtr_t &problem)
shared_ptr< Cost > CostPtr_t
Definition: fwd.hh:287
shared_ptr< GradientBased > GradientBasedPtr_t
Definition: fwd.hh:289
pinocchio::value_type value_type
Definition: fwd.hh:174
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
Eigen::Matrix< value_type, 1, Eigen::Dynamic > rowvector_t
Definition: fwd.hh:223
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:221
shared_ptr< WeighedDistance > WeighedDistancePtr_t
Definition: fwd.hh:226
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
pinocchio::vector_t vector_t
Definition: fwd.hh:220
pinocchio::size_type size_type
Definition: fwd.hh:173
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:197
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:107
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:167
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:166
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
Definition: bi-rrt-planner.hh:35