From b22055a1ce746751d64d9e444feeb16f430e8170 Mon Sep 17 00:00:00 2001 From: FyloZ Date: Thu, 11 Apr 2024 13:30:35 -0400 Subject: [PATCH] Finito! --- labo_ik/IKApplication.cpp | 85 ++++++++++++++++----------------------- labo_ik/IKSolver.cpp | 43 +++++++++----------- 2 files changed, 53 insertions(+), 75 deletions(-) diff --git a/labo_ik/IKApplication.cpp b/labo_ik/IKApplication.cpp index 1c5c409..2b503fc 100644 --- a/labo_ik/IKApplication.cpp +++ b/labo_ik/IKApplication.cpp @@ -19,6 +19,7 @@ #include #define _USE_MATH_DEFINES + #include #include @@ -26,22 +27,20 @@ using namespace nanogui; -namespace -{ +namespace { // Random number generator. // Returns a random value such that _min <= val <= _max // template - static inline _Scalar rand(_Scalar _min, _Scalar _max) - { + static inline _Scalar rand(_Scalar _min, _Scalar _max) { static std::random_device rdev; static std::default_random_engine re(rdev()); // Using uniform distribution (recommended since C++11) typedef typename std::conditional< - std::is_floating_point<_Scalar>::value, - std::uniform_real_distribution<_Scalar>, - std::uniform_int_distribution<_Scalar>>::type dist_type; + std::is_floating_point<_Scalar>::value, + std::uniform_real_distribution<_Scalar>, + std::uniform_int_distribution<_Scalar>>::type dist_type; dist_type uni(_min, _max); return static_cast<_Scalar>(uni(re)); } @@ -49,8 +48,7 @@ namespace } IKApplication::IKApplication() : Screen(Vector2i(1280, 720), "GTI320 Labo sur la cinematique inverse"), -m_targetPos() -{ + m_targetPos() { m_armature = std::make_unique(); m_ikSolver = std::make_unique(m_armature.get(), m_targetPos); @@ -62,46 +60,41 @@ m_targetPos() m_window->set_layout(new GroupLayout()); m_canvas = std::make_unique(this); - m_canvas->set_background_color({ 255, 255, 255, 255 }); - m_canvas->set_size({ 1080, 600 }); + m_canvas->set_background_color({255, 255, 255, 255}); + m_canvas->set_size({1080, 600}); - Window* controls = new Window(this, "Controls"); + Window *controls = new Window(this, "Controls"); controls->set_position(Vector2i(1020, 10)); controls->set_layout(new GroupLayout()); - Widget* tools = new Widget(controls); + Widget *tools = new Widget(controls); tools->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Middle, 0, 5)); m_targetUI = std::make_unique(tools, m_targetPos); const int numLinks = m_armature->links.size(); - for (int i = 0; i < numLinks; ++i) - { - gti320::Link* link = m_armature->links[i]; - if (!link->isEndEffector()) - { + for (int i = 0; i < numLinks; ++i) { + gti320::Link *link = m_armature->links[i]; + if (!link->isEndEffector()) { m_linkUIArr.push_back(new LinkUI(m_armature->links[i], i, tools)); } } - Button* solveButton = new Button(tools, "IK solve"); - solveButton->set_callback([this] - { - ikSolve(); - }); + Button *solveButton = new Button(tools, "IK solve"); + solveButton->set_callback([this] { + ikSolve(); + }); - Button* resetButton = new Button(tools, "Reset"); - resetButton->set_callback([this] - { - reset(); - }); + Button *resetButton = new Button(tools, "Reset"); + resetButton->set_callback([this] { + reset(); + }); perform_layout(); reset(); } -bool IKApplication::keyboard_event(int key, int scancode, int action, int modifiers) -{ +bool IKApplication::keyboard_event(int key, int scancode, int action, int modifiers) { if (Screen::keyboard_event(key, scancode, action, modifiers)) return true; if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) { @@ -111,8 +104,7 @@ bool IKApplication::keyboard_event(int key, int scancode, int action, int modifi return false; } -void IKApplication::draw(NVGcontext* ctx) -{ +void IKApplication::draw(NVGcontext *ctx) { assert(m_armature->root != nullptr); m_armature->updateKinematics(); @@ -121,12 +113,10 @@ void IKApplication::draw(NVGcontext* ctx) Screen::draw(ctx); } -void IKApplication::reset() -{ +void IKApplication::reset() { // Reset all joints to zero angle // - for (gti320::Link* l : m_armature->links) - { + for (gti320::Link *l: m_armature->links) { l->eulerAng.setZero(); } @@ -136,23 +126,19 @@ void IKApplication::reset() // Update UI // - for (LinkUI* ui : m_linkUIArr) - { + for (LinkUI *ui: m_linkUIArr) { ui->onArmatureChanged(); } } -void IKApplication::ikSolve() -{ +void IKApplication::ikSolve() { m_ikSolver->solve(); - for (LinkUI* ui : m_linkUIArr) - { + for (LinkUI *ui: m_linkUIArr) { ui->onArmatureChanged(); } } -void IKApplication::initializeArmature() -{ +void IKApplication::initializeArmature() { // Initialize the armature // gti320::Vector3f angs; @@ -161,19 +147,19 @@ void IKApplication::initializeArmature() // Root t.setZero(); - gti320::Link* link0 = new gti320::Link(nullptr, angs, t); + gti320::Link *link0 = new gti320::Link(nullptr, angs, t); // First joint t(1) = 1.5f; - gti320::Link* link1 = new gti320::Link(link0, angs, t); + gti320::Link *link1 = new gti320::Link(link0, angs, t); // Second joint t(1) = 0.75f; - gti320::Link* link2 = new gti320::Link(link1, angs, t); + gti320::Link *link2 = new gti320::Link(link1, angs, t); // End-effector t(1) = 0.25f; - gti320::Link* link3 = new gti320::Link(link2, angs, t); + gti320::Link *link3 = new gti320::Link(link2, angs, t); m_armature->links.push_back(link0); m_armature->links.push_back(link1); @@ -182,8 +168,7 @@ void IKApplication::initializeArmature() m_armature->root = link0; } -void IKApplication::initializeTarget() -{ +void IKApplication::initializeTarget() { m_targetPos(0) = 1.0f; m_targetPos(1) = 1.0f; m_targetPos(2) = 0.0f; diff --git a/labo_ik/IKSolver.cpp b/labo_ik/IKSolver.cpp index 8a25cd8..9bd2ece 100644 --- a/labo_ik/IKSolver.cpp +++ b/labo_ik/IKSolver.cpp @@ -1,6 +1,5 @@ #pragma once -#include #include "IKSolver.h" #include "Armature.h" #include "SVD.h" @@ -31,16 +30,6 @@ float IKSolver::getError(Vector3f &dx) const { return ddx.norm(); } -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); - } -} - template float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) { _Scalar product = 0; @@ -63,43 +52,47 @@ void IKSolver::solve() { // 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(); + Vector3f ri = endEffector->globalPosition() - link->globalPosition(); - jacobian(m_J, link, shift, i); + 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); + } } - // TODO Compute the error between the current end effector + // 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: + // Compute the change in the joint angles by solving: // df/dtheta * delta_theta = delta_x // where df/dtheta is the Jacobian matrix. auto svd = SVD(m_J); svd.decompose(); -// svd.getSigma() * svd.getV().transpose() * d_theta - svd.getU().transpose() * dx; - + int m = svd.getSigma().size(); int rank = 0; - for (int i = 0; i < svd.getSigma().size(); i++) { + for (int i = 0; i < m; i++) { if (svd.getSigma()(i) == 0) { break; } rank++; } - auto d_theta = Vector(rank); - auto z = Vector(rank); + + auto z = Vector(m); for (int i = 0; i < rank; i++) { - auto ui = svd.getU().block(i, 0, 3, 1); + auto ui = svd.getU().block(0, i, 3, 1); auto si = svd.getSigma()(i); z(i) = dotP(ui, dx) / si; } + Vector d_theta = svd.getV() * z; - d_theta = svd.getV() * z; - - // TODO Perform gradient descent method with line search + // Perform gradient descent method with line search // to move the end effector toward the target position. // // Hint: use the Armature::unpack() and Armature::pack() functions @@ -115,5 +108,5 @@ void IKSolver::solve() { m_armature->updateKinematics(); alpha /= 2; - } while (getError(dx) < error); + } while (getError(dx) >= error); }