Impl

.idea/encodings.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Encoding">
+    <file url="file://$PROJECT_DIR$/labo_ik/SVD.h" charset="UTF-8" />
+  </component>
+</project>

labo_ik/CMakeLists.txt

@@ -23,8 +23,9 @@ include_directories(${PROJECT_SOURCE_DIR}/../labo01/src ${COMMON_INCLUDES})
 find_package(OpenGL REQUIRED)
 
 # Add .cpp and .h files
-set(HEADERS IKApplication.h IKGLCanvas.h LinkUI.h TargetUI.h Armature.h IKSolver.h SVD.h)
-set(SOURCE main.cpp IKApplication.cpp IKGLCanvas.cpp LinkUI.cpp TargetUI.cpp Armature.cpp IKSolver.cpp)
+set(HEADERS IKApplication.h IKGLCanvas.h LinkUI.h TargetUI.h Armature.h IKSolver.h)
+set(SOURCE main.cpp IKApplication.cpp IKGLCanvas.cpp LinkUI.cpp TargetUI.cpp Armature.cpp IKSolver.cpp
+        SVD.h)
 add_executable(labo_ik ${SOURCE} ${HEADERS})
 
 target_link_libraries(labo_ik nanogui OpenGL ${NANOGUI_EXTRA_LIBS})

labo_ik/IKSolver.cpp

@@ -41,6 +41,15 @@ void jacobian(Jacobianf &m_J, Link *link, Vector3f &ri, int i) {
     }
 }
 
+template<typename _Scalar, int _Rows, int _Storage>
+float dotP(SubMatrix<_Scalar, _Rows, Dynamic, _Storage> a, Vector<_Scalar, _Rows> b) {
+    _Scalar product = 0;
+    for (int i = 0; i < b.size(); i++) {
+        product += a(i, 0) * b(i);
+    }
+    return product;
+}
+
 void IKSolver::solve() {
     const int numLinks = m_armature->links.size();
     const int dim = 3 * (numLinks);
@@ -67,8 +76,28 @@ void IKSolver::solve() {
     // TODO Compute the change in the joint angles by solving:
     //    df/dtheta * delta_theta = delta_x
     //  where df/dtheta is the Jacobian matrix.
-    //    
-    //
+    auto svd = SVD(m_J);
+    svd.decompose();
+
+//    svd.getSigma() * svd.getV().transpose() * d_theta - svd.getU().transpose() * dx;
+
+    int rank = 0;
+    for (int i = 0; i < svd.getSigma().size(); i++) {
+        if (svd.getSigma()(i) == 0) {
+            break;
+        }
+
+        rank++;
+    }
+    auto d_theta = Vector<float, Dynamic>(rank);
+    auto z = Vector<float, Dynamic>(rank);
+    for (int i = 0; i < rank; i++) {
+        auto ui = svd.getU().block(i, 0, 3, 1);
+        auto si = svd.getSigma()(i);
+        z(i) = dotP(ui, dx) / si;
+    }
+
+    d_theta = svd.getV() * z;
 
     // TODO Perform gradient descent method with line search
     //      to move the end effector toward the target position.
@@ -78,6 +107,13 @@ void IKSolver::solve() {
     // 
     //   Hint: whenever you change the joint angles, you must also call
     //   armature->updateKinematics() to compute the global position.
-    //    
+    float alpha = 1;
+    Vector<float, Dynamic> initial_theta;
+    m_armature->pack(initial_theta);
+    do {
+        m_armature->unpack(initial_theta + alpha * d_theta);
+        m_armature->updateKinematics();
 
+        alpha /= 2;
+    } while (getError(dx) < error);
 }

labo_ik/SVD.h

@@ -47,7 +47,7 @@ namespace gti320
     public:
 
         explicit SVD(const Matrix<_Scalar, _Rows, _Cols, _Storage>& _A) :
-            m_U(_A), m_V(_A.cols(), _A.cols()), m_S(_A.cols())
+                m_U(_A), m_V(_A.cols(), _A.cols()), m_S(_A.cols())
         {
         }
 
@@ -327,7 +327,7 @@ namespace gti320
             const int ncols = m_U.cols();
             const int nrows = m_U.rows();
             Vector<_Scalar> su(nrows), sv(ncols);
-            do { inc *= 3; inc++; } while (inc <= ncols);   // Sort using Shell’s sort.
+            do { inc *= 3; inc++; } while (inc <= ncols);   // Sort using Shell<EFBFBD>s sort.
 
             do {
                 inc /= 3;