Merge pull request #4 from manumerous/feature/external-torque-cost

External torque cost

Author: manumerous

.github/workflows/build_test.yml

@@ -1,12 +1,13 @@
-name: Build Tests
+name: Full Install & Build Tests
 
 on:
   push:
     branches:
       - master
   pull_request:
     branches:
-      - "*"
+      - '**'  # double asterisk matches any branch
+    types: [opened, synchronize, reopened]
 
 jobs:
   build:
@@ -30,15 +31,20 @@ jobs:
           restore-keys: |
             ${{ runner.os }}-apt-
 
-      - name: Install Dependencies
+      - name: Install Base Dependencies
         run: |
           sudo apt-get update && sudo apt-get upgrade -y
-          # Install dependencies listed in dependencies.txt
-          # Make sure dependencies.txt is at the root of your repo or update the path accordingly
-          xargs -a dependencies.txt sudo apt-get install -y
+          sudo apt-get update && apt-get install -y --no-install-recommends \
+          curl gnupg2 lsb-release software-properties-common \
+          locales \
+          git \
+          x11-apps \
+          mesa-utils \
+          gettext-base 
 
-          # Install additional ROS packages
-          sudo apt-get install -y ros-jazzy-ament-cmake-clang-format ros-jazzy-joint-state-publisher-gui ros-jazzy-xacro ros-jazzy-mcap-vendor ros-jazzy-interactive-markers
+      - name: Install Project Dependencies
+        run: |
+          envsubst < dependencies.txt | xargs sudo apt-get install -y --no-install-recommends
 
       - name: Initialize submodules
         run: |

humanoid_nmpc/humanoid_centroidal_mpc/src/CentroidalMpcInterface.cpp

@@ -205,8 +205,6 @@ void CentroidalMpcInterface::setupOptimalControlProblem() {
     problemPtr_->softConstraintPtr->add(footName + "_frictionForceCone", factory.getFrictionForceConeConstraint(i));
     problemPtr_->softConstraintPtr->add(footName + "_contactMomentXY",
                                         factory.getContactMomentXYConstraint(i, footName + "_contact_moment_XY_constraint"));
-    problemPtr_->equalityConstraintPtr->add(footName + "_contactMomentZ",
-                                            factory.getContactMomentZConstraint(i, footName + "_contact_moment_Z_constraint"));
     problemPtr_->equalityConstraintPtr->add(footName + "_zeroWrench", factory.getZeroWrenchConstraint(i));
     problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getStanceFootConstraint(*eeKinematicsPtr, i));
     problemPtr_->equalityConstraintPtr->add(footName + "_normalVelocity", getNormalVelocityConstraint(*eeKinematicsPtr, i));
@@ -219,6 +217,7 @@ void CentroidalMpcInterface::setupOptimalControlProblem() {
     problemPtr_->costPtr->add(footTrackingCostName, std::unique_ptr<StateInputCost>(new CentroidalMpcEndEffectorFootCost(
                                                         *referenceManagerPtr_, footTrackingCostWeights, *pinocchioInterfacePtr_,
                                                         *mpcRobotModelADPtr_, i, footTrackingCostName, modelSettings_)));
+    problemPtr_->costPtr->add(footName + "_ExternalTorqueQuadraticCost", factory.getExternalTorqueQuadraticCost(i));
   }
 
   // Pre-computation

humanoid_nmpc/humanoid_common_mpc/CMakeLists.txt

