edit.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
9 /*
10  * edit.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
12  */
13 
14 #include "chain.h"
15 
16 /*
17  * create chain
18  */
19 int Chain::BeginCreateChain(int append)
20 {
21  if(in_create_chain)
22  {
23  cerr << "Chain::BeginCreateChain - called after BeginCreateChain()" << endl;
24  return -1;
25  }
26  if(!append) Clear();
27  in_create_chain = true;
28  clear_data();
29  return 0;
30 }
31 
33 {
34  if(!in_create_chain)
35  {
36  cerr << "Chain::RemoveJoint - attempted to edit the chain before calling BeginCreateChain" << endl;
37  return -1;
38  }
39  if(!j->parent) return -1;
40  return j->parent->remove_child(j);
41 }
42 
44 {
45  Joint* prev = 0;
46  j->parent = 0;
47  if(child == j)
48  {
49  child = j->brother;
50  j->brother = 0;
51  return 0;
52  }
53  for(prev=child; prev && prev->brother!=j; prev=prev->brother);
54  if(!prev) return -1;
55  prev->brother = j->brother;
56  j->brother = 0;
57  return 0;
58 }
59 
60 Joint* Chain::AddRoot(const char* name, const fVec3& grav)
61 {
62  if(!in_create_chain)
63  {
64  cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
65  return NULL;
66  }
67  if(root)
68  {
69  cerr << "Chain::AddRoot - root is already set" << endl;
70  return NULL;
71  }
72  Joint* r;
73  if(name)
74  {
75  r = new Joint(name, JFIXED);
76  }
77  else
78  {
79  r = new Joint("space", JFIXED);
80  }
81  r->loc_lin_acc.set(grav);
82  r->chain = this;
83  root = r;
84  return r;
85 }
86 
88 {
89  if(!in_create_chain)
90  {
91  cerr << "Chain::AddRoot - attempted to add a root before calling BeginCreateChain" << endl;
92  return -1;
93  }
94  if(root)
95  {
96  cerr << "Chain::AddRoot - root is already set" << endl;
97  return -1;
98  }
99  r->chain = this;
100  r->j_type = JFIXED;
101  root = r;
102  return 0;
103 }
104 
105 int Chain::AddJoint(Joint* target, const char* parent_name, const char* charname)
106 {
107  Joint* p = FindJoint(parent_name, charname);
108  if(!strcmp(parent_name, root->name)) p = root;
109  if(!p)
110  {
111  cerr << "Chain::AddJoint - parent " << parent_name << " not found for " << target->name << endl;
112  return -1;
113  }
114  return AddJoint(target, p);
115 }
116 
117 int Chain::AddJoint(Joint* target, Joint* p)
118 {
119  if(!in_create_chain)
120  {
121  cerr << "Chain::AddJoint - attempted to add a joint before calling BeginCreateChain" << endl;
122  return -1;
123  }
124  if(!root)
125  {
126  cerr << "Chain::AddJoint - root is not set" << endl;
127  return -1;
128  }
129  if(!p)
130  {
131  cerr << "Chain::AddJoint - parent is NULL" << endl;
132  return -1;
133  }
134  if(!p->chain)
135  {
136  cerr << "Joint::AddJoint - parent joint is not added yet" << endl;
137  return -1;
138  }
139  if(FindJoint(target->name))
140  {
141  cerr << "Joint::AddJoint - joint " << target->name << " already exists" << endl;
142  return -1;
143  }
144  if(!target) return 0;
145  target->chain = this;
146  if(p)
147  {
148  p->add_child(target);
149  }
150  else
151  {
152  root = target;
153  }
154  return 0;
155 }
156 
158 {
159  c->parent = this;
160  c->brother = child;
161  child = c;
162 }
163 
164 Joint* Chain::AddJoint(JointData* joint_data, const char* charname)
165 {
166  Joint* j = new Joint(joint_data, charname);
167  if(AddJoint(j, joint_data->parent_name, charname)) return 0;
168  return j;
169 }
170 
171 #if 0
172 int Chain::CreateSerial(int num_joint, const JointData& joint_data,
173  const char* charname, Joint* parent_joint)
174 {
175  if(!joint_data.name) return -1;
176  if(!root) AddRoot();
177  Joint* last_joint = root;
178  if(parent_joint) last_joint = parent_joint;
179  int i;
180  for(i=0; i<num_joint; i++)
181  {
182  JointData* jdata = new JointData(joint_data);
183  jdata->name = new char [strlen(joint_data.name) + 5];
184  sprintf(jdata->name, "%s%04d", joint_data.name, i+1);
185  jdata->parent_name = new char [strlen(last_joint->basename) + 1];
186  strcpy(jdata->parent_name, last_joint->basename);
187  Joint* new_joint = AddJoint(jdata, charname);
188  cerr << new_joint->name << " added to " << last_joint->basename << endl;
189  last_joint = new_joint;
190  delete jdata;
191  }
192  return 0;
193 }
194 
195 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)
196 {
197  int i;
198  fVec3 cur_pos_offset(init_pos);
199  fMat33 cur_att_offset(init_att);
200  for(i=0; i<num_char; i++)
201  {
202  char* charname = new char [strlen(charname_base) + 5];
203  sprintf(charname, "%s%04d", charname_base, i+init_num);
204  Load(prmname, charname);
205  Joint* char_root = FindCharacterRoot(charname);
206  fVec3 tmp, tmpp;
207  fMat33 tmpr;
208  tmp.mul(cur_att_offset, pos_offset);
209  cur_pos_offset += tmp;
210  tmpr.mul(cur_att_offset, att_offset);
211  cur_att_offset.set(tmpr);
212  tmpp.add(char_root->rel_pos, cur_pos_offset);
213  tmpr.mul(char_root->rel_att, cur_att_offset);
214  char_root->SetJointValue(tmpp, tmpr);
215  cerr << charname << ": " << char_root->rel_pos << endl << char_root->rel_att << endl;
216  delete[] charname;
217  }
218  return 0;
219 }
220 #endif
221 
222 #ifdef SEGA
224 #else
225 int Chain::EndCreateChain(SceneGraph* sg)
226 #endif
227 {
228  if(!in_create_chain)
229  {
230  cerr << "Chain::EndCreateChain - called before BeginCreateChain()" << endl;
231  return -1;
232  }
233 #ifdef SEGA
234  init();
235 #else
236  init(sg);
237 #endif
238  in_create_chain = false;
239  return 0;
240 }
241 
242 #ifndef SEGA
243 void Chain::set_relative_positions(SceneGraph* sg)
244 {
245  root->abs_pos.zero();
246  root->abs_att.identity();
249 }
250 
251 void Chain::calc_abs_positions(Joint* cur, SceneGraph* sg)
252 {
253  if(!cur) return;
254  TransformNode* tnode = sg->findTransformNode(cur->name);
255 // cerr << "---- calc_abs_positions: " << cur->name << endl;
256  if(cur->parent && tnode)
257  {
258  float abs_pos[3], abs_att[3][3], abs_scale[3];
259  get_abs_matrix(tnode, abs_pos, abs_att, abs_scale);
260  cur->abs_pos(0) = abs_pos[0];
261  cur->abs_pos(1) = abs_pos[1];
262  cur->abs_pos(2) = abs_pos[2];
263  cur->abs_att(0,0) = abs_att[0][0];
264  cur->abs_att(0,1) = abs_att[0][1];
265  cur->abs_att(0,2) = abs_att[0][2];
266  cur->abs_att(1,0) = abs_att[1][0];
267  cur->abs_att(1,1) = abs_att[1][1];
268  cur->abs_att(1,2) = abs_att[1][2];
269  cur->abs_att(2,0) = abs_att[2][0];
270  cur->abs_att(2,1) = abs_att[2][1];
271  cur->abs_att(2,2) = abs_att[2][2];
272 // cerr << cur->abs_pos << endl;
273 // cerr << cur->abs_att << endl;
274  }
275  calc_abs_positions(cur->brother, sg);
276  calc_abs_positions(cur->child, sg);
277 }
278 
279 void Chain::calc_rel_positions(Joint* cur, SceneGraph* sg)
280 {
281  if(!cur) return;
282  TransformNode* tnode = sg->findTransformNode(cur->name);
283 // cerr << "---- calc_rel_positions: " << cur->name << endl;
284  if(cur->parent && tnode)
285  {
286  static fVec3 pp, rel_pos;
287  static fMat33 tr, rel_att;
288  pp.sub(cur->abs_pos, cur->parent->abs_pos);
289  tr.tran(cur->parent->abs_att);
290  rel_pos.mul(tr, pp);
291  rel_att.mul(tr, cur->abs_att);
292  // set rel_pos, rel_att, rel_ep
293  cur->rel_pos.set(rel_pos);
294  cur->rel_att.set(rel_att);
295  cur->rel_ep.set(rel_att);
296  // set init_pos and init_att
297  cur->init_pos.set(rel_pos);
298  cur->init_att.set(rel_att);
299 // cerr << "--- " << cur->name << endl;
300 // cerr << rel_pos << endl;
301 // cerr << rel_att << endl;
302  }
303  calc_rel_positions(cur->brother, sg);
304  calc_rel_positions(cur->child, sg);
305 }
306 #endif
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)
Automatically generate multiple identical chains.
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
3x3 matrix class.
Definition: fMatrix3.h:29
int c
Definition: autoplay.py:16
int remove_child(Joint *j)
Definition: edit.cpp:43
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: init.cpp:22
Joint * child
pointer to the child joint
Definition: chain.h:685
Joint * AddRoot(const char *name=0, const fVec3 &grav=fVec3(0.0, 0.0, 9.8))
Add the (unique) root joint.
Definition: edit.cpp:60
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
void set_relative_positions(SceneGraph *sg)
Definition: edit.cpp:243
int in_create_chain
true if between BeginCreateChain() and EndCreateChain().
Definition: chain.h:473
int RemoveJoint(Joint *j)
disconnect joint j from its parent
Definition: edit.cpp:32
png_infop png_charpp name
Definition: png.h:2382
void calc_abs_positions(Joint *cur, SceneGraph *sg)
Definition: edit.cpp:251
fVec3 loc_lin_acc
linear acceleration in local frame
Definition: chain.h:746
fVec3 init_pos
origin of the joint value (for prismatic joints)
Definition: chain.h:697
Joint * root
Chain information.
Definition: chain.h:466
char * parent_name
parent joint&#39;s name
Definition: chain.h:129
int Load(const char *fname, const char *charname=0)
Load the chain from a file in original (*.prm) format.
Definition: load.cpp:26
void identity()
Identity matrix.
Definition: fMatrix3.cpp:230
int BeginCreateChain(int append=false)
Indicates begining of creating a kinematic chain.
Definition: edit.cpp:19
void add_child(Joint *j)
Definition: edit.cpp:157
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
png_uint_32 i
Definition: png.h:2735
fEulerPara rel_ep
Euler parameter representation of rel_att (for 0/3/6 DOF joints)
Definition: chain.h:702
char * basename
joint base name (without the character name)
Definition: chain.h:692
void set(const fVec3 &v, double s)
Set the elements.
Definition: fEulerPara.h:75
fVec3 rel_pos
(initial) position in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:700
JointType j_type
joint type
Definition: chain.h:694
fMat33 abs_att
absolute orientation
Definition: chain.h:742
Chain * chain
Definition: chain.h:769
char * name
joint name
Definition: chain.h:128
char * name
joint name (including the character name)
Definition: chain.h:691
int EndCreateChain(SceneGraph *sg=NULL)
End editing a chain.
Definition: edit.cpp:225
fMat33 init_att
origin of the joint value (for rotational joints)
Definition: chain.h:698
Joint * brother
pointer to the brother joint
Definition: chain.h:684
int AddJoint(Joint *target, Joint *p)
Add a new joint target as a child of joint p.
Definition: edit.cpp:117
fixed (0DOF)
Definition: chain.h:40
Classes for defining open/closed kinematic chains.
fMat33 rel_att
(initial) orientation in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:701
void calc_rel_positions(Joint *cur, SceneGraph *sg)
Definition: edit.cpp:279
std::string sprintf(char const *__restrict fmt,...)
Temporary storage for basic joint information.
Definition: chain.h:61
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
int SetJointValue(double _q)
Definition: joint.cpp:538
int CreateSerial(int num_joint, const JointData &joint_data, const char *charname=0, Joint *parent_joint=0)
Automatically generate a serial chain.
friend class Joint
Definition: chain.h:146
virtual int clear_data()
Clear arrays only; don&#39;t delete joints.
Definition: vary.cpp:127
Joint * FindJoint(const char *jname, const char *charname=0)
Find a joint from name.
Definition: chain.cpp:391
3-element vector class.
Definition: fMatrix3.h:206
fVec3 abs_pos
absolute position
Definition: chain.h:741
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
The class for representing a joint.
Definition: chain.h:538
Joint * parent
pointer to the parent joint
Definition: chain.h:683
virtual void Clear()
Remove all joints and clear all parameters.
Definition: chain.cpp:87
Joint * FindCharacterRoot(const char *charname)
Find the root joint of the character with name charname.
Definition: chain.cpp:429


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03