1+ #ifndef MOCO_MOCOFRAMEDISTANCECONSTRAINT_H
2+ #define MOCO_MOCOFRAMEDISTANCECONSTRAINT_H
3+ /* -------------------------------------------------------------------------- *
4+ * OpenSim Moco: MocoFrameDistanceConstraint.h *
5+ * -------------------------------------------------------------------------- *
6+ * Copyright (c) 2019 Stanford University and the Authors *
7+ * *
8+ * Author(s): Nicholas Bianco *
9+ * *
10+ * Licensed under the Apache License, Version 2.0 (the "License"); you may *
11+ * not use this file except in compliance with the License. You may obtain a *
12+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 *
13+ * *
14+ * Unless required by applicable law or agreed to in writing, software *
15+ * distributed under the License is distributed on an "AS IS" BASIS, *
16+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
17+ * See the License for the specific language governing permissions and *
18+ * limitations under the License. *
19+ * -------------------------------------------------------------------------- */
20+
21+ #include " MocoConstraint.h"
22+ #include " osimMocoDLL.h"
23+
24+ namespace OpenSim {
25+
26+ class OSIMMOCO_API MocoFrameDistanceConstraintPair : public Object {
27+ OpenSim_DECLARE_CONCRETE_OBJECT (MocoFrameDistanceConstraintPair, Object);
28+
29+ public:
30+ OpenSim_DECLARE_PROPERTY (frame1_path, std::string,
31+ " The first model frame path of the pair."
32+ OpenSim_DECLARE_PROPERTY (frame2_path, std::string,
33+ " The second model frame path of the pair."
34+ OpenSim_DECLARE_PROPERTY (minimum_distance, double ,
35+ " The minimum distance apart that the two frame origins can be."
36+ OpenSim_DECLARE_PROPERTY (maximum_distance, double ,
37+ " The maximum distance apart that the two frame origins can be."
38+
39+ MocoFrameDistanceConstraintPair ();
40+ MocoFrameDistanceConstraintPair (std::string firstFramePath,
41+ std::string secondFramePath, double minimum_distance,
42+ double maximum_distance);
43+
44+ private:
45+ void constructProperties ();
46+ };
47+
48+ // / This path constraint enforces that the distance between the origins of pairs
49+ // / of model frames is kept between minimum and maximum bounds. Frame pairs and
50+ // / their bounds are specified via a MocoFrameDistancConstraintPair.
51+ // / Any model component derived from Frame is valid to be included in a frame
52+ // / pair, and any number of frame pairs may be append to this constraint via
53+ // / addFramePair().
54+ // /
55+ // / This constraint can be used as a simple method for preventing bodies in your
56+ // / model from intersecting during an optimization. For example, the
57+ // / following prevents feet from intersecting during a walking optimization:
58+ // / @code
59+ // / distance = problem.addPathConstraint<MocoFrameDistanceConstraint>();
60+ // / distance.setName("minimum_distance");
61+ // / SimTK::Real inf = SimTK::Infinity;
62+ // / distance.addFramePair('/bodyset/calcn_l', '/bodyset/calcn_r', 0.1, inf);
63+ // / distance.addFramePair('/bodyset/toes_l', '/bodyset/toes_r', 0.1, inf);
64+ // / distance.addFramePair('/bodyset/calcn_l', '/bodyset/toes_r', 0.1, inf);
65+ // / distance.addFramePair('/bodyset/toes_l', '/bodyset/calcn_r', 0.1, inf);
66+ // / @endcode
67+ // /
68+ // / @note This class represents a path constraint, *not* a model kinematic
69+ // / constraint. Therefore, there are no Lagrange multipliers or constraint
70+ // / forces associated with this constraint. The model's force elements
71+ // / (including actuators) must generate the forces necessary for satisfying this
72+ // / constraint.
73+ // /
74+ // / @ingroup mocopathcon
75+ class OSIMMOCO_API MocoFrameDistanceConstraint : public MocoPathConstraint {
76+ OpenSim_DECLARE_CONCRETE_OBJECT (
77+ MocoFrameDistanceConstraint, MocoPathConstraint);
78+
79+ public:
80+ MocoFrameDistanceConstraint ();
81+
82+ void addFramePair (MocoFrameDistanceConstraintPair pair) {
83+ append_frame_pairs (std::move (pair));
84+ }
85+ void addFramePair (const std::string& frame1_path,
86+ const std::string& frame2_path, double minimum_distance,
87+ double maximum_distance) {
88+ append_frame_pairs (MocoFrameDistanceConstraintPair (frame1_path,
89+ frame2_path, minimum_distance, maximum_distance));
90+ }
91+
92+ protected:
93+ void initializeOnModelImpl (
94+ const Model& model, const MocoProblemInfo&) const override ;
95+ void calcPathConstraintErrorsImpl (
96+ const SimTK::State& state, SimTK::Vector& errors) const override ;
97+
98+ private:
99+ OpenSim_DECLARE_LIST_PROPERTY (frame_pairs,
100+ MocoFrameDistanceConstraintPair,
101+ " Pairs of frames whose origins are constrained to be within minimum "
102+ " and maximum bounds."
103+
104+ void constructProperties ();
105+ mutable std::vector<std::pair<SimTK::ReferencePtr<const Frame>,
106+ SimTK::ReferencePtr<const Frame>>> m_frame_pairs;
107+ };
108+
109+ } // namespace OpenSim
110+
111+ #endif // MOCO_MOCOFRAMEDISTANCECONSTRAINT_H
0 commit comments