ik.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * The University of Tokyo
 */
#ifndef __IK_H__
#define __IK_H__

#include <chain.h>

class IKConstraint;
class IKHandle;

//#define MEASURE_TIME

//#define MAX_CONSTRAINT 128
#define MIN_JOINT_WEIGHT 0.01

#define MAX_CONDITION_NUMBER 100.0

static const char joint_name_separator = '_';

class IK
        : virtual public Chain
{
        friend class IKConstraint;
        friend class IKHandle;
        friend class IKDesire;
public:
        enum ConstType {
                HANDLE_CONSTRAINT,  
                DESIRE_CONSTRAINT,  
                COM_CONSTRAINT,     
                SCALAR_JOINT_LIMIT_CONSTRAINT,  
                IK_NUM_CONSTRAINT_TYPES,
        };
        enum Priority {
                HIGH_PRIORITY = 0,  
                HIGH_IF_POSSIBLE,   
                LOW_PRIORITY,       
                N_PRIORITY_TYPES,
        };
        enum ConstIndex {
                HAVE_CONSTRAINT,    
                NO_CONSTRAINT,      
        };
        
        IK();
        ~IK();

        int NumConstraints() {
                return n_constraints;
        }
        int NumConstraints(ConstType t);

        int LoadMarkerXML(const char* fname, const char* _charname);

        int AddConstraint(IKConstraint* _constraint);
        int RemoveConstraint(int _id);
        int RemoveAllConstraints();

        double Update(double timestep);

        double Update(double max_timestep, double min_timestep, double max_integ_error = DEFAULT_MAX_INTEG_ERROR);

        IKConstraint* FindConstraint(ConstType _type, const char* jname, const char* charname = 0);

        IKConstraint* FindConstraint(int _id);

        int ConstraintIndex(ConstType _type, const char* jname);
        int ConstraintIndex(int _id);

        int ConstraintID(ConstType _type, const char* jname);
        int ConstraintID(int _index);

        int ResetAllConstraints();

        int ResetConstraints(ConstType t);

        int SetJointWeight(const char* jname, double _weight);
        int SetJointWeight(const char* jname, const fVec& _weight);

        int EnableDesire(const char* jname);
        int DisableDesire(const char* jname);
        int SetDesireGain(const char* jname, double _gain);

        void SetMaxConditionNumber(double cn) {
                max_condnum = cn;
        }
        double GetMaxConditionNumber() {
                return max_condnum;
        }

        void SetConstraintScale(double _scale, const char* charname = 0);
        void SetCharacterScale(double _scale, const char* charname = 0);


        IKHandle* AddMarker(const std::string& label, const std::string& linkname, const std::string& charname, const fVec3& rel_pos);
#ifndef SEGA
        IKHandle* AddMarker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos = 0.0);
        int SaveMarkers(const char* fname);
        int EditMarker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos = 0.0);
#endif
protected:
#ifdef SEGA
        int init();
        int myinit();
#else
        int init(SceneGraph* sg);
        int myinit(SceneGraph* sg);
        int load_markers(SceneGraph* sg, Joint* rj);
        int load_markers(TransformNode* marker_top);
        IKHandle* add_marker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos);
        IKHandle* add_marker(TransformNode* tn);
        int edit_marker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos);
        int save_marker(TransformNode* top_tnode, IKHandle* h);
#endif

        int calc_jacobian();

        int calc_feedback();
        
        double solve_ik();

        int copy_jacobian();

        void set_character_scale(Joint* jnt, double _scale, const char* charname);

        int n_constraints;

        int n_total_const;

        IKConstraint** constraints;

        int n_assigned_constraints;
        int assign_constraints(int _n);

        fMat J[N_PRIORITY_TYPES];  
        fVec fb[N_PRIORITY_TYPES]; 
        fVec weight[N_PRIORITY_TYPES]; 
        int n_const[N_PRIORITY_TYPES]; 
        int n_all_const;

        double max_condnum;

        fVec joint_weights;

};

class IKConstraint
{
        friend class IK;
public:

        IKConstraint(IK* _ik, const char* _jname, Joint* _jnt, IK::Priority _pri, double _gain) {
                ik = _ik;
                joint_name = 0;
                enabled = true;
                active = true;
                if(_jname)
                {
                        joint_name = new char [strlen(_jname) + 1];
                        strcpy(joint_name, _jname);
                }
                joint = _jnt;
                n_const = 0;
                priority = _pri;
                gain = _gain;
                i_const = -1;
                id = -1;
                is_dropped = false;
        }
        
        virtual ~IKConstraint() {
                if(joint_name) delete[] joint_name;
        }

        virtual IK::ConstType GetType() = 0;

        virtual int Reset() {
                return 0;
        }

        void Enable() {
                enabled = true;
        }
        void Disable() {
                enabled = false;
        }

        void Activate() {
                active = true;
        }
        void Diactivate() {
                active = false;
        }
        int Active() {
                return active;
        }

        void SetGain(double _gain) {
                gain = _gain;
        }
        double GetGain() {
                return gain;
        }