@@ -82,11 +82,11 @@ add_library(${PROJECT_NAME}
         src/constraint/EndEffectorKinematicsTwistConstraint.cpp
         src/constraint/FrictionForceConeConstraint.cpp
         src/constraint/ContactMomentXYConstraintCppAd.cpp
-        src/constraint/ContactMomentZConstraint.cpp
         src/constraint/EndEffectorKinematicsLinearVelConstraint.cpp
         src/cost/StateInputQuadraticCost.cpp
         src/cost/EndEffectorKinematicCostHelpers.cpp
         src/cost/EndEffectorKinematicsQuadraticCost.cpp
+        src/cost/ExternalTorqueQuadraticCostAD.cpp
         src/command/TargetTrajectoriesCalculatorBase.cpp
         src/gait/Gait.cpp
         src/gait/GaitSchedule.cpp

humanoid_nmpc/humanoid_common_mpc/include/humanoid_common_mpc/HumanoidCostConstraintFactory.h

@@ -72,12 +72,12 @@ class HumanoidCostConstraintFactory {
 
   std::unique_ptr<StateInputCost> getContactMomentXYConstraint(size_t contactPointIndex, const std::string& name) const;
 
-  std::unique_ptr<StateInputConstraint> getContactMomentZConstraint(size_t contactPointIndex, const std::string& name) const;
-
   std::unique_ptr<StateInputConstraint> getZeroWrenchConstraint(size_t contactPointIndex) const;
 
   std::unique_ptr<StateInputCost> getFrictionForceConeConstraint(size_t contactPointIndex) const;
 
+  std::unique_ptr<StateInputCost> getExternalTorqueQuadraticCost(size_t contactPointIndex) const;
+
  private:
   std::string taskFile_;
   std::string referenceFile_;

humanoid_nmpc/humanoid_common_mpc/include/humanoid_common_mpc/HumanoidPreComputation.h

@@ -68,7 +68,7 @@ class HumanoidPreComputation : public PreComputation {
  protected:
   HumanoidPreComputation(const HumanoidPreComputation& rhs);
 
-  void updatePinocchioFramePlacements(const vector_t& generalizedCoordinates);
+  void updatePinocchioModelKinematics(const vector_t& generalizedCoordinates);
 
   PinocchioInterface pinocchioInterface_;
   const SwingTrajectoryPlanner* swingTrajectoryPlannerPtr_;

humanoid_nmpc/humanoid_common_mpc/include/humanoid_common_mpc/constraint/ContactMomentZConstraint.h renamed to humanoid_nmpc/humanoid_common_mpc/include/humanoid_common_mpc/cost/ExternalTorqueQuadraticCostAD.h

@@ -1,5 +1,5 @@
 /******************************************************************************
-Copyright (c) 2024, 1X Technologies. All rights reserved.
+Copyright (c) 2025, Manuel Yves Galliker. All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
@@ -29,51 +29,67 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include <ocs2_core/constraint/StateInputConstraintCppAd.h>
+#include <ocs2_core/cost/StateInputGaussNewtonCostAd.h>
 #include <ocs2_pinocchio_interface/PinocchioInterface.h>
+#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+#include <pinocchio/algorithm/frames.hpp>
 
 #include "humanoid_common_mpc/common/ModelSettings.h"
 #include "humanoid_common_mpc/common/MpcRobotModelBase.h"
-#include "humanoid_common_mpc/common/Types.h"
 #include "humanoid_common_mpc/reference_manager/SwitchedModelReferenceManager.h"
 
+#include "humanoid_common_mpc/cost/EndEffectorKinematicCostHelpers.h"
+
 namespace ocs2::humanoid {
 
-class ContactMomentZConstraint final : public StateInputConstraintCppAd {
+class ExternalTorqueQuadraticCostAD : public ocs2::StateInputCostGaussNewtonAd {
  public:
-  /*
-   * Constructor
-   * @param [in] referenceManager : Switched model ReferenceManager.
-   * @param [in] contactPointIndex : The 3 DoF contact index.
-   */
-  ContactMomentZConstraint(const SwitchedModelReferenceManager& referenceManager,
-                           const PinocchioInterface& pinocchioInterface,
-                           const MpcRobotModelBase<ad_scalar_t>& mpcRobotModel,
-                           size_t contactPointIndex,
-                           std::string costName,
-                           const ModelSettings& modelSettings);
-
-  ~ContactMomentZConstraint() override = default;
-  ContactMomentZConstraint* clone() const override { return new ContactMomentZConstraint(*this); }
+  struct Config {
+    std::vector<std::string> activeJointNames;
+    vector_t weights;
+  };
+
+  ExternalTorqueQuadraticCostAD(size_t endEffectorIndex,
+                                Config config,
+                                const SwitchedModelReferenceManager& referenceManager,
+                                const PinocchioInterface& pinocchioInterface,
+                                const MpcRobotModelBase<ad_scalar_t>& mpcRobotModelAD,
+                                const ModelSettings& modelSettings);
+
+  ~ExternalTorqueQuadraticCostAD() override = default;
+  ExternalTorqueQuadraticCostAD* clone() const override { return new ExternalTorqueQuadraticCostAD(*this); }
+
+  virtual vector_t getParameters(scalar_t time,
+                                 const TargetTrajectories& targetTrajectories,
+                                 const PreComputation& preComputation) const override;
 
   bool isActive(scalar_t time) const override;
-  void setActive(bool active) override { isActive_ = active; }
-  bool getActive() const override { return isActive_; }
-  size_t getNumConstraints(scalar_t time) const override { return n_constraints; }
+  void setActive(bool active) { isActive_ = active; }
+  bool getActive() const { return isActive_; }
 
- private:
-  ContactMomentZConstraint(const ContactMomentZConstraint& rhs);
+  void getWeights(vector_t& weights) const { weights = sqrtWeights_.cwiseProduct(sqrtWeights_); }
+  void setWeights(const vector_t& weights) { sqrtWeights_ = weights.cwiseSqrt(); }
 
-  ad_vector_t constraintFunction(ad_scalar_t time,
+  static ExternalTorqueQuadraticCostAD::Config loadConfigFromFile(const std::string& filename,
+                                                                  const std::string& fieldname,
+                                                                  bool verbose = true);
+
+ protected:
+  ExternalTorqueQuadraticCostAD(const ExternalTorqueQuadraticCostAD& other);
+
+  ad_vector_t costVectorFunction(ad_scalar_t time,
                                  const ad_vector_t& state,
                                  const ad_vector_t& input,
-                                 const ad_vector_t& parameters) const override;
+                                 const ad_vector_t& parameters) override;
 
+  const size_t contactPointIndex_;
+  const pinocchio::FrameIndex frameID_;
+  const size_t n_parameters_;
+  vector_t sqrtWeights_;
+  const std::vector<std::string> activeJointNames_;
   const SwitchedModelReferenceManager* referenceManagerPtr_;
   PinocchioInterfaceCppAd pinocchioInterfaceCppAd_;
-  const MpcRobotModelBase<ad_scalar_t>* mpcRobotModelPtr_;
-  const size_t contactPointIndex_;
-  static const int n_constraints = 1;
+  const std::unique_ptr<MpcRobotModelBase<ad_scalar_t>> mpcRobotModelADPtr;
   bool isActive_ = true;
 };
 

humanoid_nmpc/humanoid_common_mpc/src/HumanoidCostConstraintFactory.cpp

@@ -46,9 +46,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <humanoid_common_mpc/cost/StateInputQuadraticCost.h>
 #include <humanoid_common_mpc/pinocchio_model/pinocchioUtils.h>
 #include "humanoid_common_mpc/constraint/ContactMomentXYConstraintCppAd.h"
-#include "humanoid_common_mpc/constraint/ContactMomentZConstraint.h"
 #include "humanoid_common_mpc/constraint/FootCollisionConstraint.h"
 #include "humanoid_common_mpc/constraint/JointLimitsSoftConstraint.h"
+#include "humanoid_common_mpc/cost/ExternalTorqueQuadraticCostAD.h"
 
 namespace ocs2::humanoid {
 
@@ -175,16 +175,6 @@ std::unique_ptr<StateInputCost> HumanoidCostConstraintFactory::getContactMomentX
 /******************************************************************************************************/
 /******************************************************************************************************/
 
-std::unique_ptr<StateInputConstraint> HumanoidCostConstraintFactory::getContactMomentZConstraint(size_t contactPointIndex,
-                                                                                                 const std::string& name) const {
-  return std::unique_ptr<StateInputConstraint>(new ContactMomentZConstraint(*referenceManagerPtr_, *pinocchioInterfacePtr_,
-                                                                            *mpcRobotModelADPtr_, contactPointIndex, name, modelSettings_));
-}
-
-/******************************************************************************************************/
-/******************************************************************************************************/
-/******************************************************************************************************/
-
 std::unique_ptr<StateInputConstraint> HumanoidCostConstraintFactory::getZeroWrenchConstraint(size_t contactPointIndex) const {
   return std::unique_ptr<StateInputConstraint>(new ZeroWrenchConstraint(*referenceManagerPtr_, contactPointIndex, *mpcRobotModelPtr_));
 }
@@ -240,4 +230,21 @@ std::unique_ptr<StateCost> HumanoidCostConstraintFactory::getTerminalCost() cons
 /******************************************************************************************************/
 /******************************************************************************************************/
 
+std::unique_ptr<StateInputCost> HumanoidCostConstraintFactory::getExternalTorqueQuadraticCost(size_t contactPointIndex) const {
+  std::string fieldName;
+  if (contactPointIndex == 0) {
+    fieldName = "left_leg_torque_cost.";
+  }
+  if (contactPointIndex == 1) {
+    fieldName = "right_leg_torque_cost.";
+  }
+  ExternalTorqueQuadraticCostAD::Config config = ExternalTorqueQuadraticCostAD::loadConfigFromFile(taskFile_, fieldName, verbose_);
+  return std::make_unique<ExternalTorqueQuadraticCostAD>(contactPointIndex, config, *referenceManagerPtr_, *pinocchioInterfacePtr_,
+                                                         *mpcRobotModelADPtr_, modelSettings_);
+}
+
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+
 }  // namespace ocs2::humanoid

humanoid_nmpc/humanoid_common_mpc/src/HumanoidPreComputation.cpp

@@ -80,12 +80,12 @@ HumanoidPreComputation* HumanoidPreComputation::clone() const {
 /******************************************************************************************************/
 /******************************************************************************************************/
 
-void HumanoidPreComputation::updatePinocchioFramePlacements(const vector_t& generalizedCoordinates) {
+void HumanoidPreComputation::updatePinocchioModelKinematics(const vector_t& q) {
   const pinocchio::Model& model = pinocchioInterface_.getModel();
   pinocchio::Data& data = pinocchioInterface_.getData();
 
   // Perform the forward kinematics over the kinematic tree
-  pinocchio::forwardKinematics(model, data, generalizedCoordinates);
+  pinocchio::forwardKinematics(model, data, q);
   pinocchio::updateFramePlacements(model, data);
 }
 
@@ -97,6 +97,8 @@ void HumanoidPreComputation::request(RequestSet request, scalar_t t, const vecto
     return;
   }
 
+  updatePinocchioModelKinematics(mpcRobotModelPtr_->getGeneralizedCoordinates(x));
+
   // lambda to set config for normal velocity constraints
   auto eeNormalVelConConfig = [&](size_t footIndex) {
     EndEffectorKinematicsLinearVelConstraint::Config config;
@@ -111,8 +113,6 @@ void HumanoidPreComputation::request(RequestSet request, scalar_t t, const vecto
   };
 
   if (request.contains(Request::Constraint)) {
-    updatePinocchioFramePlacements(mpcRobotModelPtr_->getGeneralizedCoordinates(x));
-
     for (size_t i = 0; i < N_CONTACTS; i++) {
       eeNormalVelConConfigs_[i] = eeNormalVelConConfig(i);
       pinocchio::FrameIndex frameID = pinocchioInterface_.getModel().getFrameId(mpcRobotModelPtr_->modelSettings.contactNames6DoF[i]);