00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
#ifndef __rdCMC_Task_h__
00030
#define __rdCMC_Task_h__
00031
00032
00033
#include "rdCMCDLL.h"
00034
#include <OpenSim/Tools/rdTools.h>
00035
#include <OpenSim/Tools/PropertyBool.h>
00036
#include <OpenSim/Tools/PropertyBoolArray.h>
00037
#include <OpenSim/Tools/PropertyInt.h>
00038
#include <OpenSim/Tools/PropertyDblArray.h>
00039
#include <OpenSim/Tools/Function.h>
00040
#include <OpenSim/Tools/FunctionSet.h>
00041
#include <OpenSim/Simulation/Model/Model.h>
00042
00043
00044
00045
00046
00056 class RDCMC_API rdCMC_Task :
public OpenSim::Object
00057 {
00058
00059
00060
00061
00062
protected:
00063
00065 OpenSim::PropertyBool _propOn;
00067 OpenSim::PropertyInt _propWRTBody;
00069 OpenSim::PropertyInt _propExpressBody;
00071 OpenSim::PropertyBoolArray _propActive;
00073 OpenSim::PropertyDblArray _propW;
00075 OpenSim::PropertyDblArray _propKP;
00077 OpenSim::PropertyDblArray _propKV;
00079 OpenSim::PropertyDblArray _propKA;
00081 OpenSim::PropertyDblArray _propR0;
00083 OpenSim::PropertyDblArray _propR1;
00085 OpenSim::PropertyDblArray _propR2;
00086
00087
00088
00089
00090
00091
00092
00094 bool &_on;
00096 int &_wrtBody;
00098 int &_expressBody;
00100 OpenSim::Array<bool> &_active;
00102 OpenSim::Array<double> &_w;
00104 OpenSim::Array<double> &_kp;
00106 OpenSim::Array<double> &_kv;
00108 OpenSim::Array<double> &_ka;
00110 OpenSim::Array<double> &_r0;
00112 OpenSim::Array<double> &_r1;
00114 OpenSim::Array<double> &_r2;
00115
00116
00118 OpenSim::Model *_model;
00119
00121 int _nTrk;
00126 OpenSim::Function *_pTrk[3];
00129 OpenSim::Function *_vTrk[3];
00132 OpenSim::Function *_aTrk[3];
00134 double _pErrLast[3];
00136 double _pErr[3];
00138 double _vErrLast[3];
00140 double _vErr[3];
00142 double _aDes[3];
00144 double _a[3];
00146 double *_j;
00148 double *_m;
00149
00150
00151
00152
00153
00154
00155
00156
public:
00157 rdCMC_Task();
00158 rdCMC_Task(DOMElement *aElement);
00159 rdCMC_Task(
const rdCMC_Task &aTaskObject);
00160
virtual ~rdCMC_Task();
00161
virtual OpenSim::Object* copy() const = 0;
00162 virtual OpenSim::Object* copy(DOMElement *aElement) const = 0;
00163 private:
00164
void setNull();
00165
void setupProperties();
00166
void copyData(const rdCMC_Task &aTaskObject);
00167
00168
00169
00170
00171 public:
00172
00173 #ifndef SWIG
00174 rdCMC_Task& operator=(const rdCMC_Task &aTaskObject);
00175 #endif
00176
00177
00178
00179
00180
00181
void setModel(OpenSim::Model *aModel);
00182 OpenSim::Model* getModel() const;
00183
00184
void setOn(
bool aTrueFalse);
00185
bool getOn() const;
00186
00187 virtual
void setParametersBasedOnName() = 0;
00188
00189
void setWRTBody(
int aBody);
00190
int getWRTBody() const;
00191
00192
void setExpressBody(
int aBody);
00193
int getExpressBody() const;
00194
00195
void setActive(
bool a0,
bool a1=false,
bool a2=false);
00196
bool getActive(
int aWhich) const;
00197
00198
void setWeight(
double aW0,
double aW1=0.0,
double aW2=0.0);
00199
double getWeight(
int aWhich) const;
00200
00201
void setKP(
double aK0,
double aK1=0.0,
double aK2=0.0);
00202
double getKP(
int aWhich) const;
00203
00204
void setKV(
double aK0,
double aK1=0.0,
double aK2=0.0);
00205
double getKV(
int aWhich) const;
00206
00207
void setKA(
double aK0,
double aK1=0.0,
double aK2=0.0);
00208
double getKA(
int aWhich) const;
00209
00210
void setDirection_0(const
double aR[3]);
00211
void getDirection_0(
double rR[3]) const;
00212
00213
void setDirection_1(const
double aR[3]);
00214
void getDirection_1(
double rR[3]) const;
00215
00216
void setDirection_2(const
double aR[3]);
00217
void getDirection_2(
double rR[3]) const;
00218
00219
int getNumTaskFunctions() const;
00220
void setTaskFunctions(OpenSim::Function *aF0,
00221 OpenSim::Function *aF1=NULL,OpenSim::Function *aF2=NULL);
00222 OpenSim::Function* getTaskFunction(
int aWhich) const;
00223
void setTaskFunctionsForVelocity(OpenSim::Function *aF0,
00224 OpenSim::Function *aF1=NULL,OpenSim::Function *aF2=NULL);
00225 OpenSim::Function* getTaskFunctionForVelocity(
int aWhich) const;
00226
void setTaskFunctionsForAcceleration(OpenSim::Function *aF0,
00227 OpenSim::Function *aF1=NULL,OpenSim::Function *aF2=NULL);
00228 OpenSim::Function* getTaskFunctionForAcceleration(
int aWhich) const;
00229
00230
double getTaskPosition(
int aWhich,
double aT) const;
00231
double getTaskVelocity(
int aWhich,
double aT) const;
00232
double getTaskAcceleration(
int aWhich,
double aT) const;
00233
00234
void setPositionErrorLast(
double aE0,
double aE1=0.0,
double aE2=0.0);
00235
double getPositionErrorLast(
int aWhich) const;
00236
void setVelocityErrorLast(
double aE0,
double aE1=0.0,
double aE2=0.0);
00237
double getVelocityErrorLast(
int aWhich) const;
00238
00239
double getPositionError(
int aWhich) const;
00240
double getVelocityError(
int aWhich) const;
00241
00242
double getDesiredAcceleration(
int aWhich) const;
00243
00244
double getAcceleration(
int aWhich) const;
00245
00246
00247
00248
00249 virtual
void computeErrors(
double aT) = 0;
00250 virtual
void computeDesiredAccelerations(
double aT) = 0;
00251 virtual
void computeDesiredAccelerations(
double aTI,
double aTF) = 0;
00252 virtual
void computeAccelerations() = 0;
00253 virtual
void computeJacobian();
00254 virtual
void computeEffectiveMassMatrix();
00255
00256
00257
00258
00259 virtual
void updateFromXMLNode();
00260
00261
00262
00263 };
00264
00265
00266
00267 #endif
00268
00269