1 #ifndef OPENSIM_COORDINATE_H_ 2 #define OPENSIM_COORDINATE_H_ 3 /* -------------------------------------------------------------------------- * 4 * OpenSim: Coordinate.h * 5 * -------------------------------------------------------------------------- * 6 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * 7 * See http://opensim.stanford.edu and the NOTICE file for more information. * 8 * OpenSim is developed at Stanford University and supported by the US * 9 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * 10 * through the Warrior Web program. * 11 * * 12 * Copyright (c) 2005-2017 Stanford University and the Authors * 13 * Author(s): Ajay Seth, Michael A. Sherman, Ayman Habib * 14 * Contributor(s): Frank C. Anderson, Jeffrey A. Reinbolt * 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 28 // INCLUDE 29 #include <OpenSim/Simulation/osimSimulationDLL.h> 30 #include <OpenSim/Common/Function.h> 31 #include <OpenSim/Simulation/Model/ModelComponent.h> 32 33 class ModifiableConstant; 34 35 namespace OpenSim { 36 37 class Function; 38 class Joint; 39 class Model; 40 41 //============================================================================= 42 //============================================================================= 43 /** 44 * A Coordinate is a ModelComponent for managing the access and behavior 45 * of a model's generalized coordinate including its value, speed and 46 * acceleration (once system accelerations have been realized). 47 * As a ModelComponent it provides resources to enable a Coordinate to be 48 * locked, prescribed, or clamped (limited to a min-to-max range). 49 * 50 * @authors Ajay Seth, Ayman Habib, Michael Sherman 51 */ 52 class OSIMSIMULATION_API Coordinate : public ModelComponent { 53 OpenSim_DECLARE_CONCRETE_OBJECT(Coordinate, ModelComponent); 54 55 public: 56 //============================================================================== 57 // PROPERTIES 58 //============================================================================== 59 OpenSim_DECLARE_PROPERTY(default_value, double, 60 "The value of this coordinate before any value has been set. " 61 "Rotational coordinate value is in radians and Translational in meters."); 62 63 OpenSim_DECLARE_PROPERTY(default_speed_value, double, 64 "The speed value of this coordinate before any value has been set. " 65 "Rotational coordinate value is in rad/s and Translational in m/s."); 66 67 OpenSim_DECLARE_LIST_PROPERTY_SIZE(range, double, 2, 68 "The minimum and maximum values that the coordinate can range between. " 69 "Rotational coordinate range in radians and Translational in meters." ); 70 71 OpenSim_DECLARE_PROPERTY(clamped, bool, 72 "Flag indicating whether or not the values of the coordinates should " 73 "be limited to the range, above." ); 74 75 OpenSim_DECLARE_PROPERTY(locked, bool, 76 "Flag indicating whether or not the values of the coordinates should " 77 "be constrained to the current (e.g. default) value, above." ); 78 79 OpenSim_DECLARE_OPTIONAL_PROPERTY(prescribed_function, Function, 80 "If specified, the coordinate can be prescribed by a function of time. " 81 "It can be any OpenSim Function with valid second order derivatives." ); 82 83 OpenSim_DECLARE_PROPERTY(prescribed, bool, 84 "Flag indicating whether or not the values of the coordinates should " 85 "be prescribed according to the function above. It is ignored if the " 86 "no prescribed function is specified." ); 87 88 OpenSim_DECLARE_PROPERTY(is_free_to_satisfy_constraints, bool, 89 "Flag identifies whether or not this coordinate can change freely when " 90 "posing the model to satisfy kinematic constraints. When true, the " 91 "coordinate's initial or specified value is ignored when considering " 92 "constraints. This allows values for important coordinates, which have " 93 "this flag set to false, to dictate the value of unimportant coordinates " 94 "if they are linked via constraints."); 95 96 //============================================================================== 97 // OUTPUTS 98 //============================================================================== 99 OpenSim_DECLARE_OUTPUT(value, double, getValue, SimTK::Stage::Model); 100 OpenSim_DECLARE_OUTPUT(speed, double, getSpeedValue, SimTK::Stage::Model); 101 OpenSim_DECLARE_OUTPUT(acceleration, double, getAccelerationValue, 102 SimTK::Stage::Acceleration); 103 104 /** Motion type that describes the motion dictated by the coordinate. 105 Specifically it describes how the coordinate can be interpreted. 106 A coordinate can be interpreted as Rotational or Translational if 107 the displacement about or along an axis is the coordinate value. 108 If the Coordinate cannot be interpreted as being either of these 109 it is flagged as Coupled. */ 110 enum MotionType 111 { 112 Undefined, 113 Rotational, 114 Translational, 115 Coupled 116 }; 117 118 119 //============================================================================= 120 // PUBLIC METHODS 121 //============================================================================= 122 /** @name Public accessor methods 123 Get and set attributes of the Coordinate **/ 124 /**@{**/ 125 126 /** access to the Coordinate's owning joint */ 127 const Joint& getJoint() const; 128 129 /** access to the generalized Coordinate's motion type 130 This can be Rotational, Translational, or Coupled (both) */ 131 MotionType getMotionType() const; 132 133 /** get the value of the Coordinate from the state */ 134 double getValue(const SimTK::State& s) const; 135 /** Set the value of the Coordinate on to the state. 136 Optional flag to enforce the constraints immediately (true by default), 137 which can adjust all coordinate values in the state to satisfy model 138 constraints. Use getValue(s) to see if/how the value was adjusted to 139 satisfy the kinematic constraints. If setting multiple Coordinate values 140 consecutively, e.g. in a loop, set the flag to false and then call 141 Model::assemble(state) once all Coordinate values have been set. 142 Alternatively, use Model::setStateVariableValues() to set all coordinate 143 values and their speeds at once followed by Model::assemble(state). 144 145 The provided value will be clamped to the coordinate's range if 146 the coordinate is clamped and enforceConstraints is true. 147 */ 148 void setValue(SimTK::State& s, double aValue, bool enforceContraints=true) const; 149 150 /** get the speed value of the Coordinate from the state */ 151 double getSpeedValue(const SimTK::State& s) const; 152 void setSpeedValue(SimTK::State& s, double aValue) const; 153 /** return the name (label) used to identify the Coordinate's speed 154 state variable. Returns the string "<coordinate_name>/speed" */ 155 const std::string& getSpeedName() const; 156 157 /** get the default value for this coordinate. This is the value 158 used if no value has been set prior to a simulation. */ getDefaultValue()159 double getDefaultValue() const { return get_default_value(); } 160 void setDefaultValue(double aDefaultValue); 161 162 /** get the default speed value for this coordinate. This is the value 163 used if no value has been set prior to a simulation. */ getDefaultSpeedValue()164 double getDefaultSpeedValue() const { return get_default_speed_value(); } setDefaultSpeedValue(double aDefaultSpeedValue)165 void setDefaultSpeedValue(double aDefaultSpeedValue) 166 { upd_default_speed_value() = aDefaultSpeedValue; } 167 168 /** get acceleration of the coordinate is dependent on having 169 realized the model and state to the acceleration stage */ 170 double getAccelerationValue(const SimTK::State& s) const; 171 172 /** determine or set whether or not the Coordinate is 173 "clamped" between a range of values. */ 174 bool getClamped(const SimTK::State& s) const; 175 void setClamped(SimTK::State& s, bool aLocked) const; 176 /** get/set whether or not the Coordinate is clamped by default */ getDefaultClamped()177 bool getDefaultClamped() const { return get_clamped(); } setDefaultClamped(bool aClamped)178 void setDefaultClamped(bool aClamped ) { upd_clamped() = aClamped; } 179 180 /** get the value for the Coordinate's range of motion */ getRangeMin()181 double getRangeMin() const {return get_range(0); } getRangeMax()182 double getRangeMax() const {return get_range(1); } 183 /** set the range with a double array of length 2 in order of 184 minimum and maximum coordinate values */ 185 void setRange(double aRange[2]); 186 void setRangeMin(double aMin); 187 void setRangeMax(double aMax); 188 189 /** determine or set whether or not the Coordinate is 190 "locked" for a given state of the Model. */ 191 bool getLocked(const SimTK::State& s) const; 192 void setLocked(SimTK::State& s, bool aLocked) const; 193 /** get/set whether or not the Coordinate is locked by default */ getDefaultLocked()194 bool getDefaultLocked() const { return get_locked(); } setDefaultLocked(bool aLocked)195 void setDefaultLocked(bool aLocked) { upd_locked() = aLocked; } 196 197 /** determine or set whether or not the Coordinate is 198 "prescribed" for a given state of the Model. */ 199 bool isPrescribed(const SimTK::State& s) const; 200 void setIsPrescribed(SimTK::State& s, bool isPrescribed ) const; 201 /** get/set whether or not the Coordinate is locked by default */ getDefaultIsPrescribed()202 bool getDefaultIsPrescribed() const {return get_prescribed();} setDefaultIsPrescribed(bool isPrescribed)203 void setDefaultIsPrescribed(bool isPrescribed ) {upd_prescribed() = isPrescribed;} 204 /** Specify an OpenSim Function specifies the prescribed motion for this 205 Coordinate as a function of time. Note, this function must provide 206 valid first and second order derivatives. */ 207 void setPrescribedFunction(const Function& function); 208 const Function& getPrescribedFunction() const; 209 210 /** Return true if coordinate is dependent on other coordinates via a coupler 211 constraint OR it has been flagged as free to change when satisfying 212 the model's kinematic constraints in general. */ 213 bool isDependent(const SimTK::State& s) const; 214 215 /** Return true if coordinate is locked, prescribed, or dependent on other coordinates */ 216 bool isConstrained(const SimTK::State& s) const; 217 218 /** @name Advanced Access to underlying Simbody system resources */ 219 /**@{**/ getMobilizerQIndex()220 int getMobilizerQIndex() const { return _mobilizerQIndex; }; getBodyIndex()221 SimTK::MobilizedBodyIndex getBodyIndex() const { return _bodyIndex; }; 222 /**@}**/ 223 224 /* For internal consistency checking. Returns the user-specified MotionType 225 serialized with pre-4.0 model files if one is provided, otherwise 226 returns MotionType::Undefined. */ 227 const MotionType& getUserSpecifiedMotionTypePriorTo40() const; 228 229 //-------------------------------------------------------------------------- 230 // CONSTRUCTION 231 //-------------------------------------------------------------------------- 232 /** default constructor*/ 233 Coordinate(); 234 235 /** Convenience constructor */ 236 Coordinate(const std::string &aName, MotionType aMotionType, 237 double defaultValue, double aRangeMin, double aRangeMax); 238 239 // Uses default (compiler-generated) destructor, copy constructor and copy 240 // assignment operator. 241 242 protected: 243 // Only model should be invoking these ModelComponent interface methods. 244 void extendAddToSystem(SimTK::MultibodySystem& system) const override; 245 //State structure is locked and now we can assign names to state variables 246 //allocated by underlying components after modeling options have been 247 //factored in. 248 void extendRealizeInstance(const SimTK::State& state) const override; 249 void extendInitStateFromProperties(SimTK::State& s) const override; 250 void extendSetPropertiesFromState(const SimTK::State& state) override; 251 252 // Only the coordinate or the joint itself can specify the owner 253 // of Coordinate 254 void setJoint(const Joint& aOwningJoint); 255 256 // Override to account for version updates in the XML format. 257 void updateFromXMLNode(SimTK::Xml::Element& aNode, 258 int versionNumber = -1) override; 259 260 261 //============================================================================= 262 // MODEL DATA 263 //============================================================================= 264 private: 265 // Class for handling state variable added (allocated) by this Component 266 class CoordinateStateVariable : public StateVariable { 267 public: 268 // Constructors 269 /** Convenience constructor for defining a Component added state variable */ CoordinateStateVariable(const std::string & name,const Component & owner,SimTK::SubsystemIndex subSysIndex,int index)270 explicit CoordinateStateVariable(const std::string& name, //state var name 271 const Component& owner, //owning component 272 SimTK::SubsystemIndex subSysIndex, 273 int index) : 274 StateVariable(name, owner, subSysIndex, index, false) {} 275 276 //override StateVariable virtual methods 277 double getValue(const SimTK::State& state) const override; 278 void setValue(SimTK::State& state, double value) const override; 279 double getDerivative(const SimTK::State& state) const override; 280 void setDerivative(const SimTK::State& state, double deriv) const override; 281 }; 282 283 // Class for handling state variable added (allocated) by this Component 284 class SpeedStateVariable : public StateVariable { 285 public: 286 // Constructors 287 /** Convenience constructor for defining a Component added state variable */ SpeedStateVariable(const std::string & name,const Component & owner,SimTK::SubsystemIndex subSysIndex,int index)288 explicit SpeedStateVariable(const std::string& name, //state var name 289 const Component& owner, //owning component 290 SimTK::SubsystemIndex subSysIndex, 291 int index) : 292 StateVariable(name, owner, subSysIndex, index, false) {} 293 294 //override StateVariable virtual methods 295 double getValue(const SimTK::State& state) const override; 296 void setValue(SimTK::State& state, double value) const override; 297 double getDerivative(const SimTK::State& state) const override; 298 void setDerivative(const SimTK::State& state, double deriv) const override; 299 }; 300 301 // All coordinates (Simbody mobility) have associated constraints that 302 // perform joint locking, prescribed motion and range of motion. 303 // Constraints are created upon setup: locked, prescribed Function 304 // and range must be set. 305 // NOTE: Changing the prescribed motion function requires topology to be realized 306 // so state is invalidated 307 // Enabling/disabling locking, prescribed motion or clamping is allowable 308 // during a simulation. 309 // The last constraint to be set takes precedence. 310 /** Indices for the constraint in Simbody. */ 311 SimTK::ResetOnCopy<SimTK::ConstraintIndex> _prescribedConstraintIndex; 312 SimTK::ResetOnCopy<SimTK::ConstraintIndex> _lockedConstraintIndex; 313 SimTK::ResetOnCopy<SimTK::ConstraintIndex> _clampedConstraintIndex; 314 315 /* MobilizedBodyIndex of the body which this coordinate serves. */ 316 SimTK::ResetOnCopy<SimTK::MobilizedBodyIndex> _bodyIndex; 317 318 /* Mobilizer Q (i.e. generalized coordinate) index for this Coordinate. */ 319 SimTK::ResetOnCopy<SimTK::MobilizerQIndex> _mobilizerQIndex; 320 321 /* Keep a reference to the SimTK function owned by the PrescribedMotion 322 Constraint, so we can change the value at which to lock the joint. */ 323 SimTK::ReferencePtr<ModifiableConstant> _lockFunction; 324 325 /* Label for the related state that is the generalized speed of 326 this coordinate. */ 327 std::string _speedName; 328 329 /* The OpenSim::Joint that owns this coordinate. */ 330 SimTK::ReferencePtr<const Joint> _joint; 331 332 /* User set MotionType from versions of OpenSim that predate 4.0 */ 333 MotionType _userSpecifiedMotionTypePriorTo40{ Undefined }; 334 335 mutable bool _lockedWarningGiven; 336 337 // PRIVATE METHODS implementing the Component interface 338 void constructProperties(); 339 void extendFinalizeFromProperties() override; 340 341 friend class CoordinateCouplerConstraint; 342 friend class Joint; 343 344 //============================================================================= 345 }; // END of class Coordinate 346 //============================================================================= 347 //============================================================================= 348 } // end of namespace OpenSim 349 350 351 #endif // OPENSIM_COORDINATE_H_ 352 353 354