Implementation of floating-base task space control #3703

nathantpickle-cfdrc · 2024-02-12T15:13:08Z

Fixes issue #3636

Brief summary of changes

New classes for task space control have been added in OpenSim/Tools. Currently the files are all prefixed with "TaskSpace", but could be renamed and/or moved as needed. Examples are located in OpenSim/Examples/TaskSpace.

A manuscript preprint with the associated mathematical derivation can be found here:

https://www.biorxiv.org/content/10.1101/2024.02.13.580044v1

Testing I've completed

All examples have been tested and are working correctly. All new functionality is located in new classes and does not affect existing code/tests.

Looking for feedback on...

Integration - should the new task classes be integrated with existing tracking tasks (IK, CMC)?
Organization - does the structure of the new code make sense? Should anything be moved/renamed?
Clarity - is all documentation clear?
Style - Are the naming/comment conventions consistent with OpenSim guidelines?

CHANGELOG.md (choose one)

will update once changes are finalized

This change is

Update 3Nov2022 See merge request Biomechanics/tools/3rd-party/opensim-core-mirror!1

… const issues

Cfdrc cmc point jacobian See merge request Biomechanics/tools/3rd-party/opensim-core-mirror!2

…to cfdrc-main

wip: Added in new task classes wip: Add in TaskSpaceTorqueController feat: All code migrated and initial example working wip: Trying to get all Stanev examples working checkpoint: Got Perreault example output to match original code checkpoint: Figured out issue with ExampleClosedKinematicChain.cpp. Jacobian hits singularity at Pi/2 wip: All examples work. Starting cleanup fix: Resolve issues with constraint null space chore: Cleaned up from debugging chore: Update license

…/tools/3rd-party/opensim-core-mirror into cfdrc-taskspace-release

aymanhab · 2024-02-13T21:32:03Z

Build failures are due to the latest PR to use sockets instead of actuator names
#3683
Sorry for the late change, but please update the code to match the latest conventions/API . Thank you @nathantpickle-cfdrc

nathantpickle-cfdrc · 2024-02-13T23:25:19Z

Build failures are due to the latest PR to use sockets instead of actuator names #3683 Sorry for the late change, but please update the code to match the latest conventions/API . Thank you @nathantpickle-cfdrc

Thanks @aymanhab, I'll take a look at it.

nickbianco

Great start, @nathantpickle-cfdrc! I used my first pass to get a feel for the layout of the files, to make some general formatting suggestions, and comment on some of the changes to existing CMC classes. On the next pass I can make more detailed file-by-file review comments. This will probably take multiple review passes given the amount of files, but what I've looked through so far seems well organized and commented, so hopefully it shouldn't take too much time.

One more high-level comment: based on the amount of files added, I think it would make sense to group everything in the Tools directory under a subdirectory called TaskSpace.

Reviewed 64 of 64 files at r1, all commit messages.
Reviewable status: all files reviewed, 44 unresolved discussions (waiting on @nathantpickle-cfdrc)

CMakeLists.txt line 803 at r1 (raw file):

if(OPENSIM_WITH_CASADI)
    find_package(casadi 3.4.4 REQUIRED
            HINTS "${OPENSIM_DEPENDENCIES_DIR}/casadi")

Was this addition necessary for creating local builds?

CMakeLists.txt line 836 at r1 (raw file):

    else()
        find_package(Ipopt REQUIRED CONFIG
                HINTS "${OPENSIM_DEPENDENCIES_DIR}/ipopt/lib/cmake/Ipopt")

Same question as above.

OpenSim/Examples/TaskSpace/ExampleAbsoluteCoordinates/CMakeLists.txt line 23 at r1 (raw file):

#     "${OpenSim_INCLUDE_DIRS}/OpenSim"
#     ${Simbody_INCLUDE_DIR}
#     "${OpenSim_INCLUDE_DIRS}/../spdlog/include")

Remove dead code?

Nice work using the existing CMake macros!

OpenSim/Examples/TaskSpace/ExampleAbsoluteCoordinates/ExampleAbsoluteCoordinates.cpp line 12 at r1 (raw file):

 *
 * @see <a href="http://ieeexplore.ieee.org/document/8074739/">[Publication]</a>
 */