        void SetPriority(IK::Priority _pri) {
                priority = _pri;
        }
        IK::Priority GetPriority() {
                return priority;
        }

        int iConst() {
                return i_const;
        }
        int nConst() {
                return n_const;
        }

        Joint* GetJoint() {
                return joint;
        }
        int ID() {
                return id;
        }
        int Dropped() {
                if(priority == IK::HIGH_IF_POSSIBLE) return is_dropped;
                return false;
        }

        virtual void SetCharacterScale(double _scale, const char* charname = 0) {
        }

protected:

        virtual int calc_jacobian();
        virtual int calc_jacobian_rotate(Joint* cur) {
                return 0;
        }
        virtual int calc_jacobian_slide(Joint* cur) {
                return 0;
        }
        virtual int calc_jacobian_sphere(Joint* cur) {
                return 0;
        }
        virtual int calc_jacobian_free(Joint* cur) {
                return 0;
        }

        int calc_jacobian(Joint* cur);
        int copy_jacobian();

        virtual int calc_feedback() = 0;

        IK* ik;
        Joint* joint;          
        char* joint_name;      
        int id;                

        IK::Priority priority; 
        double gain;           
        fVec weight;           
        int n_const;           
        int enabled;
        int active;
        
        fMat J;                
        fVec fb;               
        int i_const;           

        int is_dropped;

private:
        int count_constraints();
};

class IKHandle
        : public IKConstraint
{
        friend class IK;
public:
        IKHandle(IK* _ik, const char* _jname, Joint* _jnt,
                         IK::ConstIndex cindex[6], IK::Priority _pri, double _gain,
                         const fVec3& _rel_pos = 0.0, const fMat33& _rel_att = 1.0,
                         Joint* _other_joint = 0)
                        : IKConstraint(_ik, _jname, _jnt, _pri, _gain),
                          rel_pos(_rel_pos), rel_att(_rel_att) {
                for(int i=0; i<6; i++)
                {
                        const_index[i] = cindex[i];
                        if(const_index[i] == IK::HAVE_CONSTRAINT)
                                n_const++;
                }
                other_joint = _other_joint;
                // compute the current absolute position/orientation
                abs_pos.zero();
                abs_att.identity();
                if(joint)
                {
                        abs_att.mul(joint->abs_att, rel_att);
                        abs_pos.mul(joint->abs_att, rel_pos);
                        abs_pos += joint->abs_pos;
                        // relative position/orientation
                        if(other_joint)
                        {
                                static fVec3 pp;
                                static fMat33 rt, att;
                                rt.tran(other_joint->abs_att);
                                att.set(abs_att);
                                pp.sub(abs_pos, other_joint->abs_pos);
                                abs_pos.mul(rt, pp);
                                abs_att.mul(rt, att);
                        }
                }
                J.resize(n_const, ik->NumDOF());
                J.zero();
                fb.resize(n_const);
                fb.zero();
                weight.resize(n_const);
                weight = 1.0;
        }
        
        ~IKHandle() {
        }

        int Reset() {
                if(joint)
                {
                        abs_att.mul(joint->abs_att, rel_att);
                        abs_pos.mul(joint->abs_att, rel_pos);
                        abs_pos += joint->abs_pos;
                        // relative position/orientation
                        if(other_joint)
                        {
                                static fVec3 pp;
                                static fMat33 rt, att;
                                rt.tran(other_joint->abs_att);
                                att.set(abs_att);
                                pp.sub(abs_pos, other_joint->abs_pos);
                                abs_pos.mul(rt, pp);
                                abs_att.mul(rt, att);
                        }
                }
                return 0;
        }
        void SetPos(const fVec3& _abs_pos) {
                abs_pos.set(_abs_pos);
        }
        void SetAtt(const fMat33& _abs_att) {
                abs_att.set(_abs_att);
        }
        
        void GetPos(fVec3& _abs_pos) {
                _abs_pos.set(abs_pos);
        }
        void GetAtt(fMat33& _abs_att) {
                _abs_att.set(abs_att);
        }

        void SetRelPos(const fVec3& _rel_pos) {
                rel_pos.set(_rel_pos);
                Reset();
        }

        IK::ConstType GetType() {
                return IK::HANDLE_CONSTRAINT;
        }

        void GetConstIndex(IK::ConstIndex _cindex[]) {
                for(int i=0; i<6; i++)
                {
                        _cindex[i] = const_index[i];
                }
        }

        void SetCharacterScale(double _scale, const char* charname = 0);

protected:
        int calc_jacobian();

        int calc_feedback();

        fVec3 abs_pos;  
        fMat33 abs_att; 

        IK::ConstIndex const_index[6]; 
        fVec3 rel_pos;
        fMat33 rel_att;

        Joint* other_joint;
};

