ETS_GTI320
/
sim_cinematique_inverse
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Finito

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
labo_ik/Armature.cpp
View File

@ -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;

15
labo_ik/Armature.h
View File

@ -1,5 +1,11 @@
#pragma once
/*
* Nom: William Nolin
* Code permanent : NOLW76060101
* Email : william.nolin.1@ens.etsmtl.ca
*/
#include "Math3D.h"
#include <vector>
@ -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().

8
labo_ik/IKApplication.cpp
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 <nanogui/window.h>

7
labo_ik/IKApplication.h
View File

@ -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 <nanogui/window.h>

6
labo_ik/IKGLCanvas.cpp
View File

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

7
labo_ik/IKGLCanvas.h
View File

@ -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 <nanogui/window.h>

38
labo_ik/IKSolver.cpp
View File

@ -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<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;
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<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() {
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);
}

8
labo_ik/IKSolver.h
View File

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

8
labo_ik/LinkUI.cpp
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)
{

3
labo_ik/LinkUI.h
View File

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

3
labo_ik/SVD.h
View File

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

8
labo_ik/TargetUI.cpp
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)
{

3
labo_ik/TargetUI.h
View File

@ -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 <nanogui/window.h>

7
labo_ik/main.cpp
View File

@ -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"