| 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 |
|
|
|