GCC Code Coverage Report

Directory: ./
File: examples/inverse-kinematics.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 41 45 91.1%
Branches: 62 124 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-6;
23
24
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::Data::Matrix6x J(6, model.nv);
25
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 J.setZero();
26
27 1 bool success = false;
28 typedef Eigen::Matrix<double, 6, 1> Vector6d;
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Vector6d err;
30
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::VectorXd v(model.nv);
31 1 for (int i = 0;; i++)
32 {
33
1/2
✓ Branch 1 taken 91 times.
✗ Branch 2 not taken.
 91 pinocchio::forwardKinematics(model, data, q);
34
1/2
✓ Branch 2 taken 91 times.
✗ Branch 3 not taken.
 91 const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes);
35
3/6
✓ Branch 1 taken 91 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 91 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 91 times.
✗ Branch 8 not taken.
 91 err = pinocchio::log6(iMd).toVector(); // in joint frame
36
3/4
✓ Branch 1 taken 91 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 90 times.
 91 if (err.norm() < eps)
37 {
38 1 success = true;
39 1 break;
40 }
41
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 90 times.
 90 if (i >= IT_MAX)
42 {
43 ✗ success = false;
44 ✗ break;
45 }
46
1/2
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
 90 pinocchio::computeJointJacobian(model, data, q, JOINT_ID, J); // J in joint frame
47
1/2
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
 90 pinocchio::Data::Matrix6 Jlog;
48
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 pinocchio::Jlog6(iMd.inverse(), Jlog);
49
3/6
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
 90 J = -Jlog * J;
50
1/2
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
 90 pinocchio::Data::Matrix6 JJt;
51
4/8
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
 90 JJt.noalias() = J * J.transpose();
52
3/6
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
 90 JJt.diagonal().array() += damp;
53
7/14
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 90 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 90 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 90 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 90 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 90 times.
✗ Branch 20 not taken.
 90 v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
54
2/4
✓ Branch 1 taken 90 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 90 times.
✗ Branch 5 not taken.
 90 q = pinocchio::integrate(model, q, v * DT);
55
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 81 times.
 90 if (!(i % 10))
56
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 9 std::cout << i << ": error = " << err.transpose() << std::endl;
57 90 }
58
59
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (success)
60 {
61
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;
62 }
63 else
64 {
65 std::cout
66 ✗ << "\nWarning: the iterative algorithm has not reached convergence to the desired precision"
67 ✗ << std::endl;
68 }
69
70
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;
71
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;
72 1 }
73