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_h__
00030
#define __rdCMC_h__
00031
00032
00033
00034
00035
00036
#include "rdCMCDLL.h"
00037
#include <OpenSim/Tools/rdTools.h>
00038
#include <OpenSim/SQP/rdSQP.h>
00039
#include <OpenSim/SQP/rdOptimizationTarget.h>
00040
#include <OpenSim/Simulation/Model/Model.h>
00041
#include <OpenSim/Simulation/Model/VectorFunctionForActuators.h>
00042
#include <OpenSim/Simulation/Control/Controller.h>
00043
#include <OpenSim/SQP/rdFSQP.h>
00044
#include "rdCMC_TaskSet.h"
00045
00046
#ifdef SWIG
00047
#ifdef RDCMC_API
00048
#undef RDCMC_API
00049
#define RDCMC_API
00050
#endif
00051
#endif
00052
00053
00054
00082 class RDCMC_API rdCMC :
public OpenSim::Controller
00083 {
00084
00085
00086
00087
protected:
00089 OpenSim::rdFSQP *_sqp;
00091 OpenSim::rdOptimizationTarget *_target;
00093 rdCMC_TaskSet *_taskSet;
00095 double _dt;
00098 double _lastDT;
00102 bool _restoreDT;
00107 double _tf;
00110 double _targetDT;
00112 bool _checkTargetTime;
00114 OpenSim::Storage *_pErrStore;
00116 OpenSim::Storage *_vErrStore;
00118 OpenSim::Storage *_stressTermWeightStore;
00120 OpenSim::ControlSet *_controlSet;
00122 OpenSim::ControlSet *_controlConstraints;
00125 bool _useCurvatureFilter;
00127 bool _useReflexes;
00129 OpenSim::ControlSet *_controlConstraintsFromReflexes;
00132 OpenSim::Array<int> _paramList;
00133
00134
00137 OpenSim::VectorFunctionForActuators *_predictor;
00139 OpenSim::Array<double> _f;
00140
00141
00142
00143
00144
00145
00146
00147
00148
public:
00149 rdCMC(OpenSim::Model *aModel,
rdCMC_TaskSet *aSet);
00150
virtual ~rdCMC();
00151
void setNull();
00152
00153
00154
00155
00156
void setControlConstraints(OpenSim::ControlSet *aCcontrolSet);
00157 OpenSim::ControlSet* getControlConstraints();
00158 OpenSim::ControlSet* getControlSet() const;
00159
rdCMC_TaskSet* getTaskSet() const;
00160 OpenSim::Array<
int>* getParameterList();
00161 OpenSim::rdFSQP* getOptimizer() const;
00162 OpenSim::rdOptimizationTarget* setOptimizationTarget(OpenSim::rdOptimizationTarget *aTarget);
00163 OpenSim::rdOptimizationTarget* getOptimizationTarget() const;
00164
void setDT(
double aDT);
00165
double getDT() const;
00166
void setTargetTime(
double aTime);
00167
double getTargetTime() const;
00168
void setTargetDT(
double aDT);
00169
double getTargetDT() const;
00170
void setCheckTargetTime(
bool aTrueFalse);
00171
bool getCheckTargetTime() const;
00172
void setActuatorForcePredictor(OpenSim::VectorFunctionForActuators *aPredictor);
00173 OpenSim::VectorFunctionForActuators* getActuatorForcePredictor();
00174 OpenSim::Storage* getPositionErrorStorage() const;
00175 OpenSim::Storage* getVelocityErrorStorage() const;
00176 OpenSim::Storage* getStressTermWeightStorage() const;
00177
void setUseCurvatureFilter(
bool aTrueFalse);
00178
bool getUseCurvatureFilter() const;
00179
void setUseReflexes(
bool aTrueFalse);
00180
bool getUseReflexes() const;
00181 virtual
void constrainControlsBasedOnReflexes(
double time,OpenSim::Array<
double> &xmin,OpenSim::Array<
double> &xmax);
00182
00183
00184
00185
00186
void restoreConfiguration(
int nqnu,const
double *yi,
double *y);
00187
void obtainActuatorEquilibrium(
double tiReal,
double dtReal,
00188 OpenSim::Array<
double> &x,OpenSim::Array<
double> &y,
bool hold);
00189
00190
00191
00192
00193 virtual
void
00194 computeInitialStates(
double &rTI,
double *rYI);
00195 virtual
void
00196 computeControls(
double &rDT,
double aT,const
double *aY,OpenSim::ControlSet &rX);
00197
00198
00199
00200
00201 static
void
00202 FilterControls(const OpenSim::ControlSet &aControlSet,
double aDT,
double aT,
00203 OpenSim::Array<
double> &rControls);
00204
00205
00206
00207 };
00208
00209
00210
00211 #endif
00212
00213