chain.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  * chain.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
12  */
13 
14 #include "chain.h"
15 #include <string.h>
16 
17 char* CharName(const char* _name)
18 {
19  char* ret = (char*)strrchr(_name, charname_separator);
20  if(ret) return ret+1;
21  return 0;
22 }
23 
25 {
26  value.resize(n_value);
27  vel.resize(n_dof);
28  acc.resize(n_dof);
29  GetJointValue(value);
30  GetJointVel(vel);
31  GetJointAcc(acc);
32  return 0;
33 }
34 
35 int Chain::SetStatus(const fVec& values, const fVec& vels, const fVec& accs)
36 {
37  SetJointValue(values);
38  SetJointVel(vels);
39  SetJointAcc(accs);
40  return 0;
41 }
42 
43 /*
44  * constructors and destructors
45  */
47 {
48  root = NULL;
49  n_value = 0;
50  n_dof = 0;
51  n_thrust = 0;
52  n_joint = 0;
53  in_create_chain = false;
54  all_value = 0;
55  all_value_dot = 0;
56  all_vel = 0;
57  all_vel_dot = 0;
58  j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
59  j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
60  init_value = 0;
61  init_vel = 0;
62  do_connect = false;
63 }
64 
66 {
67  if(root) delete root;
68  if(all_value) delete[] all_value;
69  if(all_value_dot) delete[] all_value_dot;
70  if(all_vel) delete[] all_vel;
71  if(all_vel_dot) delete[] all_vel_dot;
72  if(j_acc_p[0]) delete[] j_acc_p[0];
73  if(j_acc_p[1]) delete[] j_acc_p[1];
74  if(j_acc_p[2]) delete[] j_acc_p[2];
75  if(j_acc_p[3]) delete[] j_acc_p[3];
76  if(j_value_dot[0]) delete[] j_value_dot[0];
77  if(j_value_dot[1]) delete[] j_value_dot[1];
78  if(j_value_dot[2]) delete[] j_value_dot[2];
79  if(j_value_dot[3]) delete[] j_value_dot[3];
80  if(init_value) delete[] init_value;
81  if(init_vel) delete[] init_vel;
82 #ifndef SEGA
84 #endif
85 }
86 
88 {
89  if(root) delete root;
90  root = 0;
91  n_value = 0;
92  n_dof = 0;
93  n_thrust = 0;
94  n_joint = 0;
95  if(all_value) delete[] all_value;
96  all_value = 0;
97  if(all_value_dot) delete[] all_value_dot;
98  all_value_dot = 0;
99  if(all_vel) delete[] all_vel;
100  all_vel = 0;
101  if(all_vel_dot) delete[] all_vel_dot;
102  all_vel_dot = 0;
103  if(j_acc_p[0]) delete[] j_acc_p[0];
104  if(j_acc_p[1]) delete[] j_acc_p[1];
105  if(j_acc_p[2]) delete[] j_acc_p[2];
106  if(j_acc_p[3]) delete[] j_acc_p[3];
107  if(j_value_dot[0]) delete[] j_value_dot[0];
108  if(j_value_dot[1]) delete[] j_value_dot[1];
109  if(j_value_dot[2]) delete[] j_value_dot[2];
110  if(j_value_dot[3]) delete[] j_value_dot[3];
111  if(init_value) delete[] init_value;
112  if(init_vel) delete[] init_vel;
113  j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
114  j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
115  init_value = 0;
116  init_vel = 0;
117  in_create_chain = false;
118  do_connect = false;
119 }
120 
122 {
123  name = 0;
124  basename = 0;
125  realname = 0;
126  cur_scale = 1.0;
127  clear();
128 }
129 
130 Joint::Joint(const char* _name, JointType jt,
131  const fVec3& rpos, const fMat33& ratt, AxisIndex ai,
132  int _t_given)
133 {
134  name = 0;
135  basename = 0;
136  realname = 0;
137  cur_scale = 1.0;
138  clear();
139  if(_name)
140  {
141  name = new char [strlen(_name) + 1];
142  strcpy(name, _name);
143  char* charname = strrchr(name, charname_separator);
144  if(charname) *charname = '\0';
145  basename = new char [strlen(name) + 1];
146  strcpy(basename, name);
147  if(charname) *charname = charname_separator;
148  }
149  t_given = _t_given;
150  switch(jt)
151  {
152  case JFIXED:
153  SetFixedJointType(rpos, ratt);
154  break;
155  case JROTATE:
156  SetRotateJointType(rpos, ratt, ai);
157  break;
158  case JSLIDE:
159  SetSlideJointType(rpos, ratt, ai);
160  break;
161  case JSPHERE:
162  SetSphereJointType(rpos, ratt);
163  break;
164  case JFREE:
165  SetFreeJointType(rpos, ratt);
166  break;
167  case JUNKNOWN:
168  break;
169  }
170 }
171 
172 Joint::Joint(JointData* jdata, const char* charname)
173 {
174  name = 0;
175  basename = 0;
176  realname = 0;
177  cur_scale = 1.0;
178  clear();
179  if(jdata->name)
180  {
181  if(charname)
182  {
183  name = new char [strlen(jdata->name) + strlen(charname) + 2];
184  sprintf(name, "%s%c%s", jdata->name, charname_separator, charname);
185  basename = new char [strlen(jdata->name) + 1];
186  strcpy(basename, jdata->name);
187  }
188  else
189  {
190  name = new char [strlen(jdata->name) + 1];
191  strcpy(name, jdata->name);
192  // character name included?
193  char* _ch = strrchr(name, charname_separator);
194  if(_ch) *_ch = '\0';
195  basename = new char [strlen(name) + 1];
196  strcpy(basename, name);
197  if(_ch) *_ch = charname_separator;
198  }
199  }
200  mass = jdata->mass;
201  inertia.set(jdata->inertia);
202  loc_com.set(jdata->com);
203  t_given = jdata->t_given;
204  switch(jdata->j_type)
205  {
206  case JFIXED:
207  SetFixedJointType(jdata->rel_pos, jdata->rel_att);
208  break;
209  case JROTATE:
210  SetRotateJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
211  break;
212  case JSLIDE:
213  SetSlideJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
214  break;
215  case JSPHERE:
216  SetSphereJointType(jdata->rel_pos, jdata->rel_att);
217  break;
218  case JFREE:
219  SetFreeJointType(jdata->rel_pos, jdata->rel_att);
220  break;
221  default:
222  break;
223  }
224 }
225 
227 {
228  if(name) delete[] name;
229  if(basename) delete[] basename;
230  if(realname) delete[] realname;
231  name = 0;
232  basename = 0;
233  realname = 0;
234  chain = NULL;
235  parent = NULL;
236  brother = NULL;
237  child = NULL;
238  real = NULL;
239  rpos_real.zero();
240  ratt_real.identity();
241  j_type = JUNKNOWN;
242  t_given = true;
243  n_dof = 0;
244  n_thrust = 0;
245  n_root_dof = 0;
246 
247  q = qd = qdd = 0.0;
248  axis.zero();
249  init_pos.zero();
250  init_att.identity();
251  rel_lin_vel.zero();
252  rel_ang_vel.zero();
253  rel_lin_acc.zero();
254  rel_ang_acc.zero();
255 
256  rel_pos.zero();
257  rel_att.identity();
258  mass = 0.0;
259  inertia.zero();
260  loc_com.zero();
261  gear_ratio = 1.0;
262  rotor_inertia = 0.0;
263  i_value = -1;
264  i_dof = -1;
265  i_thrust = -1;
266  i_joint = -1;
267 
268  abs_pos.zero();
269  abs_att.identity();
270  loc_lin_vel.zero();
271  loc_ang_vel.zero();
272  loc_lin_acc.zero();
273  loc_ang_acc.zero();
274  loc_com_acc.zero();
275  loc_com_vel.zero();
276 
277  p_lin_vel.zero();
278  p_ep_dot.zero();
279  p_ang_vel.zero();
280  p_lin_acc.zero();
281  p_ang_acc.zero();
282 
283  ext_force.zero();
284  ext_moment.zero();
285  joint_f.zero();
286  joint_n.zero();
287 
288  tau=0.0;
289 
290 }
291 
293 {
294  if(name) delete[] name;
295  if(basename) delete[] basename;
296  if(realname) delete[] realname;
297  if(brother) delete brother;
298  if(child) delete child;
299 }
300 
301 void Joint::SetJointData(JointData* jdata, const char* charname)
302 {
303  clear();
304  if(jdata->name)
305  {
306  name = new char [strlen(jdata->name) + 1];
307  strcpy(name, jdata->name);
308  }
309  mass = jdata->mass;
310  inertia.set(jdata->inertia);
311  loc_com.set(jdata->com);
312  t_given = jdata->t_given;
313  switch(jdata->j_type)
314  {
315  case JFIXED:
316  SetFixedJointType(jdata->rel_pos, jdata->rel_att);
317  break;
318  case JROTATE:
319  SetRotateJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
320  break;
321  case JSLIDE:
322  SetSlideJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
323  break;
324  case JSPHERE:
325  SetSphereJointType(jdata->rel_pos, jdata->rel_att);
326  break;
327  case JFREE:
328  SetFreeJointType(jdata->rel_pos, jdata->rel_att);
329  break;
330  default:
331  break;
332  }
333 }
334 
335 /*
336  * utilities
337  */
338 int Chain::GetJointNameList(char**& jnames)
339 {
340  jnames = NULL;
341  if(in_create_chain)
342  {
343  cerr << "Chain::GetJointNameList - error: cannot be called between BeginCreateChain() and EndCreateChain()" << endl;
344  return -1;
345  }
346  if(n_joint > 0)
347  {
348  jnames = new char* [n_joint];
349  root->get_joint_name_list(jnames);
350  }
351  return n_joint;
352 }
353 
354 void Joint::get_joint_name_list(char** jnames)
355 {
356  if(i_joint >= 0)
357  {
358  jnames[i_joint] = new char [strlen(name) + 1];
359  strcpy(jnames[i_joint], name);
360  }
361  child->get_joint_name_list(jnames);
362  brother->get_joint_name_list(jnames);
363 }
364 
366 {
367  joints = NULL;
368  if(in_create_chain)
369  {
370  cerr << "Chain::GetJointList - error: cannot be called between BeginCreateChain() and EndCreateChain()" << endl;
371  return -1;
372  }
373  if(n_joint > 0)
374  {
375  joints = new Joint* [n_joint];
376  root->get_joint_list(joints);
377  }
378  return n_joint;
379 }
380 
382 {
383  if(i_joint >= 0 && !real)
384  {
385  joints[i_joint] = this;
386  }
387  child->get_joint_list(joints);
388  brother->get_joint_list(joints);
389 }
390 
391 Joint* Chain::FindJoint(const char* n, const char* charname)
392 {
393  return root->find_joint(n, charname);
394 }
395 
396 Joint* Joint::find_joint(const char* n, const char* charname)
397 {
398  if(charname)
399  {
400  char* mych = CharName();
401  if(mych
402  && !strcmp(basename, n)
403  && !strcmp(mych, charname)) return this;
404  }
405  else
406  {
407  if(!strcmp(name, n)) return this;
408  }
409  Joint* ret;
410  if((ret = child->find_joint(n, charname))) return ret;
411  if((ret = brother->find_joint(n, charname))) return ret;
412  return NULL;
413 }
414 
416 {
417  return root->find_joint(_id);
418 }
419 
421 {
422  if(i_joint == _id) return this;
423  Joint* ret;
424  if((ret = child->find_joint(_id))) return ret;
425  if((ret = brother->find_joint(_id))) return ret;
426  return NULL;
427 }
428 
429 Joint* Chain::FindCharacterRoot(const char* charname)
430 {
431  if(!root) return 0;
432  Joint* j;
433  for(j=root->child; j; j=j->brother)
434  {
435  char* ch = j->CharName();
436  if(ch && !strcmp(ch, charname)) return j;
437  }
438  return 0;
439 }
440 
442 {
443  return child->descendant_dof();
444 }
445 
447 {
448  int ret1 = brother->descendant_dof();
449  int ret2 = child->descendant_dof();
450  return (ret1 + ret2 + n_dof);
451 }
452 
454 {
455  return (1+child->descendant_num_joints());
456 }
457 
459 {
460  int ret1 = brother->descendant_num_joints();
461  int ret2 = child->descendant_num_joints();
462  return (ret1 + ret2 + 1);
463 }
464 
466 {
467  return is_descendant(child, target);
468 }
469 
470 int Joint::is_descendant(Joint* cur, Joint* target)
471 {
472  if(!cur) return false;
473  if(cur == target) return true;
474  if(is_descendant(cur->brother, target)) return true;
475  if(is_descendant(cur->child, target)) return true;
476  return false;
477 }
478 
480 {
481  Joint* p;
482  for(p=this; p; p=p->parent)
483  {
484  if(p == target) return true;
485  }
486  return false;
487 }
~Joint()
Definition: chain.cpp:292
3x3 matrix class.
Definition: fMatrix3.h:29
ret1
Joint * child
pointer to the child joint
Definition: chain.h:685
int SetJointValue(const fVec &values)
Set all joint values.
Definition: joint.cpp:415
Definition: chain.h:39
void clear(CorbaSequence &seq)
JointType j_type
joint type
Definition: chain.h:130
void get_joint_name_list(char **jnames)
Definition: chain.cpp:354
char * CharName(const char *_name)
Extracts the character name from a joint name.
Definition: chain.cpp:17
int GetJointNameList(char **&jnames)
Obtain a list of joint names.
Definition: chain.cpp:338
int SetStatus(const fVec &value, const fVec &vel, const fVec &acc)
Set current joint values, velocities, and accelerations.
Definition: chain.cpp:35
int isAscendant(Joint *target)
Identifies whether the target joint is a direct ascendant.
Definition: chain.cpp:479
void clear_scale_object_list()
int in_create_chain
true if between BeginCreateChain() and EndCreateChain().
Definition: chain.h:473
int is_descendant(Joint *cur, Joint *target)
Definition: chain.cpp:470
double * init_value
Definition: chain.h:462
png_infop png_charpp name
Definition: png.h:2382
fMat33 rel_att
initial orientation in parent joint&#39;s frame
Definition: chain.h:132
png_voidp int value
Definition: png.h:2113
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
Joint * root
Chain information.
Definition: chain.h:466
int SetJointAcc(const fVec &accs)
Definition: joint.cpp:497
double * j_value_dot[4]
for 4-th order Runge-Kutta
Definition: chain.h:460
fVec3 com
link center of mass in local frame
Definition: chain.h:136
double mass
link mass
Definition: chain.h:134
int n_joint
Definition: chain.h:469
static char charname_separator
Definition: dims_common.h:19
int GetJointVel(fVec &vels)
Definition: joint.cpp:172
void SetJointData(JointData *jdata, const char *charname)
Definition: chain.cpp:301
int isDescendant(Joint *target)
Identifies whether the target joint is a direct descendant.
Definition: chain.cpp:465
int SetJointVel(const fVec &vels)
Set all joint velocities/accelerations.
Definition: joint.cpp:456
int DescendantNumJoints()
Total number of joints of the descendants (end link side).
Definition: chain.cpp:453
int n_thrust
total DOF of the joints with t_given = false
Definition: chain.h:470
Joint * find_joint(const char *n, const char *charname)
Definition: chain.cpp:396
int SaveStatus(fVec &value, fVec &vel, fVec &acc)
Save current joint values, velocities, and accelerations.
Definition: chain.cpp:24
int GetJointAcc(fVec &accs)
Definition: joint.cpp:214
spherical (3DOF)
Definition: chain.h:43
int descendant_num_joints()
Definition: chain.cpp:458
std::string basename(const std::string name)
virtual ~Chain()
Definition: chain.cpp:65
double ** all_value
Pointers to the integration variables.
Definition: chain.h:455
int n_dof
Definition: chain.h:468
double ** all_vel_dot
Definition: chain.h:458
ret2
void get_joint_list(Joint **joints)
Definition: chain.cpp:381
void clear()
Definition: chain.cpp:226
char * name
joint name
Definition: chain.h:128
int do_connect
true after Connect() was called; application (or subclass) must reset the flag
Definition: chain.h:476
int n_value
Definition: chain.h:467
fMat33 inertia
link inertia
Definition: chain.h:135
void resize(int i)
Change the size.
Definition: fMatrix.h:511
Vector of generic size.
Definition: fMatrix.h:491
Joint * brother
pointer to the brother joint
Definition: chain.h:684
double * init_vel
Definition: chain.h:463
fixed (0DOF)
Definition: chain.h:40
Classes for defining open/closed kinematic chains.
Joint()
Definition: chain.cpp:121
double ** all_value_dot
Definition: chain.h:456
prismatic (1DOF)
Definition: chain.h:42
double ** all_vel
Definition: chain.h:457
int DescendantDOF()
Total DOF of the descendants (end link side).
Definition: chain.cpp:441
std::string sprintf(char const *__restrict fmt,...)
char * CharName() const
Returns the character name.
Definition: chain.h:648
Temporary storage for basic joint information.
Definition: chain.h:61
int GetJointValue(fVec &values)
Obtain the joint values/velocities/accelerations.
Definition: joint.cpp:124
int GetJointList(Joint **&joints)
Obtain a list of pointers to all joints.
Definition: chain.cpp:365
JointType
Enums for joint types.
Definition: chain.h:38
AxisIndex
Direction of a 1-DOF joint.
Definition: chain.h:48
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
rotational (1DOF)
Definition: chain.h:41
double * j_acc_p[4]
Definition: chain.h:461
The class for representing a joint.
Definition: chain.h:538
int descendant_dof()
Definition: chain.cpp:446
Chain()
Definition: chain.cpp:46
fVec3 rel_pos
initial position in parent joint&#39;s frame
Definition: chain.h:131
Joint * parent
pointer to the parent joint
Definition: chain.h:683
free (6DOF)
Definition: chain.h:44
virtual void Clear()
Remove all joints and clear all parameters.
Definition: chain.cpp:87
int t_given
if true, the joint is torque controlled, otherwise position controlled (high-gain control) ...
Definition: chain.h:137
AxisIndex axis_index
direction of the joint axis (only for 1DOF joints)
Definition: chain.h:133
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:02