00001
00002
00003
00004
00005
00006
00007
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
00433
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