From 4fb568bb0526844486fcb970344abc3e64fc75db Mon Sep 17 00:00:00 2001 From: william Date: Fri, 12 Apr 2024 15:25:52 -0400 Subject: [PATCH] Finito --- labo_ik/Armature.cpp | 6 ++++++ labo_ik/Armature.h | 15 ++++++--------- labo_ik/IKApplication.cpp | 8 +++++++- labo_ik/IKApplication.h | 7 +++---- labo_ik/IKGLCanvas.cpp | 6 ++++++ labo_ik/IKGLCanvas.h | 7 +++---- labo_ik/IKSolver.cpp | 38 ++++++++++++++++++++++++++++++++------ labo_ik/IKSolver.h | 8 +++++++- labo_ik/LinkUI.cpp | 8 +++++++- labo_ik/LinkUI.h | 3 +++ labo_ik/SVD.h | 3 +++ labo_ik/TargetUI.cpp | 8 +++++++- labo_ik/TargetUI.h | 3 +++ labo_ik/main.cpp | 7 +++---- 14 files changed, 96 insertions(+), 31 deletions(-) diff --git a/labo_ik/Armature.cpp b/labo_ik/Armature.cpp index fdc5f51..be652e2 100644 --- a/labo_ik/Armature.cpp +++ b/labo_ik/Armature.cpp @@ -1,5 +1,11 @@ #pragma once +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + #include "Armature.h" using namespace gti320; diff --git a/labo_ik/Armature.h b/labo_ik/Armature.h index 30aef35..2b1bf90 100644 --- a/labo_ik/Armature.h +++ b/labo_ik/Armature.h @@ -1,5 +1,11 @@ #pragma once +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + #include "Math3D.h" #include @@ -31,15 +37,6 @@ namespace gti320 // 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/IKApplication.cpp b/labo_ik/IKApplication.cpp index 2b503fc..e3ef34c 100644 --- a/labo_ik/IKApplication.cpp +++ b/labo_ik/IKApplication.cpp @@ -1,4 +1,10 @@ -#include "IKApplication.h" +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + +#include "IKApplication.h" #include "IKGLCanvas.h" #include diff --git a/labo_ik/IKApplication.h b/labo_ik/IKApplication.h index 0470f05..16b97f4 100644 --- a/labo_ik/IKApplication.h +++ b/labo_ik/IKApplication.h @@ -5,10 +5,9 @@ * * @brief Application class for labo 2. * - * Nom: - * Code permanent : - * Email : - * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include diff --git a/labo_ik/IKGLCanvas.cpp b/labo_ik/IKGLCanvas.cpp index 2e09f69..b95441b 100644 --- a/labo_ik/IKGLCanvas.cpp +++ b/labo_ik/IKGLCanvas.cpp @@ -1,3 +1,9 @@ +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + #include "IKGLCanvas.h" #include "IKApplication.h" diff --git a/labo_ik/IKGLCanvas.h b/labo_ik/IKGLCanvas.h index a93d786..29dd6c2 100644 --- a/labo_ik/IKGLCanvas.h +++ b/labo_ik/IKGLCanvas.h @@ -5,10 +5,9 @@ * * @brief Canvas class for drawing OpenGL scenes. * - * Nom: - * Code permanent : - * Email : - * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include diff --git a/labo_ik/IKSolver.cpp b/labo_ik/IKSolver.cpp index 9bd2ece..8fba565 100644 --- a/labo_ik/IKSolver.cpp +++ b/labo_ik/IKSolver.cpp @@ -1,5 +1,11 @@ #pragma once +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + #include "IKSolver.h" #include "Armature.h" #include "SVD.h" @@ -14,7 +20,7 @@ IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_arma float IKSolver::getError(Vector3f &dx) const { - // TODO Compute the error between the current end effector + // Compute the error between the current end effector // position and the target position dx.setZero(); @@ -30,8 +36,15 @@ float IKSolver::getError(Vector3f &dx) const { return ddx.norm(); } +// NOUVELLE FONCTION +// Produit scalaire entre une sous-matrice et un vecteur, permettant d'éviter +// la copie de données dans un nouveau vecteur pour utiliser l'opérateur fait dans le premier laboratoire. +// La sous matrice doit avoir au moins une colonne, et toutes les autres seront ignorées. template -float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) { +float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> &a, Vector<_Scalar, _Rows> &b) { + assert(a.cols() >= 1); + assert(a.rows() == b.rows()); + _Scalar product = 0; for (int i = 0; i < b.size(); i++) { product += a(i, 0) * b(i); @@ -39,6 +52,21 @@ float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows return product; } +// NOUVELLE FONCTION +// Produit vectoriel entre une sous-matrice et un vecteur. +// La sous-matrice doit comporter au moins une colonne et trois rangées, et la sous-matrice résultante doit contenir au moins une colonne. +template +void crossP(SubMatrix &result, + SubMatrix &a, Vector3f &b) { + assert(result.cols() >= 1); + assert(a.rows() >= 3); + assert(a.cols() >= 1); + + result(0, 0) = a(1, 0) * b(2) - a(2, 0) * b(1); + result(1, 0) = a(2, 0) * b(0) - a(0, 0) * b(2); + result(2, 0) = a(0, 0) * b(1) - a(1, 0) * b(0); +} + void IKSolver::solve() { const int numLinks = m_armature->links.size(); const int dim = 3 * (numLinks); @@ -57,9 +85,7 @@ void IKSolver::solve() { for (int j = 0; j < 3; j++) { auto ji = m_J.block(0, i * 3 + j, 3, 1); auto oij = link->M.block(0, j, 3, 1); - ji(0, 0) = oij(1, 0) * ri(2) - oij(2, 0) * ri(1); - ji(1, 0) = oij(2, 0) * ri(0) - oij(0, 0) * ri(2); - ji(2, 0) = oij(0, 0) * ri(1) - oij(1, 0) * ri(0); + crossP(ji, oij, ri); } } @@ -108,5 +134,5 @@ void IKSolver::solve() { m_armature->updateKinematics(); alpha /= 2; - } while (getError(dx) >= error); + } while (getError(dx) >= error && alpha > 0); } diff --git a/labo_ik/IKSolver.h b/labo_ik/IKSolver.h index 713c8ac..1a44e20 100644 --- a/labo_ik/IKSolver.h +++ b/labo_ik/IKSolver.h @@ -1,4 +1,10 @@ - #pragma once +#pragma once + +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ #include "Math3D.h" diff --git a/labo_ik/LinkUI.cpp b/labo_ik/LinkUI.cpp index 379fd3c..8902d9a 100644 --- a/labo_ik/LinkUI.cpp +++ b/labo_ik/LinkUI.cpp @@ -1,4 +1,10 @@ -#include "LinkUI.h" +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + +#include "LinkUI.h" LinkUI::LinkUI(gti320::Link* _link, int _id, nanogui::Widget* _parent) : link(_link), id(_id) { diff --git a/labo_ik/LinkUI.h b/labo_ik/LinkUI.h index c199ecf..1035de6 100644 --- a/labo_ik/LinkUI.h +++ b/labo_ik/LinkUI.h @@ -5,6 +5,9 @@ * * @brief User interface for armature links. * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include diff --git a/labo_ik/SVD.h b/labo_ik/SVD.h index 6f422f6..29f0d13 100644 --- a/labo_ik/SVD.h +++ b/labo_ik/SVD.h @@ -5,6 +5,9 @@ * * @brief Singular value decomposition. * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include "Matrix.h" diff --git a/labo_ik/TargetUI.cpp b/labo_ik/TargetUI.cpp index 102618d..d166074 100644 --- a/labo_ik/TargetUI.cpp +++ b/labo_ik/TargetUI.cpp @@ -1,4 +1,10 @@ -#include "TargetUI.h" +/* + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca + */ + +#include "TargetUI.h" TargetUI::TargetUI(nanogui::Widget* _parent, gti320::Vector3f& _target) : target(_target) { diff --git a/labo_ik/TargetUI.h b/labo_ik/TargetUI.h index 6181122..3f9c55b 100644 --- a/labo_ik/TargetUI.h +++ b/labo_ik/TargetUI.h @@ -5,6 +5,9 @@ * * @brief User interface for end-effector target. * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include diff --git a/labo_ik/main.cpp b/labo_ik/main.cpp index e7ee20f..3039dc2 100644 --- a/labo_ik/main.cpp +++ b/labo_ik/main.cpp @@ -3,10 +3,9 @@ * * @brief GTI320 Labo 2 - creates a NanoGUI application. * - * Nom: - * Code permanent : - * Email : - * + * Nom: William Nolin + * Code permanent : NOLW76060101 + * Email : william.nolin.1@ens.etsmtl.ca */ #include "IKApplication.h"