hpp-bezier-com-traj
4.9.0
Multi contact trajectory generation for the COM using Bezier curves
|
Classes | |
struct | Constraints |
Used to define the constraints on the trajectory generation problem. Flags are used to constrain initial and terminal com positions an derivatives. Additionally, the maximum acceleration can be bounded. More... | |
struct | ContactData |
Contact data contains all the contact information relative to a contact phase: contact points and normals (within Equilibrium object), as well as any additional kinematic and angular constraints. More... | |
struct | ProblemData |
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost functions. Also,a list of ContactData defines the different phases of the problem. While the method can handle any phase greater than one, using more than three phases is probably too constraining. More... | |
struct | ResultDataCOMTraj |
Specialized ResultData that computes the Bezier curves corresponding to the computed trajectory. More... | |
struct | waypoint_t |
Typedefs | |
typedef solvers::ResultData | ResultData |
typedef double | value_type |
typedef Eigen::Matrix< value_type, 3, 3 > | Matrix3 |
typedef Eigen::Matrix< value_type, 6, 3 > | Matrix63 |
typedef Eigen::Matrix< value_type, 3, 9 > | Matrix39 |
typedef Eigen::Matrix< value_type, Eigen::Dynamic, 3 > | MatrixX3 |
typedef Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > | MatrixXX |
typedef centroidal_dynamics::Vector3 | Vector3 |
typedef centroidal_dynamics::Vector6 | Vector6 |
typedef centroidal_dynamics::VectorX | VectorX |
typedef Eigen::Ref< Vector3 > | Ref_vector3 |
typedef Eigen::Ref< VectorX > | Ref_vectorX |
typedef Eigen::Ref< MatrixX3 > | Ref_matrixX3 |
typedef Eigen::Ref< MatrixXX > | Ref_matrixXX |
typedef Matrix63 | matrix6_t |
typedef Vector6 | point6_t |
typedef Matrix3 | matrix3_t |
typedef Vector3 | point3_t |
typedef Eigen::Vector3d | point_t |
typedef std::pair< matrix6_t, point6_t > | waypoint6_t |
waypoint_t a waypoint is composed of a 6*3 matrix that depend on the variable x, and of a 6d vector independent of x, such that each control point of the target bezier curve is given by pi = wix * x + wis More... | |
typedef std::pair< matrix3_t, point3_t > | waypoint3_t |
typedef std::pair< Matrix39, point3_t > | waypoint9_t |
typedef curves::bezier_curve< double, double, true, point_t > | bezier_t |
typedef curves::bezier_curve< double, double, true, waypoint_t > | bezier_wp_t |
typedef curves::bezier_curve< double, double, true, point6_t > | bezier6_t |
typedef std::vector< std::pair< double, int > > | T_time |
typedef T_time::const_iterator | CIT_time |
typedef std::pair< double, point3_t > | coefs_t |
typedef coefs_t(* | evalCurveAtTime) (const std::vector< point_t > &pi, double t) |
typedef std::map< ConstraintFlag, evalCurveAtTime > | T_evalCurveAtTime |
typedef T_evalCurveAtTime::const_iterator | CIT_evalCurveAtTime |
typedef coefs_t(* | evalAccCurveAtTime) (const std::vector< point_t > &pi, double T, double t) |
typedef std::map< ConstraintFlag, evalAccCurveAtTime > | T_evalAccCurveAtTime |
typedef T_evalAccCurveAtTime::const_iterator | CIT_evalAccCurveAtTime |
typedef waypoint_t(* | evalCurveWaypointAtTime) (const std::vector< point_t > &pi, double t) |
typedef std::map< ConstraintFlag, evalCurveWaypointAtTime > | T_evalCurveWaypointAtTime |
typedef T_evalCurveWaypointAtTime::const_iterator | CIT_evalCurveWaypointAtTime |
typedef waypoint_t(* | evalVelCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef std::map< ConstraintFlag, evalVelCurveWaypointAtTime > | T_evalVelCurveWaypointAtTime |
typedef T_evalVelCurveWaypointAtTime::const_iterator | CIT_evalVelCurveWaypointAtTime |
typedef waypoint_t(* | evalAccCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef std::map< ConstraintFlag, evalAccCurveWaypointAtTime > | T_evalAccCurveWaypointAtTime |
typedef T_evalAccCurveWaypointAtTime::const_iterator | CIT_evalAccCurveWaypointAtTime |
typedef waypoint_t(* | evalJerkCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef std::map< ConstraintFlag, evalJerkCurveWaypointAtTime > | T_evalJerkCurveWaypointAtTime |
typedef T_evalJerkCurveWaypointAtTime::const_iterator | CIT_evalJerkCurveWaypointAtTime |
typedef std::vector< point_t >(* | compConsWp) (const ProblemData &pData, double T) |
typedef std::map< ConstraintFlag, compConsWp > | T_compConsWp |
typedef T_compConsWp::const_iterator | CIT_compConsWp |
typedef std::vector< waypoint_t >(* | compVelWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::map< ConstraintFlag, compVelWp > | T_compVelWp |
typedef T_compVelWp::const_iterator | CIT_compVelWp |
typedef std::vector< waypoint_t >(* | compAccWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::map< ConstraintFlag, compAccWp > | T_compAccWp |
typedef T_compAccWp::const_iterator | CIT_compAccWp |
typedef std::vector< waypoint_t >(* | compJerkWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::map< ConstraintFlag, compJerkWp > | T_compJerkWp |
typedef T_compJerkWp::const_iterator | CIT_compJerkWp |
typedef bezier_wp_t::t_point_t(* | compWp) (const ProblemData &pData, double T) |
typedef std::map< ConstraintFlag, compWp > | T_compWp |
typedef T_compWp::const_iterator | CIT_compWp |
typedef coefs_t(* | compFinalVelP) (const ProblemData &pData, double T) |
typedef std::map< ConstraintFlag, compFinalVelP > | T_compFinalVelP |
typedef T_compFinalVelP::const_iterator | CIT_compFinalVelP |
typedef std::pair< MatrixXX, VectorX >(* | compVelCost) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::map< ConstraintFlag, compVelCost > | T_compVelCost |
typedef T_compVelCost::const_iterator | CIT_compVelCost |
Functions | |
BEZIER_COM_TRAJ_DLLAPI std::vector< waypoint6_t > | ComputeDiscretizedWaypoints (const std::vector< waypoint6_t > &wps, const std::vector< curves::Bern< double > > &bernstein, int numSteps) |
ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve, computes a discretization of the curve. More... | |
BEZIER_COM_TRAJ_DLLAPI std::pair< MatrixXX, VectorX > | compute6dControlPointInequalities (const ContactData &cData, const std::vector< waypoint6_t > &wps, const std::vector< waypoint6_t > &wpL, const bool useAngMomentum, bool &fail) |
compute6dControlPointInequalities Given linear and angular control waypoints, compute the inequality matrices A and b, A x <= b that constrain the desired control point x. More... | |
BEZIER_COM_TRAJ_DLLAPI std::pair< MatrixXX, VectorX > | compute6dControlPointEqualities (const ContactData &cData, const std::vector< waypoint6_t > &wps, const std::vector< waypoint6_t > &wpL, const bool useAngMomentum, bool &fail) |
compute6dControlPointEqualities Given linear and angular control waypoints, compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact forces Beta. More... | |
BEZIER_COM_TRAJ_DLLAPI ResultData | solve (Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds, const solvers::SolverType solver=solvers::SOLVER_QUADPROG) |
solve x' h x + 2 g' x, subject to A*x <= b using quadprog More... | |
BEZIER_COM_TRAJ_DLLAPI ResultData | solve (Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, const solvers::SolverType solver=solvers::SOLVER_QUADPROG) |
solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog More... | |
BEZIER_COM_TRAJ_DLLAPI ResultData | solve (const std::pair< MatrixXX, VectorX > &Ab, const std::pair< MatrixXX, VectorX > &Hg, const VectorX &init, const solvers::SolverType solver=solvers::SOLVER_QUADPROG) |
solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3 More... | |
BEZIER_COM_TRAJ_DLLAPI ResultData | solve (const std::pair< MatrixXX, VectorX > &Ab, const std::pair< MatrixXX, VectorX > &Dd, const std::pair< MatrixXX, VectorX > &Hg, Cref_vectorX minBounds, Cref_vectorX maxBounds, const VectorX &init, const solvers::SolverType solver=solvers::SOLVER_QUADPROG) |
solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog, with x of fixed dimension 3 More... | |
template<typename Point > | |
BEZIER_COM_TRAJ_DLLAPI std::vector< std::pair< double, Point > > | computeDiscretizedWaypoints (const ProblemData &pData, double T, const T_time &timeArray) |
template<typename Point > | |
BEZIER_COM_TRAJ_DLLAPI std::vector< std::pair< double, Point > > | computeDiscretizedAccelerationWaypoints (const ProblemData &pData, double T, const T_time &timeArray) |
ConstraintFlag | operator~ (ConstraintFlag a) |
ConstraintFlag | operator| (ConstraintFlag a, ConstraintFlag b) |
ConstraintFlag | operator& (ConstraintFlag a, ConstraintFlag b) |
ConstraintFlag | operator^ (ConstraintFlag a, ConstraintFlag b) |
ConstraintFlag & | operator|= (ConstraintFlag &a, ConstraintFlag b) |
ConstraintFlag & | operator&= (ConstraintFlag &a, ConstraintFlag b) |
ConstraintFlag & | operator^= (ConstraintFlag &a, ConstraintFlag b) |
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj | solve0step (const ProblemData &pData, const std::vector< double > &Ts, const double timeStep=-1) |
solve0step Tries to solve the 0-step capturability problem. Given the current contact phase, a COM position, and an initial velocity, tries to compute a feasible COM trajectory that stops the character without falling. In this specific implementation, the considered constraints are: init position and velocity, 0 velocity constraints (acceleration constraints are ignored) More... | |
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj | computeCOMTrajFixedSize (const ProblemData &pData, const VectorX &Ts, const unsigned int pointsPerPhase=3) |
computeCOMTraj Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity, try to compute the CoM trajectory (as a Bezier curve) that connect them More... | |
BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj | computeCOMTraj (const ProblemData &pData, const VectorX &Ts, const double timeStep=-1, const solvers::SolverType solver=solvers::SOLVER_QUADPROG) |
computeCOMTraj Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity, try to compute the CoM trajectory (as a Bezier curve) that connect them More... | |
template<typename Path > | |
ResultDataCOMTraj | solveEndEffector (const ProblemData &pData, const Path &path, const double T, const double weightDistance, bool useVelCost=true) |
solveEndEffector Tries to produce a trajectory represented as a bezier curve that satisfy position, velocity and acceleration constraint for the initial and final point and that follow as close as possible the input trajectory More... | |
coefs_t | initCoefs () |
std::vector< bezier_t::point_t > | computeConstantWaypointsInitPredef (const ProblemData &pData, double T) |
std::vector< bezier_t::point_t > | computeConstantWaypointsGoalPredef (const ProblemData &pData, double T) |
void | computeConstraintsMatrix (const ProblemData &pData, const std::vector< waypoint_t > &wps_acc, const std::vector< waypoint_t > &wps_vel, const VectorX &acc_bounds, const VectorX &vel_bounds, MatrixXX &A, VectorX &b, const std::vector< waypoint_t > &wps_jerk=std::vector< waypoint_t >(), const VectorX &jerk_bounds=VectorX(DIM_POINT)) |
std::pair< MatrixXX, VectorX > | computeDistanceCostFunction (size_t numPoints, const ProblemData &pData, double T, std::vector< point3_t > pts_path) |
template<typename Path > | |
std::pair< MatrixXX, VectorX > | computeDistanceCostFunction (size_t numPoints, const ProblemData &pData, double T, const Path &path) |
void | computeC_of_T (const ProblemData &pData, double T, ResultDataCOMTraj &res) |
void | computeVelCostFunctionDiscretized (int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g) |
void | computeAccelerationCostFunctionDiscretized (int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g) |
void | computeJerkCostFunctionDiscretized (int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g) |
std::pair< MatrixXX, VectorX > | computeEndEffectorConstraints (const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi) |
template<typename Path > | |
std::pair< MatrixXX, VectorX > | computeEndEffectorCost (const ProblemData &pData, const Path &path, const double T, const double weightDistance, bool, std::vector< bezier_t::point_t > pi) |
template<typename T > | |
T | initwp () |
waypoint_t | initwp (const size_t rows, const size_t cols) |
waypoint_t | operator+ (const waypoint_t &w1, const waypoint_t &w2) |
waypoint_t | operator- (const waypoint_t &w1, const waypoint_t &w2) |
waypoint_t | operator* (const double k, const waypoint_t &w) |
waypoint_t | operator* (const waypoint_t &w, const double k) |
BEZIER_COM_TRAJ_DLLAPI std::vector< curves::Bern< double > > | ComputeBersteinPolynoms (const unsigned int degree) |
Compute the Bernstein polynoms for a given degree. More... | |
template<typename Bezier , typename Point > | |
BEZIER_COM_TRAJ_DLLAPI Bezier | computeBezierCurve (const ConstraintFlag &flag, const double T, const std::vector< Point > &pi, const Point &x) |
given the constraints of the problem, and a set of waypoints, return the bezier curve corresponding More... | |
T_time | computeDiscretizedTimeFixed (const VectorX &phaseTimings, const unsigned int pointsPerPhase) |
computeDiscretizedTime build an array of discretized points in time, such that there is the same number of point in each phase. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size() More... | |
T_time | computeDiscretizedTime (const VectorX &phaseTimings, const double timeStep) |
computeDiscretizedTime build an array of discretized points in time, given the timestep. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size() More... | |
void | printQHullFile (const std::pair< MatrixXX, VectorX > &Ab, VectorX intPoint, const std::string &fileName, bool clipZ=false) |
write a polytope describe by A x <= b linear constraints in a given filename More... | |
BEZIER_COM_TRAJ_DLLAPI Matrix3 | skew (point_t_tC x) |
skew symmetric matrix More... | |
int | Normalize (Ref_matrixXX A, Ref_vectorX b) |
normalize inequality constraints More... | |
coefs_t | evaluateCurveAtTime (const ProblemData &pData, const std::vector< point_t > &pi, double t) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x) More... | |
coefs_t | evaluateVelocityCurveAtTime (const ProblemData &pData, const std::vector< point_t > &pi, double T, double t) |
evaluateVelocityCurveAtTime compute the expression of the point on the curve dc at t, defined by the waypoint pi and one free waypoint (x) More... | |
coefs_t | evaluateAccelerationCurveAtTime (const ProblemData &pData, const std::vector< point_t > &pi, double T, double t) |
evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x) More... | |
coefs_t | evaluateJerkCurveAtTime (const ProblemData &pData, const std::vector< point_t > &pi, double T, double t) |
evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x) More... | |
std::vector< point_t > | computeConstantWaypoints (const ProblemData &pData, double T) |
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states More... | |
bezier_wp_t::t_point_t | computeConstantWaypointsSymbolic (const ProblemData &pData, double T) |
computeConstantWaypointsSymbolic compute the constant waypoints of c(t) defined by the constraints on initial and final states More... | |
std::vector< waypoint_t > | computeVelocityWaypoints (const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >()) |
computeWwaypoints compute the constant waypoints of dc(t) defined by the constraints on initial and final states More... | |
std::vector< waypoint_t > | computeAccelerationWaypoints (const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >()) |
computeWwaypoints compute the constant waypoints of ddc(t) defined by the constraints on initial and final states More... | |
std::vector< waypoint_t > | computeJerkWaypoints (const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >()) |
computeWwaypoints compute the constant waypoints of dddc(t) defined by the constraints on initial and final states More... | |
waypoint_t | evaluateCurveWaypointAtTime (const ProblemData &pData, const std::vector< point_t > &pi, double t) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x) More... | |
waypoint_t | evaluateVelocityCurveWaypointAtTime (const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x) More... | |
waypoint_t | evaluateAccelerationCurveWaypointAtTime (const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x) More... | |
waypoint_t | evaluateJerkCurveWaypointAtTime (const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x) More... | |
bezier_wp_t::t_point_t | computeWwaypoints (const ProblemData &pData, double T) |
computeConstantWaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states More... | |
coefs_t | computeFinalVelocityPoint (const ProblemData &pData, double T) |
int | dimVar (const ProblemData &pData) |
std::pair< MatrixXX, VectorX > | computeVelocityCost (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
computeVelocityCost the matrices H and g defining a cost that minimise the integral of the squared velocity More... | |
MatrixXX | initMatrixA (const int dimH, const std::vector< waypoint6_t > &wps, const long int extraConstraintSize, const bool useAngMomentum) |
MatrixXX | initMatrixD (const int colG, const std::vector< waypoint6_t > &wps, const long int extraConstraintSize, const bool useAngMomentum) |
void | addKinematic (Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin, Cref_vectorX kin, const long int otherConstraintIndex) |
void | addAngularMomentum (Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang, Cref_vectorX ang) |
int | removeZeroRows (Ref_matrixXX &A, Ref_vectorX &b) |
ResultData | solve (Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds, const solvers::SolverType solver) |
ResultData | solve (Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, const solvers::SolverType solver) |
bezier_wp_t::t_point_t | computeDiscretizedWwaypoints (const ProblemData &pData, double T, const T_time &timeArray) |
std::pair< MatrixXX, VectorX > | dynamicStabilityConstraints_cross (const MatrixXX &mH, const VectorX &h, const Vector3 &g, const coefs_t &c, const coefs_t &ddc) |
std::pair< MatrixXX, VectorX > | dynamicStabilityConstraints (const MatrixXX &mH, const VectorX &h, const Vector3 &g, const waypoint_t &w) |
std::vector< int > | stepIdPerPhase (const T_time &timeArray) |
long int | computeNumIneq (const ProblemData &pData, const VectorX &Ts, const std::vector< int > &phaseSwitch) |
void | updateH (const ProblemData &pData, const ContactData &phase, MatrixXX &mH, VectorX &h, int &dimH) |
void | assignStabilityConstraintsForTimeStep (MatrixXX &mH, VectorX &h, const waypoint_t &wp_w, const int dimH, long int &id_rows, MatrixXX &A, VectorX &b, const Vector3 &g) |
void | assignStabilityConstraintsForTimeStepForce (const waypoint_t &wp_w, const long int rowIdx, long int &id_cols, const centroidal_dynamics::Matrix6X mG, MatrixXX &D, VectorX &d, const double mass, const Vector3 &g) |
void | switchContactPhase (const ProblemData &pData, MatrixXX &A, VectorX &b, MatrixXX &mH, VectorX &h, const waypoint_t &wp_w, const ContactData &phase, long int &id_rows, int &dimH) |
long int | assignStabilityConstraints (const ProblemData &pData, MatrixXX &A, VectorX &b, const T_time &timeArray, const double t_total, const std::vector< int > &stepIdForPhase) |
void | assignKinematicConstraints (const ProblemData &pData, MatrixXX &A, VectorX &b, const T_time &timeArray, const double t_total, const std::vector< int > &stepIdForPhase, long int &id_rows) |
void | assignAccelerationConstraints (const ProblemData &pData, MatrixXX &A, VectorX &b, const T_time &timeArray, const double t_total, long int &id_rows) |
std::pair< MatrixXX, VectorX > | computeConstraintsOneStep (const ProblemData &pData, const VectorX &Ts, const double t_total, const T_time &timeArray) |
void | computeFinalAcceleration (ResultDataCOMTraj &res) |
void | computeFinalVelocity (ResultDataCOMTraj &res) |
std::pair< MatrixXX, VectorX > | genCostFunction (const ProblemData &pData, const VectorX &Ts, const double T, const T_time &timeArray, const long int &dim) |
ResultDataCOMTraj | genTraj (ResultData resQp, const ProblemData &pData, const double T) |
long int | computeNumIneqContinuous (const ProblemData &pData, const VectorX &Ts, const int degree, const int w_degree, const bool useDD) |
computeNumIneqContinuous compute the number of inequalitie required by all the phases More... | |
long int | computeNumEqContinuous (const ProblemData &pData, const int w_degree, const bool useForce, long int &forceVarDim) |
computeNumEqContinuous compute the number of equalities required by all the phases More... | |
std::pair< MatrixXX, VectorX > | computeConstraintsContinuous (const ProblemData &pData, const VectorX &Ts, std::pair< MatrixXX, VectorX > &Dd, VectorX &minBounds, VectorX &) |
waypoint6_t | w0 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &p0X, const Matrix3 &, const Matrix3 &, const double alpha) |
waypoint6_t | w1 (point_t_tC p0, point_t_tC p1, point_t_tC, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha) |
waypoint6_t | w2 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha) |
waypoint6_t | w3 (point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha) |
waypoint6_t | w4 (point_t_tC, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha) |
waypoint6_t | u0 (point_t_tC l0, const double alpha) |
waypoint6_t | u1 (point_t_tC l0, const double alpha) |
waypoint6_t | u2 (point_t_tC l0, const double alpha) |
waypoint6_t | u3 (point_t_tC, const double alpha) |
waypoint6_t | u4 (point_t_tC, const double) |
int | computeNumSteps (const double T, const double timeStep) |
std::vector< waypoint6_t > | ComputeAllWaypoints (point_t_tC p0, point_t_tC dc0, point_t_tC g, const double T, const double timeStep) |
std::vector< waypoint6_t > | ComputeAllWaypointsAngularMomentum (point_t_tC l0, const double T, const double timeStep) |
std::pair< MatrixXX, VectorX > | compute6dControlPointInequalities (const ContactData &cData, point_t_tC c0, point_t_tC dc0, point_t_tC l0, const bool useAngMomentum, const double T, const double timeStep, bool &fail) |
std::pair< MatrixXX, VectorX > | computeCostFunction (point_t_tC p0, point_t_tC l0, const bool useAngMomentum) |
void | computeRealCost (const ProblemData &pData, ResultData &resData) |
void | computeC_of_T (const ProblemData &pData, const std::vector< double > &Ts, ResultDataCOMTraj &res) |
void | computedL_of_T (const ProblemData &pData, const std::vector< double > &Ts, ResultDataCOMTraj &res) |
template<> | |
waypoint9_t | initwp< waypoint9_t > () |
template<> | |
waypoint6_t | initwp< waypoint6_t > () |
template<> | |
waypoint3_t | initwp< waypoint3_t > () |
Variables | |
const typedef Eigen::Ref< const Vector3 > & | Cref_vector3 |
const typedef Eigen::Ref< const Vector6 > & | Cref_vector6 |
const typedef Eigen::Ref< const VectorX > & | Cref_vectorX |
const typedef Eigen::Ref< const MatrixXX > & | Cref_matrixXX |
const typedef Eigen::Ref< const MatrixX3 > & | Cref_matrixX3 |
const typedef Eigen::Ref< const point_t > & | point_t_tC |
const int | DIM_POINT = 3 |
const bool | verbose = false |
typedef curves::bezier_curve<double, double, true, point6_t> bezier_com_traj::bezier6_t |
typedef curves::bezier_curve<double, double, true, point_t> bezier_com_traj::bezier_t |
typedef curves::bezier_curve<double, double, true, waypoint_t> bezier_com_traj::bezier_wp_t |
typedef T_compAccWp::const_iterator bezier_com_traj::CIT_compAccWp |
typedef T_compConsWp::const_iterator bezier_com_traj::CIT_compConsWp |
typedef T_compFinalVelP::const_iterator bezier_com_traj::CIT_compFinalVelP |
typedef T_compJerkWp::const_iterator bezier_com_traj::CIT_compJerkWp |
typedef T_compVelCost::const_iterator bezier_com_traj::CIT_compVelCost |
typedef T_compVelWp::const_iterator bezier_com_traj::CIT_compVelWp |
typedef T_compWp::const_iterator bezier_com_traj::CIT_compWp |
typedef T_evalAccCurveAtTime::const_iterator bezier_com_traj::CIT_evalAccCurveAtTime |
typedef T_evalAccCurveWaypointAtTime::const_iterator bezier_com_traj::CIT_evalAccCurveWaypointAtTime |
typedef T_evalCurveAtTime::const_iterator bezier_com_traj::CIT_evalCurveAtTime |
typedef T_evalCurveWaypointAtTime::const_iterator bezier_com_traj::CIT_evalCurveWaypointAtTime |
typedef T_evalJerkCurveWaypointAtTime::const_iterator bezier_com_traj::CIT_evalJerkCurveWaypointAtTime |
typedef T_evalVelCurveWaypointAtTime::const_iterator bezier_com_traj::CIT_evalVelCurveWaypointAtTime |
typedef T_time::const_iterator bezier_com_traj::CIT_time |
typedef std::pair< double, point3_t > bezier_com_traj::coefs_t |
typedef std::vector<waypoint_t>(* bezier_com_traj::compAccWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::vector<point_t>(* bezier_com_traj::compConsWp) (const ProblemData &pData, double T) |
typedef coefs_t(* bezier_com_traj::compFinalVelP) (const ProblemData &pData, double T) |
typedef std::vector<waypoint_t>(* bezier_com_traj::compJerkWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::pair<MatrixXX, VectorX>(* bezier_com_traj::compVelCost) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef std::vector<waypoint_t>(* bezier_com_traj::compVelWp) (const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi) |
typedef bezier_wp_t::t_point_t(* bezier_com_traj::compWp) (const ProblemData &pData, double T) |
typedef coefs_t(* bezier_com_traj::evalAccCurveAtTime) (const std::vector< point_t > &pi, double T, double t) |
typedef waypoint_t(* bezier_com_traj::evalAccCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef waypoint_t(* bezier_com_traj::evalCurveWaypointAtTime) (const std::vector< point_t > &pi, double t) |
typedef waypoint_t(* bezier_com_traj::evalJerkCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef waypoint_t(* bezier_com_traj::evalVelCurveWaypointAtTime) (const std::vector< point_t > &pi, const double T, double t) |
typedef Eigen::Matrix<value_type, 3, 3> bezier_com_traj::Matrix3 |
typedef Eigen::Matrix<value_type, 3, 9> bezier_com_traj::Matrix39 |
typedef Matrix3 bezier_com_traj::matrix3_t |
typedef Eigen::Matrix<value_type, 6, 3> bezier_com_traj::Matrix63 |
typedef Matrix63 bezier_com_traj::matrix6_t |
typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> bezier_com_traj::MatrixX3 |
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> bezier_com_traj::MatrixXX |
typedef Vector3 bezier_com_traj::point3_t |
typedef Vector6 bezier_com_traj::point6_t |
typedef Eigen::Vector3d bezier_com_traj::point_t |
typedef Eigen::Ref<MatrixX3> bezier_com_traj::Ref_matrixX3 |
typedef Eigen::Ref<MatrixXX> bezier_com_traj::Ref_matrixXX |
typedef Eigen::Ref<Vector3> bezier_com_traj::Ref_vector3 |
typedef Eigen::Ref<VectorX> bezier_com_traj::Ref_vectorX |
typedef solvers::ResultData bezier_com_traj::ResultData |
typedef std::map<ConstraintFlag, compAccWp> bezier_com_traj::T_compAccWp |
typedef std::map<ConstraintFlag, compConsWp> bezier_com_traj::T_compConsWp |
typedef std::map<ConstraintFlag, compFinalVelP> bezier_com_traj::T_compFinalVelP |
typedef std::map<ConstraintFlag, compJerkWp> bezier_com_traj::T_compJerkWp |
typedef std::map<ConstraintFlag, compVelCost> bezier_com_traj::T_compVelCost |
typedef std::map<ConstraintFlag, compVelWp> bezier_com_traj::T_compVelWp |
typedef std::map<ConstraintFlag, compWp> bezier_com_traj::T_compWp |
typedef std::map<ConstraintFlag, evalAccCurveAtTime> bezier_com_traj::T_evalAccCurveAtTime |
typedef std::map<ConstraintFlag, evalAccCurveWaypointAtTime> bezier_com_traj::T_evalAccCurveWaypointAtTime |
typedef std::map<ConstraintFlag, evalCurveAtTime> bezier_com_traj::T_evalCurveAtTime |
typedef std::map<ConstraintFlag, evalCurveWaypointAtTime> bezier_com_traj::T_evalCurveWaypointAtTime |
typedef std::map<ConstraintFlag, evalJerkCurveWaypointAtTime> bezier_com_traj::T_evalJerkCurveWaypointAtTime |
typedef std::map<ConstraintFlag, evalVelCurveWaypointAtTime> bezier_com_traj::T_evalVelCurveWaypointAtTime |
typedef std::vector<std::pair<double, int> > bezier_com_traj::T_time |
typedef double bezier_com_traj::value_type |
typedef centroidal_dynamics::Vector3 bezier_com_traj::Vector3 |
typedef centroidal_dynamics::Vector6 bezier_com_traj::Vector6 |
typedef centroidal_dynamics::VectorX bezier_com_traj::VectorX |
typedef std::pair<matrix3_t, point3_t> bezier_com_traj::waypoint3_t |
typedef std::pair<matrix6_t, point6_t> bezier_com_traj::waypoint6_t |
waypoint_t a waypoint is composed of a 6*3 matrix that depend on the variable x, and of a 6d vector independent of x, such that each control point of the target bezier curve is given by pi = wix * x + wis
typedef std::pair<Matrix39, point3_t> bezier_com_traj::waypoint9_t |
void bezier_com_traj::addAngularMomentum | ( | Ref_matrixXX | A, |
Ref_vectorX | b, | ||
Cref_matrixX3 | Ang, | ||
Cref_vectorX | ang | ||
) |
void bezier_com_traj::addKinematic | ( | Ref_matrixXX | A, |
Ref_vectorX | b, | ||
Cref_matrixX3 | Kin, | ||
Cref_vectorX | kin, | ||
const long int | otherConstraintIndex | ||
) |
void bezier_com_traj::assignAccelerationConstraints | ( | const ProblemData & | pData, |
MatrixXX & | A, | ||
VectorX & | b, | ||
const T_time & | timeArray, | ||
const double | t_total, | ||
long int & | id_rows | ||
) |
void bezier_com_traj::assignKinematicConstraints | ( | const ProblemData & | pData, |
MatrixXX & | A, | ||
VectorX & | b, | ||
const T_time & | timeArray, | ||
const double | t_total, | ||
const std::vector< int > & | stepIdForPhase, | ||
long int & | id_rows | ||
) |
long int bezier_com_traj::assignStabilityConstraints | ( | const ProblemData & | pData, |
MatrixXX & | A, | ||
VectorX & | b, | ||
const T_time & | timeArray, | ||
const double | t_total, | ||
const std::vector< int > & | stepIdForPhase | ||
) |
void bezier_com_traj::assignStabilityConstraintsForTimeStep | ( | MatrixXX & | mH, |
VectorX & | h, | ||
const waypoint_t & | wp_w, | ||
const int | dimH, | ||
long int & | id_rows, | ||
MatrixXX & | A, | ||
VectorX & | b, | ||
const Vector3 & | g | ||
) |
void bezier_com_traj::assignStabilityConstraintsForTimeStepForce | ( | const waypoint_t & | wp_w, |
const long int | rowIdx, | ||
long int & | id_cols, | ||
const centroidal_dynamics::Matrix6X | mG, | ||
MatrixXX & | D, | ||
VectorX & | d, | ||
const double | mass, | ||
const Vector3 & | g | ||
) |
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> bezier_com_traj::compute6dControlPointEqualities | ( | const ContactData & | cData, |
const std::vector< waypoint6_t > & | wps, | ||
const std::vector< waypoint6_t > & | wpL, | ||
const bool | useAngMomentum, | ||
bool & | fail | ||
) |
compute6dControlPointEqualities Given linear and angular control waypoints, compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact forces Beta.
cData | data for the current contact phase |
wps | waypoints or the linear part of the trajectory |
wpL | waypoints or the angular part of the trajectory |
useAngMomentum | whether the angular momentum is consider or equal to 0 |
fail | set to true if problem is found infeasible |
std::pair< MatrixXX, VectorX > bezier_com_traj::compute6dControlPointInequalities | ( | const ContactData & | cData, |
const std::vector< waypoint6_t > & | wps, | ||
const std::vector< waypoint6_t > & | wpL, | ||
const bool | useAngMomentum, | ||
bool & | fail | ||
) |
compute6dControlPointInequalities Given linear and angular control waypoints, compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
cData | data for the current contact phase |
wps | waypoints or the linear part of the trajectory |
wpL | waypoints or the angular part of the trajectory |
useAngMomentum | whether the angular momentum is consider or equal to 0 |
fail | set to true if problem is found infeasible |
std::pair<MatrixXX, VectorX> bezier_com_traj::compute6dControlPointInequalities | ( | const ContactData & | cData, |
point_t_tC | c0, | ||
point_t_tC | dc0, | ||
point_t_tC | l0, | ||
const bool | useAngMomentum, | ||
const double | T, | ||
const double | timeStep, | ||
bool & | fail | ||
) |
void bezier_com_traj::computeAccelerationCostFunctionDiscretized | ( | int | numPoints, |
const ProblemData & | pData, | ||
double | T, | ||
MatrixXX & | H, | ||
VectorX & | g | ||
) |
std::vector< waypoint_t > bezier_com_traj::computeAccelerationWaypoints | ( | const ProblemData & | pData, |
const double | T, | ||
std::vector< bezier_t::point_t > | pi = std::vector<bezier_t::point_t>() |
||
) |
computeWwaypoints compute the constant waypoints of ddc(t) defined by the constraints on initial and final states
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
pData | |
T |
std::vector<waypoint6_t> bezier_com_traj::ComputeAllWaypoints | ( | point_t_tC | p0, |
point_t_tC | dc0, | ||
point_t_tC | g, | ||
const double | T, | ||
const double | timeStep | ||
) |
std::vector<waypoint6_t> bezier_com_traj::ComputeAllWaypointsAngularMomentum | ( | point_t_tC | l0, |
const double | T, | ||
const double | timeStep | ||
) |
std::vector< curves::Bern< double > > bezier_com_traj::ComputeBersteinPolynoms | ( | const unsigned int | degree | ) |
Compute the Bernstein polynoms for a given degree.
degree | required degree |
BEZIER_COM_TRAJ_DLLAPI Bezier bezier_com_traj::computeBezierCurve | ( | const ConstraintFlag & | flag, |
const double | T, | ||
const std::vector< Point > & | pi, | ||
const Point & | x | ||
) |
given the constraints of the problem, and a set of waypoints, return the bezier curve corresponding
pData | problem data |
T | total trajectory time |
pis | list of waypoints |
void bezier_com_traj::computeC_of_T | ( | const ProblemData & | pData, |
const std::vector< double > & | Ts, | ||
ResultDataCOMTraj & | res | ||
) |
void bezier_com_traj::computeC_of_T | ( | const ProblemData & | pData, |
double | T, | ||
ResultDataCOMTraj & | res | ||
) |
ResultDataCOMTraj bezier_com_traj::computeCOMTraj | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const double | timeStep = -1 , |
||
const solvers::SolverType | solver = solvers::SOLVER_QUADPROG |
||
) |
computeCOMTraj Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity, try to compute the CoM trajectory (as a Bezier curve) that connect them
pData | problem Data. |
Ts | timelength of each contact phase. Should be the same length as pData.contacts |
timeStep | time step used by the discretization, if -1 : use the continuous fomulation |
solver | solver used to perform optimization. WARNING: if the continuous force formulation is used, it is highly recommended to use the SOLVER_GLPK solver if available and a quadratic cost is not necessary, as these solvers are increasely more computationnaly efficient for the problem |
ResultDataCOMTraj bezier_com_traj::computeCOMTrajFixedSize | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const unsigned int | pointsPerPhase = 3 |
||
) |
computeCOMTraj Tries to solve the one step problem : Given two or three contact phases, an initial and final com position and velocity, try to compute the CoM trajectory (as a Bezier curve) that connect them
pData | problem Data. |
Ts | timelength of each contact phase. Should be the same legnth as pData.contacts |
timeStep | time step used by the discretization |
std::vector< point_t > bezier_com_traj::computeConstantWaypoints | ( | const ProblemData & | pData, |
double | T | ||
) |
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
pData | |
T |
std::vector<bezier_t::point_t> bezier_com_traj::computeConstantWaypointsGoalPredef | ( | const ProblemData & | pData, |
double | T | ||
) |
std::vector<bezier_t::point_t> bezier_com_traj::computeConstantWaypointsInitPredef | ( | const ProblemData & | pData, |
double | T | ||
) |
bezier_wp_t::t_point_t bezier_com_traj::computeConstantWaypointsSymbolic | ( | const ProblemData & | pData, |
double | T | ||
) |
computeConstantWaypointsSymbolic compute the constant waypoints of c(t) defined by the constraints on initial and final states
pData | |
T |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeConstraintsContinuous | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
std::pair< MatrixXX, VectorX > & | Dd, | ||
VectorX & | minBounds, | ||
VectorX & | |||
) |
(pData.contacts_.begin()->contactPhase_->getAlgorithm() == centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
void bezier_com_traj::computeConstraintsMatrix | ( | const ProblemData & | pData, |
const std::vector< waypoint_t > & | wps_acc, | ||
const std::vector< waypoint_t > & | wps_vel, | ||
const VectorX & | acc_bounds, | ||
const VectorX & | vel_bounds, | ||
MatrixXX & | A, | ||
VectorX & | b, | ||
const std::vector< waypoint_t > & | wps_jerk = std::vector<waypoint_t>() , |
||
const VectorX & | jerk_bounds = VectorX(DIM_POINT) |
||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeConstraintsOneStep | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const double | t_total, | ||
const T_time & | timeArray | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeCostFunction | ( | point_t_tC | p0, |
point_t_tC | l0, | ||
const bool | useAngMomentum | ||
) |
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > bezier_com_traj::computeDiscretizedAccelerationWaypoints | ( | const ProblemData & | pData, |
double | T, | ||
const T_time & | timeArray | ||
) |
T_time bezier_com_traj::computeDiscretizedTime | ( | const VectorX & | phaseTimings, |
const double | timeStep | ||
) |
computeDiscretizedTime build an array of discretized points in time, given the timestep. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size()
phaseTimings | |
timeStep |
T_time bezier_com_traj::computeDiscretizedTimeFixed | ( | const VectorX & | phaseTimings, |
const unsigned int | pointsPerPhase | ||
) |
computeDiscretizedTime build an array of discretized points in time, such that there is the same number of point in each phase. Doesn't contain t=0, is of size pointsPerPhase*phaseTimings.size()
phaseTimings | |
pointsPerPhase |
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > bezier_com_traj::computeDiscretizedWaypoints | ( | const ProblemData & | pData, |
double | T, | ||
const T_time & | timeArray | ||
) |
std::vector< waypoint6_t > bezier_com_traj::ComputeDiscretizedWaypoints | ( | const std::vector< waypoint6_t > & | wps, |
const std::vector< curves::Bern< double > > & | bernstein, | ||
int | numSteps | ||
) |
ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve, computes a discretization of the curve.
wps | original waypoints |
bernstein | berstein polynoms for |
numSteps | desired number of wayoints |
bezier_wp_t::t_point_t bezier_com_traj::computeDiscretizedWwaypoints | ( | const ProblemData & | pData, |
double | T, | ||
const T_time & | timeArray | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeDistanceCostFunction | ( | size_t | numPoints, |
const ProblemData & | pData, | ||
double | T, | ||
const Path & | path | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeDistanceCostFunction | ( | size_t | numPoints, |
const ProblemData & | pData, | ||
double | T, | ||
std::vector< point3_t > | pts_path | ||
) |
void bezier_com_traj::computedL_of_T | ( | const ProblemData & | pData, |
const std::vector< double > & | Ts, | ||
ResultDataCOMTraj & | res | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeEndEffectorConstraints | ( | const ProblemData & | pData, |
const double | T, | ||
std::vector< bezier_t::point_t > | pi | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::computeEndEffectorCost | ( | const ProblemData & | pData, |
const Path & | path, | ||
const double | T, | ||
const double | weightDistance, | ||
bool | , | ||
std::vector< bezier_t::point_t > | pi | ||
) |
void bezier_com_traj::computeFinalAcceleration | ( | ResultDataCOMTraj & | res | ) |
void bezier_com_traj::computeFinalVelocity | ( | ResultDataCOMTraj & | res | ) |
coefs_t bezier_com_traj::computeFinalVelocityPoint | ( | const ProblemData & | pData, |
double | T | ||
) |
void bezier_com_traj::computeJerkCostFunctionDiscretized | ( | int | numPoints, |
const ProblemData & | pData, | ||
double | T, | ||
MatrixXX & | H, | ||
VectorX & | g | ||
) |
std::vector< waypoint_t > bezier_com_traj::computeJerkWaypoints | ( | const ProblemData & | pData, |
const double | T, | ||
std::vector< bezier_t::point_t > | pi = std::vector<bezier_t::point_t>() |
||
) |
computeWwaypoints compute the constant waypoints of dddc(t) defined by the constraints on initial and final states
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
pData | |
T |
long int bezier_com_traj::computeNumEqContinuous | ( | const ProblemData & | pData, |
const int | w_degree, | ||
const bool | useForce, | ||
long int & | forceVarDim | ||
) |
computeNumEqContinuous compute the number of equalities required by all the phases
pData | |
w_degree | //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf waypoints_c0_dc0_dc1 ) |
useForce | whether double description or force formulation is used to compute dynamic constraints) |
forceVarDim | reference value that stores the total force variable size |
long int bezier_com_traj::computeNumIneq | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const std::vector< int > & | phaseSwitch | ||
) |
long int bezier_com_traj::computeNumIneqContinuous | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const int | degree, | ||
const int | w_degree, | ||
const bool | useDD | ||
) |
computeNumIneqContinuous compute the number of inequalitie required by all the phases
pData | |
Ts | |
degree | |
w_degree | //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf waypoints_c0_dc0_dc1 ) |
useDD | whether double description or force formulation is used to compute dynamic constraints) |
int bezier_com_traj::computeNumSteps | ( | const double | T, |
const double | timeStep | ||
) |
void bezier_com_traj::computeRealCost | ( | const ProblemData & | pData, |
ResultData & | resData | ||
) |
void bezier_com_traj::computeVelCostFunctionDiscretized | ( | int | numPoints, |
const ProblemData & | pData, | ||
double | T, | ||
MatrixXX & | H, | ||
VectorX & | g | ||
) |
std::pair< MatrixXX, VectorX > bezier_com_traj::computeVelocityCost | ( | const ProblemData & | pData, |
double | T, | ||
std::vector< bezier_t::point_t > | pi | ||
) |
computeVelocityCost the matrices H and g defining a cost that minimise the integral of the squared velocity
pData | |
T |
std::vector< waypoint_t > bezier_com_traj::computeVelocityWaypoints | ( | const ProblemData & | pData, |
const double | T, | ||
std::vector< bezier_t::point_t > | pi = std::vector<bezier_t::point_t>() |
||
) |
computeWwaypoints compute the constant waypoints of dc(t) defined by the constraints on initial and final states
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial and final states
pData | |
T |
bezier_wp_t::t_point_t bezier_com_traj::computeWwaypoints | ( | const ProblemData & | pData, |
double | T | ||
) |
computeConstantWaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
computeWwaypoints compute the constant waypoints of w(t) defined by the constraints on initial and final states
pData | |
T |
int bezier_com_traj::dimVar | ( | const ProblemData & | pData | ) |
This file is used to choose the correct expressions of the curves waypoints, depending on the options set in ProblemData.constraints
std::pair<MatrixXX, VectorX> bezier_com_traj::dynamicStabilityConstraints | ( | const MatrixXX & | mH, |
const VectorX & | h, | ||
const Vector3 & | g, | ||
const waypoint_t & | w | ||
) |
std::pair<MatrixXX, VectorX> bezier_com_traj::dynamicStabilityConstraints_cross | ( | const MatrixXX & | mH, |
const VectorX & | h, | ||
const Vector3 & | g, | ||
const coefs_t & | c, | ||
const coefs_t & | ddc | ||
) |
coefs_t bezier_com_traj::evaluateAccelerationCurveAtTime | ( | const ProblemData & | pData, |
const std::vector< point_t > & | pi, | ||
double | T, | ||
double | t | ||
) |
evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
waypoint_t bezier_com_traj::evaluateAccelerationCurveWaypointAtTime | ( | const ProblemData & | pData, |
const double | T, | ||
const std::vector< point_t > & | pi, | ||
double | t | ||
) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
coefs_t bezier_com_traj::evaluateCurveAtTime | ( | const ProblemData & | pData, |
const std::vector< point_t > & | pi, | ||
double | t | ||
) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
This file is used to choose the correct expressions of the curves waypoints, depending on the options set in ProblemData.constraints
pi | constant waypoints of the curve |
t | param (normalized !) |
pi | constant waypoints of the curve |
t | param (normalized !) |
waypoint_t bezier_com_traj::evaluateCurveWaypointAtTime | ( | const ProblemData & | pData, |
const std::vector< point_t > & | pi, | ||
double | t | ||
) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
coefs_t bezier_com_traj::evaluateJerkCurveAtTime | ( | const ProblemData & | pData, |
const std::vector< point_t > & | pi, | ||
double | T, | ||
double | t | ||
) |
evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
waypoint_t bezier_com_traj::evaluateJerkCurveWaypointAtTime | ( | const ProblemData & | pData, |
const double | T, | ||
const std::vector< point_t > & | pi, | ||
double | t | ||
) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
coefs_t bezier_com_traj::evaluateVelocityCurveAtTime | ( | const ProblemData & | pData, |
const std::vector< point_t > & | pi, | ||
double | T, | ||
double | t | ||
) |
evaluateVelocityCurveAtTime compute the expression of the point on the curve dc at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
waypoint_t bezier_com_traj::evaluateVelocityCurveWaypointAtTime | ( | const ProblemData & | pData, |
const double | T, | ||
const std::vector< point_t > & | pi, | ||
double | t | ||
) |
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint pi and one free waypoint (x)
pi | constant waypoints of the curve |
t | param (normalized !) |
std::pair<MatrixXX, VectorX> bezier_com_traj::genCostFunction | ( | const ProblemData & | pData, |
const VectorX & | Ts, | ||
const double | T, | ||
const T_time & | timeArray, | ||
const long int & | dim | ||
) |
ResultDataCOMTraj bezier_com_traj::genTraj | ( | ResultData | resQp, |
const ProblemData & | pData, | ||
const double | T | ||
) |
coefs_t bezier_com_traj::initCoefs | ( | ) |
MatrixXX bezier_com_traj::initMatrixA | ( | const int | dimH, |
const std::vector< waypoint6_t > & | wps, | ||
const long int | extraConstraintSize, | ||
const bool | useAngMomentum | ||
) |
MatrixXX bezier_com_traj::initMatrixD | ( | const int | colG, |
const std::vector< waypoint6_t > & | wps, | ||
const long int | extraConstraintSize, | ||
const bool | useAngMomentum | ||
) |
T bezier_com_traj::initwp | ( | ) |
waypoint_t bezier_com_traj::initwp | ( | const size_t | rows, |
const size_t | cols | ||
) |
waypoint3_t bezier_com_traj::initwp< waypoint3_t > | ( | ) |
waypoint6_t bezier_com_traj::initwp< waypoint6_t > | ( | ) |
waypoint9_t bezier_com_traj::initwp< waypoint9_t > | ( | ) |
int bezier_com_traj::Normalize | ( | Ref_matrixXX | A, |
Ref_vectorX | b | ||
) |
normalize inequality constraints
|
inline |
|
inline |
waypoint_t bezier_com_traj::operator* | ( | const double | k, |
const waypoint_t & | w | ||
) |
waypoint_t bezier_com_traj::operator* | ( | const waypoint_t & | w, |
const double | k | ||
) |
waypoint_t bezier_com_traj::operator+ | ( | const waypoint_t & | w1, |
const waypoint_t & | w2 | ||
) |
waypoint_t bezier_com_traj::operator- | ( | const waypoint_t & | w1, |
const waypoint_t & | w2 | ||
) |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
void bezier_com_traj::printQHullFile | ( | const std::pair< MatrixXX, VectorX > & | Ab, |
VectorX | intPoint, | ||
const std::string & | fileName, | ||
bool | clipZ = false |
||
) |
write a polytope describe by A x <= b linear constraints in a given filename
int bezier_com_traj::removeZeroRows | ( | Ref_matrixXX & | A, |
Ref_vectorX & | b | ||
) |
Matrix3 bezier_com_traj::skew | ( | point_t_tC | x | ) |
skew symmetric matrix
ResultData bezier_com_traj::solve | ( | const std::pair< MatrixXX, VectorX > & | Ab, |
const std::pair< MatrixXX, VectorX > & | Dd, | ||
const std::pair< MatrixXX, VectorX > & | Hg, | ||
Cref_vectorX | minBounds, | ||
Cref_vectorX | maxBounds, | ||
const VectorX & | init, | ||
const solvers::SolverType | solver = solvers::SOLVER_QUADPROG |
||
) |
solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog, with x of fixed dimension 3
Ab | Inequality matrix and vector |
Dd | Equality matrix and vector |
Hg | Cost matrix and vector |
minBounds | lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x. Unbounded elements should be equal to -std::numeric_limits<double>::infinity(); |
maxBounds | upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x Unbounded elements should be equal to std::numeric_limits<double>::infinity(); |
solver | solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved |
ResultData bezier_com_traj::solve | ( | const std::pair< MatrixXX, VectorX > & | Ab, |
const std::pair< MatrixXX, VectorX > & | Hg, | ||
const VectorX & | init, | ||
const solvers::SolverType | solver = solvers::SOLVER_QUADPROG |
||
) |
solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3
Ab | Inequality matrix and vector |
Hg | Cost matrix and vector |
BEZIER_COM_TRAJ_DLLAPI ResultData bezier_com_traj::solve | ( | Cref_matrixXX | A, |
Cref_vectorX | b, | ||
Cref_matrixXX | D, | ||
Cref_vectorX | d, | ||
Cref_matrixXX | H, | ||
Cref_vectorX | g, | ||
Cref_vectorX | initGuess, | ||
const solvers::SolverType | solver = solvers::SOLVER_QUADPROG |
||
) |
solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog
A | Inequality matrix |
b | Inequality vector |
D | Equality matrix |
d | Equality vector |
H | Cost matrix |
g | cost Vector |
ResultData bezier_com_traj::solve | ( | Cref_matrixXX | A, |
Cref_vectorX | b, | ||
Cref_matrixXX | H, | ||
Cref_vectorX | g, | ||
Cref_vectorX | initGuess, | ||
const solvers::SolverType | solver | ||
) |
BEZIER_COM_TRAJ_DLLAPI ResultData bezier_com_traj::solve | ( | Cref_matrixXX | A, |
Cref_vectorX | b, | ||
Cref_matrixXX | H, | ||
Cref_vectorX | g, | ||
Cref_vectorX | initGuess, | ||
Cref_vectorX | minBounds, | ||
Cref_vectorX | maxBounds, | ||
const solvers::SolverType | solver = solvers::SOLVER_QUADPROG |
||
) |
solve x' h x + 2 g' x, subject to A*x <= b using quadprog
A | Inequality matrix |
b | Inequality vector |
H | Cost matrix |
g | cost Vector |
x | initGuess initial guess |
minBounds | lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x. Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP; |
maxBounds | upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or a size equal to x Unbounded elements should be higher or lower than solvers::UNBOUNDED_DOWN; |
solver | solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved |
ResultData bezier_com_traj::solve | ( | Cref_matrixXX | A, |
Cref_vectorX | ci0, | ||
Cref_matrixXX | D, | ||
Cref_vectorX | d, | ||
Cref_matrixXX | H, | ||
Cref_vectorX | g, | ||
Cref_vectorX | initGuess, | ||
Cref_vectorX | minBounds, | ||
Cref_vectorX | maxBounds, | ||
const solvers::SolverType | solver | ||
) |
ResultDataCOMTraj bezier_com_traj::solve0step | ( | const ProblemData & | pData, |
const std::vector< double > & | Ts, | ||
const double | timeStep = -1 |
||
) |
solve0step Tries to solve the 0-step capturability problem. Given the current contact phase, a COM position, and an initial velocity, tries to compute a feasible COM trajectory that stops the character without falling. In this specific implementation, the considered constraints are: init position and velocity, 0 velocity constraints (acceleration constraints are ignored)
pData | problem Data. Should contain only one contact phase. |
Ts | timelength of each contact phase. Should only contain one value |
timeStep | time that the solver has to stop. |
ResultDataCOMTraj bezier_com_traj::solveEndEffector | ( | const ProblemData & | pData, |
const Path & | path, | ||
const double | T, | ||
const double | weightDistance, | ||
bool | useVelCost = true |
||
) |
solveEndEffector Tries to produce a trajectory represented as a bezier curve that satisfy position, velocity and acceleration constraint for the initial and final point and that follow as close as possible the input trajectory
pData | problem Data. |
path | the path to follow, the class Path must implement the operator (double t) , t \in [0,1] return a Vector3 that give the position on the path for a given time |
T | time lenght of the trajectory |
timeStep | time that the solver has to stop |
std::vector<int> bezier_com_traj::stepIdPerPhase | ( | const T_time & | timeArray | ) |
void bezier_com_traj::switchContactPhase | ( | const ProblemData & | pData, |
MatrixXX & | A, | ||
VectorX & | b, | ||
MatrixXX & | mH, | ||
VectorX & | h, | ||
const waypoint_t & | wp_w, | ||
const ContactData & | phase, | ||
long int & | id_rows, | ||
int & | dimH | ||
) |
waypoint6_t bezier_com_traj::u0 | ( | point_t_tC | l0, |
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::u1 | ( | point_t_tC | l0, |
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::u2 | ( | point_t_tC | l0, |
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::u3 | ( | point_t_tC | , |
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::u4 | ( | point_t_tC | , |
const double | |||
) |
void bezier_com_traj::updateH | ( | const ProblemData & | pData, |
const ContactData & | phase, | ||
MatrixXX & | mH, | ||
VectorX & | h, | ||
int & | dimH | ||
) |
waypoint6_t bezier_com_traj::w0 | ( | point_t_tC | p0, |
point_t_tC | p1, | ||
point_t_tC | g, | ||
const Matrix3 & | p0X, | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::w1 | ( | point_t_tC | p0, |
point_t_tC | p1, | ||
point_t_tC | , | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const Matrix3 & | gX, | ||
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::w2 | ( | point_t_tC | p0, |
point_t_tC | p1, | ||
point_t_tC | g, | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const Matrix3 & | gX, | ||
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::w3 | ( | point_t_tC | p0, |
point_t_tC | p1, | ||
point_t_tC | g, | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const double | alpha | ||
) |
waypoint6_t bezier_com_traj::w4 | ( | point_t_tC | , |
point_t_tC | p1, | ||
point_t_tC | g, | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const Matrix3 & | , | ||
const double | alpha | ||
) |
const typedef Eigen::Ref<const MatrixX3>& bezier_com_traj::Cref_matrixX3 |
const typedef Eigen::Ref<const MatrixXX>& bezier_com_traj::Cref_matrixXX |
const typedef Eigen::Ref<const Vector3>& bezier_com_traj::Cref_vector3 |
const typedef Eigen::Ref<const Vector6>& bezier_com_traj::Cref_vector6 |
const typedef Eigen::Ref<const VectorX>& bezier_com_traj::Cref_vectorX |
const int bezier_com_traj::DIM_POINT = 3 |
const typedef Eigen::Ref<const point_t>& bezier_com_traj::point_t_tC |
const bool bezier_com_traj::verbose = false |