Cinématique inverse de l'armature
This commit is contained in:
parent
80375e68de
commit
b4e8eee37f
|
@ -7,62 +7,65 @@ using namespace gti320;
|
||||||
// Constructor
|
// Constructor
|
||||||
//
|
//
|
||||||
Link::Link(Link *_parent, const Vector3f &_eulerAng, const Vector3f &_trans) :
|
Link::Link(Link *_parent, const Vector3f &_eulerAng, const Vector3f &_trans) :
|
||||||
parent(_parent), eulerAng(_eulerAng), trans(_trans)
|
parent(_parent), eulerAng(_eulerAng), trans(_trans) {
|
||||||
{
|
if (parent != nullptr) {
|
||||||
if (parent != nullptr)
|
|
||||||
{
|
|
||||||
parent->enfants.push_back(this);
|
parent->enfants.push_back(this);
|
||||||
}
|
}
|
||||||
M.setIdentity();
|
M.setIdentity();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Link::forward()
|
void Link::forward() {
|
||||||
{
|
// Create a rotation matrix from the Euler angles
|
||||||
// TODO Create a rotation matrix from the Euler angles
|
|
||||||
// of the current link.
|
// of the current link.
|
||||||
|
// Create a local 4D rigid transformation matrix from the
|
||||||
// TODO Create a local 4D rigid transformation matrix from the
|
|
||||||
// 3D rotation matrix and translation of the current link.
|
// 3D rotation matrix and translation of the current link.
|
||||||
|
Matrix<float, 4, 4> local;
|
||||||
|
local.setIdentity();
|
||||||
|
local.block(0, 0, 3, 3) = makeRotation(eulerAng(0), eulerAng(1), eulerAng(2));
|
||||||
|
local(0, 3) = trans(0);
|
||||||
|
local(1, 3) = trans(1);
|
||||||
|
local(2, 3) = trans(2);
|
||||||
|
|
||||||
// TODO Update the global transformation for the link using the
|
// Update the global transformation for the link using the
|
||||||
// parent's rigid transformation matrix and the local transformation.
|
// parent's rigid transformation matrix and the local transformation.
|
||||||
// Hint : the parent matrix should be post-multiplied.
|
// Hint : the parent matrix should be post-multiplied.
|
||||||
// Hint : the root does not have a parent. You must consider this case.
|
// Hint : the root does not have a parent. You must consider this case.
|
||||||
|
if (isRoot()) {
|
||||||
|
M = local;
|
||||||
|
} else {
|
||||||
|
M = parent->M * local;
|
||||||
|
}
|
||||||
|
|
||||||
// TODO Update the transformation of child links
|
// Update the transformation of child links
|
||||||
// by recursion.
|
// by recursion.
|
||||||
|
for (const auto &child: enfants) {
|
||||||
|
child->forward();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
Armature::Armature() : links(), root(nullptr)
|
Armature::Armature() : links(), root(nullptr) {
|
||||||
{
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Armature::~Armature()
|
Armature::~Armature() {
|
||||||
{
|
for (Link *l: links) {
|
||||||
for (Link* l : links)
|
|
||||||
{
|
|
||||||
delete l;
|
delete l;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Armature::updateKinematics()
|
void Armature::updateKinematics() {
|
||||||
{
|
|
||||||
assert(root != nullptr);
|
assert(root != nullptr);
|
||||||
root->forward();
|
root->forward();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Armature::pack(Vector<float, Dynamic>& theta)
|
void Armature::pack(Vector<float, Dynamic> &theta) {
|
||||||
{
|
|
||||||
// TODO Collect the Euler angles of each link and put them
|
// TODO Collect the Euler angles of each link and put them
|
||||||
// into the dense vector @a theta
|
// into the dense vector @a theta
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Armature::unpack(const Vector<float, Dynamic>& theta)
|
void Armature::unpack(const Vector<float, Dynamic> &theta) {
|
||||||
{
|
|
||||||
const int numLinks = links.size();
|
const int numLinks = links.size();
|
||||||
assert(theta.size() == 3 * numLinks);
|
assert(theta.size() == 3 * numLinks);
|
||||||
|
|
||||||
|
|
|
@ -27,7 +27,7 @@ set(HEADERS IKApplication.h IKGLCanvas.h LinkUI.h TargetUI.h Armature.h IKSolver
|
||||||
set(SOURCE main.cpp IKApplication.cpp IKGLCanvas.cpp LinkUI.cpp TargetUI.cpp Armature.cpp IKSolver.cpp)
|
set(SOURCE main.cpp IKApplication.cpp IKGLCanvas.cpp LinkUI.cpp TargetUI.cpp Armature.cpp IKSolver.cpp)
|
||||||
add_executable(labo_ik ${SOURCE} ${HEADERS})
|
add_executable(labo_ik ${SOURCE} ${HEADERS})
|
||||||
|
|
||||||
target_link_libraries(labo_ik nanogui opengl32 ${NANOGUI_EXTRA_LIBS})
|
target_link_libraries(labo_ik nanogui OpenGL ${NANOGUI_EXTRA_LIBS})
|
||||||
|
|
||||||
if(MSVC)
|
if(MSVC)
|
||||||
set_property(TARGET labo_ik PROPERTY VS_DEBUGGER_WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/labo_ik)
|
set_property(TARGET labo_ik PROPERTY VS_DEBUGGER_WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/labo_ik)
|
||||||
|
|
Loading…
Reference in New Issue