chain.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 __CHAIN_H__
#define __CHAIN_H__

#include <dims_common.h>
#include <fMatrix3.h>
#include <fEulerPara.h>
#ifndef SEGA
#include <SceneGraph.h>
#endif
#include <list>
#include <cstring>
#include <cstdio>
#include "hrpModelExportDef.h"

#define DEFAULT_MIN_TIMESTEP 1e-6
#define DEFAULT_MAX_INTEG_ERROR 1e-4

char* CharName(const char* _name);

enum JointType {
        JUNKNOWN = 0,
        JFIXED,   
        JROTATE,  
        JSLIDE,   
        JSPHERE,  
        JFREE,    
};

enum AxisIndex {
        AXIS_NULL = -1,
        AXIS_X,
        AXIS_Y,
        AXIS_Z,
};

class Joint;

struct JointData
{
        JointData() {
                name = NULL;
                parent_name = NULL;
                j_type = JUNKNOWN;
                rel_pos.zero();
                rel_att.identity();
                axis_index = AXIS_NULL;
                mass = 0.0;
                inertia.zero();
                com.zero();
        }
        
        JointData(const char* _name, const char* _parent_name, JointType _j_type,
                          const fVec3& _rpos, const fMat33& _ratt, AxisIndex _axis_index,
                          double _mass, const fMat33& _inertia, const fVec3& _com,
                          int _t_given = true) {
                name = NULL;
                parent_name = NULL;
                if(_name)
                {
                        name = new char [strlen(_name) + 1];
                        strcpy(name, _name);
                }
                if(_parent_name)
                {
                        parent_name = new char [strlen(_parent_name) + 1];
                        strcpy(parent_name, _parent_name);
                }
                j_type = _j_type;
                rel_pos.set(_rpos);
                rel_att.set(_ratt);
                axis_index = _axis_index;
                mass = _mass;
                inertia.set(_inertia);
                com.set(_com);
                t_given = _t_given;
        }
        JointData(const JointData& ref) {
                name = NULL;
                parent_name = NULL;
                if(ref.name)
                {
                        name = new char [strlen(ref.name)];
                        strcpy(name, ref.name);
                }
                if(ref.parent_name)
                {
                        parent_name = new char [strlen(ref.parent_name) + 1];
                        strcpy(parent_name, ref.parent_name);
                }
                j_type = ref.j_type;
                rel_pos.set(ref.rel_pos);
                rel_att.set(ref.rel_att);
                axis_index = ref.axis_index;
                mass = ref.mass;
                inertia.set(ref.inertia);
                com.set(ref.com);
                t_given = ref.t_given;
        }
        
        ~JointData() {
                if(name) delete[] name;
                if(parent_name) delete[] parent_name;
        }

        char* name;         
        char* parent_name;  
        JointType j_type;   
        fVec3 rel_pos;      
        fMat33 rel_att;     
        AxisIndex axis_index; 
        double mass;        
        fMat33 inertia;     
        fVec3 com;          
        int t_given;        
};

class HRPBASE_EXPORT Chain
{
        friend class Joint;
public:
        Chain();
        virtual ~Chain();

        Joint* Root() {
                return root;
        }


        Joint* FindJoint(const char* jname, const char* charname = 0);

        Joint* FindJoint(int _id);

        Joint* FindCharacterRoot(const char* charname);


        int BeginCreateChain(int append = false);


        Joint* AddRoot(const char* name = 0, const fVec3& grav = fVec3(0.0, 0.0, 9.8));


        int AddRoot(Joint* r);


        int Load(const char* fname, const char* charname = 0);

        int LoadXML(const char* fname, const char* charname = 0);


        int AddJoint(Joint* target, Joint* p);

        int AddJoint(Joint* target, const char* parent_name, const char* charname = 0);


        Joint* AddJoint(JointData* joint_data, const char* charname = 0);


        int CreateSerial(int num_joint, const JointData& joint_data, const char* charname = 0, Joint* parent_joint = 0);

        int CreateParallel(int num_char, const char* prmname, const char* charname,
                                           const fVec3& init_pos = 0.0, const fMat33& init_att = 1.0, 
                                           const fVec3& pos_offset = 0.0, const fMat33& att_offset = 1.0,
                                           int init_num = 0);

