/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/kinematics/inverse/inverse_bone_traits.h

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_BONE_TRAITS_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_BONE_TRAITS_H
//
// OpenTissue Template Library
// - A generic toolbox for physics-based modeling and simulation.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_euler_angles.h> // needed for ZYZ_euler_angles

#include <cassert>

namespace OpenTissue
{
  namespace kinematics
  {
    namespace inverse
    {
      
      template<typename base_bone_traits >
      class BoneTraits 
        : public base_bone_traits
        {
        public:
          
          typedef typename base_bone_traits::transform_type               transform_type;
          typedef typename base_bone_traits::vector3_type                 vector3_type;
          typedef typename base_bone_traits::math_types::real_type        real_type;
          typedef typename base_bone_traits::math_types::value_traits     value_traits;
          
          typedef enum {hinge_type, slider_type, ball_type}               joint_type;
          
        protected:
          
          joint_type            m_type;              
          real_type             m_theta[6];          
          real_type             m_min_theta[6];      
          real_type             m_max_theta[6];      
          vector3_type          m_u;                 
          
        public:
          
          BoneTraits()
          : m_type(ball_type)
          , m_u( value_traits::zero(), value_traits::zero(), value_traits::one() )
          {
            // Create some default values
            for(size_t i=0;i<6;++i)
            {
              m_theta[i] = value_traits::zero();
              m_min_theta[i] = -value_traits::pi();
              m_max_theta[i] =  value_traits::pi();    
            }
            
            
          }
          
          joint_type const & type() const  { return this->m_type; }
          joint_type       & type()        { return this->m_type; }
          
          vector3_type const & u() const  { return this->m_u; }
          vector3_type       & u()        { return this->m_u; }
          
          real_type const & theta(size_t const & i) const 
          {
            assert( i < this->active_dofs() || !"theta(): invalid index value");
            return m_theta[i];
          }
          
          real_type      & theta(size_t const & i)
          {
            assert( i < this->active_dofs() || !"theta(): invalid index value");
            return m_theta[i];
          }
          
          size_t active_dofs() const 
          {
            if(this->m_type==hinge_type)
              return 1;
            if(this->m_type==slider_type)
              return 1;
            if(this->m_type==ball_type)
              return 3;
            return 0;
          }
          
          real_type const & min_joint_limit(size_t const & i) const 
          {
            assert( i < this->active_dofs() || !"min_joint_limit(): invalid index value");
            return m_min_theta[i];
          }
          
          real_type      & min_joint_limit(size_t const & i)
          {
            assert( i < this->active_dofs() || !"min_joint_limit(): invalid index value");
            return m_min_theta[i];
          }
          
          real_type const & max_joint_limit(size_t const & i) const 
          {
            assert( i < this->active_dofs() || !"max_joint_limit(): invalid index value");
            return m_max_theta[i];
          }
          
          real_type      & max_joint_limit(size_t const & i)
          {
            assert( i < this->active_dofs() || !"max_joint_limit(): invalid index value");
            return m_max_theta[i];
          }
          
          template<typename bone_type, typename chain_type, typename matrix_range>
          static void compute_jacobian( bone_type & bone, chain_type & chain, matrix_range & J )
          {
            assert(J.size1() == chain.get_goal_dimension() || !"compute_jacobian() invalid dimension");
            assert(J.size2() == bone.active_dofs()         || !"compute_jacobian() invalid dimension");
            
            // Get tool frame in world coordinate system
            vector3_type p = base_bone_traits::transform_point(  chain.get_end_effector()->absolute(),  chain.p_local() );
            p = p - bone.absolute().T();// new term that was missing and may also be in the article
            vector3_type i = unit(base_bone_traits::transform_vector( chain.get_end_effector()->absolute(),  chain.x_local() ));
            vector3_type j = unit(base_bone_traits::transform_vector( chain.get_end_effector()->absolute(),  chain.y_local() ));
            
            if(bone.type()==hinge_type)
            {
              // Get joint axis in world coordinate frame
              vector3_type u = bone.u(); // If bone is root then joint axis is already in world coordinate frame
              if(bone.parent())
              {
                u = base_bone_traits::transform_vector( bone.parent()->absolute(),  bone.u() );
              }
              
              vector3_type uXp = cross(u, p);
              
              J(0,0) = uXp(0);
              J(1,0) = uXp(1);
              J(2,0) = uXp(2);
              
              if(!chain.only_position() )// we need to do the orientation derivatives also
              {
                vector3_type uXi = cross(u, i);
                vector3_type uXj = cross(u, j);
                
                J(3,0) = uXi(0);
                J(4,0) = uXi(1);
                J(5,0) = uXi(2);
                
                J(6,0) = uXj(0);
                J(7,0) = uXj(1);
                J(8,0) = uXj(2);
              }
            }
            else if(bone.type()==slider_type)
            {
              // Get joint axis in world coordinate frame
              vector3_type u = bone.u(); // If bone is root then joint axis is already in world coordinate frame
              if(bone.parent())
              {
                u = base_bone_traits::transform_vector( bone.parent()->absolute(),  bone.u() );
              }
              
              J(0,0) = u(0);
              J(1,0) = u(1);
              J(2,0) = u(2);
              
              if(!chain.only_position() )// we need to do the orientation derivatives also
              {
                J(3,0) = value_traits::zero();
                J(4,0) = value_traits::zero();
                J(5,0) = value_traits::zero();
                J(6,0) = value_traits::zero();
                J(7,0) = value_traits::zero();
                J(8,0) = value_traits::zero();
              }
            }
            else if(bone.type()==ball_type)
            {
              vector3_type ii = vector3_type(value_traits::one(),value_traits::zero(),value_traits::zero());
              vector3_type jj = vector3_type(value_traits::zero(),value_traits::one(),value_traits::zero());
              vector3_type kk = vector3_type(value_traits::zero(),value_traits::zero(),value_traits::one());
              
              // Extract joint angles
              real_type phi   = bone.theta(0);
              real_type psi   = bone.theta(1);
              //real_type theta = bone.theta(2);
              
              // Compute the instantaneous axis of rotation (in parent frame)
              vector3_type u = kk;
              vector3_type v = OpenTissue::math::Rz(phi)*jj;
              vector3_type w = OpenTissue::math::Rz(phi)*OpenTissue::math::Ry(psi)*kk;
              
              // Transform instantaneous rotation axes into world coordinate frame
              if(bone.parent())
              {
                vector3_type tmp = base_bone_traits::transform_vector( bone.parent()->absolute(),  u );
                u = unit(tmp);
                tmp = base_bone_traits::transform_vector( bone.parent()->absolute(),  v );
                v = unit(tmp);
                tmp = base_bone_traits::transform_vector( bone.parent()->absolute(),  w );
                w = unit(tmp);
              }
              
              vector3_type uXp = cross(u, p);
              vector3_type vXp = cross(v, p);
              vector3_type wXp = cross(w, p);
              
              // setup phi part
              J(0,0) = uXp(0);
              J(1,0) = uXp(1);
              J(2,0) = uXp(2);
              
              // setup psi part
              J(0,1) = vXp(0);
              J(1,1) = vXp(1);
              J(2,1) = vXp(2);
              
              // setup theta part
              J(0,2) = wXp(0);
              J(1,2) = wXp(1);
              J(2,2) = wXp(2);
              
              if( ! chain.only_position() ) // we need to do the orientation derivatives also
              {
                vector3_type uXi = cross(u, i);
                vector3_type uXj = cross(u, j);
                
                vector3_type vXi = cross(v, i);
                vector3_type vXj = cross(v, j);
                
                vector3_type wXi = cross(w, i);
                vector3_type wXj = cross(w, j);
                
                J(3,0) = uXi(0);
                J(4,0) = uXi(1);
                J(5,0) = uXi(2);
                
                J(6,0) = uXj(0);
                J(7,0) = uXj(1);
                J(8,0) = uXj(2);
                
                J(3,1) = vXi(0);
                J(4,1) = vXi(1);
                J(5,1) = vXi(2);
                
                J(6,1) = vXj(0);
                J(7,1) = vXj(1);
                J(8,1) = vXj(2);
                
                J(3,2) = wXi(0);
                J(4,2) = wXi(1);
                J(5,2) = wXi(2);
                
                J(6,2) = wXj(0);
                J(7,2) = wXj(1);
                J(8,2) = wXj(2);
              }
            }
            else
            {
              assert(false || !"compute_jacobian(): unknown joint type");
            }
          }
          
          template<typename bone_type, typename vector_range>
          static void set_theta(bone_type & bone, vector_range const & sub_theta)
          {
            assert(sub_theta.size() == bone.active_dofs() || !"set_theta() invalid dimension");
            
            if(bone.type() == hinge_type)
            {
              bone.theta(0) = sub_theta(0);
              bone.relative().Q() = OpenTissue::math::Ru(bone.theta(0), bone.u() );
            }
            else if(bone.type() == slider_type)
            {
              bone.theta(0) = sub_theta(0);
              bone.relative().T() = bone.theta(0) * bone.u();
            }
            else if(bone.type() == ball_type)
            {
              real_type phi   = (bone.theta(0) = sub_theta(0) );
              real_type psi   = (bone.theta(1) = sub_theta(1) );
              real_type theta = (bone.theta(2) = sub_theta(2) );
              
              bone.relative().Q() = OpenTissue::math::Rz( phi )*OpenTissue::math::Ry( psi )*OpenTissue::math::Rz(theta);
            }
            else
            {
              assert(false || !"set_theta(): unknown joint type");
            }
          }
          
          template<typename bone_type, typename vector_range>
          static void get_theta(bone_type const & bone, vector_range & sub_theta)
          {
            assert(sub_theta.size() == bone.active_dofs()         || !"get_theta() invalid dimension");
            
            if(bone.type() == hinge_type)
            {
              sub_theta(0) = bone.theta(0);
            }
            else if(bone.type() == slider_type)
            {
              sub_theta(0) = bone.theta(0);
            }
            else if(bone.type() == ball_type)
            {
              sub_theta(0) = bone.theta(0);
              sub_theta(1) = bone.theta(1);
              sub_theta(2) = bone.theta(2);
            }
            else
            {
              assert(false || !"get_theta(): unknown joint type");
            }
          }
          
          template<typename bone_type, typename vector_range>
          static void get_joint_limits(bone_type const & bone, vector_range & sub_min,vector_range & sub_max)
          {
            assert(sub_min.size() == bone.active_dofs()         || !"get_joint_limits() invalid dimension");
            assert(sub_max.size() == bone.active_dofs()         || !"get_joint_limits() invalid dimension");
            
            if(bone.type() == hinge_type)
            {
              sub_min(0) = bone.min_joint_limit(0);
              sub_max(0) = bone.max_joint_limit(0);
            }
            else if(bone.type() == slider_type)
            {
              sub_min(0) = bone.min_joint_limit(0);
              sub_max(0) = bone.max_joint_limit(0);
            }
            else if(bone.type() == ball_type)
            {
              sub_min(0) = bone.min_joint_limit(0);
              sub_min(1) = bone.min_joint_limit(1);
              sub_min(2) = bone.min_joint_limit(2);
              
              sub_max(0) = bone.max_joint_limit(0);
              sub_max(1) = bone.max_joint_limit(1);
              sub_max(2) = bone.max_joint_limit(2);
            }
            else
            {
              assert(false || !"get_joint_limits(): unknown joint type");
            }
          }
          
          template<typename bone_type, typename vector_range>
          static void joint_limit_projection( bone_type const & bone, vector_range & sub_theta)
          {
            using std::max;
            using std::min;
            
            assert(sub_theta.size() == bone.active_dofs() || !"joint_limit_projection() invalid dimension");
            assert(sub_theta.size() == bone.active_dofs() || !"joint_limit_projection() invalid dimension");
            
            if(bone.type() == hinge_type || bone.type() == slider_type)
            {
              sub_theta(0) = max( bone.min_joint_limit(0), min(bone.max_joint_limit(0), sub_theta(0) ) );
            }
            else if(bone.type() == ball_type)
            {
              sub_theta(0) = max( bone.min_joint_limit(0), min(bone.max_joint_limit(0), sub_theta(0)) );
              sub_theta(1) = max( bone.min_joint_limit(1), min(bone.max_joint_limit(1), sub_theta(1)) );
              sub_theta(2) = max( bone.min_joint_limit(2), min(bone.max_joint_limit(2), sub_theta(2)) );
            }
            else
            {
              assert(false || !"joint_limit_projection(): unknown joint type");
            }
          }
          
          
        };
      
      
      
