1 /* -------------------------------------------------------------------------- *
2 * OpenSim: SMC_Joint.cpp *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5 * See http://opensim.stanford.edu and the NOTICE file for more information. *
6 * OpenSim is developed at Stanford University and supported by the US *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8 * through the Warrior Web program. *
9 * *
10 * Copyright (c) 2005-2017 Stanford University and the Authors *
11 * *
12 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
13 * not use this file except in compliance with the License. You may obtain a *
14 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
15 * *
16 * Unless required by applicable law or agreed to in writing, software *
17 * distributed under the License is distributed on an "AS IS" BASIS, *
18 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
19 * See the License for the specific language governing permissions and *
20 * limitations under the License. *
21 * -------------------------------------------------------------------------- */
22 //
23 // This software, originally developed by Realistic Dynamics, Inc., was
24 // transferred to Stanford University on November 1, 2006.
25 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26
27
28 //=============================================================================
29 // INCLUDES
30 //=============================================================================
31 #include "SMC_Joint.h"
32 #include <OpenSim/Common/Function.h>
33
34 using namespace std;
35 using namespace OpenSim;
36
37 //=============================================================================
38 // STATICS
39 //=============================================================================
40
41
42 //=============================================================================
43 // CONSTRUCTOR(S) AND DESTRUCTOR
44 //=============================================================================
45 //_____________________________________________________________________________
46 /**
47 * Destructor.
48 */
~SMC_Joint()49 SMC_Joint::~SMC_Joint()
50 {
51 }
52
53 //_____________________________________________________________________________
54 /**
55 * Construct a task for a specified generalized coordinate.
56 *
57 * @param aQID ID of the generalized coordinate to be tracked.
58 * @todo Instead of an integer id, the name of the coordinate
59 * should be used.
60 */
SMC_Joint(const string & aCoordinateName)61 SMC_Joint::SMC_Joint(const string &aCoordinateName) :
62 CMC_Joint(aCoordinateName),
63 _s(_propS.getValueDbl())
64 {
65 setNull();
66 }
67
68 //_____________________________________________________________________________
69 /**
70 * Copy constructor.
71 *
72 * @param aTask Joint task to be copied.
73 */
SMC_Joint(const SMC_Joint & aTask)74 SMC_Joint::SMC_Joint(const SMC_Joint &aTask) :
75 CMC_Joint(aTask),
76 _s(_propS.getValueDbl())
77 {
78 setNull();
79 copyData(aTask);
80 }
81
82
83 //=============================================================================
84 // CONSTRUCTION
85 //=============================================================================
86 //_____________________________________________________________________________
87 /**
88 * Set NULL values for all member variables.
89 */
90 void SMC_Joint::
setNull()91 setNull()
92 {
93 setupProperties();
94 _s = 100.0;
95 }
96 //_____________________________________________________________________________
97 /**
98 * Set up serialized member variables.
99 */
100 void SMC_Joint::
setupProperties()101 setupProperties()
102 {
103 _propS.setComment("Parameter for specifying the boundary"
104 "of the surface error. The default for this parameter is 100.0."
105 " Generally, this parameter can have a value in the range of 1.0 to 1000.0.");
106 _propS.setName("surface_error_boundary");
107 _propertySet.append(&_propS);
108 }
109
110
111 //_____________________________________________________________________________
112 /**
113 * Copy only the member data of specified object.
114 */
115 void SMC_Joint::
copyData(const SMC_Joint & aTask)116 copyData(const SMC_Joint &aTask)
117 {
118 _s = aTask._s;
119 }
120
121
122 //=============================================================================
123 // OPERATORS
124 //=============================================================================
125 //-----------------------------------------------------------------------------
126 // ASSIGNMENT
127 //-----------------------------------------------------------------------------
128 //_____________________________________________________________________________
129 /**
130 * Assignment operator.
131 *
132 * @param aTask Object to be copied.
133 * @return Reference to the altered object.
134 */
135 SMC_Joint& SMC_Joint::
operator =(const SMC_Joint & aTask)136 operator=(const SMC_Joint &aTask)
137 {
138 // BASE CLASS
139 CMC_Joint::operator =(aTask);
140
141 // DATA
142 copyData(aTask);
143
144 return(*this);
145 }
146
147 //=============================================================================
148 // GET AND SET
149 //=============================================================================
150
151
152 //=============================================================================
153 // COMPUTATIONS
154 //=============================================================================
155 //_____________________________________________________________________________
156 /**
157 * Compute the desired accelerations.
158 * This method assumes that the states have been set for the model.
159 *
160 * @param aT Time at which the desired accelerations are to be computed in
161 * real time units.
162 * @see Model::set()
163 * @see Model::setStates()
164 */
165 void SMC_Joint::
computeDesiredAccelerations(const SimTK::State & state,double aT)166 computeDesiredAccelerations( const SimTK::State& state, double aT)
167 {
168 _aDes = SimTK::NaN;
169
170 // CHECK
171 if(_model==NULL) return;
172 if(_pTrk[0]==NULL) return;
173
174 // COMPUTE ERRORS
175 computeErrors(state, aT);
176
177 // Term 1: Experimental Acceleration
178 double a;
179 if(_aTrk[0]==NULL) {
180 std::vector<int> derivComponents(2);
181 derivComponents[0]=0;
182 derivComponents[1]=0;
183 a = (_ka)[0]*_pTrk[0]->calcDerivative(derivComponents,SimTK::Vector(1,aT));
184 } else {
185 a = (_ka)[0]*_aTrk[0]->calcValue(SimTK::Vector(1,aT));
186 }
187
188 // Surface Error
189 double s = -_vErr[0] -(_kv)[0]*_pErr[0];
190
191 // Term 2: Velocity
192 double v = (_kv)[0]*_vErr[0];
193
194 // Term 3: Robust Term
195 double r = (_kp)[0] * tanh(s/_s);
196
197 // DESIRED ACCELERATION
198 _aDes[0] = a + v - r;
199
200 // PRINT
201 //printf("SMC_Joint.computeDesiredAcceleration:\n");
202 //printf("%s: t=%lf aDes=%lf a=%lf vErr=%lf pErr=%lf\n",getName(),t,_aDes[0],
203 // _pTrk[0]->evaluate(2,t),_vErr[0],_pErr[0]);
204 }
205