Logo Search packages:      
Sourcecode: blender version File versions

IK_QJacobianSolver.h

/**
 * $Id: IK_QJacobianSolver.h,v 1.3 2002/10/30 02:06:30 mein 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 *****
 */

#ifndef NAN_INCLUDED_IK_QJacobianSolver_h

#define NAN_INCLUDED_IK_QJacobianSolver_h

/**
 * @author Laurence Bourn
 * @date 28/6/2001
 */

#include "TNT/cmat.h"
#include <vector>
#include "MT_Vector3.h"
#include "IK_QChain.h"

00046 class IK_QJacobianSolver {

public :

      /**
       * Create a new IK_QJacobianSolver on the heap
       * @return A newly created IK_QJacobianSolver you take ownership of the object
     * and responsibility for deleting it
       */


      static 
            IK_QJacobianSolver *
      New(
      );

      /**
       * Compute a solution for a chain.
     * @param chain Reference to the chain to modify
       * @param g_position Reference to the goal position.
       * @param g_pose -not used- Reference to the goal pose. 
       * @param tolerance The maximum allowed distance between solution
     * and goal for termination.
       * @param max_iterations should be in the range (50 - 500) 
     * @param max_angle_change The maximum change in the angle vector 
     * of the chain (0.1 is a good value)
       *
     * @return True iff goal position reached.
     */

            bool
      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
      );

      ~IK_QJacobianSolver(
      );


private :

      /**
       * Private constructor to force use of New()
       */

      IK_QJacobianSolver(
      );


      /** 
       * Compute the inverse jacobian matrix of the chain.
       * Uses a damped least squares solution when the matrix is 
       * is approaching singularity
     */

            int
      ComputeInverseJacobian(
            IK_QChain &chain,
            const MT_Scalar x_length,
            const MT_Scalar max_angle_change
      );

            void
      ComputeBetas(
            IK_QChain &chain,
            const MT_Vector3 d_pos
      );

      /** 
       * Updates the angles of the chain with the newly
       * computed values
     */

            void
      UpdateChain(
            IK_QChain &chain
      );
      
      /**
       * Make sure all the matrices are of the correct size
     */

            void
      ArmMatrices(
            int dof
      );

      /**
       * Quick check to see if all the segments are parallel
       * to the goal direction.
       */

            bool
      ParallelCheck(
            const IK_QChain &chain,
            const MT_Vector3 goal
      ) const;
      


private :

      /// the vector of intermediate betas
00154       TNT::Vector<MT_Scalar> m_beta;

      /// the vector of computed angle changes
00157       TNT::Vector<MT_Scalar> m_d_theta;

      /// the constraint gradients for each angle.
00160       TNT::Vector<MT_Scalar> m_dh;

      /// Space required for SVD computation

00164       TNT::Vector<MT_Scalar> m_svd_w;
    TNT::Vector<MT_Scalar> m_svd_work_space;        
      TNT::Matrix<MT_Scalar> m_svd_v;
      TNT::Matrix<MT_Scalar> m_svd_u;

      TNT::Matrix<MT_Scalar> m_svd_w_diag;
      TNT::Matrix<MT_Scalar> m_svd_v_t;
      TNT::Matrix<MT_Scalar> m_svd_u_t;
      TNT::Matrix<MT_Scalar> m_svd_inverse;
      TNT::Matrix<MT_Scalar> m_svd_temp1;


};

#endif


Generated by  Doxygen 1.6.0   Back to index