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 |
|
✗ |
int main(int /* argc */, char ** /* argv */) |
10 |
|
|
{ |
11 |
|
✗ |
pinocchio::Model model; |
12 |
|
✗ |
pinocchio::buildModels::manipulator(model); |
13 |
|
✗ |
pinocchio::Data data(model); |
14 |
|
|
|
15 |
|
✗ |
const int JOINT_ID = 6; |
16 |
|
✗ |
const pinocchio::SE3 oMdes(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1., 0., 1.)); |
17 |
|
|
|
18 |
|
✗ |
Eigen::VectorXd q = pinocchio::neutral(model); |
19 |
|
✗ |
const double eps = 1e-4; |
20 |
|
✗ |
const int IT_MAX = 1000; |
21 |
|
✗ |
const double DT = 1e-1; |
22 |
|
✗ |
const double damp = 1e-6; |
23 |
|
|
|
24 |
|
✗ |
pinocchio::Data::Matrix6x J(6, model.nv); |
25 |
|
✗ |
J.setZero(); |
26 |
|
|
|
27 |
|
✗ |
bool success = false; |
28 |
|
|
typedef Eigen::Matrix<double, 6, 1> Vector6d; |
29 |
|
✗ |
Vector6d err; |
30 |
|
✗ |
Eigen::VectorXd v(model.nv); |
31 |
|
✗ |
for (int i = 0;; i++) |
32 |
|
|
{ |
33 |
|
✗ |
pinocchio::forwardKinematics(model, data, q); |
34 |
|
✗ |
const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(oMdes); |
35 |
|
✗ |
err = pinocchio::log6(iMd).toVector(); // in joint frame |
36 |
|
✗ |
if (err.norm() < eps) |
37 |
|
|
{ |
38 |
|
✗ |
success = true; |
39 |
|
✗ |
break; |
40 |
|
|
} |
41 |
|
✗ |
if (i >= IT_MAX) |
42 |
|
|
{ |
43 |
|
✗ |
success = false; |
44 |
|
✗ |
break; |
45 |
|
|
} |
46 |
|
✗ |
pinocchio::computeJointJacobian(model, data, q, JOINT_ID, J); // J in joint frame |
47 |
|
✗ |
pinocchio::Data::Matrix6 Jlog; |
48 |
|
✗ |
pinocchio::Jlog6(iMd.inverse(), Jlog); |
49 |
|
✗ |
J = -Jlog * J; |
50 |
|
✗ |
pinocchio::Data::Matrix6 JJt; |
51 |
|
✗ |
JJt.noalias() = J * J.transpose(); |
52 |
|
✗ |
JJt.diagonal().array() += damp; |
53 |
|
✗ |
v.noalias() = -J.transpose() * JJt.ldlt().solve(err); |
54 |
|
✗ |
q = pinocchio::integrate(model, q, v * DT); |
55 |
|
✗ |
if (!(i % 10)) |
56 |
|
✗ |
std::cout << i << ": error = " << err.transpose() << std::endl; |
57 |
|
|
} |
58 |
|
|
|
59 |
|
✗ |
if (success) |
60 |
|
|
{ |
61 |
|
✗ |
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 |
|
✗ |
std::cout << "\nresult: " << q.transpose() << std::endl; |
71 |
|
✗ |
std::cout << "\nfinal error: " << err.transpose() << std::endl; |
72 |
|
|
} |
73 |
|
|
|