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;
73 joint_weights.resize(n_dof);
91 char* marker_top_full_name;
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);
100 marker_top_full_name =
new char [strlen(marker_top_name) + 1];
101 strcpy(marker_top_full_name, marker_top_name);
103 TransformNode* marker_top = sg->findTransformNode(marker_top_full_name);
106 delete[] marker_top_full_name;
114 for(
n=marker_top->getChildNodes();
n && !
n->getName();
n=
n->getChildNodes());
117 for(tn=
n; tn; tn=tn->next())
119 if(tn->isTransformNode())
128 char* fullname =
new char [strlen(tn->getName()) + 1];
129 strcpy(fullname, tn->getName());
134 body_name =
new char [ch_start - fullname + 1];
135 strncpy(body_name, fullname, ch_start-fullname);
136 body_name[ch_start-fullname] =
'\0';
140 body_name =
new char [strlen(fullname) + 1];
141 strcpy(body_name, fullname);
150 char* joint_name =
new char [strlen(j_start)];
151 strcpy(joint_name, j_start+1);
156 marker_name =
new char [j_start - body_name + strlen(ch_start) + 1];
157 strncpy(marker_name, body_name, j_start-body_name);
159 marker_name[j_start-body_name+1] =
'\0';
160 strcat(marker_name, ch_start+1);
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';
170 joint =
FindJoint(joint_name, ch_start+1);
178 delete[] marker_name;
187 char* marker_joint_name =
new char [strlen(marker_name) + 1];
188 sprintf(marker_joint_name,
"%s", marker_name);
190 delete[] marker_joint_name;
201 delete[] marker_name;
206 float fpos[3], fatt[3][3], fscale[3];
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];
234 cerr <<
"marker " << marker_name <<
" added to " << joint->
name <<
" (abs_pos = " << abs_pos <<
", rel_pos = " << new_joint->
rel_pos <<
")" << endl;
239 delete[] marker_name;
247 if(!pjoint)
return 0;
267 cerr <<
"marker " << fullname <<
" added " << new_joint->
rel_pos << endl;
276 ret =
add_marker(marker_name, parent_joint, abs_pos);
284 char* fullname =
new char [strlen(marker_name) + strlen(parent_joint->
name) + 2];
286 TransformNode* tnode =
new TransformNode;
287 tnode->setName(fullname);
288 tnode->setTranslation(abs_pos(0), abs_pos(1), abs_pos(2));
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;
320 marker->joint_name =
new char [strlen(body_name) + 1];
321 strcpy(
marker->joint_name, body_name);
325 if(
marker->joint->parent == parent_joint)
return 0;
327 cerr <<
"fixed to " << parent_joint->
name << endl;
339 SceneGraph* sg =
new SceneGraph;
341 TransformNode* top_tnode =
new TransformNode;
342 top_tnode->setName(marker_top_name);
343 sg->addNode(top_tnode);
349 char* fname =
new char [strlen(_fname) + 1];
350 strcpy(fname, _fname);
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';
371 parent_name[parent_name_length] =
'\0';
373 delete[] marker_name;
374 delete[] parent_name;
382 TransformNode* tnode =
new TransformNode();
383 tnode->setName(t_name);
385 top_tnode->addChildNode(tnode);
404 cerr <<
"assign_constraints(" << _n <<
")" << endl;
423 if(save)
delete[] save;
424 cerr <<
"<-" << endl;
435 _constraint->
id = id;
442 if(index < 0)
return -1;
467 static char fullname[256];
472 strcpy(fullname, jname);
580 if(_gain < 0.0)
return -1;
592 cerr <<
"desire constraint at " << jname <<
" enabled" << endl;
601 cerr <<
"desire constraint at " << jname <<
" disabled" << endl;
626 if((!charname || strstr(jnt->
name, charname)) && jnt->
n_dof > 0)
656 if(charname && !strstr(
joint->
name, charname))
return;
678 if(charname && !strstr(
joint->
name, charname))
return;