ik.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 __IK_H__
00017 #define __IK_H__
00018 
00019 #include <chain.h>
00020 
00021 class IKConstraint;
00022 class IKHandle;
00023 
00024 //#define MEASURE_TIME
00025 
00026 //#define MAX_CONSTRAINT 128
00027 #define MIN_JOINT_WEIGHT 0.01
00028 
00029 #define MAX_CONDITION_NUMBER 100.0
00030 
00031 static const char joint_name_separator = '_';
00032 
00037 class IK
00038         : virtual public Chain
00039 {
00040         friend class IKConstraint;
00041         friend class IKHandle;
00042         friend class IKDesire;
00043 public:
00047         enum ConstType {
00048                 HANDLE_CONSTRAINT,  
00049                 DESIRE_CONSTRAINT,  
00050                 COM_CONSTRAINT,     
00051                 SCALAR_JOINT_LIMIT_CONSTRAINT,  
00052                 IK_NUM_CONSTRAINT_TYPES,
00053         };
00057         enum Priority {
00058                 HIGH_PRIORITY = 0,  
00059                 HIGH_IF_POSSIBLE,   
00060                 LOW_PRIORITY,       
00061                 N_PRIORITY_TYPES,
00062         };
00066         enum ConstIndex {
00067                 HAVE_CONSTRAINT,    
00068                 NO_CONSTRAINT,      
00069         };
00070         
00072         IK();
00074         ~IK();
00075 
00077         int NumConstraints() {
00078                 return n_constraints;
00079         }
00081         int NumConstraints(ConstType t);
00082 
00084         int LoadMarkerXML(const char* fname, const char* _charname);
00085 
00091         int AddConstraint(IKConstraint* _constraint);
00097         int RemoveConstraint(int _id);
00101         int RemoveAllConstraints();
00102 
00108         double Update(double timestep);
00109 
00111         double Update(double max_timestep, double min_timestep, double max_integ_error = DEFAULT_MAX_INTEG_ERROR);
00112 
00120         IKConstraint* FindConstraint(ConstType _type, const char* jname, const char* charname = 0);
00121 
00126         IKConstraint* FindConstraint(int _id);
00127 
00132         int ConstraintIndex(ConstType _type, const char* jname);
00137         int ConstraintIndex(int _id);
00138 
00143         int ConstraintID(ConstType _type, const char* jname);
00148         int ConstraintID(int _index);
00149 
00151         int ResetAllConstraints();
00152 
00154         int ResetConstraints(ConstType t);
00155 
00157         int SetJointWeight(const char* jname, double _weight);
00159         int SetJointWeight(const char* jname, const fVec& _weight);
00160 
00162         int EnableDesire(const char* jname);
00164         int DisableDesire(const char* jname);
00166         int SetDesireGain(const char* jname, double _gain);
00167 
00169         void SetMaxConditionNumber(double cn) {
00170                 max_condnum = cn;
00171         }
00173         double GetMaxConditionNumber() {
00174                 return max_condnum;
00175         }
00176 
00178         void SetConstraintScale(double _scale, const char* charname = 0);
00180         void SetCharacterScale(double _scale, const char* charname = 0);
00181 
00183 
00190         IKHandle* AddMarker(const std::string& label, const std::string& linkname, const std::string& charname, const fVec3& rel_pos);
00191 #ifndef SEGA
00192         IKHandle* AddMarker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos = 0.0);
00193         int SaveMarkers(const char* fname);
00194         int EditMarker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos = 0.0);
00195 #endif
00196 protected:
00197 #ifdef SEGA
00198         int init();
00199         int myinit();
00200 #else
00201         int init(SceneGraph* sg);
00202         int myinit(SceneGraph* sg);
00203         int load_markers(SceneGraph* sg, Joint* rj);
00204         int load_markers(TransformNode* marker_top);
00205         IKHandle* add_marker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos);
00206         IKHandle* add_marker(TransformNode* tn);
00207         int edit_marker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos);
00208         int save_marker(TransformNode* top_tnode, IKHandle* h);
00209 #endif
00210 
00212         int calc_jacobian();
00213 
00214         int calc_feedback();
00215         
00217         double solve_ik();
00218 
00219         int copy_jacobian();
00220 
00221         void set_character_scale(Joint* jnt, double _scale, const char* charname);
00222 
00224         int n_constraints;
00225 
00227         int n_total_const;
00228 
00230         IKConstraint** constraints;
00231 
00232         int n_assigned_constraints;
00233         int assign_constraints(int _n);
00234 
00235         fMat J[N_PRIORITY_TYPES];  
00236         fVec fb[N_PRIORITY_TYPES]; 
00237         fVec weight[N_PRIORITY_TYPES]; 
00238         int n_const[N_PRIORITY_TYPES]; 
00239         int n_all_const;
00240 
00242         double max_condnum;
00243 
00245         fVec joint_weights;
00246 
00247 };
00248 
00253 class IKConstraint
00254 {
00255         friend class IK;
00256 public:
00258 
00266         IKConstraint(IK* _ik, const char* _jname, Joint* _jnt, IK::Priority _pri, double _gain) {
00267                 ik = _ik;
00268                 joint_name = 0;
00269                 enabled = true;
00270                 active = true;
00271                 if(_jname)
00272                 {
00273                         joint_name = new char [strlen(_jname) + 1];
00274                         strcpy(joint_name, _jname);
00275                 }
00276                 joint = _jnt;
00277                 n_const = 0;
00278                 priority = _pri;
00279                 gain = _gain;
00280                 i_const = -1;
00281                 id = -1;
00282                 is_dropped = false;
00283         }
00284         
00286         virtual ~IKConstraint() {
00287                 if(joint_name) delete[] joint_name;
00288         }
00289 
00291         virtual IK::ConstType GetType() = 0;
00292 
00293         virtual int Reset() {
00294                 return 0;
00295         }
00296 
00298         void Enable() {
00299                 enabled = true;
00300         }
00302         void Disable() {
00303                 enabled = false;
00304         }
00305 
00307         void Activate() {
00308                 active = true;
00309         }
00311         void Diactivate() {
00312                 active = false;
00313         }
00315         int Active() {
00316                 return active;
00317         }
00318 
00320         void SetGain(double _gain) {
00321                 gain = _gain;
00322         }
00324         double GetGain() {
00325                 return gain;
00326         }
00327 
00329         void SetPriority(IK::Priority _pri) {
00330                 priority = _pri;
00331         }
00333         IK::Priority GetPriority() {
00334                 return priority;
00335         }
00336 
00337         int iConst() {
00338                 return i_const;
00339         }
00340         int nConst() {
00341                 return n_const;
00342         }
00343 
00344         Joint* GetJoint() {
00345                 return joint;
00346         }
00347         int ID() {
00348                 return id;
00349         }
00350         int Dropped() {
00351                 if(priority == IK::HIGH_IF_POSSIBLE) return is_dropped;
00352                 return false;
00353         }
00354 
00355         virtual void SetCharacterScale(double _scale, const char* charname = 0) {
00356         }
00357 
00358 protected:
00360 
00368         virtual int calc_jacobian();
00369         virtual int calc_jacobian_rotate(Joint* cur) {
00370                 return 0;
00371         }
00372         virtual int calc_jacobian_slide(Joint* cur) {
00373                 return 0;
00374         }
00375         virtual int calc_jacobian_sphere(Joint* cur) {
00376                 return 0;
00377         }
00378         virtual int calc_jacobian_free(Joint* cur) {
00379                 return 0;
00380         }
00381 
00383         int calc_jacobian(Joint* cur);
00385         int copy_jacobian();
00386 
00388         virtual int calc_feedback() = 0;
00389 
00390         IK* ik;
00391         Joint* joint;          
00392         char* joint_name;      
00393         int id;                
00394 
00395         IK::Priority priority; 
00396         double gain;           
00397         fVec weight;           
00398         int n_const;           
00399         int enabled;
00400         int active;
00401         
00402         fMat J;                
00403         fVec fb;               
00404         int i_const;           
00405 
00406         int is_dropped;
00407 
00408 private:
00409         int count_constraints();
00410 };
00411 
00416 class IKHandle
00417         : public IKConstraint
00418 {
00419         friend class IK;
00420 public:
00433         IKHandle(IK* _ik, const char* _jname, Joint* _jnt,
00434                          IK::ConstIndex cindex[6], IK::Priority _pri, double _gain,
00435                          const fVec3& _rel_pos = 0.0, const fMat33& _rel_att = 1.0,
00436                          Joint* _other_joint = 0)
00437                         : IKConstraint(_ik, _jname, _jnt, _pri, _gain),
00438                           rel_pos(_rel_pos), rel_att(_rel_att) {
00439                 for(int i=0; i<6; i++)
00440                 {
00441                         const_index[i] = cindex[i];
00442                         if(const_index[i] == IK::HAVE_CONSTRAINT)
00443                                 n_const++;
00444                 }
00445                 other_joint = _other_joint;
00446                 // compute the current absolute position/orientation
00447                 abs_pos.zero();
00448                 abs_att.identity();
00449                 if(joint)
00450                 {
00451                         abs_att.mul(joint->abs_att, rel_att);
00452                         abs_pos.mul(joint->abs_att, rel_pos);
00453                         abs_pos += joint->abs_pos;
00454                         // relative position/orientation
00455                         if(other_joint)
00456                         {
00457                                 static fVec3 pp;
00458                                 static fMat33 rt, att;
00459                                 rt.tran(other_joint->abs_att);
00460                                 att.set(abs_att);
00461                                 pp.sub(abs_pos, other_joint->abs_pos);
00462                                 abs_pos.mul(rt, pp);
00463                                 abs_att.mul(rt, att);
00464                         }
00465                 }
00466                 J.resize(n_const, ik->NumDOF());
00467                 J.zero();
00468                 fb.resize(n_const);
00469                 fb.zero();
00470                 weight.resize(n_const);
00471                 weight = 1.0;
00472         }
00473         
00474         ~IKHandle() {
00475         }
00476 
00478         int Reset() {
00479                 if(joint)
00480                 {
00481                         abs_att.mul(joint->abs_att, rel_att);
00482                         abs_pos.mul(joint->abs_att, rel_pos);
00483                         abs_pos += joint->abs_pos;
00484                         // relative position/orientation
00485                         if(other_joint)
00486                         {
00487                                 static fVec3 pp;
00488                                 static fMat33 rt, att;
00489                                 rt.tran(other_joint->abs_att);
00490                                 att.set(abs_att);
00491                                 pp.sub(abs_pos, other_joint->abs_pos);
00492                                 abs_pos.mul(rt, pp);
00493                                 abs_att.mul(rt, att);
00494                         }
00495                 }
00496                 return 0;
00497         }
00499         void SetPos(const fVec3& _abs_pos) {
00500                 abs_pos.set(_abs_pos);
00501         }
00503         void SetAtt(const fMat33& _abs_att) {
00504                 abs_att.set(_abs_att);
00505         }
00506         
00508         void GetPos(fVec3& _abs_pos) {
00509                 _abs_pos.set(abs_pos);
00510         }
00512         void GetAtt(fMat33& _abs_att) {
00513                 _abs_att.set(abs_att);
00514         }
00515 
00517         void SetRelPos(const fVec3& _rel_pos) {
00518                 rel_pos.set(_rel_pos);
00519                 Reset();
00520         }
00521 
00522         IK::ConstType GetType() {
00523                 return IK::HANDLE_CONSTRAINT;
00524         }
00525 
00526         void GetConstIndex(IK::ConstIndex _cindex[]) {
00527                 for(int i=0; i<6; i++)
00528                 {
00529                         _cindex[i] = const_index[i];
00530                 }
00531         }
00532 
00533         void SetCharacterScale(double _scale, const char* charname = 0);
00534 
00535 protected:
00536         int calc_jacobian();
00537 
00538         int calc_feedback();
00539 
00540         fVec3 abs_pos;  
00541         fMat33 abs_att; 
00542 
00543         IK::ConstIndex const_index[6]; 
00544         fVec3 rel_pos;
00545         fMat33 rel_att;
00546 
00548         Joint* other_joint;
00549 };
00550 
00555 class IKDesire
00556         : public IKConstraint
00557 {
00558         friend class IK;
00559 public:
00560         IKDesire(IK* _ik, const char* _jname, Joint* _jnt,
00561                          IK::Priority _pri, double _gain)
00562                         : IKConstraint(_ik, _jname, _jnt, _pri, _gain) {
00563                 q_des = 0.0;
00564                 att_des.identity();
00565                 pos_des.zero();
00566                 if(joint && joint->n_dof > 0)
00567                 {
00568                         n_const = joint->n_dof;
00569                         J.resize(joint->n_dof, ik->NumDOF());
00570                         J.zero();
00571                         fb.resize(joint->n_dof);
00572                         fb.zero();
00573                         weight.resize(n_const);
00574                         weight = 1.0;
00575                         switch(joint->j_type)
00576                         {
00577                         case JROTATE:
00578                         case JSLIDE:
00579                                 joint->GetJointValue(q_des);
00580                                 break;
00581                         case JSPHERE:
00582                                 joint->GetJointValue(att_des);
00583                                 break;
00584                         case JFREE:
00585                                 joint->GetJointValue(pos_des, att_des);
00586                                 break;
00587                         default:
00588                                 break;
00589                         }
00590                 }
00591         }
00592         
00593         ~IKDesire() {
00594         }
00595 
00596         int Reset() {
00597                 if(joint && joint->n_dof > 0)
00598                 {
00599                         switch(joint->j_type)
00600                         {
00601                         case JROTATE:
00602                         case JSLIDE:
00603                                 joint->GetJointValue(q_des);
00604                                 break;
00605                         case JSPHERE:
00606                                 joint->GetJointValue(att_des);
00607                                 break;
00608                         case JFREE:
00609                                 joint->GetJointValue(pos_des, att_des);
00610                                 break;
00611                         default:
00612                                 break;
00613                         }
00614                 }
00615                 return 0;
00616         }
00617 
00618         IK::ConstType GetType() {
00619                 return IK::DESIRE_CONSTRAINT;
00620         }
00621 
00622         void SetDesire(double _q) {
00623                 q_des = _q;
00624         }
00625         void SetDesire(const fMat33& _att) {
00626                 att_des.set(_att);
00627         }
00628         void SetDesire(const fVec3& _pos, const fMat33& _att) {
00629                 pos_des.set(_pos);
00630                 att_des.set(_att);
00631         }
00632         void SetDesire(const fVec3& _pos) {
00633                 pos_des.set(_pos);
00634         }
00635 
00636         void SetCharacterScale(double _scale, const char* charname = 0);
00637 protected:
00638         int calc_jacobian_rotate(Joint* cur);
00639         int calc_jacobian_slide(Joint* cur);
00640         int calc_jacobian_sphere(Joint* cur);
00641         int calc_jacobian_free(Joint* cur);
00642 
00643         int calc_feedback();
00644 
00645         double q_des;    
00646         fMat33 att_des;  
00647         fVec3 pos_des;   
00648 };
00649 
00654 class IKCom
00655         : public IKConstraint
00656 {
00657 public:
00658         IKCom(IK* _ik, const char* _charname, IK::ConstIndex cindex[3],
00659                          IK::Priority _pri, double _gain)
00660                         : IKConstraint(_ik, 0, 0, _pri, _gain) {
00661                 charname = 0;
00662                 if(_charname)
00663                 {
00664                         charname = new char [strlen(_charname)+1];
00665                         strcpy(charname, _charname);
00666                 }
00667                 n_const = 0;
00668                 for(int i=0; i<3; i++)
00669                 {
00670                         const_index[i] = cindex[i];
00671                         if(const_index[i] == IK::HAVE_CONSTRAINT)
00672                                 n_const++;
00673                 }
00674                 fb.resize(n_const);
00675                 des_com.zero();
00676                 cur_com.zero();
00677                 if(_ik)
00678                 {
00679                         _ik->CalcPosition();
00680                         _ik->TotalCOM(cur_com, _charname);
00681                         des_com.set(cur_com);
00682                 }
00683         }
00684         ~IKCom() {
00685                 if(charname) delete[] charname;
00686         }
00687 
00688         IK::ConstType GetType() {
00689                 return IK::COM_CONSTRAINT;
00690         }
00691         int Reset() {
00692                 ik->TotalCOM(des_com, charname);
00693                 return 0;
00694         }
00695         void SetPos(const fVec3& p) {
00696                 des_com.set(p);
00697         }
00698 
00699 protected:
00700         int calc_jacobian();
00701         int calc_feedback();
00702 
00703         IK::ConstIndex const_index[3];  
00704         char* charname;  
00705         fVec3 des_com;   
00706         fVec3 cur_com;   
00707 };
00708 
00713 class IKScalarJointLimit
00714         : public IKConstraint
00715 {
00716         friend class IK;
00717 public:
00718         IKScalarJointLimit(IK* _ik, const char* _jname, Joint* _jnt,
00719                                            IK::Priority _pri, double _gain)
00720                         : IKConstraint(_ik, _jname, _jnt, _pri, _gain) {
00721                 min_limit = false;
00722                 max_limit = false;
00723                 q_min = 0.0;
00724                 q_max = 0.0;
00725                 if(joint && (joint->j_type == JROTATE || joint->j_type == JSLIDE))
00726                 {
00727                         n_const = 1;
00728                         J.resize(1, ik->NumDOF());
00729                         J.zero();
00730                         fb.resize(1);
00731                         fb.zero();
00732                         weight.resize(1);
00733                         weight = 1.0;
00734                 }
00735                 else
00736                 {
00737                         enabled = false;
00738                 }
00739         }
00740         
00741         ~IKScalarJointLimit() {
00742         }
00743 
00744         IK::ConstType GetType() {
00745                 return IK::SCALAR_JOINT_LIMIT_CONSTRAINT;
00746         }
00747 
00748         void SetMax(double _q_max) {
00749                 q_max = _q_max;
00750                 max_limit = true;
00751         }
00752         void SetMin(double _q_min) {
00753                 q_min = _q_min;
00754                 min_limit = true;
00755         }
00756         double GetMax() {
00757                 return q_max;
00758         }
00759         double GetMin() {
00760                 return q_min;
00761         }
00762 
00763         void SetCharacterScale(double _scale, const char* charname = 0);
00764 protected:
00765         int calc_jacobian_rotate(Joint* cur);
00766         int calc_jacobian_slide(Joint* cur);
00767 
00768         int calc_feedback();
00769 
00770         int min_limit, max_limit;
00771         double q_min, q_max;
00772 };
00773 
00774 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:17