|
1 | 1 | /****************************************************************************** |
2 | | -Copyright (c) 2024, 1X Technologies. All rights reserved. |
| 2 | +Copyright (c) 2025, Manuel Yves Galliker. All rights reserved. |
3 | 3 |
|
4 | 4 | Redistribution and use in source and binary forms, with or without |
5 | 5 | 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. |
29 | 29 |
|
30 | 30 | #pragma once |
31 | 31 |
|
32 | | -#include <ocs2_core/constraint/StateInputConstraintCppAd.h> |
| 32 | +#include <ocs2_core/cost/StateInputGaussNewtonCostAd.h> |
33 | 33 | #include <ocs2_pinocchio_interface/PinocchioInterface.h> |
| 34 | +#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h> |
| 35 | +#include <pinocchio/algorithm/frames.hpp> |
34 | 36 |
|
35 | 37 | #include "humanoid_common_mpc/common/ModelSettings.h" |
36 | 38 | #include "humanoid_common_mpc/common/MpcRobotModelBase.h" |
37 | | -#include "humanoid_common_mpc/common/Types.h" |
38 | 39 | #include "humanoid_common_mpc/reference_manager/SwitchedModelReferenceManager.h" |
39 | 40 |
|
| 41 | +#include "humanoid_common_mpc/cost/EndEffectorKinematicCostHelpers.h" |
| 42 | + |
40 | 43 | namespace ocs2::humanoid { |
41 | 44 |
|
42 | | -class ContactMomentZConstraint final : public StateInputConstraintCppAd { |
| 45 | +class ExternalTorqueQuadraticCostAD : public ocs2::StateInputCostGaussNewtonAd { |
43 | 46 | public: |
44 | | - /* |
45 | | - * Constructor |
46 | | - * @param [in] referenceManager : Switched model ReferenceManager. |
47 | | - * @param [in] contactPointIndex : The 3 DoF contact index. |
48 | | - */ |
49 | | - ContactMomentZConstraint(const SwitchedModelReferenceManager& referenceManager, |
50 | | - const PinocchioInterface& pinocchioInterface, |
51 | | - const MpcRobotModelBase<ad_scalar_t>& mpcRobotModel, |
52 | | - size_t contactPointIndex, |
53 | | - std::string costName, |
54 | | - const ModelSettings& modelSettings); |
55 | | - |
56 | | - ~ContactMomentZConstraint() override = default; |
57 | | - ContactMomentZConstraint* clone() const override { return new ContactMomentZConstraint(*this); } |
| 47 | + struct Config { |
| 48 | + std::vector<std::string> activeJointNames; |
| 49 | + vector_t weights; |
| 50 | + }; |
| 51 | + |
| 52 | + ExternalTorqueQuadraticCostAD(size_t endEffectorIndex, |
| 53 | + Config config, |
| 54 | + const SwitchedModelReferenceManager& referenceManager, |
| 55 | + const PinocchioInterface& pinocchioInterface, |
| 56 | + const MpcRobotModelBase<ad_scalar_t>& mpcRobotModelAD, |
| 57 | + const ModelSettings& modelSettings); |
| 58 | + |
| 59 | + ~ExternalTorqueQuadraticCostAD() override = default; |
| 60 | + ExternalTorqueQuadraticCostAD* clone() const override { return new ExternalTorqueQuadraticCostAD(*this); } |
| 61 | + |
| 62 | + virtual vector_t getParameters(scalar_t time, |
| 63 | + const TargetTrajectories& targetTrajectories, |
| 64 | + const PreComputation& preComputation) const override; |
58 | 65 |
|
59 | 66 | bool isActive(scalar_t time) const override; |
60 | | - void setActive(bool active) override { isActive_ = active; } |
61 | | - bool getActive() const override { return isActive_; } |
62 | | - size_t getNumConstraints(scalar_t time) const override { return n_constraints; } |
| 67 | + void setActive(bool active) { isActive_ = active; } |
| 68 | + bool getActive() const { return isActive_; } |
63 | 69 |
|
64 | | - private: |
65 | | - ContactMomentZConstraint(const ContactMomentZConstraint& rhs); |
| 70 | + void getWeights(vector_t& weights) const { weights = sqrtWeights_.cwiseProduct(sqrtWeights_); } |
| 71 | + void setWeights(const vector_t& weights) { sqrtWeights_ = weights.cwiseSqrt(); } |
66 | 72 |
|
67 | | - ad_vector_t constraintFunction(ad_scalar_t time, |
| 73 | + static ExternalTorqueQuadraticCostAD::Config loadConfigFromFile(const std::string& filename, |
| 74 | + const std::string& fieldname, |
| 75 | + bool verbose = true); |
| 76 | + |
| 77 | + protected: |
| 78 | + ExternalTorqueQuadraticCostAD(const ExternalTorqueQuadraticCostAD& other); |
| 79 | + |
| 80 | + ad_vector_t costVectorFunction(ad_scalar_t time, |
68 | 81 | const ad_vector_t& state, |
69 | 82 | const ad_vector_t& input, |
70 | | - const ad_vector_t& parameters) const override; |
| 83 | + const ad_vector_t& parameters) override; |
71 | 84 |
|
| 85 | + const size_t contactPointIndex_; |
| 86 | + const pinocchio::FrameIndex frameID_; |
| 87 | + const size_t n_parameters_; |
| 88 | + vector_t sqrtWeights_; |
| 89 | + const std::vector<std::string> activeJointNames_; |
72 | 90 | const SwitchedModelReferenceManager* referenceManagerPtr_; |
73 | 91 | PinocchioInterfaceCppAd pinocchioInterfaceCppAd_; |
74 | | - const MpcRobotModelBase<ad_scalar_t>* mpcRobotModelPtr_; |
75 | | - const size_t contactPointIndex_; |
76 | | - static const int n_constraints = 1; |
| 92 | + const std::unique_ptr<MpcRobotModelBase<ad_scalar_t>> mpcRobotModelADPtr; |
77 | 93 | bool isActive_ = true; |
78 | 94 | }; |
79 | 95 |
|
|
0 commit comments