chain.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #ifndef __CHAIN_H__
00017 #define __CHAIN_H__
00018 
00019 #include <dims_common.h>
00020 #include <fMatrix3.h>
00021 #include <fEulerPara.h>
00022 #ifndef SEGA
00023 #include <SceneGraph.h>
00024 #endif
00025 #include <list>
00026 #include <cstring>
00027 #include <cstdio>
00028 #include "hrpModelExportDef.h"
00029 
00031 #define DEFAULT_MIN_TIMESTEP 1e-6
00032 #define DEFAULT_MAX_INTEG_ERROR 1e-4
00033 
00035 char* CharName(const char* _name);
00036 
00038 enum JointType {
00039         JUNKNOWN = 0,
00040         JFIXED,   
00041         JROTATE,  
00042         JSLIDE,   
00043         JSPHERE,  
00044         JFREE,    
00045 };
00046 
00048 enum AxisIndex {
00049         AXIS_NULL = -1,
00050         AXIS_X,
00051         AXIS_Y,
00052         AXIS_Z,
00053 };
00054 
00055 class Joint;
00056 
00061 struct JointData
00062 {
00063         JointData() {
00064                 name = NULL;
00065                 parent_name = NULL;
00066                 j_type = JUNKNOWN;
00067                 rel_pos.zero();
00068                 rel_att.identity();
00069                 axis_index = AXIS_NULL;
00070                 mass = 0.0;
00071                 inertia.zero();
00072                 com.zero();
00073         }
00074         
00075         JointData(const char* _name, const char* _parent_name, JointType _j_type,
00076                           const fVec3& _rpos, const fMat33& _ratt, AxisIndex _axis_index,
00077                           double _mass, const fMat33& _inertia, const fVec3& _com,
00078                           int _t_given = true) {
00079                 name = NULL;
00080                 parent_name = NULL;
00081                 if(_name)
00082                 {
00083                         name = new char [strlen(_name) + 1];
00084                         strcpy(name, _name);
00085                 }
00086                 if(_parent_name)
00087                 {
00088                         parent_name = new char [strlen(_parent_name) + 1];
00089                         strcpy(parent_name, _parent_name);
00090                 }
00091                 j_type = _j_type;
00092                 rel_pos.set(_rpos);
00093                 rel_att.set(_ratt);
00094                 axis_index = _axis_index;
00095                 mass = _mass;
00096                 inertia.set(_inertia);
00097                 com.set(_com);
00098                 t_given = _t_given;
00099         }
00100         JointData(const JointData& ref) {
00101                 name = NULL;
00102                 parent_name = NULL;
00103                 if(ref.name)
00104                 {
00105                         name = new char [strlen(ref.name)];
00106                         strcpy(name, ref.name);
00107                 }
00108                 if(ref.parent_name)
00109                 {
00110                         parent_name = new char [strlen(ref.parent_name) + 1];
00111                         strcpy(parent_name, ref.parent_name);
00112                 }
00113                 j_type = ref.j_type;
00114                 rel_pos.set(ref.rel_pos);
00115                 rel_att.set(ref.rel_att);
00116                 axis_index = ref.axis_index;
00117                 mass = ref.mass;
00118                 inertia.set(ref.inertia);
00119                 com.set(ref.com);
00120                 t_given = ref.t_given;
00121         }
00122         
00123         ~JointData() {
00124                 if(name) delete[] name;
00125                 if(parent_name) delete[] parent_name;
00126         }
00127 
00128         char* name;         
00129         char* parent_name;  
00130         JointType j_type;   
00131         fVec3 rel_pos;      
00132         fMat33 rel_att;     
00133         AxisIndex axis_index; 
00134         double mass;        
00135         fMat33 inertia;     
00136         fVec3 com;          
00137         int t_given;        
00138 };
00139 
00144 class HRPBASE_EXPORT Chain
00145 {
00146         friend class Joint;
00147 public:
00148         Chain();
00149         virtual ~Chain();
00150 
00151         Joint* Root() {
00152                 return root;
00153         }
00154 
00156 
00164         Joint* FindJoint(const char* jname, const char* charname = 0);
00165 
00167         Joint* FindJoint(int _id);
00168 
00170         Joint* FindCharacterRoot(const char* charname);
00171 
00173 
00178         int BeginCreateChain(int append = false);
00179 
00181 
00187         Joint* AddRoot(const char* name = 0, const fVec3& grav = fVec3(0.0, 0.0, 9.8));
00188 
00190 
00195         int AddRoot(Joint* r);
00196 
00198 
00204         int Load(const char* fname, const char* charname = 0);
00205 
00207         int LoadXML(const char* fname, const char* charname = 0);
00208 
00210 
00216         int AddJoint(Joint* target, Joint* p);
00218 
00225         int AddJoint(Joint* target, const char* parent_name, const char* charname = 0);
00226 
00228 
00234         Joint* AddJoint(JointData* joint_data, const char* charname = 0);
00235 
00237 
00244         int CreateSerial(int num_joint, const JointData& joint_data, const char* charname = 0, Joint* parent_joint = 0);
00246 
00257         int CreateParallel(int num_char, const char* prmname, const char* charname,
00258                                            const fVec3& init_pos = 0.0, const fMat33& init_att = 1.0, 
00259                                            const fVec3& pos_offset = 0.0, const fMat33& att_offset = 1.0,
00260                                            int init_num = 0);
00261 
00263         int RemoveJoint(Joint* j);
00264 
00266 
00270 #ifdef SEGA
00271         int EndCreateChain();
00272 #else
00273         int EndCreateChain(SceneGraph* sg = NULL);
00274 #endif
00275 
00277 
00281         int GetJointNameList(char**& jnames);
00282 
00284 
00288         int GetJointList(Joint**& joints);
00289 
00291 
00296         int SetJointValue(const fVec& values);
00297 
00299 
00303         int SetJointVel(const fVec& vels);
00304         int SetJointAcc(const fVec& accs);
00305 
00307 
00311         int SetJointForce(const fVec& forces);
00312 
00314         int ClearJointForce();
00315 
00317         int ClearExtForce();
00318         
00320         int GetJointValue(fVec& values);
00321         int GetJointVel(fVec& vels);
00322         int GetJointAcc(fVec& accs);
00323 
00325         int GetJointForce(fVec& forces);
00326 
00328         void CalcPosition();
00329         void CalcVelocity();
00330         void CalcAcceleration();
00331 
00333 
00337         void InvDyn(fVec& tau);
00338 
00340         virtual void Clear();
00341 
00343         int NumDOF() {
00344                 return n_dof;
00345         }
00347         int NumJoint() {
00348                 return n_joint;
00349         }
00351         int NumValue() {
00352                 return n_value;
00353         }
00354 
00356 
00362         double TotalCOM(fVec3& com, const char* chname = 0);
00363 
00365 
00372         double ComJacobian(fMat& J, fVec3& com, const char* chname = 0);
00373 
00375         int Integrate(double timestep);
00376 
00378 
00386         int IntegrateAdaptive(double& timestep, int step, double min_timestep = DEFAULT_MIN_TIMESTEP, double max_integ_error = DEFAULT_MAX_INTEG_ERROR);
00387 
00389 
00394         int IntegrateRK4(double timestep, int step);
00395 
00397 
00401         int IntegrateValue(double timestep);
00402         int IntegrateVelocity(double timestep);
00403         int IntegrateRK4Value(double timestep, int step);
00404         int IntegrateRK4Velocity(double timestep, int step);
00405 
00407         int Save(const char* fname, const char* charname = 0) const;
00408         int Save(ostream& ost, const char* charname = 0) const;
00409 
00411         int SaveXML(const char* fname, const char* charname = 0) const;
00412         int SaveXML(ostream& ost, const char* charname = 0) const;
00413 
00415         int SaveStatus(fVec& value, fVec& vel, fVec& acc);
00416 
00418         int SetStatus(const fVec& value, const fVec& vel, const fVec& acc);
00419 
00421         int Connect(Joint* virtual_joint, Joint* parent_joint);
00423         int Disconnect(Joint* j);
00424 
00426         int SetTorqueGiven(Joint* _joint, int _tg);
00428         int SetAllTorqueGiven(int _tg);
00430         int SetCharacterTorqueGiven(const char* charname, int _tg);
00431 
00432         // compute relative position/orientation from absolute values
00433         // make sure to call CalcPosition() first to update parent
00434         int set_abs_position_orientation(Joint* jnt, const fVec3& abs_pos, const fMat33& abs_att);
00435 protected:
00437 #ifdef SEGA
00438         virtual int init();
00439 #else
00440         virtual int init(SceneGraph* sg);
00441         void set_relative_positions(SceneGraph* sg);
00442         void calc_abs_positions(Joint* cur, SceneGraph* sg);
00443         void calc_rel_positions(Joint* cur, SceneGraph* sg);
00444 #endif
00445 
00446         virtual int clear_data();
00447 
00449 
00455         double** all_value;
00456         double** all_value_dot;
00457         double** all_vel;
00458         double** all_vel_dot;
00460         double* j_value_dot[4];
00461         double* j_acc_p[4];
00462         double* init_value;
00463         double* init_vel;
00464 
00466         Joint* root;
00467         int n_value;
00468         int n_dof;
00469         int n_joint;
00470         int n_thrust;  
00471 
00473         int in_create_chain;
00474 
00476         int do_connect;
00477 
00478         void set_all_torque_given(Joint* cur, int _tg);
00479 
00480 #ifndef SEGA
00481 
00482 public:
00483         struct scale_object
00484         {
00485                 scale_object(): joint_name("") {
00486                         scale = 1.0;
00487                 }
00488                 scale_object(const scale_object& ref): joint_name(ref.joint_name) {
00489                         scale = ref.scale;
00490                 }
00491                 ~scale_object() {
00492                 }
00493                 
00494                 void set_joint_name(const char* _joint_name, const char* _char_name) {
00495                         joint_name = "";
00496                         if(_joint_name)
00497                         {
00498                                 joint_name = std::string(_joint_name);
00499                                 if(_char_name)
00500                                 {
00501                                         std::string sep(1, charname_separator);
00502                                         joint_name.append(sep);
00503                                         joint_name.append(std::string(_char_name));
00504                                 }
00505                         }
00506                 }
00507                 
00508                 double scale;
00509                 std::string joint_name;
00510         };
00511 
00512         void add_scale_object(const scale_object& _s);
00513         void clear_scale_object_list();
00514 
00515         void ApplyGeomScale(SceneGraph* sg);
00516 protected:
00517         std::list<scale_object> scale_object_list;
00518 
00519         void init_scale(SceneGraph* sg);
00520         virtual void apply_scale();
00521         void apply_scale(const scale_object& _s);
00522         void reset_scale();
00523         void init_scale_sub(Node* node);
00524 
00525         void apply_scale_top(Joint* top, double scale);
00526         void apply_scale_sub(Joint* cur, double scale);
00527         void reset_scale_sub(Joint* jnt);
00528 
00529         void _apply_scale(Joint* jnt, double scale);
00530         void apply_geom_scale(SceneGraph* sg, Joint* cur);
00531 #endif
00532 };
00533 
00538 class HRPBASE_EXPORT Joint
00539 {
00540         friend class Chain;
00541 public:
00542         Joint();
00543         Joint(const char* name, JointType jt = JUNKNOWN, 
00544                   const fVec3& rpos = 0.0, const fMat33& ratt = 1.0,
00545                   AxisIndex _axis_index = AXIS_NULL,
00546                   int t_given = true);
00547         Joint(JointData* jdata, const char* charname);
00548         ~Joint();
00549 
00550         void SetJointData(JointData* jdata, const char* charname);
00551         void UpdateJointType();
00552 
00558         int SetJointValue(double _q);
00559         int SetJointVel(double _qd);
00560         int SetJointAcc(double _qdd);
00561         int GetJointValue(double& _q);
00562         int GetJointVel(double& _qd);
00563         int GetJointAcc(double& _qdd);
00564         int SetJointForce(double _tau);
00565         int GetJointForce(double& _tau);
00573         int SetJointValue(const fMat33& r);
00574         int SetJointValue(const fEulerPara& ep);
00575         int SetJointVel(const fVec3& rd);
00576         int SetJointAcc(const fVec3& rdd);
00577         int SetJointAcc(double ax, double ay, double az);
00578         int GetJointValue(fMat33& r);
00579         int GetJointVel(fVec3& rd);
00580         int GetJointAcc(fVec3& rdd);
00581         int SetJointForce(const fVec3& _n3);
00582         int GetJointForce(fVec3& _n3);
00590         int SetJointValue(const fVec3& p, const fMat33& r);
00591         int SetJointValue(const fVec3& p, const fEulerPara& ep);
00592         int SetJointVel(const fVec3& pd, const fVec3& rd);
00593         int SetJointAcc(const fVec3& pdd, const fVec3& rdd);
00594         int SetJointAcc(double lx, double ly, double lz, double ax, double ay, double az);
00595         int GetJointValue(fVec3& p, fMat33& r);
00596         int GetJointVel(fVec3& pd, fVec3& rd);
00597         int GetJointAcc(fVec3& pdd, fVec3& rdd);
00598         int SetJointForce(const fVec3& _f3, const fVec3& _n3);
00599         int GetJointForce(fVec3& _f3, fVec3& _n3);
00602 
00603         int SetJointPosition(const fVec3& p);
00605         int SetJointOrientation(const fMat33& r);
00606 
00608         void SetRotateJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai);
00610         void SetSlideJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai);
00612         void SetSphereJointType(const fVec3& rpos, const fMat33& ratt);
00614         void SetFixedJointType(const fVec3& rpos, const fMat33& ratt);
00616         void SetFreeJointType(const fVec3& rpos, const fMat33& ratt);
00617 
00619         int CalcJacobian(fMat& J);
00620 
00622         int CalcJacobian2(fMat* J2);
00623 
00625         int CalcJdot(fVec& jdot);
00626 
00628         int DescendantDOF();
00629 
00631         int DescendantNumJoints();
00632 
00634         int isDescendant(Joint* target);
00635 
00637         int isAscendant(Joint* target);
00638 
00640         double TotalMass();
00641 
00643         double Scale() {
00644                 return cur_scale;
00645         }
00646 
00648         char* CharName() const {
00649                 char* ret = strrchr(name, charname_separator);
00650                 if(ret) return ret+1;
00651                 return 0;
00652         }
00653 
00655         void SetName(const char* _name, const char* _charname = 0) {
00656                 if(name) delete[] name;
00657                 name = 0;
00658                 if(basename) delete[] basename;
00659                 basename = 0;
00660                 if(_name)
00661                 {
00662                         if(_charname)
00663                         {
00664                                 name = new char [strlen(_name) + strlen(_charname) + 2];
00665                                 sprintf(name, "%s%c%s", _name, charname_separator, _charname);
00666                         }
00667                         else
00668                         {
00669                                 name = new char [strlen(_name) + 1];
00670                                 strcpy(name, _name);
00671                         }
00672                         char* ch = strrchr(name, charname_separator);
00673                         if(ch) *ch = '\0';
00674                         basename = new char [strlen(name) + 1];
00675                         strcpy(basename, name);
00676                         if(ch) *ch = charname_separator;
00677                 }
00678         }
00679 
00683         Joint* parent;    
00684         Joint* brother;   
00685         Joint* child;     
00686         
00687         Joint* real;      
00688         fVec3 rpos_real;  
00689         fMat33 ratt_real; 
00690 
00691         char* name;        
00692         char* basename;    
00693         char* realname;    
00694         JointType j_type;  
00695 
00696         fVec3 axis;        
00697         fVec3 init_pos;    
00698         fMat33 init_att;   
00699 
00700         fVec3 rel_pos;     
00701         fMat33 rel_att;    
00702         fEulerPara rel_ep; 
00703 
00704         double mass;       
00705         fMat33 inertia;    
00706         fVec3 loc_com;     
00707         double gear_ratio;
00708         double rotor_inertia;
00709         int t_given;       
00710 
00715         int n_dof;         
00716         int n_thrust;      
00717         int n_root_dof;    
00718 
00719         int i_value;       
00720         int i_dof;         
00721         int i_thrust;      
00722         int i_joint;       
00723 
00724         double q;       
00725         double qd;      
00726         double qdd;     
00727 
00728         fVec3 rel_lin_vel;
00729         fVec3 rel_ang_vel;
00730         fVec3 rel_lin_acc;
00731         fVec3 rel_ang_acc;
00732 
00733         double tau;   
00734         fVec3 tau_f;  
00735         fVec3 tau_n;  
00736         fVec3 ext_force;  
00737         fVec3 ext_moment; 
00738 
00741         fVec3 abs_pos;      
00742         fMat33 abs_att;     
00743         fVec3 loc_lin_vel;  
00744         fVec3 loc_ang_vel;  
00745         fVec3 loc_com_vel;  
00746         fVec3 loc_lin_acc;  
00747         fVec3 loc_ang_acc;  
00748         fVec3 loc_com_acc;  
00749 
00755         fVec3 joint_f, total_f;
00756         fVec3 joint_n, total_n;
00761         fVec3 p_lin_vel;
00762         fEulerPara p_ep_dot;
00763         fVec3 p_ang_vel;
00764         fVec3 p_lin_acc;
00765         fVec3 p_ang_acc;
00768 protected:
00769         Chain* chain;
00770 
00771         double total_mass();
00772 
00773         void init();
00774         void init_arrays();
00775         void init_virtual();
00776         void _init_virtual();
00777 
00778         void clear_data();
00779 
00780         int pre_integrate();
00781         int post_integrate();
00782 
00783         void clear();
00784 
00785         int set_joint_value(const fVec& values);
00786         int set_joint_vel(const fVec& vels);
00787         int set_joint_acc(const fVec& accs);
00788         int set_joint_force(const fVec& forces);
00789 
00790         int get_joint_value(fVec& values);
00791         int get_joint_vel(fVec& vels);
00792         int get_joint_acc(fVec& accs);
00793         int get_joint_force(fVec& forces);
00794 
00795         void calc_position();
00796         void calc_velocity();
00797         void calc_acceleration();
00798 
00799         void inv_dyn();
00800         void inv_dyn_1();
00801         void inv_dyn_2();
00802         void calc_joint_force(fVec& tau);
00803 
00804         void get_joint_name_list(char** jnames);
00805         void get_joint_list(Joint** joints);
00806 
00807         void add_child(Joint* j);
00808         int remove_child(Joint* j);
00809         Joint* find_joint(const char* n, const char* charname);
00810         Joint* find_joint(int _id);
00811 
00812         int calc_jacobian(fMat& J, Joint* target);
00813         int calc_jacobian_rotate(fMat& J, Joint* target);
00814         int calc_jacobian_slide(fMat& J, Joint* target);
00815         int calc_jacobian_sphere(fMat& J, Joint* target);
00816         int calc_jacobian_free(fMat& J, Joint* target);
00817 
00818         int calc_jacobian_2(fMat* J, Joint* target);
00819         int calc_jacobian_2_rotate_sub(fMat* J, Joint* target, Joint* j1);
00820         int calc_jacobian_2_slide_sub(fMat* J, Joint* target, Joint* j1);
00821         int calc_jacobian_2_sphere_sub(fMat* J, Joint* target, Joint* j1);
00822         int calc_jacobian_2_free_sub(fMat* J, Joint* target, Joint* j1);
00823 
00824         int calc_jacobian_2_rotate_rotate(fMat* J, Joint* target, Joint* j1, const fVec3& axis1, int index1, const fVec3& axis2, int index2);
00825         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);
00826         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);
00827 
00828 
00829         int descendant_dof();
00830         int descendant_num_joints();
00831         int is_descendant(Joint* cur, Joint* target);
00832         
00833         int save(ostream& ost, int indent, const char* charname) const;
00834         int save_xml(ostream& ost, int indent, const char* charname) const;
00835 
00836         int clear_joint_force();
00837         int clear_ext_force();
00838 
00839         double total_com(fVec3& com, const char* chname);
00840         double com_jacobian(fMat& J, fVec3& com, const char* chname);
00841 
00842 private:
00843         double cur_scale;
00844 };
00845 
00846 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:52