14 typedef std::pair<double, point3_t>
coefs_t;
30 template <
typename Path>
32 const double weightDistance,
bool useVelCost =
true);
37 c.second = point3_t::Zero();
44 std::vector<bezier_t::point_t> pts;
45 pts.push_back(pData.
c0_);
46 pts.push_back((pData.
dc0_ * T / n) + pData.
c0_);
48 (n * n * pData.
c0_ - n * pData.
c0_ + 2 * n * pData.
dc0_ * T - 2 * pData.
dc0_ * T + pData.
ddc0_ * T * T) /
51 (n * n * pData.
c0_ - n * pData.
c0_ + 3 * n * pData.
dc0_ * T - 3 * pData.
dc0_ * T + 3 * pData.
ddc0_ * T * T) /
60 pts.push_back(pData.
c1_);
67 std::vector<bezier_t::point_t> pts;
68 pts.push_back(pData.
c0_);
77 (n * n * pData.
c1_ - n * pData.
c1_ - 3 * n * pData.
dc1_ * T + 3 * pData.
dc1_ * T + 3 * pData.
ddc1_ * T * T) /
80 (n * n * pData.
c1_ - n * pData.
c1_ - 2 * n * pData.
dc1_ * T + 2 * pData.
dc1_ * T + pData.
ddc1_ * T * T) /
82 pts.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
83 pts.push_back(pData.
c1_);
88 const std::vector<waypoint_t>& wps_vel,
const VectorX& acc_bounds,
90 const std::vector<waypoint_t>& wps_jerk = std::vector<waypoint_t>(),
92 assert(acc_bounds.size() ==
DIM_POINT &&
"Acceleration bounds should have the same dimension as the points");
93 assert(vel_bounds.size() ==
DIM_POINT &&
"Velocity bounds should have the same dimension as the points");
94 assert(jerk_bounds.size() ==
DIM_POINT &&
"Jerk bounds should have the same dimension as the points");
95 const int DIM_VAR =
dimVar(pData);
99 for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
100 if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_acc++;
102 for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
103 if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_vel++;
105 for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
106 if (wpcit->first.isZero(std::numeric_limits<double>::epsilon())) empty_jerk++;
110 (2 *
DIM_POINT * (wps_acc.size() - empty_acc + wps_vel.size() - empty_vel + wps_jerk.size() - empty_jerk)) +
114 b = VectorX::Zero(A.rows());
118 for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
119 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
126 for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin(); wpcit != wps_acc.end(); ++wpcit) {
127 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
135 for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
136 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
143 for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin(); wpcit != wps_vel.end(); ++wpcit) {
144 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
152 for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
153 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
160 for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin(); wpcit != wps_jerk.end(); ++wpcit) {
161 if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
170 MatrixXX mxz = MatrixXX::Zero(DIM_VAR, DIM_VAR);
172 VectorX nxz = VectorX::Zero(DIM_VAR);
173 while (j < (DIM_VAR)) {
175 nxz[j] = -std::min(pData.
c0_[2], pData.
c1_[2]);
178 A.block(i *
DIM_POINT, 0, DIM_VAR, DIM_VAR) = mxz;
183 assert((i *
DIM_POINT + DIM_VAR) == A.rows() &&
"Constraints matrix were not correctly initialized");
193 std::vector<point3_t> pts_path) {
194 assert(numPoints == pts_path.size() &&
"Pts_path size must be equal to numPoints");
195 double step = 1. / (double)(numPoints - 1);
201 for (
size_t i = 0; i < numPoints; ++i) {
208 g += ((c_wp.
second - pk).transpose() * c_wp.
first).transpose();
210 double norm = H.norm();
213 return std::make_pair(H, g);
216 template <
typename Path>
219 double step = 1. /(double)(numPoints - 1);
220 std::vector<point3_t> pts_path;
221 for (
size_t i = 0; i < numPoints; ++i) pts_path.push_back(path(((
double)i * step)));
230 else if (
dimVar(pData) == 9) {
231 wps[4] = res.x.segment<3>(0);
232 wps[5] = res.x.segment<3>(3);
233 wps[6] = res.x.segment<3>(6);
234 }
else if (
dimVar(pData) == 15) {
235 wps[4] = res.x.segment<3>(0);
236 wps[5] = res.x.segment<3>(3);
237 wps[6] = res.x.segment<3>(6);
238 wps[7] = res.x.segment<3>(9);
239 wps[8] = res.x.segment<3>(12);
242 if (
verbose) std::cout <<
"bezier curve created, size = " << res.
c_of_t_.size_ << std::endl;
246 double step = 1. / (numPoints - 1);
247 std::vector<waypoint_t> cks;
249 for (
int i = 0; i < numPoints; ++i) {
253 g = VectorX::Zero(
dimVar(pData));
254 for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
257 for (
int i = 0; i < (
dimVar(pData) / 3); ++i) {
258 H.block<3, 3>(i * 3, i * 3) += Matrix3::Identity() * ckcit->first(0, i * 3) * ckcit->first(0, i * 3);
259 g.segment<3>(i * 3) += ckcit->second.segment<3>(0) * ckcit->first(0, i * 3);
273 double step = 1. / (numPoints - 1);
274 std::vector<waypoint_t> cks;
276 for (
int i = 0; i < numPoints; ++i) {
280 g = VectorX::Zero(
dimVar(pData));
281 for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
282 H += (ckcit->first.transpose() * ckcit->first);
283 g += ckcit->first.transpose() * ckcit->second;
295 double step = 1. / (numPoints - 1);
297 std::vector<waypoint_t> cks;
299 for (
int i = 0; i < numPoints; ++i) {
303 g = VectorX::Zero(
dimVar(pData));
304 for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin(); ckcit != cks.end(); ++ckcit) {
305 H += (ckcit->first.transpose() * ckcit->first);
306 g += ckcit->first.transpose() * ckcit->second;
318 std::vector<bezier_t::point_t> pi) {
323 Vector3 jerk_bounds(10000, 10000, 10000);
324 Vector3 acc_bounds(10000, 10000, 10000);
325 Vector3 vel_bounds(10000, 10000, 10000);
329 return std::make_pair(A, b);
332 template <
typename Path>
334 const double weightDistance,
bool ,
335 std::vector<bezier_t::point_t> pi) {
336 assert(weightDistance >= 0. && weightDistance <= 1. &&
"WeightDistance must be between 0 and 1");
337 double weightSmooth = 1. - weightDistance;
338 const int DIM_VAR =
dimVar(pData);
342 std::pair<MatrixXX, VectorX> Hg_smooth, Hg_rrt;
344 if (weightDistance > 0)
345 Hg_rrt = computeDistanceCostFunction<Path>(50, pData, T, path);
347 Hg_rrt.first = MatrixXX::Zero(DIM_VAR, DIM_VAR);
348 Hg_rrt.second = VectorX::Zero(DIM_VAR);
359 H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
360 g = VectorX::Zero(DIM_VAR);
361 H = weightSmooth * (Hg_smooth.first) + weightDistance * Hg_rrt.first;
362 g = weightSmooth * (Hg_smooth.second) + weightDistance * Hg_rrt.second;
366 return std::make_pair(H, g);
369 template <
typename Path>
371 const double weightDistance,
bool useVelCost) {
372 if (
verbose) std::cout <<
"solve end effector, T = " << T << std::endl;
377 std::cout <<
"End eff A = " << std::endl << Ab.first << std::endl;
378 std::cout <<
"End eff b = " << std::endl << Ab.second << std::endl;
379 std::cout <<
"End eff H = " << std::endl << Hg.first << std::endl;
380 std::cout <<
"End eff g = " << std::endl << Hg.second << std::endl;
381 std::cout <<
"Dim Var = " <<
dimVar(pData) << std::endl;
382 std::cout <<
"Dim H = " << Hg.first.rows() <<
" x " << Hg.first.cols() << std::endl;
383 std::cout <<
"Dim g = " << Hg.second.rows() << std::endl;
384 std::cout <<
"Dim A = " << Ab.first.rows() <<
" x " << Ab.first.cols() << std::endl;
385 std::cout <<
"Dim b = " << Ab.first.rows() << std::endl;
391 if (
verbose) std::cout <<
"Init = " << std::endl << init.transpose() << std::endl;
396 if (resQp.success_) {
404 std::cout <<
"Solved, success = " << res.success_ <<
" x = " << res.x.transpose() << std::endl;
405 std::cout <<
"Final cost : " << resQp.cost_ << std::endl;