diff options
author | Hans Lambermont <hans@lambermont.dyndns.org> | 2002-10-12 15:37:38 +0400 |
---|---|---|
committer | Hans Lambermont <hans@lambermont.dyndns.org> | 2002-10-12 15:37:38 +0400 |
commit | 12315f4d0e0ae993805f141f64cb8c73c5297311 (patch) | |
tree | 59b45827cd8293cfb727758989c7a74b40183974 /intern/iksolver/intern/IK_JacobianSolver.cpp |
Initial revisionv2.25
Diffstat (limited to 'intern/iksolver/intern/IK_JacobianSolver.cpp')
-rw-r--r-- | intern/iksolver/intern/IK_JacobianSolver.cpp | 267 |
1 files changed, 267 insertions, 0 deletions
diff --git a/intern/iksolver/intern/IK_JacobianSolver.cpp b/intern/iksolver/intern/IK_JacobianSolver.cpp new file mode 100644 index 00000000000..de4531f1677 --- /dev/null +++ b/intern/iksolver/intern/IK_JacobianSolver.cpp @@ -0,0 +1,267 @@ +/** + * $Id$ + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "IK_JacobianSolver.h" + +#include "TNT/svd.h" + +using namespace std; + + IK_JacobianSolver * +IK_JacobianSolver:: +New( +){ + return new IK_JacobianSolver(); +} + + bool +IK_JacobianSolver:: +Solve( + IK_Chain &chain, + const MT_Vector3 &g_position, + const MT_Vector3 &g_pose, + const MT_Scalar tolerance, + const int max_iterations, + const MT_Scalar max_angle_change +){ + + ArmMatrices(chain.DoF()); + + for (int iterations = 0; iterations < max_iterations; iterations++) { + + + MT_Vector3 end_effector = chain.EndEffector(); + + MT_Vector3 d_pos = g_position - end_effector; + const MT_Scalar x_length = d_pos.length(); + + if (x_length < tolerance) { + return true; + } + + chain.ComputeJacobian(); + ComputeInverseJacobian(chain,x_length,max_angle_change); + + ComputeBetas(chain,d_pos); + UpdateChain(chain); + chain.UpdateGlobalTransformations(); + } + + return false; +}; + +IK_JacobianSolver:: +~IK_JacobianSolver( +){ + // nothing to do +} + + +IK_JacobianSolver:: +IK_JacobianSolver( +){ + // nothing to do +}; + + void +IK_JacobianSolver:: +ComputeBetas( + IK_Chain &chain, + const MT_Vector3 d_pos +){ + + m_beta = 0; + + m_beta[0] = d_pos.x(); + m_beta[1] = d_pos.y(); + m_beta[2] = d_pos.z(); + + TNT::matmult(m_d_theta,m_svd_inverse,m_beta); + +}; + + + int +IK_JacobianSolver:: +ComputeInverseJacobian( + IK_Chain &chain, + const MT_Scalar x_length, + const MT_Scalar max_angle_change +) { + + int dimension = 0; + + m_svd_u = 0; + + // copy first 3 rows of jacobian into m_svd_u + + int row, column; + + for (row = 0; row < 3; row ++) { + for (column = 0; column < chain.Jacobian().num_cols(); column ++) { + m_svd_u[row][column] = chain.Jacobian()[row][column]; + } + } + + m_svd_w = 0; + m_svd_v = 0; + + TNT::SVD(m_svd_u,m_svd_w,m_svd_v); + + // invert the SVD and compute inverse + + TNT::transpose(m_svd_v,m_svd_v_t); + TNT::transpose(m_svd_u,m_svd_u_t); + + // Compute damped least squares inverse of pseudo inverse + // Compute damping term lambda + + // Note when lambda is zero this is equivalent to the + // least squares solution. This is fine when the m_jjt is + // of full rank. When m_jjt is near to singular the least squares + // inverse tries to minimize |J(dtheta) - dX)| and doesn't + // try to minimize dTheta. This results in eratic changes in angle. + // Damped least squares minimizes |dtheta| to try and reduce this + // erratic behaviour. + + // We don't want to use the damped solution everywhere so we + // only increase lamda from zero as we approach a singularity. + + // find the smallest non-zero m_svd_w value + + int i = 0; + + MT_Scalar w_min = MT_INFINITY; + + // anything below epsilon is treated as zero + + MT_Scalar epsilon = 1e-10; + + for ( i = 0; i <m_svd_w.size() ; i++) { + + if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) { + w_min = m_svd_w[i]; + } + } + MT_Scalar lambda = 0; + + MT_Scalar d = x_length/max_angle_change; + + if (w_min <= d/2) { + lambda = d/2; + } else + if (w_min < d) { + lambda = sqrt(w_min*(d - w_min)); + } else { + lambda = 0; + } + + lambda *= lambda; + + for (i= 0; i < m_svd_w.size(); i++) { + if (m_svd_w[i] < epsilon) { + m_svd_w[i] = 0; + } else { + m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda); + } + } + + m_svd_w_diag.diagonal(m_svd_w); + + // FIXME optimize these matrix multiplications + // using the fact that m_svd_w_diag is diagonal! + + TNT::matmult(m_svd_temp1,m_svd_w_diag,m_svd_u_t); + TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1); + return dimension; + +} + + void +IK_JacobianSolver:: +UpdateChain( + IK_Chain &chain +){ + + // iterate through the set of angles and + // update their values from the d_thetas + + int n; + vector<IK_Segment> &segs = chain.Segments(); + + int chain_dof = chain.DoF(); + int seg_ind = 0; + for (n=0; n < chain_dof;seg_ind ++) { + n += segs[seg_ind].IncrementAngles(m_d_theta.begin() + n); + } +}; + + void +IK_JacobianSolver:: +ArmMatrices( + int dof +){ + + m_beta.newsize(dof); + m_d_theta.newsize(dof); + + m_svd_u.newsize(dof,dof); + m_svd_v.newsize(dof,dof); + m_svd_w.newsize(dof); + + m_svd_u = 0; + m_svd_v = 0; + m_svd_w = 0; + + m_svd_u_t.newsize(dof,dof); + m_svd_v_t.newsize(dof,dof); + m_svd_w_diag.newsize(dof,dof); + m_svd_inverse.newsize(dof,dof); + m_svd_temp1.newsize(dof,dof); + +}; + + + + + + + + + + + + + + + + + |