ik.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  * ik.cpp
11  * Create: Katsu Yamane, 03.07.10
12  */
13 
14 #include "ik.h"
15 #include <string>
16 
18 {
20  n_constraints = 0;
21  n_total_const = 0;
22  for(int m=0; m<N_PRIORITY_TYPES; m++)
23  n_const[m] = 0;
24  n_all_const = 0;
26  assign_constraints(100);
27 }
28 
30 {
31 #ifdef MEASURE_TIME
32  extern int n_update;
33  extern double calc_jacobian_time;
34  extern double solve_ik_time;
35  extern double high_constraint_time;
36  extern double low_constraint_time;
37  cerr << "n_update = " << n_update << endl;
38  cerr << "calc_jacobian = " << calc_jacobian_time << endl;
39  cerr << "solve_ik = " << solve_ik_time << endl;
40  cerr << "high_constraint = " << high_constraint_time << endl;
41  cerr << "low_constraint = " << low_constraint_time << endl;
42 #endif
43 
44  for(int i=0; i<n_assigned_constraints; i++)
45  {
46  if(constraints[i]) delete constraints[i];
47  }
48  delete[] constraints;
49 }
50 
51 #ifdef SEGA
52 int IK::init()
53 #else
54 int IK::init(SceneGraph* sg)
55 #endif
56 {
57 #ifdef SEGA
58  Chain::init();
59  myinit();
60 #else
61  Chain::init(sg);
62  myinit(sg);
63 #endif
64  return 0;
65 }
66 
67 #ifdef SEGA
68 int IK::myinit()
69 #else
70 int IK::myinit(SceneGraph* sg)
71 #endif
72 {
74  joint_weights = 1.0; // default weights
75 #ifndef SEGA
76  // load markers from sg
77  if(sg)
78  {
79  CalcPosition();
80  Joint* j;
81  for(j=root->child; j; j=j->brother)
82  load_markers(sg, j);
83  }
84 #endif
85  return 0;
86 }
87 
88 #ifndef SEGA
89 int IK::load_markers(SceneGraph* sg, Joint* rj)
90 {
91  char* marker_top_full_name;
92  char* char_name = rj->CharName();
93  if(char_name)
94  {
95  marker_top_full_name = new char [strlen(marker_top_name) + strlen(char_name) + 2];
96  sprintf(marker_top_full_name, "%s%c%s", marker_top_name, charname_separator, char_name);
97  }
98  else
99  {
100  marker_top_full_name = new char [strlen(marker_top_name) + 1];
101  strcpy(marker_top_full_name, marker_top_name);
102  }
103  TransformNode* marker_top = sg->findTransformNode(marker_top_full_name);
104  if(marker_top)
105  load_markers(marker_top);
106  delete[] marker_top_full_name;
107  return 0;
108 }
109 
110 int IK::load_markers(TransformNode* marker_top)
111 {
112  // find first transform node
113  Node* n;
114  for(n=marker_top->getChildNodes(); n && !n->getName(); n=n->getChildNodes());
115  if(!n) return 0;
116  Node* tn;
117  for(tn=n; tn; tn=tn->next())
118  {
119  if(tn->isTransformNode())
120  add_marker((TransformNode*)tn);
121  }
122  return 0;
123 }
124 
125 IKHandle* IK::add_marker(TransformNode* tn)
126 {
127  // analyze name {marker name}_{link name}:{character name}
128  char* fullname = new char [strlen(tn->getName()) + 1];
129  strcpy(fullname, tn->getName());
130  char* ch_start = strrchr(fullname, charname_separator);
131  char* body_name;
132  if(ch_start)
133  {
134  body_name = new char [ch_start - fullname + 1];
135  strncpy(body_name, fullname, ch_start-fullname);
136  body_name[ch_start-fullname] = '\0';
137  }
138  else // no character name
139  {
140  body_name = new char [strlen(fullname) + 1];
141  strcpy(body_name, fullname);
142  }
143  char* j_start = strrchr(body_name, joint_name_separator);
144  if(!j_start)
145  {
146  delete[] body_name;
147  delete[] fullname;
148  return 0;
149  }
150  char* joint_name = new char [strlen(j_start)];
151  strcpy(joint_name, j_start+1);
152 
153  char* marker_name;
154  if(ch_start)
155  {
156  marker_name = new char [j_start - body_name + strlen(ch_start) + 1];
157  strncpy(marker_name, body_name, j_start-body_name);
158  marker_name[j_start-body_name] = charname_separator;
159  marker_name[j_start-body_name+1] = '\0';
160  strcat(marker_name, ch_start+1);
161  }
162  else
163  {
164  marker_name = new char [j_start - body_name + 1];
165  strncpy(marker_name, body_name, j_start-body_name);
166  marker_name[j_start-body_name] = '\0';
167  }
168  Joint* joint;
169  if(ch_start)
170  joint = FindJoint(joint_name, ch_start+1);
171  else
172  joint = FindJoint(joint_name);
173  if(!joint)
174  {
175  delete[] body_name;
176  delete[] fullname;
177  delete[] joint_name;
178  delete[] marker_name;
179  return 0;
180  }
181  // create new fixed joint as a child of joint
182  Joint* new_joint;
183  if(ch_start)
184  {
185 // char* marker_joint_name = new char [strlen(body_name) + strlen(ch_start) + 1];
186 // sprintf(marker_joint_name, "%s%s", body_name, ch_start);
187  char* marker_joint_name = new char [strlen(marker_name) + 1];
188  sprintf(marker_joint_name, "%s", marker_name);
189  new_joint = new Joint(marker_joint_name, JFIXED);
190  delete[] marker_joint_name;
191  }
192  else
193  {
194  new_joint = new Joint(body_name, JFIXED);
195  }
196  if(AddJoint(new_joint, joint))
197  {
198  delete[] body_name;
199  delete[] fullname;
200  delete[] joint_name;
201  delete[] marker_name;
202  delete new_joint;
203  return 0;
204  }
205  // set relative position
206  float fpos[3], fatt[3][3], fscale[3];
207  fVec3 abs_pos;
208  fMat33 abs_att;
209  get_abs_matrix(tn, fpos, fatt, fscale);
210  abs_pos(0) = fpos[0];
211  abs_pos(1) = fpos[1];
212  abs_pos(2) = fpos[2];
213  abs_att(0,0) = fatt[0][0];
214  abs_att(0,1) = fatt[0][1];
215  abs_att(0,2) = fatt[0][2];
216  abs_att(1,0) = fatt[1][0];
217  abs_att(1,1) = fatt[1][1];
218  abs_att(1,2) = fatt[1][2];
219  abs_att(2,0) = fatt[2][0];
220  abs_att(2,1) = fatt[2][1];
221  abs_att(2,2) = fatt[2][2];
222  set_abs_position_orientation(new_joint, abs_pos, abs_att);
223  // add handle constraint
224  ConstIndex cindex[6] = {
231  };
232  IKHandle* h = new IKHandle(this, marker_name, new_joint, cindex, LOW_PRIORITY, 10.0);
233  AddConstraint(h);
234  cerr << "marker " << marker_name << " added to " << joint->name << " (abs_pos = " << abs_pos << ", rel_pos = " << new_joint->rel_pos << ")" << endl;
235 // cerr << " joint pos = " << joint->abs_pos << endl;
236  delete[] body_name;
237  delete[] fullname;
238  delete[] joint_name;
239  delete[] marker_name;
240  return h;
241 }
242 
243 // global version
244 IKHandle* IK::AddMarker(const std::string& label, const std::string& linkname, const std::string& charname, const fVec3& rel_pos)
245 {
246  Joint* pjoint = FindJoint(linkname.c_str(), charname.c_str());
247  if(!pjoint) return 0;
248  std::string fullname = label + charname_separator + charname;
249  Joint* new_joint = new Joint(fullname.c_str(), JFIXED);
250  if(AddJoint(new_joint, pjoint))
251  {
252  delete new_joint;
253  return 0;
254  }
255  new_joint->rel_pos.set(rel_pos);
256  // add handle constraint
257  ConstIndex cindex[6] = {
264  };
265  IKHandle* h = new IKHandle(this, fullname.c_str(), new_joint, cindex, LOW_PRIORITY, 10.0);
266  AddConstraint(h);
267  cerr << "marker " << fullname << " added " << new_joint->rel_pos << endl;
268  return h;
269 }
270 
271 IKHandle* IK::AddMarker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
272 {
273  IKHandle* ret;
274  in_create_chain = true;
275  clear_data();
276  ret = add_marker(marker_name, parent_joint, abs_pos);
277  init(0);
278  in_create_chain = false;
279  return ret;
280 }
281 
282 IKHandle* IK::add_marker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
283 {
284  char* fullname = new char [strlen(marker_name) + strlen(parent_joint->name) + 2];
285  sprintf(fullname, "%s%c%s", marker_name, joint_name_separator, parent_joint->name);
286  TransformNode* tnode = new TransformNode;
287  tnode->setName(fullname);
288  tnode->setTranslation(abs_pos(0), abs_pos(1), abs_pos(2));
289  IKHandle* ret = add_marker(tnode);
290  delete[] fullname;
291  delete tnode;
292  return ret;
293 }
294 
295 int IK::EditMarker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
296 {
297  int ret;
298  in_create_chain = true;
299  clear_data();
300  ret = edit_marker(marker, body_name, parent_joint, abs_pos);
301  init(0);
302  in_create_chain = false;
303  return ret;
304 }
305 
306 int IK::edit_marker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
307 {
308  // change name
309  char* ch_start = strrchr(parent_joint->name, charname_separator);
310  if(marker->joint_name) delete[] marker->joint_name;
311  marker->joint_name = 0;
312  if(ch_start)
313  {
314  marker->joint_name = new char [strlen(body_name) + strlen(ch_start) + 1];
315  sprintf(marker->joint_name, "%s%s", body_name, ch_start);
316  cerr << "new name = " << marker->joint_name << endl;
317  }
318  else
319  {
320  marker->joint_name = new char [strlen(body_name) + 1];
321  strcpy(marker->joint_name, body_name);
322  }
323  marker->joint->SetName(marker->joint_name);
324  // set parent joint
325  if(marker->joint->parent == parent_joint) return 0;
326  // disconnect and connect
327  cerr << "fixed to " << parent_joint->name << endl;
328  RemoveJoint(marker->joint);
329  AddJoint(marker->joint, parent_joint);
330  fMat33 abs_att;
331  abs_att.identity();
332  set_abs_position_orientation(marker->joint, abs_pos, abs_att);
333  return 0;
334 }
335 
336 int IK::SaveMarkers(const char* _fname)
337 {
338  CalcPosition();
339  SceneGraph* sg = new SceneGraph;
340  int i;
341  TransformNode* top_tnode = new TransformNode;
342  top_tnode->setName(marker_top_name);
343  sg->addNode(top_tnode);
344  for(i=0; i<n_constraints; i++)
345  {
346  if(constraints[i]->GetType() == HANDLE_CONSTRAINT)
347  save_marker(top_tnode, (IKHandle*)constraints[i]);
348  }
349  char* fname = new char [strlen(_fname) + 1];
350  strcpy(fname, _fname);
351  sg->save(fname);
352  delete sg;
353  delete[] fname;
354  return 0;
355 }
356 
357 int IK::save_marker(TransformNode* top_tnode, IKHandle* h)
358 {
359  char* char_name = h->joint->CharName();
360  char* t_name;
361  if(char_name)
362  {
363  int marker_name_length = strlen(h->joint_name) - strlen(char_name) - 1;
364  int parent_name_length = strlen(h->joint->parent->name) - strlen(char_name) - 1;
365  char* marker_name = new char [marker_name_length + 1];
366  char* parent_name = new char [parent_name_length + 1];
367  t_name = new char [marker_name_length + parent_name_length + 2];
368  strncpy(marker_name, h->joint_name, marker_name_length);
369  marker_name[marker_name_length] = '\0';
370  strncpy(parent_name, h->joint->parent->name, parent_name_length);
371  parent_name[parent_name_length] = '\0';
372  sprintf(t_name, "%s%c%s", marker_name, joint_name_separator, parent_name);
373  delete[] marker_name;
374  delete[] parent_name;
375  }
376  else
377  {
378  t_name = new char [strlen(h->joint_name) + strlen(h->joint->parent->name) + 2];
379  sprintf(t_name, "%s%c%s", h->joint_name, joint_name_separator, h->joint->parent->name);
380  }
381  CalcPosition();
382  TransformNode* tnode = new TransformNode();
383  tnode->setName(t_name);
384  tnode->setTranslation(h->joint->abs_pos(0), h->joint->abs_pos(1), h->joint->abs_pos(2));
385  top_tnode->addChildNode(tnode);
386  delete[] t_name;
387  return 0;
388 }
389 
390 #endif
391 
393 {
394  int count = 0;
395  for(int i=0; i<n_constraints; i++)
396  {
397  if(constraints[i]->GetType() == t) count++;
398  }
399  return count;
400 }
401 
403 {
404  cerr << "assign_constraints(" << _n << ")" << endl;
405  int i;
406  IKConstraint** save = 0;
408  {
409  save = new IKConstraint* [n_assigned_constraints];
410  for(i=0; i<n_assigned_constraints; i++)
411  save[i] = constraints[i];
412  delete[] constraints;
413  }
414  constraints = 0;
415  if(_n > 0)
416  {
417  constraints = new IKConstraint* [_n];
418  for(i=0; i<_n; i++) constraints[i] = 0;
419  for(i=0; i<n_assigned_constraints; i++)
420  constraints[i] = save[i];
421  }
423  if(save) delete[] save;
424  cerr << "<-" << endl;
425  return 0;
426 }
427 
429 {
430 // if(!_constraint->joint) return -1;
433  int id = n_total_const++;
434  constraints[n_constraints++] = _constraint;
435  _constraint->id = id;
436  return id;
437 }
438 
440 {
441  int index = ConstraintIndex(_id);
442  if(index < 0) return -1;
443  if(constraints[index]) delete constraints[index];
444  constraints[index] = 0;
445  int i;
446  for(i=index+1; i<n_constraints; i++)
447  {
448  constraints[i-1] = constraints[i];
449  }
450  constraints[--n_constraints] = 0;
451  return 0;
452 }
453 
455 {
456  for(int i=0; i<n_constraints; i++)
457  {
458  if(constraints[i]) delete constraints[i];
459  constraints[i] = 0;
460  }
461  n_constraints = 0;
462  return 0;
463 }
464 
465 IKConstraint* IK::FindConstraint(ConstType _type, const char* jname, const char* charname)
466 {
467  static char fullname[256];
468  if(charname) {
469  sprintf(fullname, "%s%c%s", jname, charname_separator, charname);
470  }
471  else {
472  strcpy(fullname, jname);
473  }
474  for(int i=0; i<n_constraints; i++)
475  {
476  if(constraints[i]->GetType() == _type &&
477  !strcmp(fullname, constraints[i]->joint_name))
478  return constraints[i];
479  }
480  return 0;
481 }
482 
484 {
485  for(int i=0; i<n_constraints; i++)
486  {
487  if(constraints[i]->id == _id)
488  return constraints[i];
489  }
490  return 0;
491 }
492 
493 int IK::ConstraintID(ConstType _type, const char* jname)
494 {
495  for(int i=0; i<n_constraints; i++)
496  {
497  if(constraints[i]->GetType() == _type &&
498  !strcmp(jname, constraints[i]->joint_name))
499  return constraints[i]->id;
500  }
501  return -1;
502 }
503 
504 int IK::ConstraintID(int _index)
505 {
506  if(_index < 0 || _index >= n_constraints)
507  return -1;
508  return constraints[_index]->id;
509 }
510 
511 int IK::ConstraintIndex(ConstType _type, const char* jname)
512 {
513  for(int i=0; i<n_constraints; i++)
514  {
515  if(constraints[i]->GetType() == _type &&
516  !strcmp(jname, constraints[i]->joint_name))
517  return i;
518  }
519  return -1;
520 }
521 
523 {
524  for(int i=0; i<n_constraints; i++)
525  {
526  if(constraints[i]->id == _id)
527  return i;
528  }
529  return -1;
530 }
531 
533 {
534  int i;
535  for(i=0; i<n_constraints; i++)
536  constraints[i]->Reset();
537  return 0;
538 }
539 
541 {
542  int i;
543  for(i=0; i<n_constraints; i++)
544  {
545  if(constraints[i]->GetType() == t)
546  constraints[i]->Reset();
547  }
548  return 0;
549 }
550 
551 int IK::SetJointWeight(const char* jname, double _weight)
552 {
553  double weight = _weight;
554  if(weight < MIN_JOINT_WEIGHT) weight = MIN_JOINT_WEIGHT;
555  Joint* jnt = FindJoint(jname);
556  if(!jnt) return -1;
557  int i;
558  for(i=0; i<jnt->n_dof; i++)
559  {
560  joint_weights(i+jnt->i_dof) = weight;
561  }
562  return 0;
563 }
564 
565 int IK::SetJointWeight(const char* jname, const fVec& _weight)
566 {
567  Joint* jnt = FindJoint(jname);
568  if(!jnt) return -1;
569  int i;
570  for(i=0; i<jnt->n_dof && i<_weight.size(); i++)
571  {
572  if(_weight(i) >= MIN_JOINT_WEIGHT)
573  joint_weights(i+jnt->i_dof) = _weight(i);
574  }
575  return 0;
576 }
577 
578 int IK::SetDesireGain(const char* jname, double _gain)
579 {
580  if(_gain < 0.0) return -1;
582  if(!des) return -1;
583  des->gain = _gain;
584  return 0;
585 }
586 
587 int IK::EnableDesire(const char* jname)
588 {
590  if(!des) return -1;
591  des->Enable();
592  cerr << "desire constraint at " << jname << " enabled" << endl;
593  return 0;
594 }
595 
596 int IK::DisableDesire(const char* jname)
597 {
599  if(!des) return -1;
600  des->Disable();
601  cerr << "desire constraint at " << jname << " disabled" << endl;
602  return 0;
603 }
604 
605 void IK::SetCharacterScale(double _scale, const char* charname)
606 {
607  SetConstraintScale(_scale, charname);
608  // set joint weights
610  joint_weights = 1.0;
611  set_character_scale(root, _scale, charname);
612 }
613 
614 void IK::SetConstraintScale(double _scale, const char* charname)
615 {
616  int i;
617  for(i=0; i<n_constraints; i++)
618  {
619  constraints[i]->SetCharacterScale(_scale, charname);
620  }
621 }
622 
623 void IK::set_character_scale(Joint* jnt, double _scale, const char* charname)
624 {
625  if(!jnt) return;
626  if((!charname || strstr(jnt->name, charname)) && jnt->n_dof > 0)
627  {
628  // rotation -> s
629  switch(jnt->j_type)
630  {
631  case JROTATE:
632  joint_weights(jnt->i_dof) = _scale;
633  break;
634  case JSPHERE:
635  joint_weights(jnt->i_dof) = _scale;
636  joint_weights(jnt->i_dof+1) = _scale;
637  joint_weights(jnt->i_dof+2) = _scale;
638  break;
639  case JFREE:
640  joint_weights(jnt->i_dof+3) = _scale;
641  joint_weights(jnt->i_dof+4) = _scale;
642  joint_weights(jnt->i_dof+5) = _scale;
643  break;
644  default:
645  break;
646  }
647  }
648  set_character_scale(jnt->brother, _scale, charname);
649  set_character_scale(jnt->child, _scale, charname);
650 }
651 
652 void IKHandle::SetCharacterScale(double _scale, const char* charname)
653 {
654  if(!joint) return;
655  if(n_const == 0) return;
656  if(charname && !strstr(joint->name, charname)) return; // not a strict check
657  int i;
659  weight = 1.0;
660  int count = 0;
661 // double s1 = 1.0/_scale;
662  double s1 = _scale;
663  for(i=0; i<6; i++)
664  {
665  if(const_index[i] == IK::HAVE_CONSTRAINT)
666  {
667  // rotation -> 1/s
668  if(i >= 3) weight(count) = s1;
669  count++;
670  }
671  }
672 }
673 
674 void IKDesire::SetCharacterScale(double _scale, const char* charname)
675 {
676  if(!joint) return;
677  if(n_const == 0) return;
678  if(charname && !strstr(joint->name, charname)) return; // not a strict check
680  if(_scale < 1.0)
681  weight = _scale;
682  else
683  weight = 1.0;
684 #if 0
685 // double s1 = 1.0/_scale;
686  double s1 = _scale;
687  // rotation -> 1/s
688  switch(joint->j_type)
689  {
690  case JROTATE:
691  weight(0) = s1;
692  break;
693  case JSPHERE:
694  weight(0) = s1;
695  weight(1) = s1;
696  weight(2) = s1;
697  break;
698  case JFREE:
699  weight(3) = s1;
700  weight(4) = s1;
701  weight(5) = s1;
702  break;
703  default:
704  break;
705  }
706 #endif
707 }
int ConstraintIndex(ConstType _type, const char *jname)
Definition: ik.cpp:511
int n_const[N_PRIORITY_TYPES]
number of constraints of each type
Definition: ik.h:238
int SetDesireGain(const char *jname, double _gain)
Set gain of desire constraint of the specific joint.
Definition: ik.cpp:578
int n_constraints
number of current constraints
Definition: ik.h:224
Base class for constraints.
Definition: ik.h:253
3x3 matrix class.
Definition: fMatrix3.h:29
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: init.cpp:22
IK()
Default constructor.
Definition: ik.cpp:17
int SetJointWeight(const char *jname, double _weight)
Set joint weight, for single-DOF joints.
Definition: ik.cpp:551
void Enable()
enable the constraint
Definition: ik.h:298
int edit_marker(IKHandle *marker, const char *body_name, Joint *parent_joint, const fVec3 &abs_pos)
Definition: ik.cpp:306
Joint * child
pointer to the child joint
Definition: chain.h:685
png_voidp s1
Definition: png.h:2110
IKHandle * AddMarker(const std::string &label, const std::string &linkname, const std::string &charname, const fVec3 &rel_pos)
Add new marker constraint.
Definition: ik.cpp:244
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
int init(SceneGraph *sg)
Initialize the parameters.
Definition: ik.cpp:54
void SetName(const char *_name, const char *_charname=0)
Change the joint name.
Definition: chain.h:655
void SetCharacterScale(double _scale, const char *charname=0)
Definition: ik.cpp:674
int ResetAllConstraints()
Reset all constraints.
Definition: ik.cpp:532
int RemoveAllConstraints()
Definition: ik.cpp:454
Position constraint.
Definition: ik.h:416
void SetCharacterScale(double _scale, const char *charname=0)
Definition: ik.cpp:652
int in_create_chain
true if between BeginCreateChain() and EndCreateChain().
Definition: chain.h:473
IKConstraint ** constraints
list of current constraints
Definition: ik.h:230
int AddConstraint(IKConstraint *_constraint)
Definition: ik.cpp:428
int RemoveJoint(Joint *j)
disconnect joint j from its parent
Definition: edit.cpp:32
IKConstraint * FindConstraint(ConstType _type, const char *jname, const char *charname=0)
Definition: ik.cpp:465
Inverse kinematics (UTPoser) class.
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
Desired joint values.
Definition: ik.h:555
Joint * root
Chain information.
Definition: chain.h:466
void set_character_scale(Joint *jnt, double _scale, const char *charname)
Definition: ik.cpp:623
void SetConstraintScale(double _scale, const char *charname=0)
Set constraint scale parameter.
Definition: ik.cpp:614
double gain
priority
Definition: ik.h:396
void identity()
Identity matrix.
Definition: fMatrix3.cpp:230
IKHandle * add_marker(const char *marker_name, Joint *parent_joint, const fVec3 &abs_pos)
Definition: ik.cpp:282
int id
ID (unique to each constraint)
Definition: ik.h:393
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
png_uint_32 i
Definition: png.h:2735
static char charname_separator
Definition: dims_common.h:19
with constraint
Definition: ik.h:67
char * joint_name
Definition: ik.h:392
int SaveMarkers(const char *fname)
Definition: ik.cpp:336
spherical (3DOF)
Definition: chain.h:43
int n_all_const
Definition: ik.h:239
without constraint
Definition: ik.h:68
fVec joint_weights
joint weights
Definition: ik.h:245
fVec3 rel_pos
(initial) position in parent joint&#39;s frame (for 0/3/6 DOF joints)
Definition: chain.h:700
void CalcPosition()
Forward kinematics.
Definition: fk.cpp:16
int marker
Definition: jpeglib.h:950
int assign_constraints(int _n)
Definition: ik.cpp:402
int i_dof
index in all DOF
Definition: chain.h:720
desired joint values
Definition: ik.h:49
Joint * joint
target joint
Definition: ik.h:391
int n_dof
Definition: chain.h:468
def j(str, encoding="cp932")
JointType j_type
joint type
Definition: chain.h:694
list index
void SetCharacterScale(double _scale, const char *charname=0)
Set character scale parameter.
Definition: ik.cpp:605
char * CharName() const
Returns the character name.
Definition: chain.h:648
char * name
joint name (including the character name)
Definition: chain.h:691
#define MIN_JOINT_WEIGHT
Definition: ik.h:27
virtual int Reset()
Definition: ik.h:293
lower priority
Definition: ik.h:60
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
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
double max_condnum
maximum condition number
Definition: ik.h:242
~IK()
Destructor.
Definition: ik.cpp:29
int RemoveConstraint(int _id)
Definition: ik.cpp:439
ConstIndex
Definition: ik.h:66
int myinit(SceneGraph *sg)
Definition: ik.cpp:70
The class representing the whole mechanism. May contain multiple characters.
Definition: chain.h:144
static const char joint_name_separator
Definition: ik.h:31
std::string sprintf(char const *__restrict fmt,...)
int NumConstraints()
Number of constraints.
Definition: ik.h:77
friend class Joint
Definition: chain.h:146
position/orientation of link
Definition: ik.h:48
virtual int clear_data()
Clear arrays only; don&#39;t delete joints.
Definition: vary.cpp:127
virtual void SetCharacterScale(double _scale, const char *charname=0)
Definition: ik.h:355
int n_total_const
number of total constraints used so far, including removed ones
Definition: ik.h:227
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
int load_markers(SceneGraph *sg, Joint *rj)
Definition: ik.cpp:89
rotational (1DOF)
Definition: chain.h:41
fVec3 abs_pos
absolute position
Definition: chain.h:741
int n_assigned_constraints
Definition: ik.h:232
The class for representing a joint.
Definition: chain.h:538
int set_abs_position_orientation(Joint *jnt, const fVec3 &abs_pos, const fMat33 &abs_att)
Definition: joint.cpp:396
int ResetConstraints(ConstType t)
Reset the constraints with the specific type.
Definition: ik.cpp:540
int DisableDesire(const char *jname)
Disable desire constraint of the specific joint.
Definition: ik.cpp:596
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
int EditMarker(IKHandle *marker, const char *body_name, Joint *parent_joint, const fVec3 &abs_pos=0.0)
Definition: ik.cpp:295
int EnableDesire(const char *jname)
Enable desire constraint of the specific joint.
Definition: ik.cpp:587
int save_marker(TransformNode *top_tnode, IKHandle *h)
Definition: ik.cpp:357
fVec weight[N_PRIORITY_TYPES]
weight of each type
Definition: ik.h:237
Joint * parent
pointer to the parent joint
Definition: chain.h:683
void Disable()
disable the constraint
Definition: ik.h:302
free (6DOF)
Definition: chain.h:44
ConstType
Definition: ik.h:47
friend class IKHandle
Definition: ik.h:41
#define MAX_CONDITION_NUMBER
Definition: ik.h:29
int ConstraintID(ConstType _type, const char *jname)
Definition: ik.cpp:493


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:23