| Directory: | ./ |
|---|---|
| File: | src/costfunction_definition.cpp |
| Date: | 2025-03-18 04:20:50 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 15 | 29 | 51.7% |
| Branches: | 23 | 90 | 25.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Copyright 2017, LAAS-CNRS | ||
| 3 | * Author: Steve Tonneau | ||
| 4 | */ | ||
| 5 | |||
| 6 | #include <hpp/bezier-com-traj/common_solve_methods.hh> | ||
| 7 | #include <hpp/bezier-com-traj/cost/costfunction_definition.hh> | ||
| 8 | #include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh> | ||
| 9 | |||
| 10 | namespace bezier_com_traj { | ||
| 11 | namespace cost { | ||
| 12 | // cost : min distance between x and midPoint : | ||
| 13 | ✗ | void computeCostMidPoint(const ProblemData& pData, const VectorX& /*Ts*/, | |
| 14 | const double /*T*/, const T_time& /*timeArray*/, | ||
| 15 | MatrixXX& H, VectorX& g) { | ||
| 16 | // cost : x' H x + 2 x g' | ||
| 17 | ✗ | Vector3 midPoint = (pData.c0_ + pData.c1_) / | |
| 18 | ✗ | 2.; // todo : replace it with point found by planning ?? | |
| 19 | ✗ | H.block<3, 3>(0, 0) = Matrix3::Identity(); | |
| 20 | ✗ | g.head<3>() = -midPoint; | |
| 21 | } | ||
| 22 | |||
| 23 | // cost : min distance between end velocity and the one computed by planning | ||
| 24 | ✗ | void computeCostEndVelocity(const ProblemData& pData, const VectorX& /*Ts*/, | |
| 25 | const double T, const T_time& /*timeArray*/, | ||
| 26 | MatrixXX& H, VectorX& g) { | ||
| 27 | ✗ | if (pData.constraints_.flag_ && END_VEL) | |
| 28 | ✗ | throw std::runtime_error( | |
| 29 | "Can't use computeCostEndVelocity as cost function when end velocity " | ||
| 30 | ✗ | "is a contraint"); | |
| 31 | ✗ | coefs_t v = computeFinalVelocityPoint(pData, T); | |
| 32 | ✗ | H.block<3, 3>(0, 0) = Matrix3::Identity() * v.first * v.first; | |
| 33 | ✗ | g.head<3>() = v.first * (v.second - pData.dc1_); | |
| 34 | } | ||
| 35 | |||
| 36 | // TODO this is temporary.The acceleration integral can be computed analitcally | ||
| 37 | 61 | void computeCostMinAcceleration(const ProblemData& pData, const VectorX& Ts, | |
| 38 | const double /*T*/, const T_time& timeArray, | ||
| 39 | MatrixXX& H, VectorX& g) { | ||
| 40 | 61 | double t_total = 0.; | |
| 41 |
3/4✓ Branch 1 taken 177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 177 times.
✓ Branch 5 taken 61 times.
|
238 | for (int i = 0; i < Ts.size(); ++i) t_total += Ts[i]; |
| 42 | std::vector<coefs_t> wps_ddc = | ||
| 43 | computeDiscretizedAccelerationWaypoints<point3_t>(pData, t_total, | ||
| 44 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
61 | timeArray); |
| 45 | // cost : x' H x + 2 x g' | ||
| 46 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | H.block<3, 3>(0, 0) = Matrix3::Zero(); |
| 47 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | g.head<3>() = Vector3::Zero(); |
| 48 |
2/2✓ Branch 1 taken 1178 times.
✓ Branch 2 taken 61 times.
|
1239 | for (size_t i = 0; i < wps_ddc.size(); ++i) { |
| 49 |
1/2✓ Branch 1 taken 1178 times.
✗ Branch 2 not taken.
|
1178 | H.block<3, 3>(0, 0) += |
| 50 |
4/8✓ Branch 3 taken 1178 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1178 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1178 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1178 times.
✗ Branch 13 not taken.
|
2356 | Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first; |
| 51 |
3/6✓ Branch 3 taken 1178 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1178 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1178 times.
✗ Branch 10 not taken.
|
1178 | g.head<3>() += wps_ddc[i].first * wps_ddc[i].second; |
| 52 | } | ||
| 53 | 61 | } | |
| 54 | |||
| 55 | typedef void (*costCompute)(const ProblemData&, const VectorX&, const double, | ||
| 56 | const T_time&, MatrixXX&, VectorX&); | ||
| 57 | typedef std::map<CostFunction, costCompute> T_costCompute; | ||
| 58 | static const T_costCompute costs = | ||
| 59 | boost::assign::map_list_of(ACCELERATION, computeCostMinAcceleration)( | ||
| 60 | DISTANCE_TRAVELED, computeCostMidPoint)(TARGET_END_VELOCITY, | ||
| 61 | computeCostEndVelocity); | ||
| 62 | |||
| 63 | 61 | void genCostFunction(const ProblemData& pData, const VectorX& Ts, | |
| 64 | const double T, const T_time& timeArray, MatrixXX& H, | ||
| 65 | VectorX& g) { | ||
| 66 |
1/2✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
|
61 | T_costCompute::const_iterator cit = costs.find(pData.costFunction_); |
| 67 |
1/2✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
|
61 | if (cit != costs.end()) |
| 68 |
1/2✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
|
61 | return cit->second(pData, Ts, T, timeArray, H, g); |
| 69 | else { | ||
| 70 | ✗ | std::cout << "Unknown cost function" << std::endl; | |
| 71 | ✗ | throw std::runtime_error("Unknown cost function"); | |
| 72 | } | ||
| 73 | } | ||
| 74 | |||
| 75 | } // namespace cost | ||
| 76 | } // namespace bezier_com_traj | ||
| 77 |