| Directory: | ./ |
|---|---|
| File: | examples/inverse-kinematics-3d.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 37 | 41 | 90.2% |
| Branches: | 55 | 110 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | #include <iostream> | ||
| 2 | |||
| 3 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 4 | #include "pinocchio/spatial/explog.hpp" | ||
| 5 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 6 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 7 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 8 | |||
| 9 | 1 | int main(int /* argc */, char ** /* argv */) | |
| 10 | { | ||
| 11 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::Model model; |
| 12 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::buildModels::manipulator(model); |
| 13 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::Data data(model); |
| 14 | |||
| 15 | 1 | const int JOINT_ID = 6; | |
| 16 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); |
| 17 | |||
| 18 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd q = pinocchio::neutral(model); |
| 19 | 1 | const double eps = 1e-4; | |
| 20 | 1 | const int IT_MAX = 1000; | |
| 21 | 1 | const double DT = 1e-1; | |
| 22 | 1 | const double damp = 1e-12; | |
| 23 | |||
| 24 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::Data::Matrix6x joint_jacobian(6, model.nv); |
| 25 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | joint_jacobian.setZero(); |
| 26 | |||
| 27 | 1 | bool success = false; | |
| 28 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::Vector3d err; |
| 29 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd v(model.nv); |
| 30 | 1 | for (int i = 0;; i++) | |
| 31 | { | ||
| 32 |
1/2✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
|
94 | pinocchio::forwardKinematics(model, data, q); |
| 33 |
1/2✓ Branch 2 taken 94 times.
✗ Branch 3 not taken.
|
94 | const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes); |
| 34 |
2/4✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
|
94 | err = iMd.translation(); // in joint frame |
| 35 |
3/4✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 93 times.
|
94 | if (err.norm() < eps) |
| 36 | { | ||
| 37 | 1 | success = true; | |
| 38 | 1 | break; | |
| 39 | } | ||
| 40 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 93 times.
|
93 | if (i >= IT_MAX) |
| 41 | { | ||
| 42 | ✗ | success = false; | |
| 43 | ✗ | break; | |
| 44 | } | ||
| 45 |
1/2✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
|
93 | pinocchio::computeJointJacobian( |
| 46 | model, data, q, JOINT_ID, joint_jacobian); // joint_jacobian expressed in the joint frame | ||
| 47 |
2/4✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
|
93 | const auto J = -joint_jacobian.topRows<3>(); // Jacobian associated with the error |
| 48 |
6/12✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 93 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 93 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 93 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 93 times.
✗ Branch 17 not taken.
|
93 | const Eigen::Matrix3d JJt = J * J.transpose() + damp * Eigen::Matrix3d::Identity(); |
| 49 |
7/14✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 93 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 93 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 93 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 93 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 93 times.
✗ Branch 20 not taken.
|
93 | v.noalias() = -J.transpose() * JJt.ldlt().solve(err); |
| 50 |
2/4✓ Branch 1 taken 93 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 93 times.
✗ Branch 5 not taken.
|
93 | q = pinocchio::integrate(model, q, v * DT); |
| 51 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 83 times.
|
93 | if (!(i % 10)) |
| 52 |
5/10✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
|
10 | std::cout << i << ": error = " << err.transpose() << std::endl; |
| 53 | 93 | } | |
| 54 | |||
| 55 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
1 | if (success) |
| 56 | { | ||
| 57 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | std::cout << "Convergence achieved!" << std::endl; |
| 58 | } | ||
| 59 | else | ||
| 60 | { | ||
| 61 | std::cout | ||
| 62 | ✗ | << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" | |
| 63 | ✗ | << std::endl; | |
| 64 | } | ||
| 65 | |||
| 66 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | std::cout << "\nresult: " << q.transpose() << std::endl; |
| 67 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | std::cout << "\nfinal error: " << err.transpose() << std::endl; |
| 68 | 1 | } | |
| 69 |