84 lines
2.4 KiB
C++
84 lines
2.4 KiB
C++
#pragma once
|
|
|
|
#include <cfloat>
|
|
#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();
|
|
}
|
|
|
|
void jacobian(Jacobianf &m_J, Link *link, Vector3f &ri, int i) {
|
|
// axes x, y et z
|
|
for (int j = 0; j < 3; j++) {
|
|
// crossP
|
|
m_J(0, i) = link->M(j, 1) * ri(2) - link->M(j, 2) * ri(1);
|
|
m_J(1, i) = link->M(j, 0) * ri(2) - link->M(j, 2) * ri(0);
|
|
m_J(2, i) = link->M(j, 0) * ri(1) - link->M(j, 1) * ri(0);
|
|
}
|
|
}
|
|
|
|
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 shift = endEffector->globalPosition() - link->globalPosition();
|
|
|
|
jacobian(m_J, link, shift, i);
|
|
}
|
|
|
|
// TODO Compute the error between the current end effector
|
|
// position and the target position by calling getError()
|
|
Vector3f dx;
|
|
float error = getError(dx);
|
|
|
|
// TODO Compute the change in the joint angles by solving:
|
|
// df/dtheta * delta_theta = delta_x
|
|
// where df/dtheta is the Jacobian matrix.
|
|
//
|
|
//
|
|
|
|
// TODO 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.
|
|
//
|
|
|
|
}
|