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

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() { 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);
} }

View File

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