sim_cinematique_inverse/labo_ik/IKSolver.cpp

61 lines
1.5 KiB
C++
Raw Normal View History

2024-04-01 17:18:18 -04:00
#pragma once
#include <cfloat>
2024-04-01 17:18:18 -04:00
#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.
//
}