      // 2009-03-26 kenny: I am not sure the name of the function reflects what it is doing?
      // 2009-03-26 kenny: The documentation is not in alignment with the implementation. In the implementation you use the relative transform, not the absolute transform. Thus, there should be no need for calling compute_poses().
      // 2009-03-26 kenny: The documentation is not in alignment with the implementation. I am not sure I understand the use-pattern, if one has performed forward kinematics, exactly what do you mean?
      // 2009-03-26 kenny: Where is the unit-test for this free template function?
      template<typename bone_type>
      inline void synchronize_bone(bone_type & bone)
      {
        using std::atan2;
        
        typedef typename bone_type::bone_traits             bone_traits;
        typedef typename bone_traits::transform_type        transform_type;
        typedef typename bone_traits::real_type             real_type;
        typedef typename bone_traits::value_traits          value_traits;
        typedef typename bone_traits::vector3_type          vector3_type;
        typedef typename transform_type::quaternion_type    quaternion_type;
        
        transform_type const & T = bone.relative();
        
        if(bone.type() == bone_traits::hinge_type)
        {
          quaternion_type Q = T.Q();
          vector3_type u = unit( Q.v() );
          // we know
          //
          // Q = [cos(theta/2), sin(theta/2) u]
          //
          real_type const ct2    = Q.s();           //---   cos(theta/2)
          real_type const st2    = length( Q.v() ); //---  |sin(theta/2)|
          real_type const theta  = value_traits::two()* atan2(st2,ct2);
          // of course it might have been -|sin(theta/2)| that were the correct solution?
          bone.u()      = u;
          bone.theta(0) = theta;
        }
        else if(bone.type() == bone_traits::slider_type)
        {
          vector3_type const u     = unit( T.T() );
          real_type    const theta = length( T.T() );
          bone.u()      = u;
          bone.theta(0) = theta;
        }
        else if(bone.type() == bone_traits::ball_type)
        {
          real_type phi     = value_traits::zero();
          real_type psi     = value_traits::zero();
          real_type theta   = value_traits::zero();
        
          quaternion_type Q = T.Q();
        
          OpenTissue::math::ZYZ_euler_angles(Q, phi, psi, theta);
      
          // Now we can store the correct joint parameter values into the bone
          bone.theta(0) = phi;
          bone.theta(1) = psi;
          bone.theta(2) = theta;
        }
      }
      
      
    } // namespace inverse
  } // namespace kinematics
} // namespace OpenTissue

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_BONE_TRAITS_H
#endif