This commit is contained in:
FyloZ 2024-04-10 22:58:40 -04:00
parent ba006846d2
commit 8b10e5ee6d
Signed by: william
GPG Key ID: 835378AE9AF4AE97
4 changed files with 50 additions and 7 deletions

6
.idea/encodings.xml Normal file
View File

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

View File

@ -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})

View File

@ -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);
}

View File

@ -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 Shells sort.
do { inc *= 3; inc++; } while (inc <= ncols); // Sort using Shell<EFBFBD>s sort.
do {
inc /= 3;