| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
#include "pinocchio/parsers/sample-models.hpp" |
||
2 |
#include "pinocchio/spatial/explog.hpp" |
||
3 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
4 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
5 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
6 |
|||
7 |
1 |
int main(int /* argc */, char ** /* argv */) |
|
8 |
{ |
||
9 |
✓✗ | 2 |
pinocchio::Model model; |
10 |
✓✗ | 1 |
pinocchio::buildModels::manipulator(model); |
11 |
✓✗ | 2 |
pinocchio::Data data(model); |
12 |
|||
13 |
1 |
const int JOINT_ID = 6; |
|
14 |
✓✗✓✗ ✓✗ |
1 |
const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); |
15 |
|||
16 |
✓✗ | 2 |
Eigen::VectorXd q = pinocchio::neutral(model); |
17 |
1 |
const double eps = 1e-4; |
|
18 |
1 |
const int IT_MAX = 1000; |
|
19 |
1 |
const double DT = 1e-1; |
|
20 |
1 |
const double damp = 1e-6; |
|
21 |
|||
22 |
✓✗ | 2 |
pinocchio::Data::Matrix6x J(6,model.nv); |
23 |
✓✗ | 1 |
J.setZero(); |
24 |
|||
25 |
1 |
bool success = false; |
|
26 |
typedef Eigen::Matrix<double, 6, 1> Vector6d; |
||
27 |
✓✗ | 1 |
Vector6d err; |
28 |
✓✗ | 1 |
Eigen::VectorXd v(model.nv); |
29 |
1 |
for (int i=0;;i++) |
|
30 |
{ |
||
31 |
✓✗ | 91 |
pinocchio::forwardKinematics(model,data,q); |
32 |
✓✗ | 91 |
const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes); |
33 |
✓✗✓✗ ✓✗ |
91 |
err = pinocchio::log6(iMd).toVector(); // in joint frame |
34 |
✓✗✓✓ |
91 |
if(err.norm() < eps) |
35 |
{ |
||
36 |
1 |
success = true; |
|
37 |
1 |
break; |
|
38 |
} |
||
39 |
✗✓ | 90 |
if (i >= IT_MAX) |
40 |
{ |
||
41 |
success = false; |
||
42 |
break; |
||
43 |
} |
||
44 |
✓✗ | 90 |
pinocchio::computeJointJacobian(model,data,q,JOINT_ID,J); // J in joint frame |
45 |
✓✗ | 90 |
pinocchio::Data::Matrix6 Jlog; |
46 |
✓✗✓✗ |
90 |
pinocchio::Jlog6(iMd.inverse(), Jlog); |
47 |
✓✗✓✗ ✓✗ |
90 |
J = -Jlog * J; |
48 |
✓✗ | 90 |
pinocchio::Data::Matrix6 JJt; |
49 |
✓✗✓✗ ✓✗✓✗ |
90 |
JJt.noalias() = J * J.transpose(); |
50 |
✓✗✓✗ ✓✗ |
90 |
JJt.diagonal().array() += damp; |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
90 |
v.noalias() = - J.transpose() * JJt.ldlt().solve(err); |
52 |
✓✗✓✗ |
90 |
q = pinocchio::integrate(model,q,v*DT); |
53 |
✓✓ | 90 |
if(!(i%10)) |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
std::cout << i << ": error = " << err.transpose() << std::endl; |
55 |
90 |
} |
|
56 |
|||
57 |
✓✗ | 1 |
if(success) |
58 |
{ |
||
59 |
✓✗✓✗ |
1 |
std::cout << "Convergence achieved!" << std::endl; |
60 |
} |
||
61 |
else |
||
62 |
{ |
||
63 |
std::cout << "\nWarning: the iterative algorithm has not reached convergence to the desired precision" << std::endl; |
||
64 |
} |
||
65 |
|||
66 |
✓✗✓✗ ✓✗✓✗ |
1 |
std::cout << "\nresult: " << q.transpose() << std::endl; |
67 |
✓✗✓✗ ✓✗✓✗ |
1 |
std::cout << "\nfinal error: " << err.transpose() << std::endl; |
68 |
1 |
} |
| Generated by: GCOVR (Version 4.2) |