For your examples, use an OpenSim-style header that includes the Apache 2.0 license. Example here: https://github.com/opensim-org/opensim-core/blob/main/OpenSim/Simulation/Model/AbstractGeometryPath.h.

OpenSim/Examples/TaskSpace/ExampleArm26/arm26.osim line 1 at r1 (raw file):

<?xml version="1.0" encoding="UTF-8" ?>

If this is a copy of the arm26 model from somewhere else in the repo, consider copying the file during the install step via CMake so we don't introduce additional copies into the codebase.

Here's an example of copy code resources from one directory to another: https://github.com/opensim-org/opensim-core/blob/main/OpenSim/Examples/Moco/example3DWalking/CMakeLists.txt.

OpenSim/Examples/TaskSpace/ExamplePerreault2001Experiment/ExamplePerreault2001Experiment.cpp line 13 at r1 (raw file):

 *
 * @see <a href="http://ieeexplore.ieee.org/document/8074739/">[Publication]</a>
 */

Use OpenSim-style header.

OpenSim/Examples/TaskSpace/ExamplePerreault2001Experiment/opensim.log line 1 at r1 (raw file):

[2024-01-18 08:29:30.166] [info] Updating Model file from 30000 to latest format...

This log file should be removed.

OpenSim/Examples/TaskSpace/ExampleStandingBalance2D/ExampleStandingBalance2D.cpp line 17 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Ah, I see you do have header format! A couple of comments:

Since you are merging this into opensim-core, the copyright line should read: "Copyright (c) 2005-2024 Stanford University and the Authors". Be sure to give yourself credit via author lines and references below the header, but the copyright line should reflect the rest of opensim-core.
You don't need "OpenSim TaskSpace" here, just "OpenSim".

These apply to all other headers in the added files.

OpenSim/Examples/TaskSpace/ExampleStandingBalance2D/ExampleStandingBalance2D.cpp line 20 at r1 (raw file):

/**
 * @file ExampleStandingBalance2D.cpp

Use camel case for all example names, e.g., exampleStandingBalance2D.cpp. You can apply camel case to the folder names as well.

OpenSim/Examples/TaskSpace/ExampleStandingBalance2D/gait2d.osim line 1 at r1 (raw file):

<?xml version="1.0" encoding="UTF-8" ?>

Duplicate model?

OpenSim/Examples/TaskSpace/ExampleUpperLimb/mobl_2016_ideal_muscles.osim line 3 at r1 (raw file):

<?xml version="1.0" encoding="UTF-8" ?>
<OpenSimDocument Version="30000">
	<Model name="MoBL_2016_Ideal_Muscles">

Similar to previous comments, avoid duplicate models wherever possible.

OpenSim/Tools/CMC_Joint.h line 65 at r1 (raw file):

    // Work Variables
    const Coordinate *_q;

Why is this const introduced here? Does it affect the CMC interface in any meaningful way?

There are several more instances of this change in the CMC classes, and this comment applies to those as well (so I don't make the same comment a bunch of times).

OpenSim/Tools/CMC_Orientation.h line 77 at r1 (raw file):

    // COMPUTATIONS
    //--------------------------------------------------------------------------
    void computeJacobian(const SimTK::State& s) override;

Why was the SimTK::State argument added here?

OpenSim/Tools/TaskSpaceBodyTask.h line 14 at r1 (raw file):

 *                                                                            *
 * Please refer to the following publication for mathematical details, and    *
 * cite this paper if you use this code in your own work:                 *

Make sure all *'s are aligned.

OpenSim/Tools/TaskSpaceBodyTask.h line 17 at r1 (raw file):

 *                                                                            *
 * Pickle and Sundararajan. "Predictive simulation of human movement in       *
 * OpenSim using floating-base task space control".                           *

Citations can be included in the doc string for classes, not in the header.

OpenSim/Tools/TaskSpaceBodyTask.h line 32 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceBodyTask.h line 83 at r1 (raw file):

    // virtual void computeJacobian(const SimTK::State& s);
    // virtual void computeBias(const SimTK::State& s);
    // virtual void update(const SimTK::State& s);

Unused code?

OpenSim/Tools/TaskSpaceBodyTask.cpp line 30 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceComputeControlsEventHandler.h line 33 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceConstraintModel.h line 33 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 1 at r1 (raw file):

#ifndef OPENSIM_TASK_SPACE_COORDINATE_TASK_H_

I used this class to make some general comments about formatted, mostly related to comments. Your comments are descriptive and clear, which is great, but some formatting changes could reduce visual clutter, especially in the .cpp file. You don't need to repeat comments in the header in the .cpp files.

The following class is a good reference for comment style: https://github.com/opensim-org/opensim-core/blob/main/OpenSim/Simulation/Model/AbstractGeometryPath.h.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 4 at r1 (raw file):

#define OPENSIM_TASK_SPACE_COORDINATE_TASK_H_
/* -------------------------------------------------------------------------- *
 *                  OpenSim: TaskSpaceCoordinateTask.h                       *

Mis-aligned *.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 32 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 36 at r1 (raw file):

//============================================================================
// INCLUDE
//============================================================================

Unnecessary for includes.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 47 at r1 (raw file):

//=============================================================================
//=============================================================================

Extra horizontal lines like these could be remove. They appear in other parts of OpenSim code, but add more clutter than organization IMO.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 92 at r1 (raw file):

     *
     * @param aTask Joint task to be copied.
     */

