Add the Frame Orientation Constraint

Moco/Moco/RegisterTypes_osimSimonFrameOrientationConstraint.cpp

@@ -0,0 +1,62 @@
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: RegisterTypes_osimMocoCustomEffortGoal.cpp                   *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2019 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Christopher Dembia                                              *
+ *                                                                            *
+ * 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 "SimonFrameOrientationConstraint.h"
+#include "RegisterTypes_osimSimonFrameOrientationConstraint.h"
+
+using namespace OpenSim;
+
+static osimSimonFrameOrientationConstraintInstantiator instantiator;
+static osimSimonFrameOrientationConstraintPairInstantiator instantiator2;
+
+OSIMMOCOCUSTOMEFFORTGOAL_API void RegisterTypes_osimMocoCustomEffortGoal() {
+    try {
+        Object::registerType(SimonFrameOrientationConstraint());
+    } catch (const std::exception& e) {
+        std::cerr << "ERROR during SimonFrameOrientationConstraint "
+                     "Object registration:\n"
+                  << e.what() << std::endl;
+    }
+
+    try {
+        Object::registerType(SimonFrameOrientationConstraintPair());
+    }
+    catch (const std::exception& e) {
+        std::cerr << "ERROR during SimonFrameOrientationConstraintPair "
+            "Object registration:\n"
+            << e.what() << std::endl;
+    }
+}
+
+
+
+
+osimSimonFrameOrientationConstraintInstantiator::osimSimonFrameOrientationConstraintInstantiator() {
+    registerDllClasses();
+}
+
+void osimSimonFrameOrientationConstraintInstantiator::registerDllClasses() {
+    RegisterTypes_osimMocoCustomEffortGoal();
+}
+
+osimSimonFrameOrientationConstraintPairInstantiator::osimSimonFrameOrientationConstraintPairInstantiator() {
+    registerDllClasses();
+}
+
+void osimSimonFrameOrientationConstraintPairInstantiator::registerDllClasses() {
+    RegisterTypes_osimMocoCustomEffortGoal();
+}

Moco/Moco/RegisterTypes_osimSimonFrameOrientationConstraint.h

