00001 #ifndef MOTIONCONTROLS_H 00002 #define MOTIONCONTROLS_H 00003 00004 class PartControl : public controlable { 00005 public: 00006 PartControl() : partFileType(0), static_(0), rigid_(1) {} 00007 00008 virtual ~PartControl() {} 00009 00010 virtual void register_at(ControlDevice& Ctrl) {} 00011 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00012 PLocCtrl = Ctrl.getSubDevice(prefix); 00013 RegisterAt(PLocCtrl,"Name",partName); 00014 RegisterAt(PLocCtrl,"FileName",partFileName); 00015 RegisterAt(PLocCtrl,"FileType",partFileType); 00016 RegisterAt(PLocCtrl,"Static",static_); 00017 RegisterAt(PLocCtrl,"Rigid",rigid_); 00018 } 00019 00020 public: 00021 00022 std::string partName, partFileName; 00023 int partFileType; 00024 bool static_, rigid_; 00025 ControlDevice PLocCtrl; 00026 }; 00027 00028 class AssemblyControl : public controlable { 00029 public: 00030 AssemblyControl() : numParts(1), numSubAssemblies(0) {} 00031 00032 virtual ~AssemblyControl() {} 00033 00034 virtual void register_at(ControlDevice& Ctrl) {} 00035 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00036 ALocCtrl = Ctrl.getSubDevice(prefix); 00037 RegisterAt(ALocCtrl,"Name",assemblyName); 00038 RegisterAt(ALocCtrl,"NumParts",numParts); 00039 RegisterAt(ALocCtrl,"NumSubAssemblies",numSubAssemblies); 00040 00041 for (register int j=0; j < maxParts; j++) { 00042 char VariableName1[16]; 00043 std::sprintf(VariableName1,"Part(%02d)",j+1); 00044 PartCtrl[j].register_at(ALocCtrl,std::string(VariableName1)); 00045 } 00046 } 00047 00048 public: 00049 00050 std::string assemblyName; 00051 int numParts, numSubAssemblies; 00052 PartControl PartCtrl[maxParts]; 00053 AssemblyControl * AssemblyCtrl[maxSubAssemblies]; 00054 ControlDevice ALocCtrl; 00055 }; 00056 00057 00058 class JointControl : public controlable { 00059 public: 00060 JointControl() : static_(0) { 00061 origin(0.); 00062 zaxis(0.); 00063 xaxis(0.); 00064 cfId.resize(10,-1); 00065 } 00066 00067 virtual ~JointControl() {} 00068 00069 virtual void register_at(ControlDevice& Ctrl) {} 00070 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00071 JLocCtrl = Ctrl.getSubDevice(prefix); 00072 RegisterAt(JLocCtrl,"Type",jointType); 00073 RegisterAt(JLocCtrl,"Static",static_); 00074 RegisterAt(JLocCtrl,"o(0)",origin(0)); 00075 RegisterAt(JLocCtrl,"o(1)",origin(1)); 00076 RegisterAt(JLocCtrl,"o(2)",origin(2)); 00077 RegisterAt(JLocCtrl,"z(0)",zaxis(0)); 00078 RegisterAt(JLocCtrl,"z(1)",zaxis(1)); 00079 RegisterAt(JLocCtrl,"z(2)",zaxis(2)); 00080 RegisterAt(JLocCtrl,"x(0)",xaxis(0)); 00081 RegisterAt(JLocCtrl,"x(1)",xaxis(1)); 00082 RegisterAt(JLocCtrl,"x(2)",xaxis(2)); 00083 00084 for (register int i = 0; i < 10; i++) { 00085 char VariableName1[16]; 00086 std::sprintf(VariableName1,"Id(%02d)",i+1); 00087 RegisterAt(JLocCtrl,std::string(VariableName1),cfId[i]); 00088 } 00089 } 00090 00091 public: 00092 00093 std::string partName, partFileName; 00094 int jointType; 00095 PType origin, zaxis, xaxis; 00096 bool static_; 00097 std::vector<int> cfId; 00098 ControlDevice JLocCtrl; 00099 }; 00100 00101 class LinkControl : public controlable { 00102 public: 00103 LinkControl() : numJoints(2), r(0.), a(0.), d(0.), t(0.) {} 00104 00105 virtual ~LinkControl() {} 00106 00107 virtual void register_at(ControlDevice& Ctrl) {} 00108 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00109 LLocCtrl = Ctrl.getSubDevice(prefix); 00110 RegisterAt(LLocCtrl,"NumJoints",numJoints); 00111 RegisterAt(LLocCtrl,"Parent",JointIds[0]); 00112 RegisterAt(LLocCtrl,"Child",JointIds[1]); 00113 RegisterAt(LLocCtrl,"Radius",r); 00114 RegisterAt(LLocCtrl,"Alpha",a); 00115 RegisterAt(LLocCtrl,"Depth",d); 00116 RegisterAt(LLocCtrl,"Theta",t); 00117 } 00118 00119 public: 00120 00121 std::string assemblyName; 00122 int numJoints; 00123 int JointIds[maxJoints]; 00124 DataType r, a, d, t; 00125 ControlDevice LLocCtrl; 00126 }; 00127 00128 00129 class ChainLinkControl : public controlable { 00130 public: 00131 ChainLinkControl() : numJoints(2), q_pre(0.), q_dot_pre(0.) {} 00132 00133 virtual ~ChainLinkControl() {} 00134 00135 virtual void register_at(ControlDevice& Ctrl) {} 00136 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00137 CLLocCtrl = Ctrl.getSubDevice(prefix); 00138 RegisterAt(CLLocCtrl,"Id",id); 00139 RegisterAt(CLLocCtrl,"NumJoints",numJoints); 00140 RegisterAt(CLLocCtrl,"ConstantPosition",constPos); 00141 RegisterAt(CLLocCtrl,"Position",q_pre); 00142 RegisterAt(CLLocCtrl,"ConstantVelocity",constVel); 00143 RegisterAt(CLLocCtrl,"Velocity",q_dot_pre); 00144 00145 } 00146 00147 public: 00148 std::string assemblyName; 00149 int numJoints, id; 00150 int constPos, constVel; 00151 DataType q_pre, q_dot_pre; 00152 ControlDevice CLLocCtrl; 00153 }; 00154 00155 class ChainTernLinkControl : public controlable { 00156 public: 00157 ChainTernLinkControl() : numJoints(2), q_pre(0.), q_dot_pre(0.) {} 00158 00159 virtual ~ChainTernLinkControl() {} 00160 00161 virtual void register_at(ControlDevice& Ctrl) {} 00162 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00163 CLLocCtrl = Ctrl.getSubDevice(prefix); 00164 RegisterAt(CLLocCtrl,"Id",id); 00165 RegisterAt(CLLocCtrl,"NumJoints",numJoints); 00166 RegisterAt(CLLocCtrl,"CLink",cLink); 00167 RegisterAt(CLLocCtrl,"NLink",nLink); 00168 RegisterAt(CLLocCtrl,"ConstantPosition",constPos); 00169 RegisterAt(CLLocCtrl,"Position",q_pre); 00170 RegisterAt(CLLocCtrl,"ConstantVelocity",constVel); 00171 RegisterAt(CLLocCtrl,"Velocity",q_dot_pre); 00172 } 00173 00174 public: 00175 00176 std::string assemblyName; 00177 int numJoints, id, cLink, nLink; 00178 int constPos, constVel; 00179 DataType q_pre, q_dot_pre; 00180 ControlDevice CLLocCtrl; 00181 }; 00182 00183 class ChainNativeLinkControl : public controlable { 00184 public: 00185 ChainNativeLinkControl() : numJoints(2), q_pre(0.), q_dot_pre(0.) {} 00186 00187 virtual ~ChainNativeLinkControl() {} 00188 00189 virtual void register_at(ControlDevice& Ctrl) {} 00190 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00191 CLLocCtrl = Ctrl.getSubDevice(prefix); 00192 RegisterAt(CLLocCtrl,"Id",id); 00193 RegisterAt(CLLocCtrl,"NumJoints",numJoints); 00194 RegisterAt(CLLocCtrl,"ConstantPosition",constPos); 00195 RegisterAt(CLLocCtrl,"Position",q_pre); 00196 RegisterAt(CLLocCtrl,"ConstantVelocity",constVel); 00197 RegisterAt(CLLocCtrl,"Velocity",q_dot_pre); 00198 } 00199 00200 public: 00201 00202 std::string assemblyName; 00203 int numJoints, id; 00204 int constPos, constVel; 00205 DataType q_pre, q_dot_pre; 00206 ControlDevice CLLocCtrl; 00207 }; 00208 00209 class DHChainControl : public controlable { 00210 public: 00211 DHChainControl() : numSubChains(0) {} 00212 00213 virtual ~DHChainControl() {} 00214 00215 virtual void register_at(ControlDevice& Ctrl) {} 00216 virtual void register_at(ControlDevice& Ctrl, const std::string& prefix) { 00217 CLocCtrl = Ctrl.getSubDevice(prefix); 00218 RegisterAt(CLocCtrl,"Name",chainName); 00219 RegisterAt(CLocCtrl,"NumLinks",numLinks); 00220 RegisterAt(CLocCtrl,"NumTernaryLinks",numTernaryLinks); 00221 RegisterAt(CLocCtrl,"NumNativeLinks",numNativeLinks); 00222 RegisterAt(CLocCtrl,"NumSubChains",numSubChains); 00223 RegisterAt(CLocCtrl,"ParentChainId",parentChainId); 00224 00225 for (register int j=0; j < maxLinks; j++) { 00226 char VariableName1[16]; 00227 std::sprintf(VariableName1,"Link(%02d)",j+1); 00228 CLLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1)); 00229 std::sprintf(VariableName1,"NativeLink(%02d)",j+1); 00230 CNLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1)); 00231 std::sprintf(VariableName1,"TernaryLink(%02d)",j+1); 00232 CTLinkCtrls[j].register_at(CLocCtrl,std::string(VariableName1)); 00233 } 00234 } 00235 00236 public: 00237 std::string chainName; 00238 int numParts, numLinks, numTernaryLinks, numNativeLinks, numSubChains, parentChainId; 00239 int DHChainCtrls[maxSubChains]; 00240 ChainLinkControl CLLinkCtrls[maxLinks]; 00241 ChainNativeLinkControl CNLinkCtrls[maxLinks]; 00242 ChainTernLinkControl CTLinkCtrls[maxLinks]; 00243 ControlDevice CLocCtrl; 00244 }; 00245 00246 #endif // MOTIONCONTROLS_H