Generic copy constructors like this one usually don't need a doc string.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 93 at r1 (raw file):

     * @param aTask Joint task to be copied.
     */
    CoordinateTask(const CoordinateTask& aTask);

If you're defining a copy constructor and destructor, you should define a copy assignment operator as well (re: https://en.cppreference.com/w/cpp/language/rule_of_three).

OpenSim/Tools/TaskSpaceCoordinateTask.h line 98 at r1 (raw file):

     * Destructor.
     */
    virtual ~CoordinateTask();

Similarly, generic destructors can go without a doc string.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 145 at r1 (raw file):

    void computeDesiredAccelerations(
            const SimTK::State& s, double aTI, double aTF) override;

Remove extra spaces (I like to let my IDE do this automatically for me).

OpenSim/Tools/TaskSpaceCoordinateTask.h line 196 at r1 (raw file):

     */
    double computeJointLimitMatrix(const SimTK::State& s);

Extra spaces.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 224 at r1 (raw file):

     */
    void setNull();

Extra spaces.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 247 at r1 (raw file):

    //--------------------------------------------------------------------------
    // OPERATORS
    //--------------------------------------------------------------------------

Comment header in the wrong location?

OpenSim/Tools/TaskSpaceCoordinateTask.h line 249 at r1 (raw file):

    //--------------------------------------------------------------------------

//==========================================================================

Can be removed.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 250 at r1 (raw file):

//==========================================================================
}; // END of class CoordinateTask

class CoordinateTask is descriptive enough here.

OpenSim/Tools/TaskSpaceCoordinateTask.h line 251 at r1 (raw file):

//==========================================================================
}; // END of class CoordinateTask
//==========================================================================

Can be removed.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 30 at r1 (raw file):

 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

Use an OpenSim-style header.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 34 at r1 (raw file):

//=============================================================================
// INCLUDES
//=============================================================================

Unnecessary.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 44 at r1 (raw file):

//=============================================================================
// STATICS
//=============================================================================

Unnecessary. Only use comment headers if content for them exists.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 49 at r1 (raw file):

// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________

Similar to my comment in the header file, I would suggest avoiding extra horizontal lines like this one.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 62 at r1 (raw file):

 * @todo Instead of an integer id, the name of the coordinate
 * should be used.
 */

Doxygen-style comments won't appear in autogenerated docs when included in .cpp files, so you can reserve them for the header files.

In general, you can use categorical comment headers like:

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================

But anything more than this will clutter the .cpp IMO.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 175 at r1 (raw file):

//-----------------------------------------------------------------------------
// ASSIGNMENT
//-----------------------------------------------------------------------------

