diff --git a/.idea/encodings.xml b/.idea/encodings.xml
new file mode 100644
index 0000000..0be61e7
--- /dev/null
+++ b/.idea/encodings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/labo_ik/CMakeLists.txt b/labo_ik/CMakeLists.txt
index 12bfa72..b246ab4 100644
--- a/labo_ik/CMakeLists.txt
+++ b/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})
diff --git a/labo_ik/IKSolver.cpp b/labo_ik/IKSolver.cpp
index 2acbc1d..8a25cd8 100644
--- a/labo_ik/IKSolver.cpp
+++ b/labo_ik/IKSolver.cpp
@@ -41,6 +41,15 @@ void jacobian(Jacobianf &m_J, Link *link, Vector3f &ri, int i) {
}
}
+template
+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(rank);
+ auto z = Vector(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 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);
}
diff --git a/labo_ik/SVD.h b/labo_ik/SVD.h
index 9597bab..6f422f6 100644
--- a/labo_ik/SVD.h
+++ b/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�s sort.
do {
inc /= 3;