sim_cinematique_inverse/labo_ik/IKSolver.cpp

113 lines
3.2 KiB
C++

#pragma once
#include "IKSolver.h"
#include "Armature.h"
#include "SVD.h"
using namespace gti320;
namespace {
}
IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_armature), m_targetPos(_targetPos), m_J() {
}
float IKSolver::getError(Vector3f &dx) const {
// TODO Compute the error between the current end effector
// position and the target position
dx.setZero();
const int numLinks = m_armature->links.size();
Link *endEffector = m_armature->links[numLinks - 1];
Vector3f f_theta = endEffector->globalPosition();
Vector3f ddx = m_targetPos - f_theta;
dx(0) = ddx(0);
dx(1) = ddx(1);
dx(2) = ddx(2);
return ddx.norm();
}
template<typename _Scalar, int _Rows, int _Storage>
float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) {
_Scalar product = 0;
for (int i = 0; i < b.size(); i++) {
product += a(i, 0) * b(i);
}
return product;
}
void IKSolver::solve() {
const int numLinks = m_armature->links.size();
const int dim = 3 * (numLinks);
m_J.resize(3, dim);
// We assume that the last link is the "end effector"
//
Link *endEffector = m_armature->links[numLinks - 1];
// Build the Jacobian matrix m_J.
// Each column corresponds to a separate link
for (int i = 0; i < numLinks; i++) {
Link *link = m_armature->links[i];
Vector3f ri = endEffector->globalPosition() - link->globalPosition();
for (int j = 0; j < 3; j++) {
auto ji = m_J.block(0, i * 3 + j, 3, 1);
auto oij = link->M.block(0, j, 3, 1);
ji(0, 0) = oij(1, 0) * ri(2) - oij(2, 0) * ri(1);
ji(1, 0) = oij(2, 0) * ri(0) - oij(0, 0) * ri(2);
ji(2, 0) = oij(0, 0) * ri(1) - oij(1, 0) * ri(0);
}
}
// Compute the error between the current end effector
// position and the target position by calling getError()
Vector3f dx;
float error = getError(dx);
// Compute the change in the joint angles by solving:
// df/dtheta * delta_theta = delta_x
// where df/dtheta is the Jacobian matrix.
auto svd = SVD(m_J);
svd.decompose();
int m = svd.getSigma().size();
int rank = 0;
for (int i = 0; i < m; i++) {
if (svd.getSigma()(i) == 0) {
break;
}
rank++;
}
auto z = Vector<float, Dynamic>(m);
for (int i = 0; i < rank; i++) {
auto ui = svd.getU().block(0, i, 3, 1);
auto si = svd.getSigma()(i);
z(i) = dotP(ui, dx) / si;
}
Vector<float, Dynamic> d_theta = svd.getV() * z;
// Perform gradient descent method with line search
// to move the end effector toward the target position.
//
// Hint: use the Armature::unpack() and Armature::pack() functions
// to set and get the joint angles of the armature.
//
// Hint: whenever you change the joint angles, you must also call
// armature->updateKinematics() to compute the global position.
float alpha = 1;
Vector<float, Dynamic> initial_theta;
m_armature->pack(initial_theta);
do {
m_armature->unpack(initial_theta + alpha * d_theta);
m_armature->updateKinematics();
alpha /= 2;
} while (getError(dx) >= error);
}