Merge branch 'master' into prep-release-0.2.0

CHANGELOG.md

@@ -1,5 +1,6 @@
 0.2.0
 =====
+- 2019-12-12: Added MocoFrameDistanceConstraint.
 
 - 2019-12-11: Add MocoOutputGoal, allowing any scalar model output to be used as
               as a goal.
@@ -41,16 +42,16 @@
               "iterate".
 
 - 2019-11-25: Update SmoothSphereHalfSpaceForce to visualize contact forces in
-              the Simbody visualizer (not the OpenSim GUI). 
+              the Simbody visualizer (not the OpenSim GUI).
 
-- 2019-11-22: Introduce TabOpUseAbsoluteStateNames to convert column labels
-              from IK solutions pre-4.0 states files to use new-style column
-              labels.
+- 2019-11-22: Introduce TabOpUseAbsoluteStateNames to convert column labels  
+              from IK solutions pre-4.0 states files to use new-style column 
+              labels. 
 
 - 2019-11-20: Added MocoAngularVelocityTrackingGoal and 
               MocoAccelerationTrackingGoal in anticipation of supporting 
               applications using IMU data in the future.
-
+              
 - 2019-11-18: Updates to report.py linewidth and legend formatting. 
 
 - 2019-11-18: Exporting controls to TimeSeriesTable via 

Moco/Bindings/OpenSimHeaders_moco.h

@@ -14,6 +14,7 @@
 #include <Moco/MocoBounds.h>
 #include <Moco/MocoCasADiSolver/MocoCasADiSolver.h>
 #include <Moco/MocoControlBoundConstraint.h>
+#include <Moco/MocoFrameDistanceConstraint.h>
 #include <Moco/MocoGoal/MocoControlGoal.h>
 #include <Moco/MocoGoal/MocoGoal.h>
 #include <Moco/MocoGoal/MocoInitialActivationGoal.h>

Moco/Bindings/moco.i

