60 lines
1.5 KiB
C++
60 lines
1.5 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();
|
||
|
|
return FLT_MAX;
|
||
|
|
}
|
||
|
|
|
||
|
|
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];
|
||
|
|
|
||
|
|
// TODO Juild the Jacobian matrix m_J.
|
||
|
|
// Each column corresponds to a separate
|
||
|
|
|
||
|
|
// TODO Compute the error between the current end effector
|
||
|
|
// position and the target position by calling getError()
|
||
|
|
//
|
||
|
|
|
||
|
|
// 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.
|
||
|
|
//
|
||
|
|
|
||
|
|
}
|