Initial revision

intern/iksolver/intern/IK_QJacobianSolver.cpp

@@ -0,0 +1,327 @@
+/**
+ * $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_QJacobianSolver.h"
+
+#include "TNT/svd.h"
+
+using namespace std;
+
+	IK_QJacobianSolver *
+IK_QJacobianSolver::
+New(
+){
+	return new IK_QJacobianSolver();
+}
+
+	bool
+IK_QJacobianSolver::
+Solve(
+	IK_QChain &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
+){
+
+	const vector<IK_QSegment> & segs = chain.Segments();
+	if (segs.size() == 0) return false;
+	
+	// compute the goal direction from the base of the chain
+	// in global coordinates
+
+	MT_Vector3 goal_dir = g_position - segs[0].GlobalSegmentStart();
+	
+
+	const MT_Scalar chain_max_extension = chain.MaxExtension();
+
+	bool do_parallel_check(false);
+
+	if (chain_max_extension < goal_dir.length()) {
+		do_parallel_check = true;
+	}
+
+	goal_dir.normalize();
+
+
+	ArmMatrices(chain.DoF());
+
+	for (int iterations = 0; iterations < max_iterations; iterations++) {
+		
+		// check to see if the chain is pointing in the right direction
+
+		if (iterations%32 && do_parallel_check && ParallelCheck(chain,goal_dir)) {
+
+			return false;
+		}
+		
+		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();
+
+        try {
+		    ComputeInverseJacobian(chain,x_length,max_angle_change);
+        }
+        catch(...) {
+            return false;
+        }
+		
+		ComputeBetas(chain,d_pos);
+		UpdateChain(chain);
+		chain.UpdateGlobalTransformations();
+	}
+
+
+	return false;
+};
+
+IK_QJacobianSolver::
+~IK_QJacobianSolver(
+){
+	// nothing to do
+}
+
+
+IK_QJacobianSolver::
+IK_QJacobianSolver(
+){
+	// nothing to do
+};
+ 
+	void
+IK_QJacobianSolver::
+ComputeBetas(
+	IK_QChain &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_QJacobianSolver::
+ComputeInverseJacobian(
+	IK_QChain &chain,
+	const MT_Scalar x_length,
+	const MT_Scalar max_angle_change
+) {
+
+	int dimension = 0;
+
+	m_svd_u = MT_Scalar(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 = MT_Scalar(0);
+	m_svd_v = MT_Scalar(0);
+
+    m_svd_work_space = MT_Scalar(0);
+
+	TNT::SVD(m_svd_u,m_svd_w,m_svd_v,m_svd_work_space);
+
+	// 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 = MT_Scalar(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 = MT_Scalar(0);
+	}
+
+
+	lambda *= lambda;
+
+	for (i= 0; i < m_svd_w.size(); i++) {
+		if (m_svd_w[i] < epsilon) {
+			m_svd_w[i] = MT_Scalar(0);
+		} else {
+			m_svd_w[i] = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
+		}
+	}
+
+
+	TNT::matmultdiag(m_svd_temp1,m_svd_w,m_svd_u_t);
+	TNT::matmult(m_svd_inverse,m_svd_v,m_svd_temp1);
+
+	return dimension;
+
+}
+
+	void
+IK_QJacobianSolver::
+UpdateChain(
+	IK_QChain &chain
+){
+
+	// iterate through the set of angles and 
+	// update their values from the d_thetas
+
+	vector<IK_QSegment> &segs = chain.Segments(); 
+	
+	int seg_ind = 0;
+	for (seg_ind = 0;seg_ind < segs.size(); seg_ind++) {
+	
+		MT_Vector3 dq;
+		dq[0] = m_d_theta[3*seg_ind];
+		dq[1] = m_d_theta[3*seg_ind + 1];
+		dq[2] = m_d_theta[3*seg_ind + 2];
+		segs[seg_ind].IncrementAngle(dq);	
+	}
+
+};
+	
+	void
+IK_QJacobianSolver::
+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_work_space.newsize(dof);
+
+	m_svd_u = MT_Scalar(0);
+	m_svd_v = MT_Scalar(0);
+	m_svd_w = MT_Scalar(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);
+
+};
+
+	bool
+IK_QJacobianSolver::
+ParallelCheck(
+	const IK_QChain &chain,
+	const MT_Vector3 goal_dir
+) const {
+	
+	// compute the start of the chain in global coordinates
+	const vector<IK_QSegment> &segs = chain.Segments(); 
+
+	int num_segs = segs.size();
+	
+	if (num_segs == 0) {
+		return false;
+	}
+
+	MT_Scalar crossp_sum = 0;
+
+	int i;
+	for (i = 0; i < num_segs; i++) {
+		MT_Vector3 global_seg_direction = segs[i].GlobalSegmentEnd() - 
+			segs[i].GlobalSegmentStart();
+
+		global_seg_direction.normalize();	
+
+		MT_Scalar crossp = (global_seg_direction.cross(goal_dir)).length();
+		crossp_sum += MT_Scalar(fabs(crossp));
+	}
+
+	if (crossp_sum < MT_Scalar(0.01)) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+