vary.cpp
Go to the documentation of this file.
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:57