This "sub-header" could be excluded.

OpenSim/Tools/TaskSpaceCoordinateTask.cpp line 411 at r1 (raw file):

//-----------------------------------------------------------------------------
// UPDATE FROM XML NODE
//-----------------------------------------------------------------------------

This "sub-header" could be excluded.

OpenSim/Tools/TaskSpaceUtilities.h line 32 at r1 (raw file):

 * -------------------------------------------------------------------------- */
#ifndef OPENSIM_TASK_SPACE_UTILITIES_H_
#define OPENSIM_TASK_SPACE_UTILITIES_H_

Header guards should go above file headers.

OpenSim/Tools/TrackingTask.h line 65 at r1 (raw file):

    /** Model. */
    const Model *_model;

Similar to the CMC class changes, why the switch to const here?

nickbianco · 2024-02-15T00:00:53Z

A few more thoughts:

There currently are no tests accompanying these new classes. Examples are good, but a proper test suite with unit tests should be included cover common interface usage and important calculations. Look to CONTRIBUTING.md and the Moco test folder for guidance here. New tests should use the Catch2 testing framework.
I'm wondering if it makes sense to break off the examples into a separate PR, given the volume of new files here. It would speed up the review cycles for the primary new classes and likely make reviewing the examples easier in the follow-up PR.

adamkewley

(forgot to submit these comments the other day)

I didn't have time to read everything yet, but here are some of the things I noticed in a few files. I can read it more as it evolves

adamkewley · 2024-02-13T08:16:12Z

OpenSim/Tools/TrackingTask.h

-    virtual void setModel(OpenSim::Model& aModel);
-    Model* getModel() const;
+    virtual void setModel(const OpenSim::Model& aModel);
+    const Model* getModel() const;


This is a nice change (no disagreements there) but it's a drive-by that breaks the API

adamkewley · 2024-02-13T08:18:08Z

OpenSim/Tools/TaskSpaceUtilities.h

+ * Pickle and Sundararajan. "Predictive simulation of human movement in       *
+ * OpenSim using floating-base task space control".                           *
+ *                                                                            *
+ * Copyright (c) 2023 CFD Research Corporation and the Authors                *


I'd consult with the primary project maintainers (e.g. @aymanhab , @nickbianco ) whether having mixed copyright notices within one project's codebase is standard or not - the legality of mixing copyrights into one final product with its own copyright constraints isn't obvious.

adamkewley · 2024-02-13T08:18:40Z

OpenSim/Tools/TaskSpaceUtilities.h

