hpp-bezier-com-traj  4.10.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 
12 
13 #include <Eigen/Dense>
14 
15 #include <vector>
16 
17 namespace bezier_com_traj {
18 
19 template <typename T>
20 T initwp();
21 waypoint_t initwp(const size_t rows, const size_t cols);
22 waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2);
23 waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2);
24 waypoint_t operator*(const double k, const waypoint_t& w);
25 waypoint_t operator*(const waypoint_t& w, const double k);
26 
27 struct waypoint_t {
30 
32 
34 
35  static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
36 
37  size_t size() const{return second.size();}
38 
39  bool isApprox(const waypoint_t& other, const value_type prec = Eigen::NumTraits<value_type>::dummy_precision()) const{
40  return first.isApprox(other.first,prec) && second.isApprox(other.second,prec);
41  }
42 
43  bool operator==(const waypoint_t& other) const{ return isApprox(other); }
44 
45  bool operator!=(const waypoint_t& other) const{ return !(*this == other); }
46 };
47 
53 BEZIER_COM_TRAJ_DLLAPI std::vector<curves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree);
54 
63 template <typename Bezier, typename Point>
64 BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag, const double T,
65  const std::vector<Point>& pi, const Point& x);
66 
75 T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase);
76 
84 T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep);
85 
91 void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint, const std::string& fileName,
92  bool clipZ = false);
93 
98 
103 
104 } // end namespace bezier_com_traj
105 
106 template <typename Bezier, typename Point>
107 Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const double T, const std::vector<Point>& pi,
108  const Point& x) {
109  std::vector<Point> wps;
110  size_t i = 0;
111  if (flag & INIT_POS) {
112  wps.push_back(pi[i]);
113  i++;
114  if (flag & INIT_VEL) {
115  wps.push_back(pi[i]);
116  i++;
117  if (flag & INIT_ACC) {
118  wps.push_back(pi[i]);
119  i++;
120  }
121  }
122  }
123  wps.push_back(x);
124  i++;
125  if (flag & (END_VEL) && !(flag & (END_POS))) {
126  wps.push_back(x);
127  i++;
128  } else {
129  if (flag & END_ACC) {
130  assert(flag & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
131  wps.push_back(pi[i]);
132  i++;
133  }
134  if (flag & END_VEL) {
135  assert(flag & END_POS && "You cannot constrain final velocity if final position is not constrained.");
136  wps.push_back(pi[i]);
137  i++;
138  }
139  if (flag & END_POS) {
140  wps.push_back(pi[i]);
141  i++;
142  }
143  }
144  return Bezier(wps.begin(), wps.end(), 0.,T);
145 }
146 
147 #endif
bezier_com_traj::waypoint_t::size
size_t size() const
Definition: utils.hh:37
bezier_com_traj::waypoint_t::operator!=
bool operator!=(const waypoint_t &other) const
Definition: utils.hh:45
INIT_ACC
INIT_ACC
Definition: flags.hh:22
bezier_com_traj::ComputeBersteinPolynoms
BEZIER_COM_TRAJ_DLLAPI std::vector< curves::Bern< double > > ComputeBersteinPolynoms(const unsigned int degree)
Compute the Bernstein polynoms for a given degree.
Definition: utils.cpp:67
BEZIER_COM_TRAJ_DLLAPI
#define BEZIER_COM_TRAJ_DLLAPI
Definition: local_config.hh:52
bezier_com_traj::computeBezierCurve
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
bezier_com_traj::initwp
T initwp()
bezier_com_traj::waypoint_t
Definition: utils.hh:27
bezier_com_traj::value_type
double value_type
Definition: definitions.hh:15
bezier_com_traj::operator+
waypoint_t operator+(const waypoint_t &w1, const waypoint_t &w2)
Definition: utils.cpp:16
bezier_com_traj::w1
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:22
bezier_com_traj::VectorX
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:23
bezier_com_traj::computeDiscretizedTime
T_time computeDiscretizedTime(const VectorX &phaseTimings, const double timeStep)
computeDiscretizedTime build an array of discretized points in time, given the timestep....
Definition: utils.cpp:90
bezier_com_traj::T_time
std::vector< std::pair< double, int > > T_time
Definition: definitions.hh:58
END_POS
END_POS
Definition: flags.hh:23
bezier_com_traj::printQHullFile
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:108
bezier_com_traj::waypoint_t::second
VectorX second
Definition: utils.hh:29
bezier_com_traj::waypoint_t::waypoint_t
waypoint_t(MatrixXX A, VectorX b)
Definition: utils.hh:33
bezier_com_traj::waypoint_t::isApprox
bool isApprox(const waypoint_t &other, const value_type prec=Eigen::NumTraits< value_type >::dummy_precision()) const
Definition: utils.hh:39
bezier_com_traj::waypoint_t::first
MatrixXX first
Definition: utils.hh:28
bezier_com_traj::point_t_tC
const typedef Eigen::Ref< const point_t > & point_t_tC
Definition: definitions.hh:42
bezier_com_traj::Matrix3
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
bezier_com_traj::waypoint_t::operator==
bool operator==(const waypoint_t &other) const
Definition: utils.hh:43
bezier_com_traj::MatrixXX
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:20
local_config.hh
INIT_VEL
INIT_VEL
Definition: flags.hh:21
bezier_com_traj::Normalize
int Normalize(Ref_matrixXX A, Ref_vectorX b)
normalize inequality constraints
Definition: common_solve_methods.cpp:72
definitions.hh
bezier_com_traj::Ref_matrixXX
Eigen::Ref< MatrixXX > Ref_matrixXX
Definition: definitions.hh:28
flags.hh
bezier_com_traj::w2
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:32
bezier_com_traj::operator*
waypoint_t operator*(const double k, const waypoint_t &w)
Definition: utils.cpp:28
END_ACC
END_ACC
Definition: flags.hh:25
bezier_com_traj::skew
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
bezier_com_traj::operator-
waypoint_t operator-(const waypoint_t &w1, const waypoint_t &w2)
Definition: utils.cpp:22
bezier_com_traj::waypoint_t::Zero
static waypoint_t Zero(size_t dim)
Definition: utils.hh:35
bezier_com_traj
Definition: common_solve_methods.hh:16
bezier_com_traj::waypoint_t::waypoint_t
waypoint_t()
Definition: utils.hh:31
END_VEL
END_VEL
Definition: flags.hh:24
INIT_POS
INIT_POS
Definition: flags.hh:20
bezier_com_traj::computeDiscretizedTimeFixed
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:73
bezier_com_traj::Ref_vectorX
Eigen::Ref< VectorX > Ref_vectorX
Definition: definitions.hh:26