From 80f413d1a471ec13a8b86be2db7deaa6fd89c4a6 Mon Sep 17 00:00:00 2001 From: ZhiWang0413 <1254739463@qq.com> Date: Wed, 23 Apr 2025 20:13:15 +0800 Subject: [PATCH] add ik --- 6_ik/CMakeLists.txt | 43 +++++++++++++ 6_ik/IkSim.cpp | 84 +++++++++++++++++++++++++ 6_ik/IkSim.h | 147 ++++++++++++++++++++++++++++++++++++++++++++ 6_ik/main.cpp | 41 ++++++++++++ CMakeLists.txt | 3 +- data/bone.off | 24 ++++++++ 6 files changed, 341 insertions(+), 1 deletion(-) 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 create mode 100644 data/bone.off 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..6c5a2eb --- /dev/null +++ b/6_ik/IkSim.cpp @@ -0,0 +1,84 @@ +#include "IkSim.h" + +bool IkSim::advance() { + // advance step + m_step++; + + Eigen::Vector2d targetPos = m_skeleton.m_pos + + Eigen::Vector2d(5 * sqrt(0.5), sqrt(0.5)) + + Eigen::Vector2d(sin(m_step/50.0), 0); // target position + + int iterNum = 10; + float alpha = 0.1; + for(int i = 0; i < iterNum; i++){ + Eigen::Vector2d currentPos = forwardKinematics(); + + Eigen::Vector2d error = targetPos - currentPos; + + double errorNorm = error.norm(); + + if (errorNorm < 0.01) { + return false; // stop if the error is small enough + } + + // construct the Jacobian matrix + Eigen::MatrixXd J = computeJacobian(targetPos); + + auto deltaTheta = alpha * J.transpose() * error; + + for (int i = 0; i < m_skeleton.m_num_bones; i++) { + m_skeleton.m_bones[i].angle += deltaTheta(i, 0); + } + } + + + return false; +} + +Eigen::MatrixXd IkSim::computeJacobian(Eigen::Vector2d targetPos) { + Eigen::MatrixXd J(2, m_skeleton.m_num_bones); + + // compute the Jacobian matrix + double accumAngle = 0; + for (int i = 0; i < m_skeleton.m_num_bones; i++) { + accumAngle += m_skeleton.m_bones[i].angle; + } + double accumX = 0; + double accumY = 0; + for (int i = m_skeleton.m_num_bones - 1; i >= 0; --i) { + double angle = m_skeleton.m_bones[i].angle; + double length = m_skeleton.m_bones[i].length; + double dx = length * cos(accumAngle); + double dy = length * sin(accumAngle); + J(0, i) = -dy; // partial derivative with respect to angle + J(1, i) = dx; // partial derivative with respect to length + accumX += dx; + accumY += dy; + accumAngle -= angle; + } + return J; +} + +Eigen::VectorXd IkSim::computeGradient() { + Eigen::VectorXd grad; + return grad; +} + +Eigen::Vector2d IkSim::forwardKinematics() { + double accumX = 0; + double accumY = 0; + + double accumAngle = 0; + for (int i = 0; i < m_skeleton.m_num_bones; i++) { + Bone& b = m_skeleton.m_bones[i]; + + accumAngle += b.angle; + double dx = b.length * cos(accumAngle); + double dy = b.length * sin(accumAngle); + + accumX += dx; + accumY += dy; + } + + return Eigen::Vector2d(accumX + m_skeleton.m_pos.x(), accumY + m_skeleton.m_pos.y()); +} \ No newline at end of file diff --git a/6_ik/IkSim.h b/6_ik/IkSim.h new file mode 100644 index 0000000..25df4b6 --- /dev/null +++ b/6_ik/IkSim.h @@ -0,0 +1,147 @@ +#include "Simulation.h" +#include +using namespace std; + +struct Bone { + Bone(double l, double a) : length(l), angle(a) {} + ~Bone() {} + double length; + double angle; +}; + +struct Skeleton { + Skeleton(): m_pos(Eigen::Vector2d(-2.5, 0)) {} + ~Skeleton() {} + void init(int num_bones) { + srand(time(NULL)); + m_num_bones = num_bones; + m_bones.clear(); + m_bones.reserve(num_bones); + for (int i = 0; i < num_bones; ++i) { + double length = 1.0; // default length + double angle = (rand() / (RAND_MAX + 1.0) * 2 - 1) * M_PI / 3; // random angle between [-pi, pi] + if (i == 0) + angle = M_PI / 4; + else + angle = M_PI / 2 * (1 - ((i % 2) * 2)); + m_bones.emplace_back(length, angle); + } + } + + int m_num_bones; // number of bones in the skeleton + Eigen::Vector2d m_pos; // position of the root joint + std::vector m_bones; // bones of the skeleton +}; + +class IkSim : public Simulation { +public: + IkSim() : Simulation() { + init(); + } + + virtual void init() override { + m_skeleton.init(5); // Initialize skeleton with 5 bones + + std::string path = "bone.off"; + m_objects.clear(); + for (int i = 0; i < m_skeleton.m_num_bones; i++) { + m_objects.push_back(RigidObject(path)); + m_objects[i].setScale(0.125); + } + + m_objects.push_back(RigidObject("sphere.off")); + m_objects[m_skeleton.m_num_bones].setScale(0.01); + m_objects[m_skeleton.m_num_bones].setPosition(Eigen::Vector3d(-2.5, 0, 0)); + + m_dt = 5e-2; + m_radius = 10; + + reset(); + } + + virtual void resetMembers() override { + + } + + void updateSkeletonPosition() { + double accumX = 0; + double accumY = 0; + + double accumAngle = 0; + for (int i = 0; i < m_skeleton.m_num_bones; i++) { + RigidObject& o = m_objects[i]; + Bone& b = m_skeleton.m_bones[i]; + + accumAngle += b.angle; + double dx = b.length * cos(accumAngle); + double dy = b.length * sin(accumAngle); + + o.setPosition(Eigen::Vector3d(m_skeleton.m_pos.x() + accumX + dx / 2, m_skeleton.m_pos.y() + accumY + dy / 2, 0)); + + Eigen::Matrix3d rotate = Eigen::AngleAxisd(accumAngle, Eigen::Vector3d(0, 0, 1)).toRotationMatrix(); + Eigen::Matrix3d rotateInv = rotate.inverse(); + + o.setRotation(rotate); + accumX += dx; + accumY += dy; + } + } + + Eigen::MatrixXd computeJacobian(Eigen::Vector2d targetPos); + + Eigen::VectorXd computeGradient(); + + Eigen::Vector2d forwardKinematics(); + + 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); + } + } + + void setRadius(float r) { m_radius = r; } + +private: + float m_radius; + Skeleton m_skeleton; + + 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..cccac93 --- /dev/null +++ b/6_ik/main.cpp @@ -0,0 +1,41 @@ +#include "IkSim.h" +#include "Gui.h" + +/* + * GUI for the earth simulation. + */ +class EarthGui : public Gui { +public: + // simulation parameters + float m_dt = 1e-2; + float m_radius = 10.0; + + IkSim* p_IkSim = NULL; + + EarthGui() { + // 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); + p_IkSim->setRadius(m_radius); + } + + + virtual void drawSimulationParameterMenu() override { + ImGui::InputFloat("Radius", &m_radius, 0, 0); + 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 EarthGui(); + + 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/data/bone.off b/data/bone.off new file mode 100644 index 0000000..3d67d5f --- /dev/null +++ b/data/bone.off @@ -0,0 +1,24 @@ +OFF +# cube.off +# A cube +8 12 0 +-4 -1 -1 + 4 -1 -1 +-4 1 -1 + 4 1 -1 +-4 -1 1 + 4 -1 1 +-4 1 1 + 4 1 1 +3 0 2 1 +3 2 3 1 +3 1 3 5 +3 3 7 5 +3 2 6 3 +3 6 7 3 +3 5 7 4 +3 7 6 4 +3 4 6 0 +3 6 2 0 +3 4 0 1 +3 4 1 5 \ No newline at end of file -- GitLab