@@ -58,6 +58,7 @@ namespace OpenSim {
 %include <Moco/MocoConstraint.h>
 
 %include <Moco/MocoControlBoundConstraint.h>
+%include <Moco/MocoFrameDistanceConstraint.h>
 
 // unique_ptr
 // ----------
@@ -221,4 +222,4 @@ moco_unique_ptr(OpenSim::PositionMotion);
 %include <Moco/Components/SmoothSphereHalfSpaceForce.h>
 %include <Moco/Components/MultivariatePolynomialFunction.h>
 
-%include <Moco/ModelOperators.h>
+%include <Moco/ModelOperators.h>

Moco/Moco/CMakeLists.txt

@@ -68,6 +68,8 @@ set(MOCO_SOURCES
         MocoConstraint.cpp
         MocoControlBoundConstraint.cpp
         MocoControlBoundConstraint.h
+        MocoFrameDistanceConstraint.h
+        MocoFrameDistanceConstraint.cpp
         Components/DeGrooteFregly2016Muscle.h
         Components/DeGrooteFregly2016Muscle.cpp
         Components/SmoothSphereHalfSpaceForce.h

Moco/Moco/Components/ModelFactory.cpp

@@ -105,8 +105,8 @@ Model ModelFactory::createPlanarPointMass() {
     model.addJoint(jointX);
 
     // The joint's x axis must point in the global "+y" direction.
-    auto* jointY = new SliderJoint("ty",
-            *intermed, Vec3(0), Vec3(0, 0, 0.5 * SimTK::Pi),
+    auto* jointY = new SliderJoint("ty", 
+            *intermed, Vec3(0), Vec3(0, 0, 0.5 * SimTK::Pi), 
             *body, Vec3(0), Vec3(0, 0, .5 * SimTK::Pi));
     auto& coordY = jointY->updCoordinate(SliderJoint::Coord::TranslationX);
     coordY.setName("ty");

Moco/Moco/MocoConstraint.h

@@ -59,7 +59,6 @@ enum class KinematicLevel {
 /// Objects of this class can only be instantiated by a MocoPhase, since
 /// information from each constraint in the model is required to ensure that
 /// the correct values are assigned to internal variables during construction.
-/// @ingroup mococonstraint
 class OSIMMOCO_API MocoKinematicConstraint {
 public:
     const MocoConstraintInfo& getConstraintInfo() const {
@@ -146,7 +145,7 @@ class OSIMMOCO_API MocoKinematicConstraint {
 /// Therefore, there is no need to clear cache variables that you create in
 /// initializeImpl(). Also, information stored in this constraint does not
 /// persist across multiple solves.
-/// @ingroup mococonstraint
+/// @ingroup mocopathcon
 class OSIMMOCO_API MocoPathConstraint : public Object {
     OpenSim_DECLARE_ABSTRACT_OBJECT(MocoPathConstraint, Object);
 

Moco/Moco/MocoControlBoundConstraint.h

@@ -38,6 +38,7 @@ class MocoProblemInfo;
 /// constrain any control signals, even if you have provided control paths.
 ///
 /// @note This class can only constrain control signals for ScalarActuator%s.
+/// @ingroup mocopathcon
 class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint {
     OpenSim_DECLARE_CONCRETE_OBJECT(
             MocoControlBoundConstraint, MocoPathConstraint);

Moco/Moco/MocoFrameDistanceConstraint.cpp

@@ -0,0 +1,109 @@
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: MocoFrameDistanceConstraint.cpp                              *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2019 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Nicholas Bianco                                                 *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
+ * not use this file except in compliance with the License. You may obtain a  *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0          *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ * -------------------------------------------------------------------------- */
+
+#include "MocoFrameDistanceConstraint.h"
+
+using namespace OpenSim;
+
+//=============================================================================
+//  MocoFrameDistanceConstraintPair
+//=============================================================================
+
+MocoFrameDistanceConstraintPair::MocoFrameDistanceConstraintPair() {
+    constructProperties();
+}
+
+MocoFrameDistanceConstraintPair::MocoFrameDistanceConstraintPair(
+        std::string frame1Path, std::string frame2Path, 
+        double minimum_distance, double maximum_distance) {
+    constructProperties();
+    set_frame1_path(frame1Path);
+    set_frame2_path(frame2Path);
+    set_minimum_distance(minimum_distance);
+    set_maximum_distance(maximum_distance);
+}
+
+void MocoFrameDistanceConstraintPair::constructProperties() {
+    constructProperty_frame1_path("");
+    constructProperty_frame2_path("");
+    constructProperty_minimum_distance(0);
+    constructProperty_maximum_distance(SimTK::Infinity);
+}
+
+//=============================================================================
+//  MocoFrameDistanceConstraint
+//=============================================================================
+
+MocoFrameDistanceConstraint::MocoFrameDistanceConstraint(){ 
+    constructProperties(); 
+}
+
+void MocoFrameDistanceConstraint::initializeOnModelImpl(
+        const Model& model, const MocoProblemInfo&) const {
+    int nFramePairs = getProperty_frame_pairs().size();
+
+    // TODO: setConstraintInfo() is not really intended for use here.
+    MocoConstraintInfo info;
+    std::vector<MocoBounds> bounds;
+    for (int i = 0; i < nFramePairs; ++i) {
+        const auto frame1_path = get_frame_pairs(i).get_frame1_path();
+        OPENSIM_THROW_IF(!model.hasComponent<Frame>(frame1_path), Exception,
+                format("Could not find frame '%s'.", frame1_path));
+        auto& frame1 = model.getComponent<Frame>(frame1_path);
+        const auto frame2_path = get_frame_pairs(i).get_frame2_path();
+        OPENSIM_THROW_IF(!model.hasComponent<Frame>(frame2_path), Exception,
+                format("Could not find frame '%s'.", frame2_path));
+        auto& frame2 = model.getComponent<Frame>(frame2_path);
+        m_frame_pairs.emplace_back(&frame1, &frame2);
+
+        const double& minimum = get_frame_pairs(i).get_minimum_distance();
+        const double& maximum = get_frame_pairs(i).get_maximum_distance();
+        OPENSIM_THROW_IF(minimum < 0, Exception, 
+                format("Expected the minimum distance for this frame pair to " 
+                    "non-negative, but it is %d.", minimum));
+        OPENSIM_THROW_IF(maximum < 0, Exception,
+                format("Expected the maximum distance for this frame pair to "
+                       "non-negative, but it is %d.", maximum));
+        OPENSIM_THROW_IF(minimum > maximum, Exception,
+                format("Expected the minimum distance for this frame pair to "
+                       "be less than or equal to the maximum distance, but "
+                       "they are %d and %d, respectively.", minimum, maximum));
+        bounds.emplace_back(SimTK::square(minimum), SimTK::square(maximum));
+    }
+
+    setNumEquations(nFramePairs);
+    info.setBounds(bounds);
+    const_cast<MocoFrameDistanceConstraint*>(this)->setConstraintInfo(info);
+}
+
+void MocoFrameDistanceConstraint::calcPathConstraintErrorsImpl(
+        const SimTK::State& state, SimTK::Vector& errors) const {
+    getModel().realizePosition(state);
+    int iconstr = 0;
+    SimTK::Vec3 relative_position;
+    for (const auto& frame_pair : m_frame_pairs) {
+        const auto& frame1_pos = frame_pair.first->getPositionInGround(state);
+        const auto& frame2_pos = frame_pair.second->getPositionInGround(state);
+        relative_position = frame2_pos - frame1_pos;
+        errors[iconstr++] = relative_position.normSqr();
+    }
+}
+
+void MocoFrameDistanceConstraint::constructProperties() { 
+    constructProperty_frame_pairs(); 
+}

Moco/Moco/MocoFrameDistanceConstraint.h

@@ -0,0 +1,111 @@
+#ifndef MOCO_MOCOFRAMEDISTANCECONSTRAINT_H
+#define MOCO_MOCOFRAMEDISTANCECONSTRAINT_H
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: MocoFrameDistanceConstraint.h                                *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2019 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Nicholas Bianco                                                 *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
+ * not use this file except in compliance with the License. You may obtain a  *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0          *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ * -------------------------------------------------------------------------- */
+
+#include "MocoConstraint.h"
+#include "osimMocoDLL.h"
+
+namespace OpenSim {
+
+class OSIMMOCO_API MocoFrameDistanceConstraintPair : public Object {
+    OpenSim_DECLARE_CONCRETE_OBJECT(MocoFrameDistanceConstraintPair, Object);
+
+public:
+    OpenSim_DECLARE_PROPERTY(frame1_path, std::string,
+            "The first model frame path of the pair.");
+    OpenSim_DECLARE_PROPERTY(frame2_path, std::string,
+            "The second model frame path of the pair.");
+    OpenSim_DECLARE_PROPERTY(minimum_distance, double,
+            "The minimum distance apart that the two frame origins can be.");
+    OpenSim_DECLARE_PROPERTY(maximum_distance, double,
+            "The maximum distance apart that the two frame origins can be.")
+
+    MocoFrameDistanceConstraintPair();
+    MocoFrameDistanceConstraintPair(std::string firstFramePath,
+            std::string secondFramePath, double minimum_distance,
+            double maximum_distance);
+
+private:
+    void constructProperties();
+};
+
+/// This path constraint enforces that the distance between the origins of pairs 
+/// of model frames is kept between minimum and maximum bounds. Frame pairs and 
+/// their bounds are specified via a MocoFrameDistancConstraintPair. 
+/// Any model component derived from Frame is valid to be included in a frame 
+/// pair, and any number of frame pairs may be append to this constraint via 
+/// addFramePair().
+/// 
+/// This constraint can be used as a simple method for preventing bodies in your
+/// model from intersecting during an optimization. For example, the
+/// following prevents feet from intersecting during a walking optimization:
+/// @code
+/// distance = problem.addPathConstraint<MocoFrameDistanceConstraint>();
+/// distance.setName("minimum_distance");
+/// SimTK::Real inf = SimTK::Infinity;
+/// distance.addFramePair('/bodyset/calcn_l', '/bodyset/calcn_r', 0.1, inf);
+/// distance.addFramePair('/bodyset/toes_l', '/bodyset/toes_r', 0.1, inf);
+/// distance.addFramePair('/bodyset/calcn_l', '/bodyset/toes_r', 0.1, inf);
+/// distance.addFramePair('/bodyset/toes_l', '/bodyset/calcn_r', 0.1, inf);
+/// @endcode
+/// 
+/// @note This class represents a path constraint, *not* a model kinematic 
+/// constraint. Therefore, there are no Lagrange multipliers or constraint
+/// forces associated with this constraint. The model's force elements 
+/// (including actuators) must generate the forces necessary for satisfying this 
+/// constraint.
+///       
+/// @ingroup mocopathcon
+class OSIMMOCO_API MocoFrameDistanceConstraint : public MocoPathConstraint {
+    OpenSim_DECLARE_CONCRETE_OBJECT(
+            MocoFrameDistanceConstraint, MocoPathConstraint);
+
+public:
+    MocoFrameDistanceConstraint();
+
+    void addFramePair(MocoFrameDistanceConstraintPair pair) {
+        append_frame_pairs(std::move(pair));
+    }
+    void addFramePair(const std::string& frame1_path, 
+            const std::string& frame2_path, double minimum_distance, 
+            double maximum_distance) {
+        append_frame_pairs(MocoFrameDistanceConstraintPair(frame1_path, 
+            frame2_path, minimum_distance, maximum_distance));
+    }
+
+protected:
+    void initializeOnModelImpl(
+            const Model& model, const MocoProblemInfo&) const override;
+    void calcPathConstraintErrorsImpl(
+            const SimTK::State& state, SimTK::Vector& errors) const override;
+
+private:
+    OpenSim_DECLARE_LIST_PROPERTY(frame_pairs, 
+            MocoFrameDistanceConstraintPair, 
+            "Pairs of frames whose origins are constrained to be within minimum "
+            "and maximum bounds.");
+
+    void constructProperties();
+    mutable std::vector<std::pair<SimTK::ReferencePtr<const Frame>,
+            SimTK::ReferencePtr<const Frame>>> m_frame_pairs;
+};
+
+} // namespace OpenSim
+
+#endif // MOCO_MOCOFRAMEDISTANCECONSTRAINT_H

Moco/Moco/MocoGoal/MocoPeriodicityGoal.h

@@ -101,8 +101,8 @@ class OSIMMOCO_API MocoPeriodicityGoal : public MocoGoal {
     OpenSim_DECLARE_CONCRETE_OBJECT(MocoPeriodicityGoal, MocoGoal);
 
 public:
-    OpenSim_DECLARE_LIST_PROPERTY(
-            state_pairs, MocoPeriodicityGoalPair, "Periodic pairs of states.");
+    OpenSim_DECLARE_LIST_PROPERTY(state_pairs, MocoPeriodicityGoalPair, 
+            "Periodic pairs of states.");
     OpenSim_DECLARE_LIST_PROPERTY(control_pairs, MocoPeriodicityGoalPair,
             "Periodic pairs of controls.");
 

Moco/Moco/MocoInverse.cpp

@@ -18,7 +18,6 @@
 
 #include "MocoInverse.h"
 
-#include "Components/ModelFactory.h"
 #include "Components/PositionMotion.h"
 #include "MocoCasADiSolver/MocoCasADiSolver.h"
 #include "MocoGoal/MocoControlGoal.h"
@@ -28,9 +27,6 @@
 #include "MocoStudy.h"
 #include "MocoUtilities.h"
 
-#include <OpenSim/Tools/InverseDynamicsTool.h>
-#include <OpenSim/Actuators/CoordinateActuator.h>
-
 using namespace OpenSim;
 
 void MocoInverse::constructProperties() {

Moco/Moco/MocoInverse.h

@@ -84,8 +84,8 @@ class MocoInverseSolution {
 /// - optim_finite_difference_scheme: forward
 ///
 /// MocoInverse minimizes the sum of squared controls and, optionally, the sum
-/// of squared activations. Currently, the costs used by MocoInverse cannot be 
-/// customized. As MocoInverse becomes more mature and general, the costs will 
+/// of squared activations. Currently, the costs used by MocoInverse cannot be
+/// customized. As MocoInverse becomes more mature and general, the costs will
 /// become more flexible.
 ///
 /// Mesh interval

Moco/Moco/MocoProblem.h

@@ -362,11 +362,11 @@ class OSIMMOCO_API MocoPhase : public Object {
     OpenSim_DECLARE_PROPERTY(default_speed_bounds, MocoBounds,
             "Bounds for coordinate speeds if not specified in "
             "state_infos (default: [-50, 50]).");
-    OpenSim_DECLARE_PROPERTY(bound_activation_from_excitation, bool,
+    OpenSim_DECLARE_PROPERTY(bound_activation_from_excitation, bool,	    
             "For muscles without explicit activation bounds, set the bounds "
-            "for muscle activation (if activation dynamics are enabled) from "
-            "the bounds for muscle control (excitation), using "
-            "min/max control if explicit control bounds are not "
+            "for muscle activation (if activation dynamics are enabled) from " 
+            "the bounds for muscle control (excitation), using "             
+            "min/max control if explicit control bounds are not "            
             "provided. (default: true).");
     OpenSim_DECLARE_LIST_PROPERTY(
             state_infos, MocoVariableInfo, "The state variables' bounds.");

Moco/Moco/RegisterTypes_osimMoco.cpp

@@ -30,6 +30,7 @@
 #include "MocoBounds.h"
 #include "MocoCasADiSolver/MocoCasADiSolver.h"
 #include "MocoControlBoundConstraint.h"
+#include "MocoFrameDistanceConstraint.h"
 #include "MocoGoal/MocoAccelerationTrackingGoal.h"
 #include "MocoGoal/MocoAngularVelocityTrackingGoal.h"
 #include "MocoGoal/MocoControlGoal.h"
@@ -101,6 +102,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() {
         Object::registerType(MocoTropterSolver());
 
         Object::registerType(MocoControlBoundConstraint());
+        Object::registerType(MocoFrameDistanceConstraint());
 
         Object::registerType(MocoCasADiSolver());