@@ -0,0 +1,42 @@
+#ifndef OPENSIM_REGISTERTYPES_OSIMSIMONFRAMEORIENTATIONCONSTRAINT_H 
+#define OPENSIM_REGISTERTYPES_OSIMSIMONFRAMEORIENTATIONCONSTRAINT_H 
+/* -------------------------------------------------------------------------- *
+ * OpenSim: RegisterTypes_osimMocoCustomEffortGoal.h                          *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2019 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Christopher Dembia                                              *
+ *                                                                            *
+ * 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 "osimSimonFrameOrientationConstraintDLL.h"
+
+extern "C" {
+
+OSIMMOCOCUSTOMEFFORTGOAL_API void RegisterTypes_osimSimonFrameOrientationConstraint();
+OSIMMOCOCUSTOMEFFORTGOAL_API void RegisterTypes_osimSimonFrameOrientationConstraintPair();
+}
+
+class osimSimonFrameOrientationConstraintInstantiator {
+public:
+    osimSimonFrameOrientationConstraintInstantiator();
+private:
+    void registerDllClasses();
+};
+
+class osimSimonFrameOrientationConstraintPairInstantiator {
+public:
+    osimSimonFrameOrientationConstraintPairInstantiator();
+private:
+    void registerDllClasses();
+};
+#endif // OPENSIM_REGISTERTYPES_OSIMSIMONFRAMEORIENTATIONCONSTRAINT_H 

Moco/Moco/SimonFrameOrientationConstraint.cpp

@@ -0,0 +1,114 @@
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: SimonFrameOrientationConstraint.cpp                                     *
+ * -------------------------------------------------------------------------- *
+ *                                                                            *
+ * Author(s): Simon Jeng                                              *
+ *                                                                            *
+ * 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 "SimonFrameOrientationConstraint.h"
+
+using namespace OpenSim;
+
+
+SimonFrameOrientationConstraintPair::SimonFrameOrientationConstraintPair() {
+    constructProperties();
+}
+
+SimonFrameOrientationConstraintPair::SimonFrameOrientationConstraintPair(
+    std::string frame1Path, std::string frame2Path,
+    double minimum_angle, double maximum_angle) {
+    constructProperties();
+    set_frame1_path(frame1Path);
+    set_frame2_path(frame2Path);
+    set_minimum_angle(minimum_angle);
+    set_maximum_angle(maximum_angle);
+}
+
+void SimonFrameOrientationConstraintPair::constructProperties() {
+    constructProperty_frame1_path("");
+    constructProperty_frame2_path("");
+    constructProperty_minimum_angle(-SimTK::Infinity);
+    constructProperty_maximum_angle(SimTK::Infinity);
+}
+
+//=============================================================================
+//  SimonFrameOrientationConstraint
+//=============================================================================
+
+SimonFrameOrientationConstraint::SimonFrameOrientationConstraint() 
+{
+    constructProperties();
+}
+SimonFrameOrientationConstraint::SimonFrameOrientationConstraint(std::string name)
+{
+    constructProperties();
+}
+void SimonFrameOrientationConstraint::initializeOnModelImpl(const Model& model, const MocoProblemInfo&) const
+{
+
+    int nFramePairs = getProperty_frame_pairs().size();
+    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,
+            "Could not find frame '{}'.", 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,
+            "Could not find frame '{}'.", 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_angle();
+        const double& maximum = get_frame_pairs(i).get_maximum_angle();
+
+        OPENSIM_THROW_IF(minimum > maximum, Exception,
+            "Expected the minimum angle for this frame pair "
+            "to be less than or equal to the maximum angle, "
+            "but they are {} and {}, respectively.",
+            minimum, maximum);
+        bounds.emplace_back(minimum, maximum);
+    }
+
+
+    setNumEquations(nFramePairs);
+    info.setBounds(bounds);
+    const_cast<SimonFrameOrientationConstraint*>(this)->setConstraintInfo(info);
+
+}
+
+void SimonFrameOrientationConstraint::calcPathConstraintErrorsImpl(const SimTK::State& state, SimTK::Vector& errors) const
+{
+    int iconstr = 0;
+
+    getModel().realizePosition(state);
+
+    for (const auto& frame_pair : m_frame_pairs) {
+        const auto& frame1_rotation = frame_pair.first->getRotationInGround(state);
+        const auto& frame2_rotation = frame_pair.second->getRotationInGround(state);
+        auto coordaxis = SimTK::CoordinateAxis(2);
+        auto frame1_angle = frame1_rotation.convertOneAxisRotationToOneAngle(coordaxis);
+        auto frame2_angle = frame2_rotation.convertOneAxisRotationToOneAngle(coordaxis);
+        auto relative_angle = frame2_angle - frame1_angle;
+
+        errors[iconstr++] = relative_angle;
+    }
+
+}
+
+void SimonFrameOrientationConstraint::constructProperties() {
+    constructProperty_frame_pairs();
+
+}

Moco/Moco/SimonFrameOrientationConstraint.h

@@ -0,0 +1,80 @@
+#ifndef OPENSIM_SIMONFRAMEORIENTATIONCONSTRAINT_H 
+#define OPENSIM_SIMONFRAMEORIENTATIONCONSTRAINT_H 
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: SimonFrameOrientationConstraint.h                                     *
+ * -------------------------------------------------------------------------- *
+ *                                                                            *
+ * Author(s): Simon Jeng                                              *
+ *                                                                            *
+ * 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 <OpenSim/Moco/osimMoco.h>
+#include "osimSimonFrameOrientationConstraintDLL.h"
+
+namespace OpenSim {
+
+class OSIMMOCOCUSTOMEFFORTGOAL_API SimonFrameOrientationConstraintPair : public Object {
+    OpenSim_DECLARE_CONCRETE_OBJECT(SimonFrameOrientationConstraintPair, 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_angle, double,
+        "The minimum distance apart that the two frame origins can be "
+        "(meters).");
+    OpenSim_DECLARE_PROPERTY(maximum_angle, double,
+        "The maximum distance apart that the two frame origins can be "
+        "(meters).")
+
+    SimonFrameOrientationConstraintPair();
+    SimonFrameOrientationConstraintPair(std::string firstFramePath,
+        std::string secondFramePath, double minimum_angle,
+        double maximum_angle);
+
+private:
+    void constructProperties();
+};
+
+class OSIMMOCOCUSTOMEFFORTGOAL_API SimonFrameOrientationConstraint : public MocoPathConstraint {
+    OpenSim_DECLARE_CONCRETE_OBJECT(SimonFrameOrientationConstraint, MocoPathConstraint);
+
+public:
+    SimonFrameOrientationConstraint();
+    SimonFrameOrientationConstraint(std::string name);
+    void addFramePair(SimonFrameOrientationConstraintPair pair) {
+        append_frame_pairs(std::move(pair));
+    }
+    void addFramePair(const std::string& frame1_path,const std::string& frame2_path, double minimum_angle,
+        double maximum_angle) {
+        append_frame_pairs(SimonFrameOrientationConstraintPair(frame1_path,frame2_path, minimum_angle,maximum_angle));
+    }
+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,
+        SimonFrameOrientationConstraintPair,
+        "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 // OPENSIM_SIMONFRAMEORIENTATIONCONSTRAINT_H 

Moco/Moco/osimSimonFrameOrientationConstraintDLL.h

@@ -0,0 +1,31 @@
+#ifndef OPENSIM_OSIMSIMONFRAMEORIENTATIONCONSTRAINTDLL_H 
+#define OPENSIM_OSIMSIMONFRAMEORIENTATIONCONSTRAINTDLL_H 
+/* -------------------------------------------------------------------------- *
+ * OpenSim: osimMocoCustomEffortGoalDLL.h                                     *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2019 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Christopher Dembia                                              *
+ *                                                                            *
+ * 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.                                             *
+ * -------------------------------------------------------------------------- */
+
+#ifndef _WIN32
+    #define OSIMMOCOCUSTOMEFFORTGOAL_API
+#else
+    #ifdef OSIMMOCOCUSTOMEFFORTGOAL_EXPORTS
+        #define OSIMMOCOCUSTOMEFFORTGOAL_API __declspec(dllexport)
+    #else
+        #define OSIMMOCOCUSTOMEFFORTGOAL_API __declspec(dllimport)
+    #endif
+#endif
+
+#endif // OPENSIM_OSIMSIMONFRAMEORIENTATIONCONSTRAINTDLL_H