From c66a4e82c16203daa4cb9a9b9548a38531b021f3 Mon Sep 17 00:00:00 2001 From: Wangzhi Date: Mon, 5 May 2025 20:46:41 +0800 Subject: [PATCH] add ik --- 6_ik/CMakeLists.txt | 43 +++++++++ 6_ik/IkSim.cpp | 91 ++++++++++++++++++ 6_ik/IkSim.h | 212 +++++++++++++++++++++++++++++++++++++++++ 6_ik/main.cpp | 39 ++++++++ CMakeLists.txt | 3 +- include/BaseObject.cpp | 12 ++- include/BaseObject.h | 6 +- 7 files changed, 399 insertions(+), 7 deletions(-) create mode 100644 6_ik/CMakeLists.txt create mode 100644 6_ik/IkSim.cpp create mode 100644 6_ik/IkSim.h create mode 100644 6_ik/main.cpp diff --git a/6_ik/CMakeLists.txt b/6_ik/CMakeLists.txt new file mode 100644 index 0000000..9decf6d --- /dev/null +++ b/6_ik/CMakeLists.txt @@ -0,0 +1,43 @@ + +cmake_minimum_required(VERSION 3.1) +project(6_ik) + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) +set(CMAKE_CXX_FLAGS "-Wall") + +# libigl +option(LIBIGL_USE_STATIC_LIBRARY "Use libigl as static library" OFF) +option(LIBIGL_WITH_ANTTWEAKBAR "Use AntTweakBar" OFF) +option(LIBIGL_WITH_CGAL "Use CGAL" OFF) +option(LIBIGL_WITH_COMISO "Use CoMiso" OFF) +option(LIBIGL_WITH_CORK "Use Cork" OFF) +option(LIBIGL_WITH_EMBREE "Use Embree" OFF) +option(LIBIGL_WITH_LIM "Use LIM" OFF) +option(LIBIGL_WITH_MATLAB "Use Matlab" OFF) +option(LIBIGL_WITH_MOSEK "Use MOSEK" OFF) +option(LIBIGL_WITH_OPENGL "Use OpenGL" ON) +option(LIBIGL_WITH_OPENGL_GLFW "Use GLFW" ON) +option(LIBIGL_WITH_OPENGL_GLFW_IMGUI "Use ImGui" ON) +option(LIBIGL_WITH_PNG "Use PNG" OFF) +option(LIBIGL_WITH_PYTHON "Use Python" OFF) +option(LIBIGL_WITH_TETGEN "Use Tetgen" OFF) +option(LIBIGL_WITH_TRIANGLE "Use Triangle" OFF) +option(LIBIGL_WITH_VIEWER "Use OpenGL viewer" ON) +option(LIBIGL_WITH_XML "Use XML" OFF) + +if (NOT LIBIGL_FOUND) + find_package(LIBIGL REQUIRED QUIET) +endif() + +# Add default project files +file(GLOB LIBFILES ${PROJECT_SOURCE_DIR}/../include/*.*) +source_group("Library Files" FILES ${LIBFILES}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../include) + +# Add your project files +file(GLOB SRCFILES *.cpp) +file(GLOB HFILES *.h) + +add_definitions(-DIGL_VIEWER_VIEWER_QUIET) +add_executable(${PROJECT_NAME} ${SRCFILES} ${HFILES} ${LIBFILES} ) +target_link_libraries(${PROJECT_NAME} igl::core igl::opengl_glfw igl::opengl_glfw_imgui) \ No newline at end of file diff --git a/6_ik/IkSim.cpp b/6_ik/IkSim.cpp new file mode 100644 index 0000000..168786e --- /dev/null +++ b/6_ik/IkSim.cpp @@ -0,0 +1,91 @@ +#include "IkSim.h" + +void Skeleton::init() { + m_bones.clear(); + + // left arm + Bone leftArm(Eigen::Vector3d(-1.2, 3, 0)); + leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(-0.5, -1.7, -0.8)); + leftArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4)); + + // right arm + Bone rightArm(Eigen::Vector3d(1.2, 3, 0)); + rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0.5, -1.7, -0.8)); + rightArm.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -1.4, 1.4)); + + // left leg + Bone leftLeg(Eigen::Vector3d(-0.7, 0, 0)); + leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2)); + leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4)); + leftLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2)); + + // right leg + Bone rightLeg(Eigen::Vector3d(0.7, 0, 0)); + rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, 0.2)); + rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, -2, -0.4)); + rightLeg.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 1.2)); + + // head + Bone head(Eigen::Vector3d(0, 3, 0)); + head.addJoint(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0.7, 0)); + + m_bones.emplace_back(leftArm); + m_bones.emplace_back(rightArm); + m_bones.emplace_back(leftLeg); + m_bones.emplace_back(rightLeg); + m_bones.emplace_back(head); +} + +std::vector IkSim::updateMotion() { + std::vector targetPos; + + Eigen::Vector3d footPos1 = Eigen::Vector3d(0, 0, 3); + Eigen::Vector3d footPos2 = Eigen::Vector3d(0, 0, -4); + + for (int i = 0; i < m_skeleton.m_bones.size(); i++) { + Bone& bone = m_skeleton.m_bones[i]; + Eigen::Vector3d endPos = m_bone_init_end[i]; + + //todo: update endPos based on m_step + + targetPos.push_back(endPos); + } + + return targetPos; +} + +bool IkSim::advance() { + // advance step + m_step++; + + int iterNum = 100; + float alpha = 0.01; + std::vector targetPos = updateMotion(); + + + //todo: update skeleton position use inverse kinematics + + return false; +} + +Eigen::MatrixXd Bone::computeJacobian(Eigen::Vector3d endPos) { + Eigen::MatrixXd J(3, m_num_joints * 3); + + // todo: compute the Jacobian matrix + + return J; +} + +Eigen::Vector3d Bone::forwardKinematics(int joint_index) { + if (joint_index < -1 || joint_index >= m_num_joints) { + printf("joint index out of range\n"); + exit(-1); + } + //todo: compute the forward kinematics of the joint in the local coordinate system of skeleton + //joint_index = -1 means the root position + //jointindex = 0 means the first joint + //joint_index = m_num_joints - 1 means the end effector position + Eigen::Vector3d pos = m_root; + + return pos; +} \ No newline at end of file diff --git a/6_ik/IkSim.h b/6_ik/IkSim.h new file mode 100644 index 0000000..4be7e0b --- /dev/null +++ b/6_ik/IkSim.h @@ -0,0 +1,212 @@ +#include "Simulation.h" +#include +using namespace std; + +inline float getRandomFloat(float min, float max) { + return min + static_cast(rand()) / (static_cast(RAND_MAX / (max - min))); +} + +struct Joint { + Eigen::Vector3d angles; // euler angles in degrees + Eigen::Vector3d extent; // length and direction of the bone + Eigen::Matrix3d toRotation() { + Eigen::Matrix3d rotation; + rotation = Eigen::AngleAxisd(angles[2] * M_PI / 180, Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(angles[1] * M_PI / 180, Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(angles[0] * M_PI / 180, Eigen::Vector3d::UnitX()); + return rotation; + } + Joint() : angles(Eigen::Vector3d(0, 0, 0)), extent(Eigen::Vector3d(1, 0, 0)) {} // default constructor + Joint(Eigen::Vector3d angles, Eigen::Vector3d extent) : angles(angles), extent(extent) {} + ~Joint() {} +}; + +struct Bone { + Bone(): m_root(Eigen::Vector3d(0, 0, 0)) {} + Bone(Eigen::Vector3d root) : m_root(root) {} + ~Bone() {} + void init(int num_joints) { + srand(time(NULL)); + m_num_joints = num_joints; + m_joints.clear(); + m_joints.reserve(num_joints); + for (int i = 0; i < num_joints; ++i) { + Eigen::Vector3d angles = Eigen::Vector3d(0, 0, 90 - (i % 2) * 180); + if (i == 0) + angles = Eigen::Vector3d(0, 0, 45); + else + angles = Eigen::Vector3d(0, 0, -45); + Eigen::Vector3d extent = Eigen::Vector3d(1, 0, 0); + m_joints.emplace_back(angles, extent); + } + } + + int m_num_joints = 0; // number of joints in the bone + Eigen::Vector3d m_root; // root position of the bone + std::vector m_joints; // joints of the bone + + Eigen::Vector3d forwardKinematics(int joint_index); + + Eigen::MatrixXd computeJacobian(Eigen::Vector3d endPos); + + void addJoint(Eigen::Vector3d angles, Eigen::Vector3d extent) { + m_joints.emplace_back(angles, extent); + m_num_joints++; + } +}; + +struct Skeleton { + Eigen::Vector3d base_pos = Eigen::Vector3d::Zero(); + std::vector m_bones; // bones of the skeleton + Skeleton(){} + ~Skeleton(){} + void init(); + void addBone(Bone& bone) { + m_bones.emplace_back(bone); + } +}; + +class IkSim : public Simulation { +public: + IkSim() : Simulation() { + init(); + } + + virtual void init() override { + m_skeleton.base_pos = Eigen::Vector3d(0, 0, 0); // Initialize root position + + m_skeleton.init(); // Initialize skeleton with 2 joints + + std::string path = "cube.off"; + m_objects.clear(); + for (int i = 0; i < m_skeleton.m_bones.size(); i++) { + Bone& bone = m_skeleton.m_bones[i]; + for (int j = 0; j < bone.m_num_joints; j++) { + m_objects.emplace_back(path); + m_objects.back().setScale(bone.m_joints[j].extent.norm() / 2, 0.2, 0.2); + } + } + + m_objects.emplace_back(path); + m_objects.back().setScale(1, 1.5, 0.5); + m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0)); + + m_objects.emplace_back("sphere.off"); + m_objects.back().setScale(0.04); + m_objects.back().setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0)); + + m_dt = 5e-2; + + reset(); + } + + void initEndVector() { + m_bone_init_end.clear(); + for (int i = 0; i < m_skeleton.m_bones.size(); i++) { + Bone& bone = m_skeleton.m_bones[i]; + Eigen::Vector3d endPos = bone.forwardKinematics(bone.m_num_joints - 1); + m_bone_init_end.push_back(endPos); + } + } + + virtual void resetMembers() override { + m_skeleton.base_pos = Eigen::Vector3d(0, 0, 0); // Initialize root position + + m_skeleton.init(); // Initialize skeleton with 2 joints + + initEndVector(); // Initialize end positions of bones + } + + std::vector updateMotion(); + + void updateSkeletonPosition() { + + int rigidIndex = 0; + for (int i = 0; i < m_skeleton.m_bones.size(); i++) { + Bone& bone = m_skeleton.m_bones[i]; + + Eigen::Vector3d pos = m_skeleton.base_pos + bone.m_root; + Eigen::Matrix3d rotate = Eigen::Matrix3d::Identity(); + + for (int j = 0; j < bone.m_num_joints; j++) { + RigidObject& o = m_objects[rigidIndex]; + Joint& b = bone.m_joints[j]; + + Eigen::Matrix3d jointRotate = bone.m_joints[j].toRotation(); + rotate = jointRotate * rotate; + Eigen::Vector3d extent = rotate * b.extent; + o.setPosition(pos + extent / 2); + + Eigen::Vector3d unit = Eigen::Vector3d(1, 0, 0); + Eigen::Vector3d normalExtent = b.extent.normalized(); + Eigen::Vector3d axis = unit.cross(normalExtent); + double angle = acos(unit.dot(normalExtent)); + + Eigen::Matrix3d extentRotate = Eigen::Matrix3d::Identity(); + if (axis.norm() > 1e-6) { + axis.normalize(); + extentRotate = Eigen::AngleAxisd(angle, axis).toRotationMatrix(); + } + o.setRotation(rotate * extentRotate); + + pos += extent; + + rigidIndex++; + } + } + + m_objects[rigidIndex].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 1.5, 0)); + + m_objects[rigidIndex + 1].setPosition(m_skeleton.base_pos + Eigen::Vector3d(0, 4, 0)); + } + + virtual void updateRenderGeometry() override { + updateSkeletonPosition(); + for (size_t i = 0; i < m_objects.size(); i++) { + RigidObject &o = m_objects[i]; + if (o.getID() < 0) { // negative id means newly created object, reverse memory for it + m_renderVs.emplace_back(); + m_renderFs.emplace_back(); + } + + m_objects[i].getMesh(m_renderVs[i], m_renderFs[i]); + } + } + + virtual bool advance() override; + + virtual void renderRenderGeometry(igl::opengl::glfw::Viewer &viewer) override { + for (size_t i = 0; i < m_objects.size(); i++) { + RigidObject &o = m_objects[i]; + if (o.getID() < 0) { + int new_id = 0; + if (i > 0) { + new_id = viewer.append_mesh(); + o.setID(new_id); + } else { + o.setID(new_id); + } + + size_t meshIndex = viewer.mesh_index(o.getID()); + viewer.data_list[meshIndex].set_face_based(true); + viewer.data_list[meshIndex].point_size = 2.0f; + viewer.data_list[meshIndex].clear(); + } + size_t meshIndex = viewer.mesh_index(o.getID()); + + viewer.data_list[meshIndex].set_mesh(m_renderVs[i], m_renderFs[i]); + viewer.data_list[meshIndex].compute_normals(); + + Eigen::MatrixXd color; + o.getColors(color); + viewer.data_list[meshIndex].set_colors(color); + } + } + +private: + Skeleton m_skeleton; + + std::vector m_bone_init_end; // initial end positions of bones + std::vector m_renderVs; // vertex positions for rendering + std::vector m_renderFs; // face indices for rendering +}; \ No newline at end of file diff --git a/6_ik/main.cpp b/6_ik/main.cpp new file mode 100644 index 0000000..b1b55da --- /dev/null +++ b/6_ik/main.cpp @@ -0,0 +1,39 @@ +#include "IkSim.h" +#include "Gui.h" + +/* + * GUI for the earth simulation. + */ +class IkGui : public Gui { +public: + // simulation parameters + float m_dt = 1e-2; + float m_radius = 10.0; + + IkSim* p_IkSim = NULL; + + IkGui() { + // create a new Earth simulation, set it in the GUI, and start the GUI + p_IkSim = new IkSim(); + setSimulation(p_IkSim); + + start(); + } + + virtual void updateSimulationParameters() override { + // change all parameters of the simulation to the values that are set in the GUI + p_IkSim->setTimestep(m_dt); + } + + + virtual void drawSimulationParameterMenu() override { + ImGui::InputFloat("dt", &m_dt, 0, 0); + } +}; + +int main(int argc, char* argv[]) { + // create a new instance of the GUI for the Earth simulation + new IkGui(); + + return 0; +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index de35e18..670b384 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,4 +36,5 @@ add_subdirectory(2-2_pendulum) add_subdirectory(3-1_spinning) add_subdirectory(3-2_collision) add_subdirectory(4_cloth) -add_subdirectory(5_fluid) \ No newline at end of file +add_subdirectory(5_fluid) +add_subdirectory(6_ik) \ No newline at end of file diff --git a/include/BaseObject.cpp b/include/BaseObject.cpp index 963b28d..ccde2b9 100644 --- a/include/BaseObject.cpp +++ b/include/BaseObject.cpp @@ -60,7 +60,11 @@ void BaseObject::recomputeCOM() { m_mesh.V = m_mesh.V.rowwise() - COM.transpose(); } -void BaseObject::setScale(double s) { m_scale = s; } +void BaseObject::setScale(Eigen::Vector3d s) { m_scale = s; } + +void BaseObject::setScale(double s) { m_scale = Eigen::Vector3d(s, s, s); } + +void BaseObject::setScale(double sX, double sY, double sZ) { m_scale = Eigen::Vector3d(sX, sY, sZ); } void BaseObject::setID(int id) { m_id = id; } @@ -80,7 +84,7 @@ void BaseObject::setRotation(const Eigen::Matrix3d& R) { void BaseObject::setColors(const Eigen::MatrixXd& C) { m_mesh.C = C; } -double BaseObject::getScale() const { return m_scale; } +Eigen::Vector3d BaseObject::getScale() const { return m_scale; } int BaseObject::getID() const { return m_id; } @@ -93,14 +97,14 @@ Eigen::Quaterniond BaseObject::getRotation() const { return m_quat; } Eigen::Matrix3d BaseObject::getRotationMatrix() const { return m_rot; } Eigen::Vector3d BaseObject::getVertexPosition(int vertexIndex) const { - return m_mesh.V.row(vertexIndex) * m_scale * + return m_mesh.V.row(vertexIndex).cwiseProduct(m_scale.transpose()) * getRotationMatrix().transpose() + getPosition().transpose(); } void BaseObject::getMesh(Eigen::MatrixXd& V, Eigen::MatrixXi& F) const { // get mesh after rotation and translation - V = (m_mesh.V * m_scale * getRotationMatrix().transpose()).rowwise() + + V = (m_mesh.V* m_scale.asDiagonal() * getRotationMatrix().transpose()).rowwise() + getPosition().transpose(); F = m_mesh.F; } diff --git a/include/BaseObject.h b/include/BaseObject.h index 9b1135e..d791fbb 100644 --- a/include/BaseObject.h +++ b/include/BaseObject.h @@ -35,7 +35,9 @@ public: void reset(); void recomputeCOM(); + void setScale(Eigen::Vector3d s); void setScale(double s); + void setScale(double sX, double sY, double sZ); void setID(int id); virtual void setType(ObjType t); void setPosition(const Eigen::Vector3d& p); @@ -43,7 +45,7 @@ public: void setRotation(const Eigen::Matrix3d& R); void setColors(const Eigen::MatrixXd& C); - double getScale() const; + Eigen::Vector3d getScale() const; int getID() const; ObjType getType() const; Eigen::Vector3d getPosition() const; @@ -65,7 +67,7 @@ protected: Mesh m_mesh; ObjType m_type; - double m_scale = 1.0; // Scale + Eigen::Vector3d m_scale = Eigen::Vector3d(1.0, 1.0, 1.0); // Scale Eigen::Vector3d m_position; // Position of the center of mass Eigen::Quaterniond m_quat; // Rotation (quaternion) Eigen::Matrix3d m_rot; // Rotation (matrix) -- GitLab