00001 /* 00002 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc. 00003 * All rights reserved. This program is made available under the terms of the 00004 * Eclipse Public License v1.0 which accompanies this distribution, and is 00005 * available at http://www.eclipse.org/legal/epl-v10.html 00006 * Contributors: 00007 * The University of Tokyo 00008 */ 00015 #include "chain.h" 00016 00017 int Chain::SetCharacterTorqueGiven(const char* charname, int _tg) 00018 { 00019 Joint* r = FindCharacterRoot(charname); 00020 if(!r) return -1; 00021 in_create_chain = true; 00022 clear_data(); 00023 // assumes that this change should not affect the character root 00024 set_all_torque_given(r->child, _tg); 00025 #ifdef SEGA 00026 init(); 00027 #else 00028 init(0); 00029 #endif 00030 in_create_chain = false; 00031 return 0; 00032 } 00033 00034 int Chain::SetTorqueGiven(Joint* _joint, int _tg) 00035 { 00036 if(!_joint) return -1; 00037 if(_joint->t_given == _tg) return 0; 00038 in_create_chain = true; 00039 clear_data(); 00040 _joint->t_given = _tg; 00041 if(_tg) 00042 { 00043 _joint->n_dof = _joint->n_thrust; 00044 _joint->n_thrust = 0; 00045 } 00046 else 00047 { 00048 _joint->n_thrust = _joint->n_dof; 00049 _joint->n_dof = 0; 00050 } 00051 #ifdef SEGA 00052 init(); 00053 #else 00054 init(0); 00055 #endif 00056 in_create_chain = false; 00057 return 0; 00058 } 00059 00060 int Chain::SetAllTorqueGiven(int _tg) 00061 { 00062 in_create_chain = true; 00063 clear_data(); 00064 set_all_torque_given(root, _tg); 00065 #ifdef SEGA 00066 init(); 00067 #else 00068 init(0); 00069 #endif 00070 in_create_chain = false; 00071 return 0; 00072 } 00073 00074 void Chain::set_all_torque_given(Joint* cur, int _tg) 00075 { 00076 if(!cur) return; 00077 if(cur->t_given != _tg) 00078 { 00079 cur->t_given = _tg; 00080 if(_tg) 00081 { 00082 cur->n_dof = cur->n_thrust; 00083 cur->n_thrust = 0; 00084 } 00085 else 00086 { 00087 cur->n_thrust = cur->n_dof; 00088 cur->n_dof = 0; 00089 } 00090 } 00091 set_all_torque_given(cur->brother, _tg); 00092 set_all_torque_given(cur->child, _tg); 00093 } 00094 00095 int Chain::Connect(Joint* virtual_joint, Joint* parent_joint) 00096 { 00097 if(!virtual_joint->realname) return -1; 00098 in_create_chain = true; 00099 clear_data(); 00100 AddJoint(virtual_joint, parent_joint); 00101 #ifdef SEGA 00102 init(); 00103 #else 00104 init(0); 00105 #endif 00106 in_create_chain = false; 00107 do_connect = true; 00108 return 0; 00109 } 00110 00111 int Chain::Disconnect(Joint* j) 00112 { 00113 if(!j->realname) return -1; // can only disconnect virtual joints 00114 in_create_chain = true; 00115 clear_data(); 00116 RemoveJoint(j); 00117 delete j; 00118 #ifdef SEGA 00119 init(); 00120 #else 00121 init(0); 00122 #endif 00123 in_create_chain = false; 00124 return 0; 00125 } 00126 00127 int Chain::clear_data() 00128 { 00129 // reset pointers 00130 if(all_value) delete[] all_value; 00131 if(all_value_dot) delete[] all_value_dot; 00132 if(all_vel) delete[] all_vel; 00133 if(all_vel_dot) delete[] all_vel_dot; 00134 if(j_acc_p[0]) delete[] j_acc_p[0]; 00135 if(j_acc_p[1]) delete[] j_acc_p[1]; 00136 if(j_acc_p[2]) delete[] j_acc_p[2]; 00137 if(j_acc_p[3]) delete[] j_acc_p[3]; 00138 if(j_value_dot[0]) delete[] j_value_dot[0]; 00139 if(j_value_dot[1]) delete[] j_value_dot[1]; 00140 if(j_value_dot[2]) delete[] j_value_dot[2]; 00141 if(j_value_dot[3]) delete[] j_value_dot[3]; 00142 if(init_value) delete[] init_value; 00143 if(init_vel) delete[] init_vel; 00144 n_value = 0; 00145 n_dof = 0; 00146 n_thrust = 0; 00147 n_joint = 0; 00148 all_value = 0; 00149 all_value_dot = 0; 00150 all_vel = 0; 00151 all_vel_dot = 0; 00152 j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0; 00153 j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0; 00154 init_value = 0; 00155 init_vel = 0; 00156 if(root) root->clear_data(); 00157 return 0; 00158 } 00159 00160 void Joint::clear_data() 00161 { 00162 if(!this) return; 00163 n_root_dof = 0; 00164 i_value = -1; 00165 i_dof = -1; 00166 i_thrust = -1; 00167 i_joint = -1; 00168 brother->clear_data(); 00169 child->clear_data(); 00170 }