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

Finito!

This commit is contained in:
FyloZ 2024-04-11 13:30:35 -04:00
parent 8b10e5ee6d
commit b22055a1ce
Signed by: william
GPG Key ID: 835378AE9AF4AE97
2 changed files with 53 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
labo_ik/IKApplication.cpp
View File

@ -19,6 +19,7 @@
#include <memory> #include <memory>
#define _USE_MATH_DEFINES #define _USE_MATH_DEFINES
#include <cmath> #include <cmath>
#include <random> #include <random>
@ -26,22 +27,20 @@
using namespace nanogui; using namespace nanogui;
namespace namespace {
{
// Random number generator. // Random number generator.
// Returns a random value such that _min <= val <= _max // Returns a random value such that _min <= val <= _max
// //
template<typename _Scalar> template<typename _Scalar>
static inline _Scalar rand(_Scalar _min, _Scalar _max) static inline _Scalar rand(_Scalar _min, _Scalar _max) {
{
static std::random_device rdev; static std::random_device rdev;
static std::default_random_engine re(rdev()); static std::default_random_engine re(rdev());
// Using uniform distribution (recommended since C++11) // Using uniform distribution (recommended since C++11)
typedef typename std::conditional< typedef typename std::conditional<
std::is_floating_point<_Scalar>::value, std::is_floating_point<_Scalar>::value,
std::uniform_real_distribution<_Scalar>, std::uniform_real_distribution<_Scalar>,
std::uniform_int_distribution<_Scalar>>::type dist_type; std::uniform_int_distribution<_Scalar>>::type dist_type;
dist_type uni(_min, _max); dist_type uni(_min, _max);
return static_cast<_Scalar>(uni(re)); return static_cast<_Scalar>(uni(re));
} }
@ -49,8 +48,7 @@ namespace
} }
IKApplication::IKApplication() : Screen(Vector2i(1280, 720), "GTI320 Labo sur la cinematique inverse"), IKApplication::IKApplication() : Screen(Vector2i(1280, 720), "GTI320 Labo sur la cinematique inverse"),
m_targetPos() m_targetPos() {
{
m_armature = std::make_unique<gti320::Armature>(); m_armature = std::make_unique<gti320::Armature>();
m_ikSolver = std::make_unique<gti320::IKSolver>(m_armature.get(), m_targetPos); m_ikSolver = std::make_unique<gti320::IKSolver>(m_armature.get(), m_targetPos);
@ -62,46 +60,41 @@ m_targetPos()
m_window->set_layout(new GroupLayout()); m_window->set_layout(new GroupLayout());
m_canvas = std::make_unique<IKGLCanvas>(this); m_canvas = std::make_unique<IKGLCanvas>(this);
m_canvas->set_background_color({ 255, 255, 255, 255 }); m_canvas->set_background_color({255, 255, 255, 255});
m_canvas->set_size({ 1080, 600 }); 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_position(Vector2i(1020, 10));
controls->set_layout(new GroupLayout()); 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)); tools->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Middle, 0, 5));
m_targetUI = std::make_unique<TargetUI>(tools, m_targetPos); m_targetUI = std::make_unique<TargetUI>(tools, m_targetPos);
const int numLinks = m_armature->links.size(); const int numLinks = m_armature->links.size();
for (int i = 0; i < numLinks; ++i) for (int i = 0; i < numLinks; ++i) {
{ gti320::Link *link = m_armature->links[i];
gti320::Link* link = m_armature->links[i]; if (!link->isEndEffector()) {
if (!link->isEndEffector())
{
m_linkUIArr.push_back(new LinkUI(m_armature->links[i], i, tools)); m_linkUIArr.push_back(new LinkUI(m_armature->links[i], i, tools));
} }
} }
Button* solveButton = new Button(tools, "IK solve"); Button *solveButton = new Button(tools, "IK solve");
solveButton->set_callback([this] solveButton->set_callback([this] {
{ ikSolve();
ikSolve(); });
});
Button* resetButton = new Button(tools, "Reset"); Button *resetButton = new Button(tools, "Reset");
resetButton->set_callback([this] resetButton->set_callback([this] {
{ reset();
reset(); });
});
perform_layout(); perform_layout();
reset(); 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)) if (Screen::keyboard_event(key, scancode, action, modifiers))
return true; return true;
if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) { 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; return false;
} }
void IKApplication::draw(NVGcontext* ctx) void IKApplication::draw(NVGcontext *ctx) {
{
assert(m_armature->root != nullptr); assert(m_armature->root != nullptr);
m_armature->updateKinematics(); m_armature->updateKinematics();
@ -121,12 +113,10 @@ void IKApplication::draw(NVGcontext* ctx)
Screen::draw(ctx); Screen::draw(ctx);
} }
void IKApplication::reset() void IKApplication::reset() {
{
// Reset all joints to zero angle // Reset all joints to zero angle
// //
for (gti320::Link* l : m_armature->links) for (gti320::Link *l: m_armature->links) {
{
l->eulerAng.setZero(); l->eulerAng.setZero();
} }
@ -136,23 +126,19 @@ void IKApplication::reset()
// Update UI // Update UI
// //
for (LinkUI* ui : m_linkUIArr) for (LinkUI *ui: m_linkUIArr) {
{
ui->onArmatureChanged(); ui->onArmatureChanged();
} }
} }
void IKApplication::ikSolve() void IKApplication::ikSolve() {
{
m_ikSolver->solve(); m_ikSolver->solve();
for (LinkUI* ui : m_linkUIArr) for (LinkUI *ui: m_linkUIArr) {
{
ui->onArmatureChanged(); ui->onArmatureChanged();
} }
} }
void IKApplication::initializeArmature() void IKApplication::initializeArmature() {
{
// Initialize the armature // Initialize the armature
// //
gti320::Vector3f angs; gti320::Vector3f angs;
@ -161,19 +147,19 @@ void IKApplication::initializeArmature()
// Root // Root
t.setZero(); t.setZero();
gti320::Link* link0 = new gti320::Link(nullptr, angs, t); gti320::Link *link0 = new gti320::Link(nullptr, angs, t);
// First joint // First joint
t(1) = 1.5f; t(1) = 1.5f;
gti320::Link* link1 = new gti320::Link(link0, angs, t); gti320::Link *link1 = new gti320::Link(link0, angs, t);
// Second joint // Second joint
t(1) = 0.75f; t(1) = 0.75f;
gti320::Link* link2 = new gti320::Link(link1, angs, t); gti320::Link *link2 = new gti320::Link(link1, angs, t);
// End-effector // End-effector
t(1) = 0.25f; 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(link0);
m_armature->links.push_back(link1); m_armature->links.push_back(link1);
@ -182,8 +168,7 @@ void IKApplication::initializeArmature()
m_armature->root = link0; m_armature->root = link0;
} }
void IKApplication::initializeTarget() void IKApplication::initializeTarget() {
{
m_targetPos(0) = 1.0f; m_targetPos(0) = 1.0f;
m_targetPos(1) = 1.0f; m_targetPos(1) = 1.0f;
m_targetPos(2) = 0.0f; m_targetPos(2) = 0.0f;

43
labo_ik/IKSolver.cpp
View File

@ -1,6 +1,5 @@
#pragma once #pragma once
#include <cfloat>
#include "IKSolver.h" #include "IKSolver.h"
#include "Armature.h" #include "Armature.h"
#include "SVD.h" #include "SVD.h"
@ -31,16 +30,6 @@ float IKSolver::getError(Vector3f &dx) const {
return ddx.norm(); 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<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) {
_Scalar product = 0; _Scalar product = 0;
@ -63,43 +52,47 @@ void IKSolver::solve() {
// Each column corresponds to a separate link // Each column corresponds to a separate link
for (int i = 0; i < numLinks; i++) { for (int i = 0; i < numLinks; i++) {
Link *link = m_armature->links[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() // position and the target position by calling getError()
Vector3f dx; Vector3f dx;
float error = getError(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 // df/dtheta * delta_theta = delta_x
// where df/dtheta is the Jacobian matrix. // where df/dtheta is the Jacobian matrix.
auto svd = SVD(m_J); auto svd = SVD(m_J);
svd.decompose(); svd.decompose();
// svd.getSigma() * svd.getV().transpose() * d_theta - svd.getU().transpose() * dx; int m = svd.getSigma().size();
int rank = 0; int rank = 0;
for (int i = 0; i < svd.getSigma().size(); i++) { for (int i = 0; i < m; i++) {
if (svd.getSigma()(i) == 0) { if (svd.getSigma()(i) == 0) {
break; break;
} }
rank++; rank++;
} }
auto d_theta = Vector<float, Dynamic>(rank);
auto z = Vector<float, Dynamic>(rank); auto z = Vector<float, Dynamic>(m);
for (int i = 0; i < rank; i++) { 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); auto si = svd.getSigma()(i);
z(i) = dotP(ui, dx) / si; z(i) = dotP(ui, dx) / si;
} }
Vector<float, Dynamic> d_theta = svd.getV() * z;
d_theta = svd.getV() * z; // Perform gradient descent method with line search
// TODO Perform gradient descent method with line search
// to move the end effector toward the target position. // to move the end effector toward the target position.
// //
// Hint: use the Armature::unpack() and Armature::pack() functions // Hint: use the Armature::unpack() and Armature::pack() functions
@ -115,5 +108,5 @@ void IKSolver::solve() {
m_armature->updateKinematics(); m_armature->updateKinematics();
alpha /= 2; alpha /= 2;
} while (getError(dx) < error); } while (getError(dx) >= error);
} }