From ba006846d2ff81ca96a38b0a45e2a21034e2bca7 Mon Sep 17 00:00:00 2001
From: FyloZ <fylozdev@gmail.com>
Date: Sat, 6 Apr 2024 20:50:36 -0400
Subject: [PATCH] Matrice jacobienne

---
 labo_ik/Armature.cpp | 28 +++++++++++++++++++++---
 labo_ik/Armature.h   | 13 +++++++++++
 labo_ik/IKSolver.cpp | 51 ++++++++++++++++++++++++++++++++------------
 3 files changed, 75 insertions(+), 17 deletions(-)

diff --git a/labo_ik/Armature.cpp b/labo_ik/Armature.cpp
index 4eb4392..fdc5f51 100644
--- a/labo_ik/Armature.cpp
+++ b/labo_ik/Armature.cpp
@@ -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) {
 
@@ -60,18 +67,33 @@ void Armature::updateKinematics() {
 }
 
 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
+    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) {
     const int numLinks = links.size();
     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
     //      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);
+    }
 }
diff --git a/labo_ik/Armature.h b/labo_ik/Armature.h
index 38f602c..30aef35 100644
--- a/labo_ik/Armature.h
+++ b/labo_ik/Armature.h
@@ -27,6 +27,19 @@ namespace gti320
         //
         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 trans;                                 // Translation giving position relative to the parent.
         Matrix4f M;                                     // Global rigid transformation of the link, computed in forward().
diff --git a/labo_ik/IKSolver.cpp b/labo_ik/IKSolver.cpp
index 9db76a3..2acbc1d 100644
--- a/labo_ik/IKSolver.cpp
+++ b/labo_ik/IKSolver.cpp
@@ -7,39 +7,62 @@
 
 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 
-    //      position and the target position 
+    //      position and the target position
     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 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];
+    Link *endEffector = m_armature->links[numLinks - 1];
 
-    // TODO Juild the Jacobian matrix m_J.
-    //      Each column corresponds to a separate 
+    // 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