Finito!
This commit is contained in:
parent
8b10e5ee6d
commit
b22055a1ce
|
@ -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;
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue