add ik

6_ik/CMakeLists.txt

+
+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

6_ik/IkSim.cpp

+#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

6_ik/IkSim.h

+#include "Simulation.h"
+#include <time.h>
+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<Bone> 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<Eigen::MatrixXd> m_renderVs;  // vertex positions for rendering
+    std::vector<Eigen::MatrixXi> m_renderFs;  // face indices for rendering
+};
\ No newline at end of file

6_ik/main.cpp

+#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

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)
+add_subdirectory(5_fluid)
\ No newline at end of file
+add_subdirectory(6_ik)
\ No newline at end of file

data/bone.off

+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