hpp-bezier-com-traj  4.14.0
Multi contact trajectory generation for the COM using Bezier curves
utils.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2018, LAAS-CNRS
3  * Author: Steve Tonneau
4  */
5 
6 #ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
7 #define BEZIER_COM_TRAJ_LIB_UTILS_H
8 
9 #include <Eigen/Dense>
13 #include <vector>
14 
15 namespace bezier_com_traj {
16 
17 template <typename T>
18 T initwp();
19 waypoint_t initwp(const size_t rows, const size_t cols);
22 waypoint_t operator*(const double k, const waypoint_t& w);
23 waypoint_t operator*(const waypoint_t& w, const double k);
24 
25 struct waypoint_t {
28 
30 
32 
33  static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
34 
35  size_t size() const { return second.size(); }
36 
37  bool isApprox(const waypoint_t& other,
38  const value_type prec =
39  Eigen::NumTraits<value_type>::dummy_precision()) const {
40  return first.isApprox(other.first, prec) &&
41  second.isApprox(other.second, prec);
42  }
43 
44  bool operator==(const waypoint_t& other) const { return isApprox(other); }
45 
46  bool operator!=(const waypoint_t& other) const { return !(*this == other); }
47 };
48 
54 BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> >
55 ComputeBersteinPolynoms(const unsigned int degree);
56 
65 template <typename Bezier, typename Point>
66 BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag,
67  const double T,
68  const std::vector<Point>& pi,
69  const Point& x);
70 
79 T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
80  const unsigned int pointsPerPhase);
81 
89 T_time computeDiscretizedTime(const VectorX& phaseTimings,
90  const double timeStep);
91 
97 void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
98  const std::string& fileName, bool clipZ = false);
99 
104 
109 
110 } // end namespace bezier_com_traj
111 
112 template <typename Bezier, typename Point>
113 Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag,
114  const double T,
115  const std::vector<Point>& pi,
116  const Point& x) {
117  std::vector<Point> wps;
118  size_t i = 0;
119  if (flag & INIT_POS) {
120  wps.push_back(pi[i]);
121  i++;
122  if (flag & INIT_VEL) {
123  wps.push_back(pi[i]);
124  i++;
125  if (flag & INIT_ACC) {
126  wps.push_back(pi[i]);
127  i++;
128  }
129  }
130  }
131  wps.push_back(x);
132  i++;
133  if (flag & (END_VEL) && !(flag & (END_POS))) {
134  wps.push_back(x);
135  i++;
136  } else {
137  if (flag & END_ACC) {
138  assert(flag & END_VEL &&
139  "You cannot constrain final acceleration if final velocity is not "
140  "constrained.");
141  wps.push_back(pi[i]);
142  i++;
143  }
144  if (flag & END_VEL) {
145  assert(flag & END_POS &&
146  "You cannot constrain final velocity if final position is not "
147  "constrained.");
148  wps.push_back(pi[i]);
149  i++;
150  }
151  if (flag & END_POS) {
152  wps.push_back(pi[i]);
153  i++;
154  }
155  }
156  return Bezier(wps.begin(), wps.end(), 0., T);
157 }
158 
159 #endif
END_ACC
Definition: flags.hh:25
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
END_POS
Definition: flags.hh:23
INIT_ACC
Definition: flags.hh:22
INIT_POS
Definition: flags.hh:20
#define BEZIER_COM_TRAJ_DLLAPI
Definition: local_config.hh:52
Definition: common_solve_methods.hh:15
int Normalize(Ref_matrixXX A, Ref_vectorX b)
normalize inequality constraints
Definition: common_solve_methods.cpp:78
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
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
double value_type
Definition: definitions.hh:16
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:62
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:17
waypoint_t operator*(const double k, const waypoint_t &w)
Definition: utils.cpp:30
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
Definition: utils.cpp:120
const Eigen::Ref< const point_t > & point_t_tC
Definition: definitions.hh:43
std::vector< std::pair< double, int > > T_time
Definition: definitions.hh:59
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 numb...
Definition: utils.cpp:81
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition: solve_0_step.cpp:23
Eigen::Ref< VectorX > Ref_vectorX
Definition: definitions.hh:27
waypoint_t operator+(const waypoint_t &w1, const waypoint_t &w2)
Definition: utils.cpp:16
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)
Definition: solve_0_step.cpp:34
waypoint_t operator-(const waypoint_t &w1, const waypoint_t &w2)
Definition: utils.cpp:23
T_time computeDiscretizedTime(const VectorX &phaseTimings, const double timeStep)
computeDiscretizedTime build an array of discretized points in time, given the timestep....
Definition: utils.cpp:100
BEZIER_COM_TRAJ_DLLAPI std::vector< ndcurves::Bern< double > > ComputeBersteinPolynoms(const unsigned int degree)
Compute the Bernstein polynoms for a given degree.
Definition: utils.cpp:73
Eigen::Ref< MatrixXX > Ref_matrixXX
Definition: definitions.hh:29
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:21
Definition: utils.hh:25
static waypoint_t Zero(size_t dim)
Definition: utils.hh:33
size_t size() const
Definition: utils.hh:35
VectorX second
Definition: utils.hh:27
bool operator==(const waypoint_t &other) const
Definition: utils.hh:44
bool isApprox(const waypoint_t &other, const value_type prec=Eigen::NumTraits< value_type >::dummy_precision()) const
Definition: utils.hh:37
MatrixXX first
Definition: utils.hh:26
waypoint_t(MatrixXX A, VectorX b)
Definition: utils.hh:31
bool operator!=(const waypoint_t &other) const
Definition: utils.hh:46
waypoint_t()
Definition: utils.hh:29