1 #ifndef OPENSIM_ROLLING_ON_SURFACE_CONSTRAINT_H_
2 #define OPENSIM_ROLLING_ON_SURFACE_CONSTRAINT_H_
3 /* -------------------------------------------------------------------------- *
4  *                   OpenSim:  RollingOnSurfaceConstraint.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                                                       *
14  *                                                                            *
15  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
16  * not use this file except in compliance with the License. You may obtain a  *
17  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
18  *                                                                            *
19  * Unless required by applicable law or agreed to in writing, software        *
20  * distributed under the License is distributed on an "AS IS" BASIS,          *
21  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
22  * See the License for the specific language governing permissions and        *
23  * limitations under the License.                                             *
24  * -------------------------------------------------------------------------- */
25 
26 
27 // INCLUDE
28 #include "UnilateralConstraint.h"
29 #include <OpenSim/Simulation/Model/PhysicalFrame.h>
30 
31 namespace OpenSim {
32 
33 //=============================================================================
34 //=============================================================================
35 /**
36  * A class implementing a collection of rolling-without-slipping and
37  * non-penetration constraints on a surface.
38  * The underlying Constraints in Simbody are:
39  *      PointInPlane to oppose penetration into the ground (unilaterally)
40  *      ConstantAngle about the normal to the enforce no twisting
41  *      NoSlip1D along one axis of the plane
42  *      NoSlip1D along the other axis
43  *
44  * mu = Coulomb Friction Coefficient
45  *
46  * Each of these constraints have conditions dependent on the reaction forces
47  * that they generate individually and collectively:
48  *   PointInPlane normal force (Fn) must be positive (in the direction of the
49  *   normal) ConstantAngle the reaction torque cannot exceed contactRadius*mu*Fn
50  *   Both NoSlip conditions are treated together, the magnitude of the combined
51  *   reaction forces (in the plane) cannot exceed mu*Fn
52 
53  * @author Ajay Seth
54  */
55 class OSIMSIMULATION_API RollingOnSurfaceConstraint
56 :   public UnilateralConstraint {
57 OpenSim_DECLARE_CONCRETE_OBJECT(RollingOnSurfaceConstraint,
58                                 UnilateralConstraint);
59 public:
60     OpenSim_DECLARE_PROPERTY(surface_normal, SimTK::Vec3,
61         "Surface normal direction in the surface body.");
62     OpenSim_DECLARE_PROPERTY(surface_height, double,
63         "Surface height in the direction of the normal in the surface body.");
64     OpenSim_DECLARE_PROPERTY(friction_coefficient, double,
65         "Coulomb friction coefficient for rolling on the surface.");
66     OpenSim_DECLARE_PROPERTY(contact_radius, double,
67         "A guess at the area of contact approximated by a circle of radius.");
68 
69     OpenSim_DECLARE_SOCKET(rolling_body, PhysicalFrame,
70         "A frame fixed to the rolling body.");
71     OpenSim_DECLARE_SOCKET(surface_body, PhysicalFrame,
72         "A frame fixed to the surface body.");
73 
74 //=============================================================================
75 // DATA
76 //=============================================================================
77 private:
78 
79     /** Get the indices of underlying constraints to access from Simbody */
80     SimTK::ResetOnCopy<std::vector<SimTK::ConstraintIndex>> _indices;
81 
82     /**  This cache acts a temporary hold for the constraint conditions when
83     time has not changed */
84     std::vector<bool> _defaultUnilateralConditions;
85 
86 //=============================================================================
87 // METHODS
88 //=============================================================================
89 public:
90     // CONSTRUCTION
91     RollingOnSurfaceConstraint();
92     virtual ~RollingOnSurfaceConstraint();
93 
94     /** %Set rolling body by its name */
95     void setRollingBodyByName(const std::string& aBodyName);
96     /** %Set surface body by its name */
97     void setSurfaceBodyByName(const std::string& aBodyName);
98 
99     /**
100     * Get whether or not the RollingOnSurfaceConstraint is enforced.
101     * Simbody multibody system instance is realized every time the isEnforced
102     * changes, BUT multiple sets to the same value have no cost.
103     *
104     * @param state  the state of the system that includes the constraint status
105     */
106     bool isEnforced(const SimTK::State& state) const override;
107 
108     /**
109     * %Set whether or not the RollingOnSurfaceConstraint is enforced.
110     * Since the constraint is composed of multiple constraints, this method can
111     * disable all the constraints, but enabling is not guaranteed. For example,
112     * if the unilateral conditions are violated the constraint will be disabled.
113     *
114     * @param state      the state to set whether the constraint is enforced or
115                         not.
116     * @param isEnforced if true the constraint is enforced.
117     */
118     bool setIsEnforced(SimTK::State& state, bool isEnforced) override;
119 
120     // This method allows finer granularity over the subconstraints according
121     // to imposed behavior
122     bool setIsEnforced(SimTK::State& state,
123                        bool isEnforced,
124                        std::vector<bool> shouldBeOn);
125 
126     /**
127     * Ask the RollingOnSurfaceConstraint for the forces it is imposing on the
128     * system. Simbody multibody system must be realized to at least
129     * Stage::Dynamics. Returns: the bodyForces on those bodies being
130     * constrained (constrainedBodies) a SpatialVec (6 components) describing
131     * resulting torque and force mobilityForces acting along constrained
132     * mobilities
133     *
134     * @param state  State of model
135     * @param bodyForcesInAncestor   Vector of SpatialVecs contain constraint
136     *                               forces
137     * @param mobilityForces     Vector of forces that act along the constrained
138     *                           mobilities associated with this constraint
139     */
140     void calcConstraintForces(const SimTK::State& state,
141                        SimTK::Vector_<SimTK::SpatialVec>& bodyForcesInAncestor,
142                        SimTK::Vector& mobilityForces) const override;
143 
144     void setContactPointForInducedAccelerations(const SimTK::State &s,
145         SimTK::Vec3 point) override;
146 
147     /** Test whether unilateral conditions are being satisfied.
148         Note: system must be realized to at least Stage::Dynamics */
149     std::vector<bool>
150     unilateralConditionsSatisfied(const SimTK::State& state) override;
151 
152 
153     /** %Set whether constraint is enforced but use cached values
154     for unilateral conditions instead of automatic reevaluation */
155     bool setIsEnforcedWithCachedUnilateralConditions(bool isEnforced,
156                                                      SimTK::State& state);
157 
158 protected:
159     /** Extend ModelComponent interface */
160     void extendConnectToModel(Model& aModel) override;
161 
162     /** Create the SimTK::Constraints: which implements this
163      * RollingOnSurfaceConstraint. */
164     void extendAddToSystem(SimTK::MultibodySystem& system) const override;
165     /** Populate the SimTK::State: with defaults for the
166      * RollingOnSurfaceConstraint. */
167     void extendInitStateFromProperties(SimTK::State& state) const override;
168     /** Given an existing SimTK::State set defaults for the
169      * RollingOnSurfaceConstraint. */
170     void extendSetPropertiesFromState(const SimTK::State& state) override;
171 
172     /** Updating XML formating to latest revision */
173     void updateFromXMLNode(SimTK::Xml::Element& aNode,
174                            int versionNumber) override;
175 
176 private:
177     void setNull();
178     /** Construct RollingOnSurfaceConstraint's properties */
179     void constructProperties();
180 
181     // References to the PhysicalFrames on the
182     SimTK::ReferencePtr<const PhysicalFrame> _rollingFrame;
183     SimTK::ReferencePtr<const PhysicalFrame> _surfaceFrame;
184 
185 
186 //=============================================================================
187 };  // END of class RollingOnSurfaceConstraint
188 //=============================================================================
189 //=============================================================================
190 
191 } // end of namespace OpenSim
192 
193 #endif // OPENSIM_ROLLING_ON_SURFACE_CONSTRAINT_H_
194 
195 
196