edit.cpp
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  */
00009 /*
00010  * edit.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
00012  */
00013 
00014 #include "chain.h"
00015 
00016 /*
00017  * create chain
00018  */
00019 int Chain::BeginCreateChain(int append)
00020 {
00021         if(in_create_chain)
00022         {
00023                 cerr << "Chain::BeginCreateChain - called after BeginCreateChain()" << endl;
00024                 return -1;
00025         }
00026         if(!append) Clear();
00027         in_create_chain = true;
00028         clear_data();
00029         return 0;
00030 }
00031 
00032 int Chain::RemoveJoint(Joint* j)
00033 {
00034         if(!in_create_chain)
00035         {
00036                 cerr << "Chain::RemoveJoint - attempted to edit the chain before calling BeginCreateChain" << endl;
00037                 return -1;
00038         }
00039         if(!j->parent) return -1;
00040         return j->parent->remove_child(j);
00041 }
00042 
00043 int Joint::remove_child(Joint* j)
00044 {
00045         Joint* prev = 0;
00046         j->parent = 0;
00047         if(child == j)
00048         {
00049                 child = j->brother;
00050                 j->brother = 0;
00051                 return 0;
00052         }
00053         for(prev=child; prev && prev->brother!=j; prev=prev->brother);
00054         if(!prev) return -1;
00055         prev->brother = j->brother;
00056         j->brother = 0;
00057         return 0;
00058 }
00059 
00060 Joint* Chain::AddRoot(const char* name, const fVec3& grav)
00061 {
00062         if(!in_create_chain)
00063         {
00064                 cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
00065                 return NULL;
00066         }
00067         if(root)
00068         {
00069                 cerr << "Chain::AddRoot - root is already set" << endl;
00070                 return NULL;
00071         }
00072         Joint* r;
00073         if(name)
00074         {
00075                 r = new Joint(name, JFIXED);
00076         }
00077         else
00078         {
00079                 r = new Joint("space", JFIXED);
00080         }
00081         r->loc_lin_acc.set(grav);
00082         r->chain = this;
00083         root = r;
00084         return r;
00085 }
00086 
00087 int Chain::AddRoot(Joint* r)
00088 {
00089         if(!in_create_chain)
00090         {
00091                 cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
00092                 return -1;
00093         }
00094         if(root)
00095         {
00096                 cerr << "Chain::AddRoot - root is already set" << endl;
00097                 return -1;
00098         }
00099         r->chain = this;
00100         r->j_type = JFIXED;
00101         root = r;
00102         return 0;
00103 }
00104 
00105 int Chain::AddJoint(Joint* target, const char* parent_name, const char* charname)
00106 {
00107         Joint* p = FindJoint(parent_name, charname);
00108         if(!strcmp(parent_name, root->name)) p = root;
00109         if(!p)
00110         {
00111                 cerr << "Chain::AddJoint - parent " << parent_name << " not found for " << target->name << endl;
00112                 return -1;
00113         }
00114         return AddJoint(target, p);
00115 }
00116 
00117 int Chain::AddJoint(Joint* target, Joint* p)
00118 {
00119         if(!in_create_chain)
00120         {
00121                 cerr << "Chain::AddJoint - attempted to add a joint before calling BeginCreateChain" << endl;
00122                 return -1;
00123         }
00124         if(!root)
00125         {
00126                 cerr << "Chain::AddJoint - root is not set" << endl;
00127                 return -1;
00128         }
00129         if(!p)
00130         {
00131                 cerr << "Chain::AddJoint - parent is NULL" << endl;
00132                 return -1;
00133         }
00134         if(!p->chain)
00135         {
00136                 cerr << "Joint::AddJoint - parent joint is not added yet" << endl;
00137                 return -1;
00138         }
00139         if(FindJoint(target->name))
00140         {
00141                 cerr << "Joint::AddJoint - joint " << target->name << " already exists" << endl;
00142                 return -1;
00143         }
00144         if(!target) return 0;
00145         target->chain = this;
00146         if(p)
00147         {
00148                 p->add_child(target);
00149         }
00150         else
00151         {
00152                 root = target;
00153         }
00154         return 0;
00155 }
00156 
00157 void Joint::add_child(Joint* c)
00158 {
00159         c->parent = this;
00160         c->brother = child;
00161         child = c;
00162 }
00163 
00164 Joint* Chain::AddJoint(JointData* joint_data, const char* charname)
00165 {
00166         Joint* j = new Joint(joint_data, charname);
00167         if(AddJoint(j, joint_data->parent_name, charname)) return 0;
00168         return j;
00169 }
00170 
00171 #if 0
00172 int Chain::CreateSerial(int num_joint, const JointData& joint_data,
00173                                                 const char* charname, Joint* parent_joint)
00174 {
00175         if(!joint_data.name) return -1;
00176         if(!root) AddRoot();
00177         Joint* last_joint = root;
00178         if(parent_joint) last_joint = parent_joint;
00179         int i;
00180         for(i=0; i<num_joint; i++)
00181         {
00182                 JointData* jdata = new JointData(joint_data);
00183                 jdata->name = new char [strlen(joint_data.name) + 5];
00184                 sprintf(jdata->name, "%s%04d", joint_data.name, i+1);
00185                 jdata->parent_name = new char [strlen(last_joint->basename) + 1];
00186                 strcpy(jdata->parent_name, last_joint->basename);
00187                 Joint* new_joint = AddJoint(jdata, charname);
00188                 cerr << new_joint->name << " added to " << last_joint->basename << endl;
00189                 last_joint = new_joint;
00190                 delete jdata;
00191         }
00192         return 0;
00193 }
00194 
00195 int Chain::CreateParallel(int num_char, const char* prmname, const char* charname_base, const fVec3& init_pos, const fMat33& init_att, const fVec3& pos_offset, const fMat33& att_offset, int init_num)
00196 {
00197         int i;
00198         fVec3 cur_pos_offset(init_pos);
00199         fMat33 cur_att_offset(init_att);
00200         for(i=0; i<num_char; i++)
00201         {
00202                 char* charname = new char [strlen(charname_base) + 5];
00203                 sprintf(charname, "%s%04d", charname_base, i+init_num);
00204                 Load(prmname, charname);
00205                 Joint* char_root = FindCharacterRoot(charname);
00206                 fVec3 tmp, tmpp;
00207                 fMat33 tmpr;
00208                 tmp.mul(cur_att_offset, pos_offset);
00209                 cur_pos_offset += tmp;
00210                 tmpr.mul(cur_att_offset, att_offset);
00211                 cur_att_offset.set(tmpr);
00212                 tmpp.add(char_root->rel_pos, cur_pos_offset);
00213                 tmpr.mul(char_root->rel_att, cur_att_offset);
00214                 char_root->SetJointValue(tmpp, tmpr);
00215                 cerr << charname << ": " << char_root->rel_pos << endl << char_root->rel_att << endl;
00216                 delete[] charname;
00217         }
00218         return 0;
00219 }
00220 #endif
00221 
00222 #ifdef SEGA
00223 int Chain::EndCreateChain()
00224 #else
00225 int Chain::EndCreateChain(SceneGraph* sg)
00226 #endif
00227 {
00228         if(!in_create_chain)
00229         {
00230                 cerr << "Chain::EndCreateChain - called before BeginCreateChain()" << endl;
00231                 return -1;
00232         }
00233 #ifdef SEGA
00234         init();
00235 #else
00236         init(sg);
00237 #endif
00238         in_create_chain = false;
00239         return 0;
00240 }
00241 
00242 #ifndef SEGA
00243 void Chain::set_relative_positions(SceneGraph* sg)
00244 {
00245         root->abs_pos.zero();
00246         root->abs_att.identity();
00247         calc_abs_positions(root, sg);
00248         calc_rel_positions(root, sg);
00249 }
00250 
00251 void Chain::calc_abs_positions(Joint* cur, SceneGraph* sg)
00252 {
00253         if(!cur) return;
00254         TransformNode* tnode = sg->findTransformNode(cur->name);
00255 //      cerr << "---- calc_abs_positions: " << cur->name << endl;
00256         if(cur->parent && tnode)
00257         {
00258                 float abs_pos[3], abs_att[3][3], abs_scale[3];
00259                 get_abs_matrix(tnode, abs_pos, abs_att, abs_scale);
00260                 cur->abs_pos(0) = abs_pos[0];
00261                 cur->abs_pos(1) = abs_pos[1];
00262                 cur->abs_pos(2) = abs_pos[2];
00263                 cur->abs_att(0,0) = abs_att[0][0];
00264                 cur->abs_att(0,1) = abs_att[0][1];
00265                 cur->abs_att(0,2) = abs_att[0][2];
00266                 cur->abs_att(1,0) = abs_att[1][0];
00267                 cur->abs_att(1,1) = abs_att[1][1];
00268                 cur->abs_att(1,2) = abs_att[1][2];
00269                 cur->abs_att(2,0) = abs_att[2][0];
00270                 cur->abs_att(2,1) = abs_att[2][1];
00271                 cur->abs_att(2,2) = abs_att[2][2];
00272 //              cerr << cur->abs_pos << endl;
00273 //              cerr << cur->abs_att << endl;
00274         }
00275         calc_abs_positions(cur->brother, sg);
00276         calc_abs_positions(cur->child, sg);
00277 }
00278 
00279 void Chain::calc_rel_positions(Joint* cur, SceneGraph* sg)
00280 {
00281         if(!cur) return;
00282         TransformNode* tnode = sg->findTransformNode(cur->name);
00283 //      cerr << "---- calc_rel_positions: " << cur->name << endl;
00284         if(cur->parent && tnode)
00285         {
00286                 static fVec3 pp, rel_pos;
00287                 static fMat33 tr, rel_att;
00288                 pp.sub(cur->abs_pos, cur->parent->abs_pos);
00289                 tr.tran(cur->parent->abs_att);
00290                 rel_pos.mul(tr, pp);
00291                 rel_att.mul(tr, cur->abs_att);
00292                 // set rel_pos, rel_att, rel_ep
00293                 cur->rel_pos.set(rel_pos);
00294                 cur->rel_att.set(rel_att);
00295                 cur->rel_ep.set(rel_att);
00296                 // set init_pos and init_att
00297                 cur->init_pos.set(rel_pos);
00298                 cur->init_att.set(rel_att);
00299 //              cerr << "--- " << cur->name << endl;
00300 //              cerr << rel_pos << endl;
00301 //              cerr << rel_att << endl;
00302         }
00303         calc_rel_positions(cur->brother, sg);
00304         calc_rel_positions(cur->child, sg);
00305 }
00306 #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:53