This commit is contained in:
william 2024-04-12 15:25:52 -04:00
parent b22055a1ce
commit 4fb568bb05
14 changed files with 96 additions and 31 deletions

View File

@ -1,5 +1,11 @@
#pragma once #pragma once
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "Armature.h" #include "Armature.h"
using namespace gti320; using namespace gti320;

View File

@ -1,5 +1,11 @@
#pragma once #pragma once
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "Math3D.h" #include "Math3D.h"
#include <vector> #include <vector>
@ -31,15 +37,6 @@ namespace gti320
// Gets the position of the link from the global rigid transformation matrix. // Gets the position of the link from the global rigid transformation matrix.
Vector3f globalPosition(); 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

@ -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 "IKGLCanvas.h"
#include <nanogui/window.h> #include <nanogui/window.h>

View File

@ -5,10 +5,9 @@
* *
* @brief Application class for labo 2. * @brief Application class for labo 2.
* *
* Nom: * Nom: William Nolin
* Code permanent : * Code permanent : NOLW76060101
* Email : * Email : william.nolin.1@ens.etsmtl.ca
*
*/ */
#include <nanogui/window.h> #include <nanogui/window.h>

View File

@ -1,3 +1,9 @@
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "IKGLCanvas.h" #include "IKGLCanvas.h"
#include "IKApplication.h" #include "IKApplication.h"

View File

@ -5,10 +5,9 @@
* *
* @brief Canvas class for drawing OpenGL scenes. * @brief Canvas class for drawing OpenGL scenes.
* *
* Nom: * Nom: William Nolin
* Code permanent : * Code permanent : NOLW76060101
* Email : * Email : william.nolin.1@ens.etsmtl.ca
*
*/ */
#include <nanogui/window.h> #include <nanogui/window.h>

View File

@ -1,5 +1,11 @@
#pragma once #pragma once
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "IKSolver.h" #include "IKSolver.h"
#include "Armature.h" #include "Armature.h"
#include "SVD.h" #include "SVD.h"
@ -14,7 +20,7 @@ IKSolver::IKSolver(Armature *_armature, Vector3f &_targetPos) : m_armature(_arma
float IKSolver::getError(Vector3f &dx) const { 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 // position and the target position
dx.setZero(); dx.setZero();
@ -30,8 +36,15 @@ float IKSolver::getError(Vector3f &dx) const {
return ddx.norm(); 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<typename _Scalar, int _Rows, int _Storage> template<typename _Scalar, int _Rows, int _Storage>
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; _Scalar product = 0;
for (int i = 0; i < b.size(); i++) { for (int i = 0; i < b.size(); i++) {
product += a(i, 0) * b(i); product += a(i, 0) * b(i);
@ -39,6 +52,21 @@ float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows
return product; 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<int _Rows, int _Columns, int _Storage>
void crossP(SubMatrix<float, 3, Dynamic, _Storage> &result,
SubMatrix<float, _Rows, _Columns, _Storage> &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() { 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);
@ -57,9 +85,7 @@ void IKSolver::solve() {
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
auto ji = m_J.block(0, i * 3 + j, 3, 1); auto ji = m_J.block(0, i * 3 + j, 3, 1);
auto oij = link->M.block(0, 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); crossP(ji, oij, ri);
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);
} }
} }
@ -108,5 +134,5 @@ void IKSolver::solve() {
m_armature->updateKinematics(); m_armature->updateKinematics();
alpha /= 2; alpha /= 2;
} while (getError(dx) >= error); } while (getError(dx) >= error && alpha > 0);
} }

View File

@ -1,5 +1,11 @@
#pragma once #pragma once
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "Math3D.h" #include "Math3D.h"
namespace gti320 namespace gti320

View File

@ -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) LinkUI::LinkUI(gti320::Link* _link, int _id, nanogui::Widget* _parent) : link(_link), id(_id)
{ {

View File

@ -5,6 +5,9 @@
* *
* @brief User interface for armature links. * @brief User interface for armature links.
* *
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/ */
#include <nanogui/window.h> #include <nanogui/window.h>

View File

@ -5,6 +5,9 @@
* *
* @brief Singular value decomposition. * @brief Singular value decomposition.
* *
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/ */
#include "Matrix.h" #include "Matrix.h"

View File

@ -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) TargetUI::TargetUI(nanogui::Widget* _parent, gti320::Vector3f& _target) : target(_target)
{ {

View File

@ -5,6 +5,9 @@
* *
* @brief User interface for end-effector target. * @brief User interface for end-effector target.
* *
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/ */
#include <nanogui/window.h> #include <nanogui/window.h>

View File

@ -3,10 +3,9 @@
* *
* @brief GTI320 Labo 2 - creates a NanoGUI application. * @brief GTI320 Labo 2 - creates a NanoGUI application.
* *
* Nom: * Nom: William Nolin
* Code permanent : * Code permanent : NOLW76060101
* Email : * Email : william.nolin.1@ens.etsmtl.ca
*
*/ */
#include "IKApplication.h" #include "IKApplication.h"