Cinématique inverse de l'armature

This commit is contained in:
FyloZ 2024-04-06 18:00:38 -04:00
parent 80375e68de
commit b4e8eee37f
Signed by: william
GPG Key ID: 835378AE9AF4AE97
2 changed files with 29 additions and 26 deletions

View File

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

View File

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