1 #ifndef SimTK_SIMBODY_MOTION_IMPL_H_ 2 #define SimTK_SIMBODY_MOTION_IMPL_H_ 3 4 /* -------------------------------------------------------------------------- * 5 * Simbody(tm) * 6 * -------------------------------------------------------------------------- * 7 * This is part of the SimTK biosimulation toolkit originating from * 8 * Simbios, the NIH National Center for Physics-Based Simulation of * 9 * Biological Structures at Stanford, funded under the NIH Roadmap for * 10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 11 * * 12 * Portions copyright (c) 2009-13 Stanford University and the Authors. * 13 * Authors: Michael Sherman * 14 * Contributors: * 15 * * 16 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 17 * not use this file except in compliance with the License. You may obtain a * 18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 19 * * 20 * Unless required by applicable law or agreed to in writing, software * 21 * distributed under the License is distributed on an "AS IS" BASIS, * 22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 23 * See the License for the specific language governing permissions and * 24 * limitations under the License. * 25 * -------------------------------------------------------------------------- */ 26 27 #include "SimTKcommon.h" 28 #include "simbody/internal/common.h" 29 #include "simbody/internal/Motion.h" 30 31 #include "SimbodyTreeState.h" 32 33 namespace SimTK { 34 35 class MobilizedBodyImpl; 36 37 38 //============================================================================== 39 // MOTION IMPL 40 //============================================================================== 41 // This is the hidden implementation class for Motion objects. This is the 42 // abstract base class to which every Motion handle points. 43 class MotionImpl : public PIMPLImplementation<Motion, MotionImpl> { 44 public: MotionImpl()45 MotionImpl() 46 : m_isDisabledByDefault(false) {} 47 48 // Default copy constructor, copy assignment, and destructor; note that the 49 // pointer to the mobilized body is not copied or deleted. 50 hasMobilizedBody()51 bool hasMobilizedBody() const {return m_mobodImpl != nullptr;} getMobilizedBodyImpl()52 const MobilizedBodyImpl& getMobilizedBodyImpl() const 53 { assert(m_mobodImpl); return *m_mobodImpl; } 54 MobilizedBodyIndex getMobilizedBodyIndex() const; 55 setMobilizedBodyImpl(MobilizedBodyImpl * mbi)56 void setMobilizedBodyImpl(MobilizedBodyImpl* mbi) 57 { assert(!m_mobodImpl); m_mobodImpl = mbi; } 58 void invalidateTopologyCache() const; 59 60 const SimbodyMatterSubsystem& getMatterSubsystem() const; 61 62 const AbstractValue& 63 getDiscreteVariable(const State& s, DiscreteVariableIndex vx) const; 64 65 AbstractValue& 66 updDiscreteVariable(State& s, DiscreteVariableIndex vx) const; 67 68 DiscreteVariableIndex 69 allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const; 70 71 template <class T> const T& getVar(const State & s,DiscreteVariableIndex vx)72 getVar(const State& s, DiscreteVariableIndex vx) const { 73 return Value<T>::downcast(getDiscreteVariable(s, vx)); 74 } 75 76 template <class T> T& updVar(State & s,DiscreteVariableIndex vx)77 updVar(State& s, DiscreteVariableIndex vx) const { 78 return Value<T>::updDowncast(updDiscreteVariable(s, vx)); 79 } 80 81 template <class T> DiscreteVariableIndex 82 allocVar(State& state, const T& initVal, 83 const Stage& stage=Stage::Instance) const 84 { 85 return allocateDiscreteVariable(state, stage, new Value<T>(initVal)); 86 } 87 88 ~MotionImpl()89 virtual ~MotionImpl() {} 90 virtual MotionImpl* clone() const = 0; 91 92 // This reports whether this Motion is holonomic (Level::Position), 93 // nonholonomic (Level::Velocity), or acceleration (Level::Acceleration). getLevel(const State & s)94 Motion::Level getLevel(const State& s) const { 95 if (isDisabled(s)) return Motion::NoLevel; 96 return getLevelVirtual(s); // ask concrete class 97 } 98 getLevelMethod(const State & s)99 Motion::Method getLevelMethod(const State& s) const { 100 if (isDisabled(s)) return Motion::NoMethod; 101 return getLevelMethodVirtual(s); 102 } 103 104 105 void disable(State& s) const; 106 void enable(State& s) const; 107 bool isDisabled(const State&) const; 108 setDisabledByDefault(bool shouldBeDisabled)109 void setDisabledByDefault(bool shouldBeDisabled) 110 { invalidateTopologyCache(); m_isDisabledByDefault=shouldBeDisabled; } isDisabledByDefault()111 bool isDisabledByDefault() const {return m_isDisabledByDefault;} 112 113 // These operators calculate prescribed positions, velocities, or 114 // accelerations given a State realized to the previous Stage. calcPrescribedPosition(const State & s,int nq,Real * q)115 void calcPrescribedPosition (const State& s, int nq, Real* q) const 116 { calcPrescribedPositionVirtual(s,nq,q); } calcPrescribedPositionDot(const State & s,int nq,Real * qdot)117 void calcPrescribedPositionDot (const State& s, int nq, Real* qdot) const 118 { calcPrescribedPositionDotVirtual(s,nq,qdot); } calcPrescribedPositionDotDot(const State & s,int nq,Real * qdotdot)119 void calcPrescribedPositionDotDot(const State& s, int nq, Real* qdotdot)const 120 { calcPrescribedPositionDotDotVirtual(s,nq,qdotdot); } calcPrescribedVelocity(const State & s,int nu,Real * u)121 void calcPrescribedVelocity (const State& s, int nu, Real* u) const 122 { calcPrescribedVelocityVirtual(s,nu,u); } calcPrescribedVelocityDot(const State & s,int nu,Real * udot)123 void calcPrescribedVelocityDot (const State& s, int nu, Real* udot) const 124 { calcPrescribedVelocityDotVirtual(s,nu,udot); } calcPrescribedAcceleration(const State & s,int nu,Real * udot)125 void calcPrescribedAcceleration (const State& s, int nu, Real* udot) const 126 { calcPrescribedAccelerationVirtual(s,nu,udot); } 127 realizeTopology(State & state)128 void realizeTopology(State& state) const 129 { realizeTopologyVirtual(state); } realizeModel(State & state)130 void realizeModel(State& state) const 131 { realizeModelVirtual(state); } realizeInstance(const State & state)132 void realizeInstance(const State& state) const 133 { realizeInstanceVirtual(state); } realizeTime(const State & state)134 void realizeTime(const State& state) const 135 { realizeTimeVirtual(state); } realizePosition(const State & state)136 void realizePosition(const State& state) const 137 { realizePositionVirtual(state); } realizeVelocity(const State & state)138 void realizeVelocity(const State& state) const 139 { realizeVelocityVirtual(state); } realizeDynamics(const State & state)140 void realizeDynamics(const State& state) const 141 { realizeDynamicsVirtual(state); } realizeAcceleration(const State & state)142 void realizeAcceleration(const State& state) const 143 { realizeAccelerationVirtual(state); } realizeReport(const State & state)144 void realizeReport(const State& state) const 145 { realizeReportVirtual(state); } 146 147 virtual Motion::Level getLevelVirtual(const State&) const = 0; getLevelMethodVirtual(const State &)148 virtual Motion::Method getLevelMethodVirtual(const State&) const 149 { return Motion::Prescribed; } 150 151 virtual void calcPrescribedPositionVirtual 152 (const State&, int nq, Real* q) const; 153 virtual void calcPrescribedPositionDotVirtual 154 (const State&, int nq, Real* qdot) const; 155 virtual void calcPrescribedPositionDotDotVirtual 156 (const State&, int nq, Real* qdotdot) const; 157 virtual void calcPrescribedVelocityVirtual 158 (const State&, int nu, Real* u) const; 159 virtual void calcPrescribedVelocityDotVirtual 160 (const State&, int nu, Real* udot) const; 161 virtual void calcPrescribedAccelerationVirtual 162 (const State&, int nu, Real* udot) const; 163 realizeTopologyVirtual(State &)164 virtual void realizeTopologyVirtual (State&) const {} realizeModelVirtual(State &)165 virtual void realizeModelVirtual (State&) const {} realizeInstanceVirtual(const State &)166 virtual void realizeInstanceVirtual (const State&) const {} realizeTimeVirtual(const State &)167 virtual void realizeTimeVirtual (const State&) const {} realizePositionVirtual(const State &)168 virtual void realizePositionVirtual (const State&) const {} realizeVelocityVirtual(const State &)169 virtual void realizeVelocityVirtual (const State&) const {} realizeDynamicsVirtual(const State &)170 virtual void realizeDynamicsVirtual (const State&) const {} realizeAccelerationVirtual(const State &)171 virtual void realizeAccelerationVirtual(const State&) const {} realizeReportVirtual(const State &)172 virtual void realizeReportVirtual (const State&) const {} 173 private: 174 ReferencePtr<MobilizedBodyImpl> m_mobodImpl; 175 bool m_isDisabledByDefault; 176 }; 177 178 179 //------------------------------------------------------------------------------ 180 // SINUSOID IMPL 181 //------------------------------------------------------------------------------ 182 class Motion::SinusoidImpl : public MotionImpl { 183 public: 184 // no default constructor SinusoidImpl(Motion::Level level,Real amplitude,Real rate,Real phase)185 SinusoidImpl(Motion::Level level, 186 Real amplitude, Real rate, Real phase) 187 : level(level), defAmplitude(amplitude), defRate(rate), 188 defPhase(phase) 189 { 190 } 191 clone()192 SinusoidImpl* clone() const override { 193 SinusoidImpl* copy = new SinusoidImpl(*this); 194 return copy; 195 } 196 getLevelVirtual(const State &)197 Motion::Level getLevelVirtual (const State&) const override 198 { return level; } getLevelMethodVirtual(const State &)199 Motion::Method getLevelMethodVirtual(const State&) const override 200 { return Motion::Prescribed; } 201 202 // Allocate variables if needed. realizeTopologyVirtual(State & state)203 void realizeTopologyVirtual(State& state) const override { 204 // None yet. 205 } 206 207 calcPrescribedPositionVirtual(const State & state,int nq,Real * q)208 void calcPrescribedPositionVirtual 209 (const State& state, int nq, Real* q) const override { 210 assert(level==Motion::Position); assert(nq==0 || q); 211 const Real t = state.getTime(); 212 const Real out = defAmplitude*std::sin(defRate*t + defPhase); 213 for (int i=0; i<nq; ++i) 214 q[i] = out; 215 } 216 calcPrescribedPositionDotVirtual(const State & state,int nq,Real * qdot)217 void calcPrescribedPositionDotVirtual 218 (const State& state, int nq, Real* qdot) const override { 219 assert(level==Motion::Position); assert(nq==0 || qdot); 220 const Real t = state.getTime(); 221 const Real outd = defAmplitude*defRate*std::cos(defRate*t + defPhase); 222 for (int i=0; i<nq; ++i) 223 qdot[i] = outd; 224 } 225 calcPrescribedPositionDotDotVirtual(const State & state,int nq,Real * qdotdot)226 void calcPrescribedPositionDotDotVirtual 227 (const State& state, int nq, Real* qdotdot) const override { 228 assert(level==Motion::Position); assert(nq==0 || qdotdot); 229 const Real t = state.getTime(); 230 const Real outdd = 231 -defAmplitude*defRate*defRate*std::sin(defRate*t + defPhase); 232 for (int i=0; i<nq; ++i) 233 qdotdot[i] = outdd; 234 } 235 calcPrescribedVelocityVirtual(const State & state,int nu,Real * u)236 void calcPrescribedVelocityVirtual 237 (const State& state, int nu, Real* u) const override { 238 assert(level==Motion::Velocity); 239 assert(nu==0 || u); 240 const Real t = state.getTime(); 241 const Real out = defAmplitude*std::sin(defRate*t + defPhase); 242 for (int i=0; i<nu; ++i) 243 u[i] = out; 244 } 245 calcPrescribedVelocityDotVirtual(const State & state,int nu,Real * udot)246 void calcPrescribedVelocityDotVirtual 247 (const State& state, int nu, Real* udot) const override { 248 assert(level==Motion::Velocity); 249 assert(nu==0 || udot); 250 const Real t = state.getTime(); 251 const Real outd = defAmplitude*defRate*std::cos(defRate*t + defPhase); 252 for (int i=0; i<nu; ++i) 253 udot[i] = outd; 254 } 255 calcPrescribedAccelerationVirtual(const State & state,int nu,Real * udot)256 void calcPrescribedAccelerationVirtual 257 (const State& state, int nu, Real* udot) const override { 258 assert(level==Motion::Acceleration); assert(nu==0 || udot); 259 const Real t = state.getTime(); 260 const Real out = defAmplitude*std::sin(defRate*t + defPhase); 261 for (int i=0; i<nu; ++i) 262 udot[i] = out; 263 } 264 private: 265 // TOPOLOGY "STATE" 266 Motion::Level level; 267 Real defAmplitude, defRate, defPhase; 268 269 // TOPOLOGY "CACHE" 270 // None yet. 271 }; 272 273 274 //------------------------------------------------------------------------------ 275 // STEADY IMPL 276 //------------------------------------------------------------------------------ 277 class Motion::SteadyImpl : public MotionImpl { 278 public: 279 // no default constructor SteadyImpl(const Vec6 & u)280 explicit SteadyImpl(const Vec6& u) : defaultU(u) {} 281 clone()282 SteadyImpl* clone() const override { 283 SteadyImpl* copy = new SteadyImpl(*this); 284 copy->currentU.invalidate(); // no sharing state variables 285 return copy; 286 } 287 setDefaultRates(const Vec6 & u)288 void setDefaultRates(const Vec6& u) { 289 invalidateTopologyCache(); 290 defaultU = u; 291 } setOneDefaultRate(MobilizerUIndex ux,Real u)292 void setOneDefaultRate(MobilizerUIndex ux, Real u) { 293 invalidateTopologyCache(); 294 defaultU[ux] = u; 295 } getOneDefaultRate(MobilizerUIndex ux)296 Real getOneDefaultRate(MobilizerUIndex ux) const { 297 return defaultU[ux]; 298 } 299 getDefaultRates()300 const Vec6& getDefaultRates() const {return defaultU;} 301 setRates(State & s,const Vec6 & u)302 void setRates(State& s, const Vec6& u) const { 303 updVar<Vec6>(s, currentU) = u; 304 } setOneRate(State & s,MobilizerUIndex ux,Real u)305 void setOneRate(State& s, MobilizerUIndex ux, Real u) const { 306 updVar<Vec6>(s, currentU)[ux] = u; 307 } 308 getOneRate(const State & s,MobilizerUIndex ux)309 Real getOneRate(const State& s, MobilizerUIndex ux) const { 310 return getVar<Vec6>(s, currentU)[ux]; 311 } 312 getLevelVirtual(const State &)313 Motion::Level getLevelVirtual (const State&) const override 314 { return Motion::Velocity; } getLevelMethodVirtual(const State &)315 Motion::Method getLevelMethodVirtual(const State&) const override 316 { return Motion::Prescribed; } 317 318 // Allocate a discrete variable to hold the constant rates. realizeTopologyVirtual(State & state)319 void realizeTopologyVirtual(State& state) const override { 320 // This is in the Topology-stage "cache" so we can write to it, 321 // but only here. 322 const_cast<DiscreteVariableIndex&>(currentU) = 323 allocVar(state, defaultU); 324 } 325 calcPrescribedVelocityVirtual(const State & state,int nu,Real * u)326 void calcPrescribedVelocityVirtual 327 (const State& state, int nu, Real* u) const override 328 { 329 assert(0 <= nu && nu <= 6); 330 assert(nu==0 || u); 331 const Vec6& uval = getVar<Vec6>(state, currentU); 332 for (int i=0; i<nu; ++i) 333 u[i] = uval[i]; 334 } 335 calcPrescribedVelocityDotVirtual(const State & state,int nu,Real * udot)336 void calcPrescribedVelocityDotVirtual 337 (const State& state, int nu, Real* udot) const override 338 { 339 assert(0 <= nu && nu <= 6); 340 assert(nu==0 || udot); 341 for (int i=0; i<nu; ++i) 342 udot[i] = 0; 343 } 344 345 private: 346 // TOPOLOGY "STATE" 347 Vec6 defaultU; 348 349 // TOPOLOGY "CACHE" 350 DiscreteVariableIndex currentU; 351 }; 352 353 354 //------------------------------------------------------------------------------ 355 // CUSTOM IMPL 356 //------------------------------------------------------------------------------ 357 class Motion::CustomImpl : public MotionImpl { 358 public: 359 // Take over ownership of the supplied heap-allocated object. 360 explicit CustomImpl(Motion::Custom::Implementation* implementation); 361 CustomImpl(const CustomImpl & src)362 CustomImpl(const CustomImpl& src) : implementation(0) { 363 if (src.implementation) 364 implementation = src.implementation->clone(); 365 } 366 clone()367 CustomImpl* clone() const override { return new CustomImpl(*this); } 368 ~CustomImpl()369 ~CustomImpl() { 370 delete implementation; 371 } 372 getLevelVirtual(const State & s)373 Motion::Level getLevelVirtual(const State& s) const override { 374 return getImplementation().getLevel(s); 375 } getLevelMethodVirtual(const State & s)376 Motion::Method getLevelMethodVirtual(const State& s) const override { 377 return getImplementation().getLevelMethod(s); 378 } 379 getImplementation()380 const Motion::Custom::Implementation& getImplementation() const { 381 assert(implementation); return *implementation; 382 } updImplementation()383 Motion::Custom::Implementation& updImplementation() { 384 assert(implementation); return *implementation; 385 } 386 calcPrescribedPositionVirtual(const State & s,int nq,Real * q)387 void calcPrescribedPositionVirtual 388 (const State& s, int nq, Real* q) const override 389 { getImplementation().calcPrescribedPosition(s,nq,q); } calcPrescribedPositionDotVirtual(const State & s,int nq,Real * qdot)390 void calcPrescribedPositionDotVirtual 391 (const State& s, int nq, Real* qdot) const override 392 { getImplementation().calcPrescribedPositionDot(s,nq,qdot); } calcPrescribedPositionDotDotVirtual(const State & s,int nq,Real * qdotdot)393 void calcPrescribedPositionDotDotVirtual 394 (const State& s, int nq, Real* qdotdot) const override 395 { getImplementation().calcPrescribedPositionDotDot(s,nq,qdotdot); } 396 calcPrescribedVelocityVirtual(const State & s,int nu,Real * u)397 void calcPrescribedVelocityVirtual 398 (const State& s, int nu, Real* u) const override 399 { getImplementation().calcPrescribedVelocity(s,nu,u); } calcPrescribedVelocityDotVirtual(const State & s,int nu,Real * udot)400 void calcPrescribedVelocityDotVirtual 401 (const State& s, int nu, Real* udot) const override 402 { getImplementation().calcPrescribedVelocityDot(s,nu,udot); } 403 calcPrescribedAccelerationVirtual(const State & s,int nu,Real * udot)404 void calcPrescribedAccelerationVirtual 405 (const State& s, int nu, Real* udot) const override 406 { getImplementation().calcPrescribedAcceleration(s,nu,udot); } 407 realizeTopologyVirtual(State & state)408 void realizeTopologyVirtual(State& state) const override { 409 getImplementation().realizeTopology(state); 410 } realizeModelVirtual(State & state)411 void realizeModelVirtual(State& state) const override { 412 getImplementation().realizeModel(state); 413 } realizeInstanceVirtual(const State & state)414 void realizeInstanceVirtual(const State& state) const override { 415 getImplementation().realizeInstance(state); 416 } realizeTimeVirtual(const State & state)417 void realizeTimeVirtual(const State& state) const override { 418 getImplementation().realizeTime(state); 419 } realizePositionVirtual(const State & state)420 void realizePositionVirtual(const State& state) const override { 421 getImplementation().realizePosition(state); 422 } realizeVelocityVirtual(const State & state)423 void realizeVelocityVirtual(const State& state) const override { 424 getImplementation().realizeVelocity(state); 425 } realizeDynamicsVirtual(const State & state)426 void realizeDynamicsVirtual(const State& state) const override { 427 getImplementation().realizeDynamics(state); 428 } realizeAccelerationVirtual(const State & state)429 void realizeAccelerationVirtual(const State& state) const override { 430 getImplementation().realizeAcceleration(state); 431 } realizeReportVirtual(const State & state)432 void realizeReportVirtual(const State& state) const override { 433 getImplementation().realizeReport(state); 434 } 435 private: 436 Motion::Custom::Implementation* implementation; 437 }; 438 439 440 } // namespace SimTK 441 442 #endif // SimTK_SIMBODY_MOTION_IMPL_H_ 443