        int RemoveJoint(Joint* j);


#ifdef SEGA
        int EndCreateChain();
#else
        int EndCreateChain(SceneGraph* sg = NULL);
#endif


        int GetJointNameList(char**& jnames);


        int GetJointList(Joint**& joints);


        int SetJointValue(const fVec& values);


        int SetJointVel(const fVec& vels);
        int SetJointAcc(const fVec& accs);


        int SetJointForce(const fVec& forces);

        int ClearJointForce();

        int ClearExtForce();
        
        int GetJointValue(fVec& values);
        int GetJointVel(fVec& vels);
        int GetJointAcc(fVec& accs);

        int GetJointForce(fVec& forces);

        void CalcPosition();
        void CalcVelocity();
        void CalcAcceleration();


        void InvDyn(fVec& tau);

        virtual void Clear();

        int NumDOF() {
                return n_dof;
        }
        int NumJoint() {
                return n_joint;
        }
        int NumValue() {
                return n_value;
        }


        double TotalCOM(fVec3& com, const char* chname = 0);


        double ComJacobian(fMat& J, fVec3& com, const char* chname = 0);

        int Integrate(double timestep);


        int IntegrateAdaptive(double& timestep, int step, double min_timestep = DEFAULT_MIN_TIMESTEP, double max_integ_error = DEFAULT_MAX_INTEG_ERROR);


        int IntegrateRK4(double timestep, int step);


        int IntegrateValue(double timestep);
        int IntegrateVelocity(double timestep);
        int IntegrateRK4Value(double timestep, int step);
        int IntegrateRK4Velocity(double timestep, int step);

        int Save(const char* fname, const char* charname = 0) const;
        int Save(ostream& ost, const char* charname = 0) const;

        int SaveXML(const char* fname, const char* charname = 0) const;
        int SaveXML(ostream& ost, const char* charname = 0) const;

        int SaveStatus(fVec& value, fVec& vel, fVec& acc);

        int SetStatus(const fVec& value, const fVec& vel, const fVec& acc);

        int Connect(Joint* virtual_joint, Joint* parent_joint);
        int Disconnect(Joint* j);

        int SetTorqueGiven(Joint* _joint, int _tg);
        int SetAllTorqueGiven(int _tg);
        int SetCharacterTorqueGiven(const char* charname, int _tg);

        // compute relative position/orientation from absolute values
        // make sure to call CalcPosition() first to update parent
        int set_abs_position_orientation(Joint* jnt, const fVec3& abs_pos, const fMat33& abs_att);
protected:
#ifdef SEGA
        virtual int init();
#else
        virtual int init(SceneGraph* sg);
        void set_relative_positions(SceneGraph* sg);
        void calc_abs_positions(Joint* cur, SceneGraph* sg);
        void calc_rel_positions(Joint* cur, SceneGraph* sg);
#endif

        virtual int clear_data();


        double** all_value;
        double** all_value_dot;
        double** all_vel;
        double** all_vel_dot;
        double* j_value_dot[4];
        double* j_acc_p[4];
        double* init_value;
        double* init_vel;

        Joint* root;
        int n_value;
        int n_dof;
        int n_joint;
        int n_thrust;  

        int in_create_chain;

        int do_connect;

        void set_all_torque_given(Joint* cur, int _tg);

#ifndef SEGA

public:
        struct scale_object
        {
                scale_object(): joint_name("") {
                        scale = 1.0;
                }
                scale_object(const scale_object& ref): joint_name(ref.joint_name) {
                        scale = ref.scale;
                }
                ~scale_object() {
                }
                
                void set_joint_name(const char* _joint_name, const char* _char_name) {
                        joint_name = "";
                        if(_joint_name)
                        {
                                joint_name = std::string(_joint_name);
                                if(_char_name)
                                {
                                        std::string sep(1, charname_separator);
                                        joint_name.append(sep);
                                        joint_name.append(std::string(_char_name));
                                }
                        }
                }
                
                double scale;
                std::string joint_name;
        };

        void add_scale_object(const scale_object& _s);
        void clear_scale_object_list();

        void ApplyGeomScale(SceneGraph* sg);
protected:
        std::list<scale_object> scale_object_list;

