vary.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  */
15 #include "chain.h"
16 
17 int Chain::SetCharacterTorqueGiven(const char* charname, int _tg)
18 {
19  Joint* r = FindCharacterRoot(charname);
20  if(!r) return -1;
21  in_create_chain = true;
22  clear_data();
23  // assumes that this change should not affect the character root
24  set_all_torque_given(r->child, _tg);
25 #ifdef SEGA
26  init();
27 #else
28  init(0);
29 #endif
30  in_create_chain = false;
31  return 0;
32 }
33 
34 int Chain::SetTorqueGiven(Joint* _joint, int _tg)
35 {
36  if(!_joint) return -1;
37  if(_joint->t_given == _tg) return 0;
38  in_create_chain = true;
39  clear_data();
40  _joint->t_given = _tg;
41  if(_tg)
42  {
43  _joint->n_dof = _joint->n_thrust;
44  _joint->n_thrust = 0;
45  }
46  else
47  {
48  _joint->n_thrust = _joint->n_dof;
49  _joint->n_dof = 0;
50  }
51 #ifdef SEGA
52  init();
53 #else
54  init(0);
55 #endif
56  in_create_chain = false;
57  return 0;
58 }
59 
61 {
62  in_create_chain = true;
63  clear_data();
65 #ifdef SEGA
66  init();
67 #else
68  init(0);
69 #endif
70  in_create_chain = false;
71  return 0;
72 }
73 
75 {
76  if(!cur) return;
77  if(cur->t_given != _tg)
78  {
79  cur->t_given = _tg;
80  if(_tg)
81  {
82  cur->n_dof = cur->n_thrust;
83  cur->n_thrust = 0;
84  }
85  else
86  {
87  cur->n_thrust = cur->n_dof;
88  cur->n_dof = 0;
89  }
90  }
91  set_all_torque_given(cur->brother, _tg);
92  set_all_torque_given(cur->child, _tg);
93 }
94 
95 int Chain::Connect(Joint* virtual_joint, Joint* parent_joint)
96 {
97  if(!virtual_joint->realname) return -1;
98  in_create_chain = true;
99  clear_data();
100  AddJoint(virtual_joint, parent_joint);
101 #ifdef SEGA
102  init();
103 #else
104  init(0);
105 #endif
106  in_create_chain = false;
107  do_connect = true;
108  return 0;
109 }
110 
112 {
113  if(!j->realname) return -1; // can only disconnect virtual joints
114  in_create_chain = true;
115  clear_data();
116  RemoveJoint(j);
117  delete j;
118 #ifdef SEGA
119  init();
120 #else
121  init(0);
122 #endif
123  in_create_chain = false;
124  return 0;
125 }
126 
128 {
129  // reset pointers
130  if(all_value) delete[] all_value;
131  if(all_value_dot) delete[] all_value_dot;
132  if(all_vel) delete[] all_vel;
133  if(all_vel_dot) delete[] all_vel_dot;
134  if(j_acc_p[0]) delete[] j_acc_p[0];
135  if(j_acc_p[1]) delete[] j_acc_p[1];
136  if(j_acc_p[2]) delete[] j_acc_p[2];
137  if(j_acc_p[3]) delete[] j_acc_p[3];
138  if(j_value_dot[0]) delete[] j_value_dot[0];
139  if(j_value_dot[1]) delete[] j_value_dot[1];
140  if(j_value_dot[2]) delete[] j_value_dot[2];
141  if(j_value_dot[3]) delete[] j_value_dot[3];
142  if(init_value) delete[] init_value;
143  if(init_vel) delete[] init_vel;
144  n_value = 0;
145  n_dof = 0;
146  n_thrust = 0;
147  n_joint = 0;
148  all_value = 0;
149  all_value_dot = 0;
150  all_vel = 0;
151  all_vel_dot = 0;
152  j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
153  j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
154  init_value = 0;
155  init_vel = 0;
156  if(root) root->clear_data();
157  return 0;
158 }
159 
161 {
162  if(!this) return;
163  n_root_dof = 0;
164  i_value = -1;
165  i_dof = -1;
166  i_thrust = -1;
167  i_joint = -1;
168  brother->clear_data();
169  child->clear_data();
170 }
char * realname
name of the real joint (for closed chains)
Definition: chain.h:693
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: init.cpp:22
Joint * child
pointer to the child joint
Definition: chain.h:685
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
void clear_data()
Definition: vary.cpp:160
int t_given
torque or motion controlled
Definition: chain.h:709
int n_thrust
DOF for motion controlled joints.
Definition: chain.h:716
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
double * init_value
Definition: chain.h:462
Joint * root
Chain information.
Definition: chain.h:466
int Disconnect(Joint *j)
Disconnect the loop at the specified virtual joint.
Definition: vary.cpp:111
double * j_value_dot[4]
for 4-th order Runge-Kutta
Definition: chain.h:460
int n_joint
Definition: chain.h:469
int n_thrust
total DOF of the joints with t_given = false
Definition: chain.h:470
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
void set_all_torque_given(Joint *cur, int _tg)
Definition: vary.cpp:74
int SetTorqueGiven(Joint *_joint, int _tg)
Change torque/motion control property of a joint.
Definition: vary.cpp:34
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
int SetCharacterTorqueGiven(const char *charname, int _tg)
Change torque/motion control property of a character.
Definition: vary.cpp:17
Joint * brother
pointer to the brother joint
Definition: chain.h:684
double * init_vel
Definition: chain.h:463
int AddJoint(Joint *target, Joint *p)
Add a new joint target as a child of joint p.
Definition: edit.cpp:117
Classes for defining open/closed kinematic chains.
double ** all_value_dot
Definition: chain.h:456
int Connect(Joint *virtual_joint, Joint *parent_joint)
Connect two links by adding a new virtual joint.
Definition: vary.cpp:95
int SetAllTorqueGiven(int _tg)
Change torque/motion control property of all joints.
Definition: vary.cpp:60
double ** all_vel
Definition: chain.h:457
virtual int clear_data()
Clear arrays only; don't delete joints.
Definition: vary.cpp:127
double * j_acc_p[4]
Definition: chain.h:461
The class for representing a joint.
Definition: chain.h:538
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:05