Matrice jacobienne

This commit is contained in:
FyloZ 2024-04-06 20:50:36 -04:00
parent b4e8eee37f
commit ba006846d2
Signed by: william
GPG Key ID: 835378AE9AF4AE97
3 changed files with 75 additions and 17 deletions

View File

@ -43,6 +43,13 @@ void Link::forward() {
} }
} }
Vector3f Link::globalPosition() {
Vector3f pos;
pos(0) = M(0, 3);
pos(1) = M(1, 3);
pos(2) = M(2, 3);
return pos;
}
Armature::Armature() : links(), root(nullptr) { Armature::Armature() : links(), root(nullptr) {
@ -60,18 +67,33 @@ void Armature::updateKinematics() {
} }
void Armature::pack(Vector<float, Dynamic> &theta) { void Armature::pack(Vector<float, Dynamic> &theta) {
// TODO Collect the Euler angles of each link and put them // Collect the Euler angles of each link and put them
// into the dense vector @a theta // into the dense vector @a theta
theta.resize(links.size() * 3);
for (int i = 0; i < links.size(); i++) {
Link *link = links[i];
int link_index = i * 3;
theta(link_index) = link->eulerAng(0);
theta(link_index + 1) = link->eulerAng(1);
theta(link_index + 2) = link->eulerAng(2);
}
} }
void Armature::unpack(const Vector<float, Dynamic> &theta) { void Armature::unpack(const Vector<float, Dynamic> &theta) {
const int numLinks = links.size(); const int numLinks = links.size();
assert(theta.size() == 3 * numLinks); assert(theta.size() == 3 * numLinks);
// TODO Extract the Euler angles contained in the // Extract the Euler angles contained in the
// dense vector @a theta and update the angles // dense vector @a theta and update the angles
// for each link in the armature. // for each link in the armature.
// for (int i = 0; i < links.size(); i++) {
Link *link = links[i];
int link_index = i * 3;
link->eulerAng(0) = theta(link_index);
link->eulerAng(1) = theta(link_index + 1);
link->eulerAng(2) = theta(link_index + 2);
}
} }

View File

@ -27,6 +27,19 @@ namespace gti320
// //
void forward(); void forward();
// NOUVELLE FONCTION
// Gets the position of the link from the global rigid transformation matrix.
Vector3f globalPosition();
// NOUVELLE FONCTION
// Gets the rotation of the link from the global rigid transformation matrix;
// inline Matrix3f globalRotation() const {
// Matrix3f rotation(M.block(0, 0, 3, 3));
// rotation.resize()
// rotation = M.block(0, 0, 3, 3);
// return M.block(0, 0, 3, 3);
// }
Vector3f eulerAng; // Euler angles giving rotation relative to the parent. Vector3f eulerAng; // Euler angles giving rotation relative to the parent.
Vector3f trans; // Translation giving position relative to the parent. Vector3f trans; // Translation giving position relative to the parent.
Matrix4f M; // Global rigid transformation of the link, computed in forward(). Matrix4f M; // Global rigid transformation of the link, computed in forward().

View File

@ -7,39 +7,62 @@
using namespace gti320; using namespace gti320;
namespace namespace {
{
} }
IKSolver::IKSolver(Armature* _armature, Vector3f& _targetPos) : m_armature(_armature), m_targetPos(_targetPos), m_J() IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_armature), m_targetPos(_targetPos), m_J() {
{
} }
float IKSolver::getError(Vector3f& dx) const float IKSolver::getError(Vector3f &dx) const {
{
// TODO Compute the error between the current end effector // TODO Compute the error between the current end effector
// position and the target position // position and the target position
dx.setZero(); dx.setZero();
return FLT_MAX;
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 IKSolver::solve() 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 numLinks = m_armature->links.size();
const int dim = 3 * (numLinks); const int dim = 3 * (numLinks);
m_J.resize(3, dim); m_J.resize(3, dim);
// We assume that the last link is the "end effector" // We assume that the last link is the "end effector"
// //
Link* endEffector = m_armature->links[numLinks - 1]; Link *endEffector = m_armature->links[numLinks - 1];
// TODO Juild the Jacobian matrix m_J. // Build the Jacobian matrix m_J.
// Each column corresponds to a separate // 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 // TODO Compute the error between the current end effector
// position and the target position by calling getError() // position and the target position by calling getError()
// Vector3f dx;
float error = getError(dx);
// TODO Compute the change in the joint angles by solving: // TODO Compute the change in the joint angles by solving:
// df/dtheta * delta_theta = delta_x // df/dtheta * delta_theta = delta_x