        void init_scale(SceneGraph* sg);
        virtual void apply_scale();
        void apply_scale(const scale_object& _s);
        void reset_scale();
        void init_scale_sub(Node* node);

        void apply_scale_top(Joint* top, double scale);
        void apply_scale_sub(Joint* cur, double scale);
        void reset_scale_sub(Joint* jnt);

        void _apply_scale(Joint* jnt, double scale);
        void apply_geom_scale(SceneGraph* sg, Joint* cur);
#endif
};

class HRPBASE_EXPORT Joint
{
        friend class Chain;
public:
        Joint();
        Joint(const char* name, JointType jt = JUNKNOWN, 
                  const fVec3& rpos = 0.0, const fMat33& ratt = 1.0,
                  AxisIndex _axis_index = AXIS_NULL,
                  int t_given = true);
        Joint(JointData* jdata, const char* charname);
        ~Joint();

        void SetJointData(JointData* jdata, const char* charname);
        void UpdateJointType();

        int SetJointValue(double _q);
        int SetJointVel(double _qd);
        int SetJointAcc(double _qdd);
        int GetJointValue(double& _q);
        int GetJointVel(double& _qd);
        int GetJointAcc(double& _qdd);
        int SetJointForce(double _tau);
        int GetJointForce(double& _tau);
        int SetJointValue(const fMat33& r);
        int SetJointValue(const fEulerPara& ep);
        int SetJointVel(const fVec3& rd);
        int SetJointAcc(const fVec3& rdd);
        int SetJointAcc(double ax, double ay, double az);
        int GetJointValue(fMat33& r);
        int GetJointVel(fVec3& rd);
        int GetJointAcc(fVec3& rdd);
        int SetJointForce(const fVec3& _n3);
        int GetJointForce(fVec3& _n3);
        int SetJointValue(const fVec3& p, const fMat33& r);
        int SetJointValue(const fVec3& p, const fEulerPara& ep);
        int SetJointVel(const fVec3& pd, const fVec3& rd);
        int SetJointAcc(const fVec3& pdd, const fVec3& rdd);
        int SetJointAcc(double lx, double ly, double lz, double ax, double ay, double az);
        int GetJointValue(fVec3& p, fMat33& r);
        int GetJointVel(fVec3& pd, fVec3& rd);
        int GetJointAcc(fVec3& pdd, fVec3& rdd);
        int SetJointForce(const fVec3& _f3, const fVec3& _n3);
        int GetJointForce(fVec3& _f3, fVec3& _n3);

        int SetJointPosition(const fVec3& p);
        int SetJointOrientation(const fMat33& r);

