Logo Search packages:      
Sourcecode: blender version File versions

IK_QJacobianSolver.cpp

/**
 * $Id: IK_QJacobianSolver.cpp,v 1.4 2003/05/01 19:52:20 sirdude Exp $
 * ***** 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 *****
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "IK_QJacobianSolver.h"

#include "TNT/svd.h"

using namespace std;

      IK_QJacobianSolver *
IK_QJacobianSolver::
00044 New(
){
      return new IK_QJacobianSolver();
}

      bool
IK_QJacobianSolver::
00051 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::
00125 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::
00150 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::
00247 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::
00270 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::
00297 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;
      }
}



Generated by  Doxygen 1.6.0   Back to index