Impl
This commit is contained in:
parent
ba006846d2
commit
8b10e5ee6d
|
@ -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>
|
|
@ -23,8 +23,9 @@ include_directories(${PROJECT_SOURCE_DIR}/../labo01/src ${COMMON_INCLUDES})
|
||||||
find_package(OpenGL REQUIRED)
|
find_package(OpenGL REQUIRED)
|
||||||
|
|
||||||
# Add .cpp and .h files
|
# Add .cpp and .h files
|
||||||
set(HEADERS IKApplication.h IKGLCanvas.h LinkUI.h TargetUI.h Armature.h IKSolver.h SVD.h)
|
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)
|
set(SOURCE main.cpp IKApplication.cpp IKGLCanvas.cpp LinkUI.cpp TargetUI.cpp Armature.cpp IKSolver.cpp
|
||||||
|
SVD.h)
|
||||||
add_executable(labo_ik ${SOURCE} ${HEADERS})
|
add_executable(labo_ik ${SOURCE} ${HEADERS})
|
||||||
|
|
||||||
target_link_libraries(labo_ik nanogui OpenGL ${NANOGUI_EXTRA_LIBS})
|
target_link_libraries(labo_ik nanogui OpenGL ${NANOGUI_EXTRA_LIBS})
|
||||||
|
|
|
@ -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() {
|
void IKSolver::solve() {
|
||||||
const int numLinks = m_armature->links.size();
|
const int numLinks = m_armature->links.size();
|
||||||
const int dim = 3 * (numLinks);
|
const int dim = 3 * (numLinks);
|
||||||
|
@ -67,8 +76,28 @@ void IKSolver::solve() {
|
||||||
// TODO Compute the change in the joint angles by solving:
|
// TODO Compute the change in the joint angles by solving:
|
||||||
// df/dtheta * delta_theta = delta_x
|
// df/dtheta * delta_theta = delta_x
|
||||||
// where df/dtheta is the Jacobian matrix.
|
// 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
|
// TODO Perform gradient descent method with line search
|
||||||
// to move the end effector toward the target position.
|
// 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
|
// Hint: whenever you change the joint angles, you must also call
|
||||||
// armature->updateKinematics() to compute the global position.
|
// 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);
|
||||||
}
|
}
|
||||||
|
|
|
@ -47,7 +47,7 @@ namespace gti320
|
||||||
public:
|
public:
|
||||||
|
|
||||||
explicit SVD(const Matrix<_Scalar, _Rows, _Cols, _Storage>& _A) :
|
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 ncols = m_U.cols();
|
||||||
const int nrows = m_U.rows();
|
const int nrows = m_U.rows();
|
||||||
Vector<_Scalar> su(nrows), sv(ncols);
|
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 {
|
do {
|
||||||
inc /= 3;
|
inc /= 3;
|
||||||
|
|
Loading…
Reference in New Issue