+#include "simbody/internal/MobilizedBody.h"
+#include "OpenSim/Simulation/Model/Model.h"
+
+/**


Comment doesn't match code

adamkewley · 2024-02-13T08:19:23Z

OpenSim/Tools/TaskSpaceUtilities.h

+ * @param model
+ * @return Matrix
+ */
+SimTK::Matrix calcM(


Seems like a helper function that should be hidden in a CPP file or in an appropriate namespace (e.g. OpenSim::detail, OpenSim::internal, or similar) so that users don't get ideas.

adamkewley · 2024-02-13T08:20:41Z

OpenSim/Tools/TaskSpaceUtilities.h

+SimTK::Matrix calcM(
+        const SimTK::State& s, const OpenSim::Model& model);
+
+/**


Similar idea for the remaining funcs here: if they get exported to downstream API users then you'll pollute the OpenSim namespace with a bunch of functions that then need to be maintained by project maintainers for backwards-compat

adamkewley · 2024-02-13T08:33:43Z

OpenSim/Tools/TaskSpaceUtilities.cpp

+    FactorSVD AInvSVD(A);
+    try {
+        AInvSVD.inverse(Ainv);
+    } catch (...) {


catch (...) with your own custom error message seems unusual, almost all exceptions coming out of SimTK inherit from std::exception, so you can usually just catch (const std::exception& ex) and append that exception's message to your own (with extra information), but what you're doing here is Pokemon-catching with your own explanation (which may mismatch the explanation given by the actual implementation)

adamkewley · 2024-02-13T08:35:14Z

OpenSim/Tools/TaskSpaceUtilities.cpp

+        }
+        Matrix SInv = diagonalize(dInv);
+        Ainv = VT.transpose() * SInv * U.transpose();
+    } catch (...) {


♪ Gotta catch em' alllllll, that's true - our courage will pull us through ♪

But seriously, catch (...) is the kind of thing that's usually reserved for a top-level application: you should be catching std::exception and ensuring information in the exception is correctly propagated upwards/swallowed.

adamkewley · 2024-02-13T08:38:17Z

OpenSim/Tools/TaskSpaceTorqueController.h

+    void extendConnectToModel(Model& model) override;
+
+    /** Selection matrix \f$ \tau^{'} = B \tau \f$ for under-actuation. */
+    mutable SimTK::Matrix B;


mutable, the sussest of keywords

adamkewley · 2024-02-13T08:39:06Z

OpenSim/Tools/TaskSpaceTorqueController.h

+    std::vector<SimTK::Vector> bt_p; 
+
+    /** Stores the applied generalized forces. */
+    mutable Storage appliedForces;


mutable, and your member naming conventions switch halfway through this: OpenSim usually uses underscore-prefixed member variables.

adamkewley · 2024-02-13T08:39:48Z

OpenSim/Tools/TaskSpaceTorqueController.h

+    void computeControls(
+            const SimTK::State& s, SimTK::Vector& controls) const override;
+
+    SimTK::Vector _tau;


Rando member field? Member fields are usually hidden in a private block

nathantpickle-cfdrc · 2024-02-15T14:51:16Z

Thanks @nickbianco and @adamkewley for the initial review! I'll get started on the suggested changes.

@nickbianco I'm on board with your suggestion to separate out the examples into a separate PR. I can remove the examples and add tests that will exercise the new classes.

CFDResearch-HATCH2020 and others added 10 commits November 4, 2022 08:03

Merge branch 'update_11-3-22' into 'master'

90718a3

Update 3Nov2022 See merge request Biomechanics/tools/3rd-party/opensim-core-mirror!1

Merge branch 'master' of https://github.com/opensim-org/opensim-core

7a2d8bd

feat: Add implementation of computeJacobian and computeBias. Also fix…

bcbbb88

… const issues

Merge branch 'cfdrc_cmc_point_jacobian' into 'cfdresearch_master'

23a1a9b

Cfdrc cmc point jacobian See merge request Biomechanics/tools/3rd-party/opensim-core-mirror!2

fix: Changes to CMake build system

9e4add8

Merge branch 'main' of https://github.com/opensim-org/opensim-core in…

622b5cd

…to cfdrc-main

Merge branch 'cfdrc-taskspace-release' of code.cfdrc.com:Biomechanics…

3eea8e2

…/tools/3rd-party/opensim-core-mirror into cfdrc-taskspace-release

chore: Minor update to license text

eb447cc

nickbianco requested changes Feb 14, 2024

View reviewed changes

adamkewley reviewed Feb 15, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implementation of floating-base task space control #3703

Implementation of floating-base task space control #3703

nathantpickle-cfdrc commented Feb 12, 2024 •

edited

aymanhab commented Feb 13, 2024

nathantpickle-cfdrc commented Feb 13, 2024

nickbianco left a comment •

edited

nickbianco commented Feb 15, 2024

adamkewley left a comment

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

adamkewley Feb 13, 2024

nathantpickle-cfdrc commented Feb 15, 2024

Implementation of floating-base task space control #3703

Are you sure you want to change the base?

Implementation of floating-base task space control #3703

Conversation

nathantpickle-cfdrc commented Feb 12, 2024 • edited

Brief summary of changes

Testing I've completed

Looking for feedback on...

CHANGELOG.md (choose one)

aymanhab commented Feb 13, 2024

nathantpickle-cfdrc commented Feb 13, 2024

nickbianco left a comment • edited

Choose a reason for hiding this comment

nickbianco commented Feb 15, 2024

adamkewley left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nathantpickle-cfdrc commented Feb 15, 2024

nathantpickle-cfdrc commented Feb 12, 2024 •

edited

nickbianco left a comment •

edited