        void SetRotateJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai);
        void SetSlideJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai);
        void SetSphereJointType(const fVec3& rpos, const fMat33& ratt);
        void SetFixedJointType(const fVec3& rpos, const fMat33& ratt);
        void SetFreeJointType(const fVec3& rpos, const fMat33& ratt);

        int CalcJacobian(fMat& J);

        int CalcJacobian2(fMat* J2);

        int CalcJdot(fVec& jdot);

        int DescendantDOF();

        int DescendantNumJoints();

        int isDescendant(Joint* target);

        int isAscendant(Joint* target);

        double TotalMass();

        double Scale() {
                return cur_scale;
        }

        char* CharName() const {
                char* ret = strrchr(name, charname_separator);
                if(ret) return ret+1;
                return 0;
        }

        void SetName(const char* _name, const char* _charname = 0) {
                if(name) delete[] name;
                name = 0;
                if(basename) delete[] basename;
                basename = 0;
                if(_name)
                {
                        if(_charname)
                        {
                                name = new char [strlen(_name) + strlen(_charname) + 2];
                                sprintf(name, "%s%c%s", _name, charname_separator, _charname);
                        }
                        else
                        {
                                name = new char [strlen(_name) + 1];
                                strcpy(name, _name);
                        }
                        char* ch = strrchr(name, charname_separator);
                        if(ch) *ch = '\0';
                        basename = new char [strlen(name) + 1];
                        strcpy(basename, name);
                        if(ch) *ch = charname_separator;
                }
        }

        Joint* parent;    
        Joint* brother;   
        Joint* child;     
        
        Joint* real;      
        fVec3 rpos_real;  
        fMat33 ratt_real; 

        char* name;        
        char* basename;    
        char* realname;    
        JointType j_type;  

        fVec3 axis;        
        fVec3 init_pos;    
        fMat33 init_att;   

        fVec3 rel_pos;     
        fMat33 rel_att;    
        fEulerPara rel_ep; 

        double mass;       
        fMat33 inertia;    
        fVec3 loc_com;     
        double gear_ratio;
        double rotor_inertia;
        int t_given;       

        int n_dof;         
        int n_thrust;      
        int n_root_dof;    

        int i_value;       
        int i_dof;         
        int i_thrust;      
        int i_joint;       

        double q;       
        double qd;      
        double qdd;     

        fVec3 rel_lin_vel;
        fVec3 rel_ang_vel;
        fVec3 rel_lin_acc;
        fVec3 rel_ang_acc;

        double tau;   
        fVec3 tau_f;  
        fVec3 tau_n;  
        fVec3 ext_force;  
        fVec3 ext_moment; 

        fVec3 abs_pos;      
        fMat33 abs_att;     
        fVec3 loc_lin_vel;  
        fVec3 loc_ang_vel;  
        fVec3 loc_com_vel;  
        fVec3 loc_lin_acc;  
        fVec3 loc_ang_acc;  
        fVec3 loc_com_acc;  

        fVec3 joint_f, total_f;
        fVec3 joint_n, total_n;
        fVec3 p_lin_vel;
        fEulerPara p_ep_dot;
        fVec3 p_ang_vel;
        fVec3 p_lin_acc;
        fVec3 p_ang_acc;
protected:
        Chain* chain;

        double total_mass();

        void init();
        void init_arrays();
        void init_virtual();
        void _init_virtual();

        void clear_data();

        int pre_integrate();
        int post_integrate();

        void clear();

        int set_joint_value(const fVec& values);
        int set_joint_vel(const fVec& vels);
        int set_joint_acc(const fVec& accs);
        int set_joint_force(const fVec& forces);

        int get_joint_value(fVec& values);
        int get_joint_vel(fVec& vels);
        int get_joint_acc(fVec& accs);
        int get_joint_force(fVec& forces);

        void calc_position();
        void calc_velocity();
        void calc_acceleration();

        void inv_dyn();
        void inv_dyn_1();
        void inv_dyn_2();
        void calc_joint_force(fVec& tau);

        void get_joint_name_list(char** jnames);
        void get_joint_list(Joint** joints);

        void add_child(Joint* j);
        int remove_child(Joint* j);
        Joint* find_joint(const char* n, const char* charname);
        Joint* find_joint(int _id);

        int calc_jacobian(fMat& J, Joint* target);
        int calc_jacobian_rotate(fMat& J, Joint* target);
        int calc_jacobian_slide(fMat& J, Joint* target);
        int calc_jacobian_sphere(fMat& J, Joint* target);
        int calc_jacobian_free(fMat& J, Joint* target);

        int calc_jacobian_2(fMat* J, Joint* target);
        int calc_jacobian_2_rotate_sub(fMat* J, Joint* target, Joint* j1);
        int calc_jacobian_2_slide_sub(fMat* J, Joint* target, Joint* j1);
        int calc_jacobian_2_sphere_sub(fMat* J, Joint* target, Joint* j1);
        int calc_jacobian_2_free_sub(fMat* J, Joint* target, Joint* j1);

        int calc_jacobian_2_rotate_rotate(fMat* J, Joint* target, Joint* j1, const fVec3& axis1, int index1, const fVec3& axis2, int index2);
        int calc_jacobian_2_slide_rotate(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index);
        int calc_jacobian_2_rotate_slide(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index);


        int descendant_dof();
        int descendant_num_joints();
        int is_descendant(Joint* cur, Joint* target);
        
        int save(ostream& ost, int indent, const char* charname) const;
        int save_xml(ostream& ost, int indent, const char* charname) const;

        int clear_joint_force();
        int clear_ext_force();

        double total_com(fVec3& com, const char* chname);
        double com_jacobian(fMat& J, fVec3& com, const char* chname);

private:
        double cur_scale;
};

#endif