class IKDesire
        : public IKConstraint
{
        friend class IK;
public:
        IKDesire(IK* _ik, const char* _jname, Joint* _jnt,
                         IK::Priority _pri, double _gain)
                        : IKConstraint(_ik, _jname, _jnt, _pri, _gain) {
                q_des = 0.0;
                att_des.identity();
                pos_des.zero();
                if(joint && joint->n_dof > 0)
                {
                        n_const = joint->n_dof;
                        J.resize(joint->n_dof, ik->NumDOF());
                        J.zero();
                        fb.resize(joint->n_dof);
                        fb.zero();
                        weight.resize(n_const);
                        weight = 1.0;
                        switch(joint->j_type)
                        {
                        case JROTATE:
                        case JSLIDE:
                                joint->GetJointValue(q_des);
                                break;
                        case JSPHERE:
                                joint->GetJointValue(att_des);
                                break;
                        case JFREE:
                                joint->GetJointValue(pos_des, att_des);
                                break;
                        default:
                                break;
                        }
                }
        }
        
        ~IKDesire() {
        }

        int Reset() {
                if(joint && joint->n_dof > 0)
                {
                        switch(joint->j_type)
                        {
                        case JROTATE:
                        case JSLIDE:
                                joint->GetJointValue(q_des);
                                break;
                        case JSPHERE:
                                joint->GetJointValue(att_des);
                                break;
                        case JFREE:
                                joint->GetJointValue(pos_des, att_des);
                                break;
                        default:
                                break;
                        }
                }
                return 0;
        }

        IK::ConstType GetType() {
                return IK::DESIRE_CONSTRAINT;
        }

        void SetDesire(double _q) {
                q_des = _q;
        }
        void SetDesire(const fMat33& _att) {
                att_des.set(_att);
        }
        void SetDesire(const fVec3& _pos, const fMat33& _att) {
                pos_des.set(_pos);
                att_des.set(_att);
        }
        void SetDesire(const fVec3& _pos) {
                pos_des.set(_pos);
        }

        void SetCharacterScale(double _scale, const char* charname = 0);
protected:
        int calc_jacobian_rotate(Joint* cur);
        int calc_jacobian_slide(Joint* cur);
        int calc_jacobian_sphere(Joint* cur);
        int calc_jacobian_free(Joint* cur);

        int calc_feedback();

        double q_des;    
        fMat33 att_des;  
        fVec3 pos_des;   
};

class IKCom
        : public IKConstraint
{
public:
        IKCom(IK* _ik, const char* _charname, IK::ConstIndex cindex[3],
                         IK::Priority _pri, double _gain)
                        : IKConstraint(_ik, 0, 0, _pri, _gain) {
                charname = 0;
                if(_charname)
                {
                        charname = new char [strlen(_charname)+1];
                        strcpy(charname, _charname);
                }
                n_const = 0;
                for(int i=0; i<3; i++)
                {
                        const_index[i] = cindex[i];
                        if(const_index[i] == IK::HAVE_CONSTRAINT)
                                n_const++;
                }
                fb.resize(n_const);
                des_com.zero();
                cur_com.zero();
                if(_ik)
                {
                        _ik->CalcPosition();
                        _ik->TotalCOM(cur_com, _charname);
                        des_com.set(cur_com);
                }
        }
        ~IKCom() {
                if(charname) delete[] charname;
        }

        IK::ConstType GetType() {
                return IK::COM_CONSTRAINT;
        }
        int Reset() {
                ik->TotalCOM(des_com, charname);
                return 0;
        }
        void SetPos(const fVec3& p) {
                des_com.set(p);
        }

protected:
        int calc_jacobian();
        int calc_feedback();

        IK::ConstIndex const_index[3];  
        char* charname;  
        fVec3 des_com;   
        fVec3 cur_com;   
};

class IKScalarJointLimit
        : public IKConstraint
{
        friend class IK;
public:
        IKScalarJointLimit(IK* _ik, const char* _jname, Joint* _jnt,
                                           IK::Priority _pri, double _gain)
                        : IKConstraint(_ik, _jname, _jnt, _pri, _gain) {
                min_limit = false;
                max_limit = false;
                q_min = 0.0;
                q_max = 0.0;
                if(joint && (joint->j_type == JROTATE || joint->j_type == JSLIDE))
                {
                        n_const = 1;
                        J.resize(1, ik->NumDOF());
                        J.zero();
                        fb.resize(1);
                        fb.zero();
                        weight.resize(1);
                        weight = 1.0;
                }
                else
                {
                        enabled = false;
                }
        }
        
        ~IKScalarJointLimit() {
        }

        IK::ConstType GetType() {
                return IK::SCALAR_JOINT_LIMIT_CONSTRAINT;
        }

        void SetMax(double _q_max) {
                q_max = _q_max;
                max_limit = true;
        }
        void SetMin(double _q_min) {
                q_min = _q_min;
                min_limit = true;
        }
        double GetMax() {
                return q_max;
        }
        double GetMin() {
                return q_min;
        }

        void SetCharacterScale(double _scale, const char* charname = 0);
protected:
        int calc_jacobian_rotate(Joint* cur);
        int calc_jacobian_slide(Joint* cur);

        int calc_feedback();

        int min_limit, max_limit;
        double q_min, q_max;
};

#endif