diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..6b3e7a4 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "submodules/qpOASES"] + path = submodules/qpOASES + url = https://github.com/coin-or/qpOASES diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt new file mode 100644 index 0000000..07e3409 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(ocs2_quadruped_controller) + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(CONTROLLER_INCLUDE_DEPENDS + pluginlib + rcpputils + controller_interface + realtime_tools + + ocs2_legged_robot_ros + ocs2_self_collision + control_input_msgs + nav_msgs + qpOASES +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach () + +add_library(${PROJECT_NAME} SHARED + src/Ocs2QuadrupedController.cpp + + # src/estimator/FromTopicEstimate.cpp + # src/estimator/LinearKalmanFilter.cpp + src/estimator/StateEstimateBase.cpp + + src/wbc/HierarchicalWbc.cpp + src/wbc/HoQp.cpp + src/wbc/WbcBase.cpp + src/wbc/WeightedWbc.cpp + + src/interface/constraint/EndEffectorLinearConstraint.cpp + src/interface/constraint/FrictionConeConstraint.cpp + src/interface/constraint/ZeroForceConstraint.cpp + src/interface/constraint/NormalVelocityConstraintCppAd.cpp + src/interface/constraint/ZeroVelocityConstraintCppAd.cpp + src/interface/constraint/SwingTrajectoryPlanner.cpp + src/interface/initialization/LeggedRobotInitializer.cpp + src/interface/SwitchedModelReferenceManager.cpp + src/interface/LeggedRobotPreComputation.cpp + src/interface/LeggedInterface.cpp + +) +target_include_directories(${PROJECT_NAME} + PUBLIC + ${qpOASES_INCLUDE_DIR} + "$" + "$") +ament_target_dependencies( + ${PROJECT_NAME} PUBLIC + ${CONTROLLER_INCLUDE_DEPENDS} +) + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif () + +ament_package() diff --git a/controllers/ocs2_quadruped_controller/LICENSE b/controllers/ocs2_quadruped_controller/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md new file mode 100644 index 0000000..7ed237e --- /dev/null +++ b/controllers/ocs2_quadruped_controller/README.md @@ -0,0 +1,7 @@ +# OCS2 Quadruped Controller + +## 2. Build +```bash +cd ~/ros2_ws +colcon build --packages-up-to ocs2_quadruped_controller +``` \ No newline at end of file diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h new file mode 100644 index 0000000..90f1471 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h @@ -0,0 +1,33 @@ +// +// Created by qiayuan on 2022/7/24. +// +#pragma once + +#include "StateEstimateBase.h" + +#include + +#pragma once +namespace ocs2::legged_robot { + using namespace ocs2; + + class FromTopicStateEstimate : public StateEstimateBase { + public: + FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics); + + void updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, + const vector3_t &linearAccelLocal, + const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance, + const matrix3_t &linearAccelCovariance) override { + }; + + vector_t update(const ros::Time &time, const ros::Duration &period) override; + + private: + void callback(const nav_msgs::Odometry::ConstPtr &msg); + + ros::Subscriber sub_; + realtime_tools::RealtimeBuffer buffer_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h new file mode 100644 index 0000000..664bcb0 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h @@ -0,0 +1,60 @@ +// +// Created by qiayuan on 2022/7/24. +// + +#pragma once + +#include "StateEstimateBase.h" + +#include +#include + +#include +#include +#include +#include + +namespace ocs2::legged_robot { + class KalmanFilterEstimate final : public StateEstimateBase { + public: + KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics); + + vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + void loadSettings(const std::string &taskFile, bool verbose); + + protected: + void updateFromTopic(); + + void callback(const nav_msgs::msg::Odometry::SharedPtr &msg); + + nav_msgs::msg::Odometry getOdomMsg(); + + vector_t feetHeights_; + + // Config + scalar_t footRadius_ = 0.02; + scalar_t imuProcessNoisePosition_ = 0.02; + scalar_t imuProcessNoiseVelocity_ = 0.02; + scalar_t footProcessNoisePosition_ = 0.002; + scalar_t footSensorNoisePosition_ = 0.005; + scalar_t footSensorNoiseVelocity_ = 0.1; + scalar_t footHeightSensorNoise_ = 0.01; + + private: + size_t numContacts_, dimContacts_, numState_, numObserve_; + + matrix_t a_, b_, c_, q_, p_, r_; + vector_t xHat_, ps_, vs_; + + // Topic + ros::Subscriber sub_; + realtime_tools::RealtimeBuffer buffer_; + tf2_ros::Buffer tfBuffer_; + tf2_ros::TransformListener tfListener_; + tf2::Transform world2odom_; + std::string frameOdom_, frameGuess_; + bool topicUpdated_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h new file mode 100644 index 0000000..f5c34b0 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -0,0 +1,81 @@ +// +// Created by qiayuan on 2021/11/15. +// +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot { + class StateEstimateBase { + public: + virtual ~StateEstimateBase() = default; + + StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics, rclcpp::Node::SharedPtr node); + + virtual void updateJointStates(const vector_t &jointPos, const vector_t &jointVel); + + virtual void updateContact(contact_flag_t contactFlag) { contactFlag_ = contactFlag; } + + virtual void updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, + const vector3_t &linearAccelLocal, + const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance, + const matrix3_t &linearAccelCovariance); + + virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0; + + size_t getMode() { return stanceLeg2ModeNumber(contactFlag_); } + + protected: + void updateAngular(const vector3_t &zyx, const vector_t &angularVel); + + void updateLinear(const vector_t &pos, const vector_t &linearVel); + + void publishMsgs(const nav_msgs::msg::Odometry &odom); + + PinocchioInterface pinocchioInterface_; + CentroidalModelInfo info_; + std::unique_ptr eeKinematics_; + + vector3_t zyxOffset_ = vector3_t::Zero(); + vector_t rbdState_; + contact_flag_t contactFlag_{}; + Eigen::Quaternion quat_; + vector3_t angularVelLocal_, linearAccelLocal_; + matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; + + rclcpp::Publisher::SharedPtr odomPub_; + rclcpp::Publisher::SharedPtr posePub_; + + rclcpp::Time lastPub_; + rclcpp::Node::SharedPtr node_; + }; + + template + T square(T a) { + return a * a; + } + + template + Eigen::Matrix quatToZyx(const Eigen::Quaternion &q) { + Eigen::Matrix zyx; + + SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999); + zyx(0) = std::atan2(2 * (q.x() * q.y() + q.w() * q.z()), + square(q.w()) + square(q.x()) - square(q.y()) - square(q.z())); + zyx(1) = std::asin(as); + zyx(2) = std::atan2(2 * (q.y() * q.z() + q.w() * q.x()), + square(q.w()) - square(q.x()) - square(q.y()) + square(q.z())); + return zyx; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h new file mode 100644 index 0000000..2c2b13c --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h @@ -0,0 +1,127 @@ +#pragma once +#pragma clang diagnostic push +#pragma ide diagnostic ignored "misc-non-private-member-variables-in-classes" +// +// Created by qiayuan on 2022/7/16. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "SwitchedModelReferenceManager.h" + +namespace ocs2::legged_robot { + class LeggedInterface : public RobotInterface { + public: + LeggedInterface(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile, + bool useHardFrictionConeConstraint = false); + + ~LeggedInterface() override = default; + + virtual void setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose); + + const OptimalControlProblem &getOptimalControlProblem() const override { return *problemPtr_; } + + const ModelSettings &modelSettings() const { return modelSettings_; } + const ddp::Settings &ddpSettings() const { return ddpSettings_; } + const mpc::Settings &mpcSettings() const { return mpcSettings_; } + const rollout::Settings &rolloutSettings() const { return rolloutSettings_; } + const sqp::Settings &sqpSettings() { return sqpSettings_; } + const ipm::Settings &ipmSettings() { return ipmSettings_; } + + const vector_t &getInitialState() const { return initialState_; } + const RolloutBase &getRollout() const { return *rolloutPtr_; } + PinocchioInterface &getPinocchioInterface() { return *pinocchioInterfacePtr_; } + const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidalModelInfo_; } + PinocchioGeometryInterface &getGeometryInterface() { return *geometryInterfacePtr_; } + + std::shared_ptr getSwitchedModelReferenceManagerPtr() const { + return referenceManagerPtr_; + } + + const Initializer &getInitializer() const override { return *initializerPtr_; } + + std::shared_ptr getReferenceManagerPtr() const override { + return referenceManagerPtr_; + } + + protected: + virtual void setupModel(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, bool verbose); + + virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose); + + virtual void setupPreComputation(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose); + + std::shared_ptr loadGaitSchedule(const std::string &file, bool verbose) const; + + std::unique_ptr getBaseTrackingCost(const std::string &taskFile, + const CentroidalModelInfo &info, bool verbose); + + matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info); + + static std::pair loadFrictionConeSettings( + const std::string &taskFile, bool verbose); + + std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, + scalar_t frictionCoefficient); + + std::unique_ptr getFrictionConeSoftConstraint(size_t contactPointIndex, + scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config & + barrierPenaltyConfig); + + std::unique_ptr > getEeKinematicsPtr(const std::vector &footNames, + const std::string &modelName); + + std::unique_ptr getZeroVelocityConstraint( + const EndEffectorKinematics &eeKinematics, + size_t contactPointIndex); + + std::unique_ptr getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, + const std::string &taskFile, + const std::string &prefix, bool verbose); + + ModelSettings modelSettings_; + mpc::Settings mpcSettings_; + ddp::Settings ddpSettings_; + sqp::Settings sqpSettings_; + ipm::Settings ipmSettings_; + const bool useHardFrictionConeConstraint_; + + std::unique_ptr pinocchioInterfacePtr_; + CentroidalModelInfo centroidalModelInfo_; + std::unique_ptr geometryInterfacePtr_; + + std::unique_ptr problemPtr_; + std::shared_ptr referenceManagerPtr_; + + rollout::Settings rolloutSettings_; + std::unique_ptr rolloutPtr_; + std::unique_ptr initializerPtr_; + + vector_t initialState_; + }; +} // namespace legged + +#pragma clang diagnostic pop diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h new file mode 100644 index 0000000..f44c088 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h @@ -0,0 +1,76 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include + +#include +#include + +#include + +#include + +#include "constraint/EndEffectorLinearConstraint.h" +#include "constraint/SwingTrajectoryPlanner.h" + +namespace ocs2::legged_robot { + /** Callback for caching and reference update */ + class LeggedRobotPreComputation : public PreComputation { + public: + LeggedRobotPreComputation(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const SwingTrajectoryPlanner &swingTrajectoryPlanner, ModelSettings settings); + + ~LeggedRobotPreComputation() override = default; + + LeggedRobotPreComputation *clone() const override { return new LeggedRobotPreComputation(*this); } + + void request(RequestSet request, scalar_t t, const vector_t &x, const vector_t &u) override; + + const std::vector &getEeNormalVelocityConstraintConfigs() const { + return eeNormalVelConConfigs_; + } + + PinocchioInterface &getPinocchioInterface() { return pinocchioInterface_; } + const PinocchioInterface &getPinocchioInterface() const { return pinocchioInterface_; } + + protected: + LeggedRobotPreComputation(const LeggedRobotPreComputation &other); + + private: + PinocchioInterface pinocchioInterface_; + CentroidalModelInfo info_; + const SwingTrajectoryPlanner *swingTrajectoryPlannerPtr_; + std::unique_ptr mappingPtr_; + const ModelSettings settings_; + + std::vector eeNormalVelConConfigs_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h new file mode 100644 index 0000000..9e0eeba --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h @@ -0,0 +1,67 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include "constraint/SwingTrajectoryPlanner.h" + +namespace ocs2::legged_robot { + /** + * Manages the ModeSchedule and the TargetTrajectories for switched model. + */ + class SwitchedModelReferenceManager : public ReferenceManager { + public: + SwitchedModelReferenceManager(std::shared_ptr gaitSchedulePtr, + std::shared_ptr swingTrajectoryPtr); + + ~SwitchedModelReferenceManager() override = default; + + void setModeSchedule(const ModeSchedule &modeSchedule) override; + + contact_flag_t getContactFlags(scalar_t time) const; + + const std::shared_ptr &getGaitSchedule() { return gaitSchedulePtr_; } + + const std::shared_ptr &getSwingTrajectoryPlanner() { return swingTrajectoryPtr_; } + + protected: + void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t &initState, + TargetTrajectories &targetTrajectories, + ModeSchedule &modeSchedule) override; + + std::shared_ptr gaitSchedulePtr_; + std::shared_ptr swingTrajectoryPtr_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h new file mode 100644 index 0000000..7b53cab --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h @@ -0,0 +1,95 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace ocs2::legged_robot { + /** + * Defines a linear constraint on an end-effector position (xee) and linear velocity (vee). + * g(xee, vee) = Ax * xee + Av * vee + b + * - For defining constraint of type g(xee), set Av to matrix_t(0, 0) + * - For defining constraint of type g(vee), set Ax to matrix_t(0, 0) + */ + class EndEffectorLinearConstraint final : public StateInputConstraint { + public: + /** + * Coefficients of the linear constraints of the form: + * g(xee, vee) = Ax * xee + Av * vee + b + */ + struct Config { + vector_t b; + matrix_t Ax; + matrix_t Av; + }; + + /** + * Constructor + * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. + * @param [in] numConstraints: The number of constraints {1, 2, 3} + * @param [in] config: The constraint coefficients, g(xee, vee) = Ax * xee + Av * vee + b + */ + EndEffectorLinearConstraint(const EndEffectorKinematics &endEffectorKinematics, size_t numConstraints, + Config config = Config()); + + ~EndEffectorLinearConstraint() override = default; + + EndEffectorLinearConstraint *clone() const override { return new EndEffectorLinearConstraint(*this); } + + /** Sets a new constraint coefficients. */ + void configure(Config &&config); + + /** Sets a new constraint coefficients. */ + void configure(const Config &config) { this->configure(Config(config)); } + + /** Gets the underlying end-effector kinematics interface. */ + EndEffectorKinematics &getEndEffectorKinematics() { return *endEffectorKinematicsPtr_; } + + size_t getNumConstraints(scalar_t time) const override { return numConstraints_; } + + vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + private: + EndEffectorLinearConstraint(const EndEffectorLinearConstraint &rhs); + + std::unique_ptr > endEffectorKinematicsPtr_; + const size_t numConstraints_; + Config config_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h new file mode 100644 index 0000000..9dd7fce --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h @@ -0,0 +1,160 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include +#include + +#include + +#include "../SwitchedModelReferenceManager.h" + +namespace ocs2::legged_robot { + /** + * Implements the constraint h(t,x,u) >= 0 + * + * frictionCoefficient * (Fz + gripperForce) - sqrt(Fx * Fx + Fy * Fy + regularization) >= 0 + * + * The gripper force shifts the origin of the friction cone down in z-direction by the amount of gripping force available. This makes it + * possible to produce tangential forces without applying a regular normal force on that foot, or to "pull" on the foot with magnitude up to + * the gripping force. + * + * The regularization prevents the constraint gradient / hessian to go to infinity when Fx = Fz = 0. It also creates a parabolic safety + * margin to the friction cone. For example: when Fx = Fy = 0 the constraint zero-crossing will be at Fz = 1/frictionCoefficient * + * sqrt(regularization) instead of Fz = 0 + * + */ + class FrictionConeConstraint final : public StateInputConstraint { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * frictionCoefficient: The coefficient of friction. + * regularization: A positive number to regulize the friction constraint. refer to the FrictionConeConstraint documentation. + * gripperForce: Gripper force in normal direction. + * hessianDiagonalShift: The Hessian shift to assure a strictly-convex quadratic constraint approximation. + */ + struct Config { + explicit Config(scalar_t frictionCoefficientParam = 0.7, scalar_t regularizationParam = 25.0, + scalar_t gripperForceParam = 0.0, + scalar_t hessianDiagonalShiftParam = 1e-6) + : frictionCoefficient(frictionCoefficientParam), + regularization(regularizationParam), + gripperForce(gripperForceParam), + hessianDiagonalShift(hessianDiagonalShiftParam) { + assert(frictionCoefficient > 0.0); + assert(regularization > 0.0); + assert(hessianDiagonalShift >= 0.0); + } + + scalar_t frictionCoefficient; + scalar_t regularization; + scalar_t gripperForce; + scalar_t hessianDiagonalShift; + }; + + /** + * Constructor + * @param [in] referenceManager : Switched model ReferenceManager. + * @param [in] config : Friction model settings. + * @param [in] contactPointIndex : The 3 DoF contact index. + * @param [in] info : The centroidal model information. + */ + FrictionConeConstraint(const SwitchedModelReferenceManager &referenceManager, Config config, + size_t contactPointIndex, + CentroidalModelInfo info); + + ~FrictionConeConstraint() override = default; + + FrictionConeConstraint *clone() const override { return new FrictionConeConstraint(*this); } + + bool isActive(scalar_t time) const override; + + size_t getNumConstraints(scalar_t time) const override { return 1; }; + + vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + /** Sets the estimated terrain normal expressed in the world frame. */ + void setSurfaceNormalInWorld(const vector3_t &surfaceNormalInWorld); + + private: + struct LocalForceDerivatives { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + matrix3_t dF_du; // derivative local force w.r.t. forces in world frame + }; + + struct ConeLocalDerivatives { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + vector3_t dCone_dF; // derivative w.r.t local force + matrix3_t d2Cone_dF2; // second derivative w.r.t local force + }; + + struct ConeDerivatives { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + vector3_t dCone_du; + matrix3_t d2Cone_du2; + }; + + FrictionConeConstraint(const FrictionConeConstraint &other) = default; + + vector_t coneConstraint(const vector3_t &localForces) const; + + LocalForceDerivatives computeLocalForceDerivatives(const vector3_t &forcesInBodyFrame) const; + + ConeLocalDerivatives computeConeLocalDerivatives(const vector3_t &localForces) const; + + ConeDerivatives computeConeConstraintDerivatives(const ConeLocalDerivatives &coneLocalDerivatives, + const LocalForceDerivatives &localForceDerivatives) const; + + matrix_t frictionConeInputDerivative(size_t inputDim, const ConeDerivatives &coneDerivatives) const; + + matrix_t frictionConeSecondDerivativeInput(size_t inputDim, const ConeDerivatives &coneDerivatives) const; + + matrix_t frictionConeSecondDerivativeState(size_t stateDim, const ConeDerivatives &coneDerivatives) const; + + const SwitchedModelReferenceManager *referenceManagerPtr_; + + const Config config_; + const size_t contactPointIndex_; + const CentroidalModelInfo info_; + + // rotation world to terrain + matrix3_t t_R_w = matrix3_t::Identity(); + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h new file mode 100644 index 0000000..702d7df --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h @@ -0,0 +1,30 @@ +// +// Created by qiayuan on 23-1-29. +// + +#pragma once + +#include + +#include "../LeggedRobotPreComputation.h" + +namespace ocs2::legged_robot { + class LeggedSelfCollisionConstraint final : public SelfCollisionConstraint { + public: + LeggedSelfCollisionConstraint(const CentroidalModelPinocchioMapping &mapping, + PinocchioGeometryInterface pinocchioGeometryInterface, + scalar_t minimumDistance) + : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface), minimumDistance) { + } + + ~LeggedSelfCollisionConstraint() override = default; + + LeggedSelfCollisionConstraint(const LeggedSelfCollisionConstraint &other) = default; + + LeggedSelfCollisionConstraint *clone() const override { return new LeggedSelfCollisionConstraint(*this); } + + const PinocchioInterface &getPinocchioInterface(const PreComputation &preComputation) const override { + return cast(preComputation).getPinocchioInterface(); + } + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h new file mode 100644 index 0000000..6b1e6c7 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h @@ -0,0 +1,78 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include + +#include "../SwitchedModelReferenceManager.h" +#include "EndEffectorLinearConstraint.h" + +namespace ocs2::legged_robot { + /** + * Specializes the CppAd version of normal velocity constraint on an end-effector position and linear velocity. + * Constructs the member EndEffectorLinearConstraint object with number of constraints of 1. + * + * See also EndEffectorLinearConstraint for the underlying computation. + */ + class NormalVelocityConstraintCppAd final : public StateInputConstraint { + public: + /** + * Constructor + * @param [in] referenceManager : Switched model ReferenceManager + * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. + * @param [in] contactPointIndex : The 3 DoF contact index. + */ + NormalVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager, + const EndEffectorKinematics &endEffectorKinematics, + size_t contactPointIndex); + + ~NormalVelocityConstraintCppAd() override = default; + + NormalVelocityConstraintCppAd *clone() const override { return new NormalVelocityConstraintCppAd(*this); } + + bool isActive(scalar_t time) const override; + + size_t getNumConstraints(scalar_t time) const override { return 1; } + + vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + private: + NormalVelocityConstraintCppAd(const NormalVelocityConstraintCppAd &rhs); + + const SwitchedModelReferenceManager *referenceManagerPtr_; + std::unique_ptr eeLinearConstraintPtr_; + const size_t contactPointIndex_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h new file mode 100644 index 0000000..c8b80d7 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h @@ -0,0 +1,118 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include + +#include +#include + +namespace ocs2::legged_robot { + class SwingTrajectoryPlanner { + public: + struct Config { + scalar_t liftOffVelocity = 0.0; + scalar_t touchDownVelocity = 0.0; + scalar_t swingHeight = 0.1; + scalar_t swingTimeScale = 0.15; + // swing phases shorter than this time will be scaled down in height and velocity + }; + + SwingTrajectoryPlanner(Config config, size_t numFeet); + + void update(const ModeSchedule &modeSchedule, scalar_t terrainHeight); + + void update(const ModeSchedule &modeSchedule, const feet_array_t &liftOffHeightSequence, + const feet_array_t &touchDownHeightSequence); + + void update(const ModeSchedule &modeSchedule, const feet_array_t &liftOffHeightSequence, + const feet_array_t &touchDownHeightSequence, + const feet_array_t &maxHeightSequence); + + scalar_t getZvelocityConstraint(size_t leg, scalar_t time) const; + + scalar_t getZpositionConstraint(size_t leg, scalar_t time) const; + + private: + /** + * Extracts for each leg the contact sequence over the motion phase sequence. + * @param phaseIDsStock + * @return contactFlagStock + */ + feet_array_t > extractContactFlags(const std::vector &phaseIDsStock) const; + + /** + * Finds the take-off and touch-down times indices for a specific leg. + * + * @param index + * @param contactFlagStock + * @return {The take-off time index for swing legs, touch-down time index for swing legs} + */ + static std::pair findIndex(size_t index, const std::vector &contactFlagStock); + + /** + * based on the input phaseIDsStock finds the start subsystem and final subsystem of the swing + * phases of the a foot in each subsystem. + * + * startTimeIndexStock: eventTimes[startTimesIndex] will be the take-off time for the requested leg. + * finalTimeIndexStock: eventTimes[finalTimesIndex] will be the touch-down time for the requested leg. + * + * @param [in] footIndex: Foot index + * @param [in] phaseIDsStock: The sequence of the motion phase IDs. + * @param [in] contactFlagStock: The sequence of the contact status for the requested leg. + * @return { startTimeIndexStock, finalTimeIndexStock} + */ + static std::pair, std::vector > updateFootSchedule( + const std::vector &contactFlagStock); + + /** + * Check if event time indices are valid + * @param leg + * @param index : phase index + * @param startIndex : liftoff event time index + * @param finalIndex : touchdown event time index + * @param phaseIDsStock : mode sequence + */ + static void checkThatIndicesAreValid(int leg, int index, int startIndex, int finalIndex, + const std::vector &phaseIDsStock); + + static scalar_t swingTrajectoryScaling(scalar_t startTime, scalar_t finalTime, scalar_t swingTimeScale); + + const Config config_; + const size_t numFeet_; + + feet_array_t > feetHeightTrajectories_; + feet_array_t > feetHeightTrajectoriesEvents_; + }; + + SwingTrajectoryPlanner::Config loadSwingTrajectorySettings(const std::string &fileName, + const std::string &fieldName = "swing_trajectory_config", + bool verbose = true); +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h new file mode 100644 index 0000000..f69f7bf --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h @@ -0,0 +1,71 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include +#include + +#include "../SwitchedModelReferenceManager.h" + +namespace ocs2::legged_robot { + class ZeroForceConstraint final : public StateInputConstraint { + public: + /* + * Constructor + * @param [in] referenceManager : Switched model ReferenceManager. + * @param [in] contactPointIndex : The 3 DoF contact index. + * @param [in] info : The centroidal model information. + */ + ZeroForceConstraint(const SwitchedModelReferenceManager &referenceManager, size_t contactPointIndex, + CentroidalModelInfo info); + + ~ZeroForceConstraint() override = default; + + ZeroForceConstraint *clone() const override { return new ZeroForceConstraint(*this); } + + bool isActive(scalar_t time) const override; + + size_t getNumConstraints(scalar_t time) const override { return 3; } + + vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + private: + ZeroForceConstraint(const ZeroForceConstraint &other) = default; + + const SwitchedModelReferenceManager *referenceManagerPtr_; + const size_t contactPointIndex_; + const CentroidalModelInfo info_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h new file mode 100644 index 0000000..4534b94 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h @@ -0,0 +1,80 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include + +#include "../SwitchedModelReferenceManager.h" +#include "EndEffectorLinearConstraint.h" + +namespace ocs2::legged_robot { + /** + * Specializes the CppAd version of zero velocity constraint on an end-effector position and linear velocity. + * Constructs the member EndEffectorLinearConstraint object with number of constraints of 3. + * + * See also EndEffectorLinearConstraint for the underlying computation. + */ + class ZeroVelocityConstraintCppAd final : public StateInputConstraint { + public: + /** + * Constructor + * @param [in] referenceManager : Switched model ReferenceManager + * @param [in] endEffectorKinematics: The kinematic interface to the target end-effector. + * @param [in] contactPointIndex : The 3 DoF contact index. + * @param [in] config: The constraint coefficients + */ + ZeroVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager, + const EndEffectorKinematics &endEffectorKinematics, + size_t contactPointIndex, + EndEffectorLinearConstraint::Config config = EndEffectorLinearConstraint::Config()); + + ~ZeroVelocityConstraintCppAd() override = default; + + ZeroVelocityConstraintCppAd *clone() const override { return new ZeroVelocityConstraintCppAd(*this); } + + bool isActive(scalar_t time) const override; + + size_t getNumConstraints(scalar_t time) const override { return 3; } + + vector_t getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const override; + + private: + ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd &rhs); + + const SwitchedModelReferenceManager *referenceManagerPtr_; + std::unique_ptr eeLinearConstraintPtr_; + const size_t contactPointIndex_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h new file mode 100644 index 0000000..c7e743d --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h @@ -0,0 +1,101 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "../SwitchedModelReferenceManager.h" + +namespace ocs2::legged_robot { + /** + * State-input tracking cost used for intermediate times + */ + class LeggedRobotStateInputQuadraticCost final : public QuadraticStateInputCost { + public: + LeggedRobotStateInputQuadraticCost(matrix_t Q, matrix_t R, CentroidalModelInfo info, + const SwitchedModelReferenceManager &referenceManager) + : QuadraticStateInputCost(std::move(Q), std::move(R)), info_(std::move(info)), + referenceManagerPtr_(&referenceManager) { + } + + ~LeggedRobotStateInputQuadraticCost() override = default; + + LeggedRobotStateInputQuadraticCost *clone() const override { + return new LeggedRobotStateInputQuadraticCost(*this); + } + + private: + LeggedRobotStateInputQuadraticCost(const LeggedRobotStateInputQuadraticCost &rhs) = default; + + std::pair getStateInputDeviation(scalar_t time, const vector_t &state, + const vector_t &input, + const TargetTrajectories &targetTrajectories) + const override { + const auto contactFlags = referenceManagerPtr_->getContactFlags(time); + const vector_t xNominal = targetTrajectories.getDesiredState(time); + const vector_t uNominal = weightCompensatingInput(info_, contactFlags); + return {state - xNominal, input - uNominal}; + } + + const CentroidalModelInfo info_; + const SwitchedModelReferenceManager *referenceManagerPtr_; + }; + + /** + * State tracking cost used for the final time + */ + class LeggedRobotStateQuadraticCost final : public QuadraticStateCost { + public: + LeggedRobotStateQuadraticCost(matrix_t Q, CentroidalModelInfo info, + const SwitchedModelReferenceManager &referenceManager) + : QuadraticStateCost(std::move(Q)), info_(std::move(info)), referenceManagerPtr_(&referenceManager) { + } + + ~LeggedRobotStateQuadraticCost() override = default; + + LeggedRobotStateQuadraticCost *clone() const override { return new LeggedRobotStateQuadraticCost(*this); } + + private: + LeggedRobotStateQuadraticCost(const LeggedRobotStateQuadraticCost &rhs) = default; + + vector_t getStateDeviation(scalar_t time, const vector_t &state, + const TargetTrajectories &targetTrajectories) const override { + const auto contactFlags = referenceManagerPtr_->getContactFlags(time); + const vector_t xNominal = targetTrajectories.getDesiredState(time); + return state - xNominal; + } + + const CentroidalModelInfo info_; + const SwitchedModelReferenceManager *referenceManagerPtr_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h new file mode 100644 index 0000000..e799d21 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h @@ -0,0 +1,63 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include +#include + +#include "../SwitchedModelReferenceManager.h" + +namespace ocs2::legged_robot { + class LeggedRobotInitializer final : public Initializer { + public: + /* + * Constructor + * @param [in] info : The centroidal model information. + * @param [in] referenceManager : Switched system reference manager. + * @param [in] extendNormalizedMomentum: If true, it extrapolates the normalized momenta; otherwise sets them to zero. + */ + LeggedRobotInitializer(CentroidalModelInfo info, const SwitchedModelReferenceManager &referenceManager, + bool extendNormalizedMomentum = false); + + ~LeggedRobotInitializer() override = default; + + LeggedRobotInitializer *clone() const override; + + void compute(scalar_t time, const vector_t &state, scalar_t nextTime, vector_t &input, + vector_t &nextState) override; + + private: + LeggedRobotInitializer(const LeggedRobotInitializer &other) = default; + + const CentroidalModelInfo info_; + const SwitchedModelReferenceManager *referenceManagerPtr_; + const bool extendNormalizedMomentum_; + }; +} diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h new file mode 100644 index 0000000..e09c7ce --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h @@ -0,0 +1,17 @@ +// +// Created by qiayuan on 22-12-23. +// +#pragma once + +#include "WbcBase.h" + +namespace ocs2::legged_robot { + class HierarchicalWbc : public WbcBase { + public: + using WbcBase::WbcBase; + + vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured, + size_t mode, + scalar_t period) override; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h new file mode 100644 index 0000000..6778724 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h @@ -0,0 +1,76 @@ +// +// Created by qiayuan on 2022/6/28. +// +// +// Ref: https://github.com/bernhardpg/quadruped_locomotion +// + +#pragma once + +#include "Task.h" + +#include + +namespace ocs2::legged_robot{ + // Hierarchical Optimization Quadratic Program + class HoQp { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using HoQpPtr = std::shared_ptr; + + explicit HoQp(const Task &task) : HoQp(task, nullptr) { + } + + HoQp(Task task, HoQpPtr higherProblem); + + matrix_t getStackedZMatrix() const { return stackedZ_; } + + Task getStackedTasks() const { return stackedTasks_; } + + vector_t getStackedSlackSolutions() const { return stackedSlackVars_; } + + vector_t getSolutions() const { + vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_; + return x; + } + + size_t getSlackedNumVars() const { return stackedTasks_.d_.rows(); } + + private: + void initVars(); + + void formulateProblem(); + + void solveProblem(); + + void buildHMatrix(); + + void buildCVector(); + + void buildDMatrix(); + + void buildFVector(); + + void buildZMatrix(); + + void stackSlackSolutions(); + + Task task_, stackedTasksPrev_, stackedTasks_; + HoQpPtr higherProblem_; + + bool hasEqConstraints_{}, hasIneqConstraints_{}; + size_t numSlackVars_{}, numDecisionVars_{}; + matrix_t stackedZPrev_, stackedZ_; + vector_t stackedSlackSolutionsPrev_, xPrev_; + size_t numPrevSlackVars_{}; + + Eigen::Matrix h_, d_; + vector_t c_, f_; + vector_t stackedSlackVars_, slackVarsSolutions_, decisionVarsSolutions_; + + // Convenience matrices that are used multiple times + matrix_t eyeNvNv_; + matrix_t zeroNvNx_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h new file mode 100644 index 0000000..615b587 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h @@ -0,0 +1,78 @@ +// +// Created by qiayuan on 2022/6/28. +// +// +// Ref: https://github.com/bernhardpg/quadruped_locomotion +// + +#pragma once + +#include + +#include + +namespace ocs2::legged_robot { + using namespace ocs2; + + class Task { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Task() = default; + + Task(matrix_t a, vector_t b, matrix_t d, vector_t f) : a_(std::move(a)), d_(std::move(d)), b_(std::move(b)), + f_(std::move(f)) { + } + + explicit Task(size_t numDecisionVars) + : Task(matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0), matrix_t::Zero(0, numDecisionVars), + vector_t::Zero(0)) { + } + + Task operator+(const Task &rhs) const { + return { + concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_), + concatenateVectors(f_, rhs.f_) + }; + } + + Task operator*(scalar_t rhs) const { + // clang-format off + return { + a_.cols() > 0 ? rhs * a_ : a_, + b_.cols() > 0 ? rhs * b_ : b_, + d_.cols() > 0 ? rhs * d_ : d_, + f_.cols() > 0 ? rhs * f_ : f_ + }; // clang-format on + } + + matrix_t a_, d_; + vector_t b_, f_; + + static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) { + if (m1.cols() <= 0) { + return m2; + } + if (m2.cols() <= 0) { + return m1; + } + assert(m1.cols() == m2.cols()); + matrix_t res(m1.rows() + m2.rows(), m1.cols()); + res << m1, m2; + return res; + } + + static vector_t concatenateVectors(const vector_t &v1, const vector_t &v2) { + if (v1.cols() <= 0) { + return v2; + } + if (v2.cols() <= 0) { + return v1; + } + assert(v1.cols() == v2.cols()); + vector_t res(v1.rows() + v2.rows()); + res << v1, v2; + return res; + } + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h new file mode 100644 index 0000000..15f2bef --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h @@ -0,0 +1,68 @@ +// +// Created by qiayuan on 2022/7/1. +// + +#pragma once + +#include "Task.h" + +#include +#include +#include + +namespace ocs2::legged_robot { + // Decision Variables: x = [\dot u^T, F^T, \tau^T]^T + class WbcBase { + using Vector6 = Eigen::Matrix; + using Matrix6 = Eigen::Matrix; + + public: + virtual ~WbcBase() = default; + + WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics); + + virtual void loadTasksSetting(const std::string &taskFile, bool verbose); + + virtual vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, + const vector_t &rbdStateMeasured, size_t mode, + scalar_t period); + + protected: + void updateMeasured(const vector_t &rbdStateMeasured); + + void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired); + + size_t getNumDecisionVars() const { return numDecisionVars_; } + + Task formulateFloatingBaseEomTask(); + + Task formulateTorqueLimitsTask(); + + Task formulateNoContactMotionTask(); + + Task formulateFrictionConeTask(); + + Task formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period); + + Task formulateSwingLegTask(); + + Task formulateContactForceTask(const vector_t &inputDesired) const; + + size_t numDecisionVars_; + PinocchioInterface pinocchioInterfaceMeasured_, pinocchioInterfaceDesired_; + CentroidalModelInfo info_; + + std::unique_ptr eeKinematics_; + CentroidalModelPinocchioMapping mapping_; + + vector_t qMeasured_, vMeasured_, inputLast_; + matrix_t j_, dj_; + contact_flag_t contactFlag_{}; + size_t numContacts_{}; + + // Task Parameters: + vector_t torqueLimits_; + scalar_t frictionCoeff_{}, swingKp_{}, swingKd_{}; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h new file mode 100644 index 0000000..0b23e5f --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h @@ -0,0 +1,28 @@ +// +// Created by qiayuan on 22-12-23. +// +#pragma once + +#include "WbcBase.h" + +namespace ocs2::legged_robot { + class WeightedWbc : public WbcBase { + public: + using WbcBase::WbcBase; + + vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured, + size_t mode, + scalar_t period) override; + + void loadTasksSetting(const std::string &taskFile, bool verbose) override; + + protected: + virtual Task formulateConstraints(); + + virtual Task formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, + scalar_t period); + + private: + scalar_t weightSwingLeg_, weightBaseAccel_, weightContactForce_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml b/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml new file mode 100644 index 0000000..e69de29 diff --git a/controllers/ocs2_quadruped_controller/package.xml b/controllers/ocs2_quadruped_controller/package.xml new file mode 100644 index 0000000..16771f7 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/package.xml @@ -0,0 +1,25 @@ + + + + ocs2_quadruped_controller + 0.0.0 + TODO: Package description + tlab-uav + Apache-2.0 + + ament_cmake + + backward_ros + controller_interface + pluginlib + control_input_msgs + qpOASES + ocs2_self_collision + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp new file mode 100644 index 0000000..0ca7f8c --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -0,0 +1,5 @@ +// +// Created by tlab-uav on 24-9-24. +// + +#include "Ocs2QuadrupedController.h" diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h new file mode 100644 index 0000000..907609d --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -0,0 +1,57 @@ +// +// Created by tlab-uav on 24-9-24. +// + +#ifndef OCS2QUADRUPEDCONTROLLER_H +#define OCS2QUADRUPEDCONTROLLER_H +#include + + +class Ocs2QuadrupedController final : public controller_interface::ControllerInterface { + +public: + CONTROLLER_INTERFACE_PUBLIC + Ocs2QuadrupedController() = default; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::return_type update( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_init() override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &previous_state) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &previous_state) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &previous_state) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &previous_state) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State &previous_state) override; + + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State &previous_state) override; + +}; + + + +#endif //OCS2QUADRUPEDCONTROLLER_H diff --git a/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp b/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp new file mode 100644 index 0000000..ee55b10 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp @@ -0,0 +1,36 @@ +// +// Created by qiayuan on 2022/7/24. +// + +#include "ocs2_quadruped_controller/estimator/FromTopiceEstimate.h" + +namespace ocs2::legged_robot { + FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics) + : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics) { + ros::NodeHandle nh; + sub_ = nh.subscribe("/ground_truth/state", 10, &FromTopicStateEstimate::callback, this); + } + + void FromTopicStateEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) { + buffer_.writeFromNonRT(*msg); + } + + vector_t FromTopicStateEstimate::update(const ros::Time & /*time*/, const ros::Duration & /*period*/) { + nav_msgs::Odometry odom = *buffer_.readFromRT(); + + updateAngular(quatToZyx(Eigen::Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, + odom.pose.pose.orientation.z)), + Eigen::Matrix(odom.twist.twist.angular.x, odom.twist.twist.angular.y, + odom.twist.twist.angular.z)); + updateLinear(Eigen::Matrix(odom.pose.pose.position.x, odom.pose.pose.position.y, + odom.pose.pose.position.z), + Eigen::Matrix(odom.twist.twist.linear.x, odom.twist.twist.linear.y, + odom.twist.twist.linear.z)); + + publishMsgs(odom); + + return rbdState_; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp new file mode 100644 index 0000000..60753ea --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -0,0 +1,277 @@ +// +// Created by qiayuan on 2022/7/24. +// + +#include +#include + +#include +#include + +#include +#include + +namespace ocs2::legged_robot { + KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics) + : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics), + numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), + dimContacts_(3 * numContacts_), + numState_(6 + dimContacts_), + numObserve_(2 * dimContacts_ + numContacts_), + tfListener_(tfBuffer_), + topicUpdated_(false) { + xHat_.setZero(numState_); + ps_.setZero(dimContacts_); + vs_.setZero(dimContacts_); + a_.setIdentity(numState_, numState_); + b_.setZero(numState_, 3); + matrix_t c1(3, 6), c2(3, 6); + c1 << matrix3_t::Identity(), matrix3_t::Zero(); + c2 << matrix3_t::Zero(), matrix3_t::Identity(); + c_.setZero(numObserve_, numState_); + for (ssize_t i = 0; i < numContacts_; ++i) { + c_.block(3 * i, 0, 3, 6) = c1; + c_.block(3 * (numContacts_ + i), 0, 3, 6) = c2; + c_(2 * dimContacts_ + i, 6 + 3 * i + 2) = 1.0; + } + c_.block(0, 6, dimContacts_, dimContacts_) = -matrix_t::Identity(dimContacts_, dimContacts_); + + q_.setIdentity(numState_, numState_); + p_ = 100. * q_; + r_.setIdentity(numObserve_, numObserve_); + feetHeights_.setZero(numContacts_); + + eeKinematics_->setPinocchioInterface(pinocchioInterface_); + + world2odom_.setRotation(tf2::Quaternion::getIdentity()); + sub_ = ros::NodeHandle().subscribe("/tracking_camera/odom/sample", 10, + &KalmanFilterEstimate::callback, this); + } + + vector_t KalmanFilterEstimate::update(const ros::Time &time, const ros::Duration &period) { + scalar_t dt = period.toSec(); + a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity(); + b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity(); + b_.block(3, 0, 3, 3) = dt * matrix3_t::Identity(); + q_.block(0, 0, 3, 3) = (dt / 20.f) * matrix3_t::Identity(); + q_.block(3, 3, 3, 3) = (dt * 9.81f / 20.f) * matrix3_t::Identity(); + q_.block(6, 6, dimContacts_, dimContacts_) = dt * matrix_t::Identity(dimContacts_, dimContacts_); + + const auto &model = pinocchioInterface_.getModel(); + auto &data = pinocchioInterface_.getData(); + size_t actuatedDofNum = info_.actuatedDofNum; + + vector_t qPino(info_.generalizedCoordinatesNum); + vector_t vPino(info_.generalizedCoordinatesNum); + qPino.setZero(); + qPino.segment<3>(3) = rbdState_.head<3>(); // Only set orientation, let position in origin. + qPino.tail(actuatedDofNum) = rbdState_.segment(6, actuatedDofNum); + + vPino.setZero(); + vPino.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity( + qPino.segment<3>(3), + rbdState_.segment<3>(info_.generalizedCoordinatesNum)); + // Only set angular velocity, let linear velocity be zero + vPino.tail(actuatedDofNum) = rbdState_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum); + + pinocchio::forwardKinematics(model, data, qPino, vPino); + pinocchio::updateFramePlacements(model, data); + + const auto eePos = eeKinematics_->getPosition(vector_t()); + const auto eeVel = eeKinematics_->getVelocity(vector_t(), vector_t()); + + matrix_t q = matrix_t::Identity(numState_, numState_); + q.block(0, 0, 3, 3) = q_.block(0, 0, 3, 3) * imuProcessNoisePosition_; + q.block(3, 3, 3, 3) = q_.block(3, 3, 3, 3) * imuProcessNoiseVelocity_; + q.block(6, 6, dimContacts_, dimContacts_) = + q_.block(6, 6, dimContacts_, dimContacts_) * footProcessNoisePosition_; + + matrix_t r = matrix_t::Identity(numObserve_, numObserve_); + r.block(0, 0, dimContacts_, dimContacts_) = + r_.block(0, 0, dimContacts_, dimContacts_) * footSensorNoisePosition_; + r.block(dimContacts_, dimContacts_, dimContacts_, dimContacts_) = + r_.block(dimContacts_, dimContacts_, dimContacts_, dimContacts_) * footSensorNoiseVelocity_; + r.block(2 * dimContacts_, 2 * dimContacts_, numContacts_, numContacts_) = + r_.block(2 * dimContacts_, 2 * dimContacts_, numContacts_, numContacts_) * footHeightSensorNoise_; + + for (int i = 0; i < numContacts_; i++) { + int i1 = 3 * i; + + int qIndex = 6 + i1; + int rIndex1 = i1; + int rIndex2 = dimContacts_ + i1; + int rIndex3 = 2 * dimContacts_ + i; + bool isContact = contactFlag_[i]; + + scalar_t high_suspect_number(100); + q.block(qIndex, qIndex, 3, 3) = (isContact ? 1. : high_suspect_number) * q.block(qIndex, qIndex, 3, 3); + r.block(rIndex1, rIndex1, 3, 3) = (isContact ? 1. : high_suspect_number) * r.block(rIndex1, rIndex1, 3, 3); + r.block(rIndex2, rIndex2, 3, 3) = (isContact ? 1. : high_suspect_number) * r.block(rIndex2, rIndex2, 3, 3); + r(rIndex3, rIndex3) = (isContact ? 1. : high_suspect_number) * r(rIndex3, rIndex3); + + ps_.segment(3 * i, 3) = -eePos[i]; + ps_.segment(3 * i, 3)[2] += footRadius_; + vs_.segment(3 * i, 3) = -eeVel[i]; + } + + vector3_t g(0, 0, -9.81); + vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linearAccelLocal_ + g; + + vector_t y(numObserve_); + y << ps_, vs_, feetHeights_; + xHat_ = a_ * xHat_ + b_ * accel; + matrix_t at = a_.transpose(); + matrix_t pm = a_ * p_ * at + q; + matrix_t cT = c_.transpose(); + matrix_t yModel = c_ * xHat_; + matrix_t ey = y - yModel; + matrix_t s = c_ * pm * cT + r; + + vector_t sEy = s.lu().solve(ey); + xHat_ += pm * cT * sEy; + + matrix_t sC = s.lu().solve(c_); + p_ = (matrix_t::Identity(numState_, numState_) - pm * cT * sC) * pm; + + matrix_t pt = p_.transpose(); + p_ = (p_ + pt) / 2.0; + + // if (p_.block(0, 0, 2, 2).determinant() > 0.000001) { + // p_.block(0, 2, 2, 16).setZero(); + // p_.block(2, 0, 16, 2).setZero(); + // p_.block(0, 0, 2, 2) /= 10.; + // } + + if (topicUpdated_) { + updateFromTopic(); + topicUpdated_ = false; + } + + updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3)); + + auto odom = getOdomMsg(); + odom.header.stamp = time; + odom.header.frame_id = "odom"; + odom.child_frame_id = "base"; + publishMsgs(odom); + + return rbdState_; + } + + void KalmanFilterEstimate::updateFromTopic() { + auto *msg = buffer_.readFromRT(); + + tf2::Transform world2sensor; + world2sensor.setOrigin(tf2::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, + msg->pose.pose.position.z)); + world2sensor.setRotation(tf2::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w)); + + if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received + { + tf2::Transform odom2sensor; + try { + geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform( + "odom", msg->child_frame_id, msg->header.stamp); + tf2::fromMsg(tf_msg.transform, odom2sensor); + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + return; + } + world2odom_ = world2sensor * odom2sensor.inverse(); + } + tf2::Transform base2sensor; + try { + geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform( + "base", msg->child_frame_id, msg->header.stamp); + tf2::fromMsg(tf_msg.transform, base2sensor); + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + return; + } + tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); + vector3_t newPos(odom2base.getOrigin().x(), odom2base.getOrigin().y(), odom2base.getOrigin().z()); + + const auto &model = pinocchioInterface_.getModel(); + auto &data = pinocchioInterface_.getData(); + + vector_t qPino(info_.generalizedCoordinatesNum); + qPino.head<3>() = newPos; + qPino.segment<3>(3) = rbdState_.head<3>(); + qPino.tail(info_.actuatedDofNum) = rbdState_.segment(6, info_.actuatedDofNum); + forwardKinematics(model, data, qPino); + updateFramePlacements(model, data); + + xHat_.segment<3>(0) = newPos; + for (size_t i = 0; i < numContacts_; ++i) { + xHat_.segment<3>(6 + i * 3) = eeKinematics_->getPosition(vector_t())[i]; + xHat_(6 + i * 3 + 2) -= footRadius_; + if (contactFlag_[i]) { + feetHeights_[i] = xHat_(6 + i * 3 + 2); + } + } + + auto odom = getOdomMsg(); + odom.header = msg->header; + odom.child_frame_id = "base"; + publishMsgs(odom); + } + + void KalmanFilterEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) { + buffer_.writeFromNonRT(*msg); + topicUpdated_ = true; + } + + nav_msgs::msg::Odometry KalmanFilterEstimate::getOdomMsg() { + nav_msgs::msg::Odometry odom; + odom.pose.pose.position.x = xHat_.segment<3>(0)(0); + odom.pose.pose.position.y = xHat_.segment<3>(0)(1); + odom.pose.pose.position.z = xHat_.segment<3>(0)(2); + odom.pose.pose.orientation.x = quat_.x(); + odom.pose.pose.orientation.y = quat_.y(); + odom.pose.pose.orientation.z = quat_.z(); + odom.pose.pose.orientation.w = quat_.w(); + odom.pose.pose.orientation.x = quat_.x(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + odom.pose.covariance[i * 6 + j] = p_(i, j); + odom.pose.covariance[6 * (3 + i) + (3 + j)] = orientationCovariance_(i * 3 + j); + } + } + // The twist in this message should be specified in the coordinate frame given by the child_frame_id: "base" + vector_t twist = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)).transpose() * xHat_.segment<3>(3); + odom.twist.twist.linear.x = twist.x(); + odom.twist.twist.linear.y = twist.y(); + odom.twist.twist.linear.z = twist.z(); + odom.twist.twist.angular.x = angularVelLocal_.x(); + odom.twist.twist.angular.y = angularVelLocal_.y(); + odom.twist.twist.angular.z = angularVelLocal_.z(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + odom.twist.covariance[i * 6 + j] = p_.block<3, 3>(3, 3)(i, j); + odom.twist.covariance[6 * (3 + i) + (3 + j)] = angularVelCovariance_(i * 3 + j); + } + } + return odom; + } + + void KalmanFilterEstimate::loadSettings(const std::string &taskFile, bool verbose) { + boost::property_tree::ptree pt; + read_info(taskFile, pt); + const std::string prefix = "kalmanFilter."; + if (verbose) { + std::cerr << "\n #### Kalman Filter Noise:"; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, footRadius_, prefix + "footRadius", verbose); + loadData::loadPtreeValue(pt, imuProcessNoisePosition_, prefix + "imuProcessNoisePosition", verbose); + loadData::loadPtreeValue(pt, imuProcessNoiseVelocity_, prefix + "imuProcessNoiseVelocity", verbose); + loadData::loadPtreeValue(pt, footProcessNoisePosition_, prefix + "footProcessNoisePosition", verbose); + loadData::loadPtreeValue(pt, footSensorNoisePosition_, prefix + "footSensorNoisePosition", verbose); + loadData::loadPtreeValue(pt, footSensorNoiseVelocity_, prefix + "footSensorNoiseVelocity", verbose); + loadData::loadPtreeValue(pt, footHeightSensorNoise_, prefix + "footHeightSensorNoise", verbose); + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp new file mode 100644 index 0000000..026f8c5 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -0,0 +1,74 @@ +// +// Created by qiayuan on 2021/11/15. +// + +#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h" + +#include +#include +#include + +#include + +namespace ocs2::legged_robot { + using namespace legged_robot; + + StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics, + rclcpp::Node::SharedPtr node) + : pinocchioInterface_(std::move(pinocchioInterface)), + info_(std::move(info)), + eeKinematics_(eeKinematics.clone()), + rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { + odomPub_ = node_->create_publisher("odom", 10); + posePub_ = node_->create_publisher("pose", 10); + } + + void StateEstimateBase::updateJointStates(const vector_t &jointPos, const vector_t &jointVel) { + rbdState_.segment(6, info_.actuatedDofNum) = jointPos; + rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = jointVel; + } + + void StateEstimateBase::updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, + const vector3_t &linearAccelLocal, const matrix3_t &orientationCovariance, + const matrix3_t &angularVelCovariance, const matrix3_t &linearAccelCovariance) { + quat_ = quat; + angularVelLocal_ = angularVelLocal; + linearAccelLocal_ = linearAccelLocal; + orientationCovariance_ = orientationCovariance; + angularVelCovariance_ = angularVelCovariance; + linearAccelCovariance_ = linearAccelCovariance; + + vector3_t zyx = quatToZyx(quat) - zyxOffset_; + vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( + zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat), angularVelLocal)); + updateAngular(zyx, angularVelGlobal); + } + + void StateEstimateBase::updateAngular(const vector3_t &zyx, const vector_t &angularVel) { + rbdState_.segment<3>(0) = zyx; + rbdState_.segment<3>(info_.generalizedCoordinatesNum) = angularVel; + } + + void StateEstimateBase::updateLinear(const vector_t &pos, const vector_t &linearVel) { + rbdState_.segment<3>(3) = pos; + rbdState_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; + } + + void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) { + rclcpp::Time time = odom.header.stamp; + scalar_t publishRate = 200; + // if (lastPub_ + rclcpp::Duration(1. / publishRate) < time) { + // lastPub_ = time; + // if (odomPub_->trylock()) { + // odomPub_->msg_ = odom; + // odomPub_->unlockAndPublish(); + // } + // if (posePub_->trylock()) { + // posePub_->msg_.header = odom.header; + // posePub_->msg_.pose = odom.pose; + // posePub_->unlockAndPublish(); + // } + // } + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp new file mode 100644 index 0000000..d634c0a --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp @@ -0,0 +1,414 @@ +// +// Created by qiayuan on 2022/7/16. +// + +#include // forward declarations must be included first. + +#include +#include + +#include "ocs2_quadruped_controller/interface/LeggedInterface.h" +#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h" +#include "ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h" +#include "ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h" +#include "ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h" +#include "ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h" +#include "ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h" +#include "ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h" +#include "ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// Boost +#include +#include + +namespace ocs2::legged_robot { + LeggedInterface::LeggedInterface(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool useHardFrictionConeConstraint) + : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { + // check that task file exists + boost::filesystem::path taskFilePath(taskFile); + if (boost::filesystem::exists(taskFilePath)) { + std::cerr << "[LeggedInterface] Loading task file: " << taskFilePath << std::endl; + } else { + throw std::invalid_argument("[LeggedInterface] Task file not found: " + taskFilePath.string()); + } + + // check that urdf file exists + boost::filesystem::path urdfFilePath(urdfFile); + if (boost::filesystem::exists(urdfFilePath)) { + std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdfFilePath << std::endl; + } else { + throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdfFilePath.string()); + } + + // check that targetCommand file exists + boost::filesystem::path referenceFilePath(referenceFile); + if (boost::filesystem::exists(referenceFilePath)) { + std::cerr << "[LeggedInterface] Loading target command settings from: " << referenceFilePath << std::endl; + } else { + throw std::invalid_argument( + "[LeggedInterface] targetCommand file not found: " + referenceFilePath.string()); + } + + bool verbose = false; + loadData::loadCppDataType(taskFile, "legged_robot_interface.verbose", verbose); + + // load setting from loading file + modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose); + mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); + ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); + sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); + ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); + rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void LeggedInterface::setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose) { + setupModel(taskFile, urdfFile, referenceFile, verbose); + + // Initial state + initialState_.setZero(centroidalModelInfo_.stateDim); + loadData::loadEigenMatrix(taskFile, "initialState", initialState_); + + setupReferenceManager(taskFile, urdfFile, referenceFile, verbose); + + // Optimal control problem + problemPtr_ = std::make_unique(); + + // Dynamics + std::unique_ptr dynamicsPtr; + dynamicsPtr = std::make_unique(*pinocchioInterfacePtr_, centroidalModelInfo_, "dynamics", + modelSettings_); + problemPtr_->dynamicsPtr = std::move(dynamicsPtr); + + // Cost terms + problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, verbose)); + + // Constraint terms + // friction cone settings + scalar_t frictionCoefficient = 0.7; + RelaxedBarrierPenalty::Config barrierPenaltyConfig; + std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(taskFile, verbose); + + for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { + const std::string &footName = modelSettings_.contactNames3DoF[i]; + std::unique_ptr > eeKinematicsPtr = + getEeKinematicsPtr({footName}, footName); + + if (useHardFrictionConeConstraint_) { + problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", + getFrictionConeConstraint(i, frictionCoefficient)); + } else { + problemPtr_->softConstraintPtr->add(footName + "_frictionCone", + getFrictionConeSoftConstraint( + i, frictionCoefficient, barrierPenaltyConfig)); + } + problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", std::unique_ptr( + new ZeroForceConstraint( + *referenceManagerPtr_, i, centroidalModelInfo_))); + problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", + getZeroVelocityConstraint(*eeKinematicsPtr, i)); + problemPtr_->equalityConstraintPtr->add( + footName + "_normalVelocity", + std::unique_ptr( + new NormalVelocityConstraintCppAd(*referenceManagerPtr_, *eeKinematicsPtr, i))); + } + + // Self-collision avoidance constraint + problemPtr_->stateSoftConstraintPtr->add("selfCollision", + getSelfCollisionConstraint( + *pinocchioInterfacePtr_, taskFile, "selfCollision", verbose)); + + setupPreComputation(taskFile, urdfFile, referenceFile, verbose); + + // Rollout + rolloutPtr_ = std::make_unique(*problemPtr_->dynamicsPtr, rolloutSettings_); + + // Initialization + constexpr bool extendNormalizedNomentum = true; + initializerPtr_ = std::make_unique(centroidalModelInfo_, *referenceManagerPtr_, + extendNormalizedNomentum); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void LeggedInterface::setupModel(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool /*verbose*/) { + // PinocchioInterface + pinocchioInterfacePtr_ = + std::make_unique( + centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames)); + + // CentroidalModelInfo + centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo( + *pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile), + centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), + modelSettings_.contactNames3DoF, + modelSettings_.contactNames6DoF); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose) { + auto swingTrajectoryPlanner = + std::make_unique( + loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); + referenceManagerPtr_ = + std::make_shared(loadGaitSchedule(referenceFile, verbose), + std::move(swingTrajectoryPlanner)); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose) { + problemPtr_->preComputationPtr = std::make_unique( + *pinocchioInterfacePtr_, centroidalModelInfo_, *referenceManagerPtr_->getSwingTrajectoryPlanner(), + modelSettings_); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::shared_ptr LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const { + const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false); + const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false); + + const auto defaultGait = [defaultModeSequenceTemplate] { + Gait gait{}; + gait.duration = defaultModeSequenceTemplate.switchingTimes.back(); + // Events: from time -> phase + std::for_each(defaultModeSequenceTemplate.switchingTimes.begin() + 1, + defaultModeSequenceTemplate.switchingTimes.end() - 1, + [&](double eventTime) { gait.eventPhases.push_back(eventTime / gait.duration); }); + // Modes: + gait.modeSequence = defaultModeSequenceTemplate.modeSequence; + return gait; + }(); + + // display + if (verbose) { + std::cerr << "\n#### Modes Schedule: "; + std::cerr << "\n#### =============================================================================\n"; + std::cerr << "Initial Modes Schedule: \n" << initModeSchedule; + std::cerr << "Default Modes Sequence Template: \n" << defaultModeSequenceTemplate; + std::cerr << "#### =============================================================================\n"; + } + + return std::make_shared(initModeSchedule, defaultModeSequenceTemplate, + modelSettings_.phaseTransitionStanceTime); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + matrix_t LeggedInterface::initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info) { + const size_t totalContactDim = 3 * info.numThreeDofContacts; + + const auto &model = pinocchioInterfacePtr_->getModel(); + auto &data = pinocchioInterfacePtr_->getData(); + const auto q = centroidal_model::getGeneralizedCoordinates(initialState_, centroidalModelInfo_); + pinocchio::computeJointJacobians(model, data, q); + pinocchio::updateFramePlacements(model, data); + + matrix_t base2feetJac(totalContactDim, info.actuatedDofNum); + for (size_t i = 0; i < info.numThreeDofContacts; i++) { + matrix_t jac = matrix_t::Zero(6, info.generalizedCoordinatesNum); + pinocchio::getFrameJacobian(model, data, model.getBodyId(modelSettings_.contactNames3DoF[i]), + pinocchio::LOCAL_WORLD_ALIGNED, jac); + base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum); + } + + matrix_t rTaskspace(info.inputDim, info.inputDim); + loadData::loadEigenMatrix(taskFile, "R", rTaskspace); + matrix_t r = rTaskspace; + // Joint velocities + r.block(totalContactDim, totalContactDim, info.actuatedDofNum, info.actuatedDofNum) = + base2feetJac.transpose() * rTaskspace.block(totalContactDim, totalContactDim, info.actuatedDofNum, + info.actuatedDofNum) * + base2feetJac; + return r; + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getBaseTrackingCost( + const std::string &taskFile, const CentroidalModelInfo &info, + bool verbose) { + matrix_t Q(info.stateDim, info.stateDim); + loadData::loadEigenMatrix(taskFile, "Q", Q); + matrix_t R = initializeInputCostWeight(taskFile, info); + + if (verbose) { + std::cerr << "\n #### Base Tracking Cost Coefficients: "; + std::cerr << "\n #### =============================================================================\n"; + std::cerr << "Q:\n" << Q << "\n"; + std::cerr << "R:\n" << R << "\n"; + std::cerr << " #### =============================================================================\n"; + } + + return std::make_unique(std::move(Q), std::move(R), info, + *referenceManagerPtr_); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::pair LeggedInterface::loadFrictionConeSettings( + const std::string &taskFile, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(taskFile, pt); + const std::string prefix = "frictionConeSoftConstraint."; + + scalar_t frictionCoefficient = 1.0; + RelaxedBarrierPenalty::Config barrierPenaltyConfig; + if (verbose) { + std::cerr << "\n #### Friction Cone Settings: "; + std::cerr << "\n #### =============================================================================\n"; + } + loadData::loadPtreeValue(pt, frictionCoefficient, prefix + "frictionCoefficient", verbose); + loadData::loadPtreeValue(pt, barrierPenaltyConfig.mu, prefix + "mu", verbose); + loadData::loadPtreeValue(pt, barrierPenaltyConfig.delta, prefix + "delta", verbose); + if (verbose) { + std::cerr << " #### =============================================================================\n"; + } + + return {frictionCoefficient, barrierPenaltyConfig}; + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getFrictionConeConstraint( + size_t contactPointIndex, scalar_t frictionCoefficient) { + FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); + return std::make_unique(*referenceManagerPtr_, frictionConeConConfig, contactPointIndex, + centroidalModelInfo_); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getFrictionConeSoftConstraint( + size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) { + return std::make_unique( + getFrictionConeConstraint(contactPointIndex, frictionCoefficient), + std::make_unique(barrierPenaltyConfig)); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr > LeggedInterface::getEeKinematicsPtr( + const std::vector &footNames, + const std::string &modelName) { + std::unique_ptr > eeKinematicsPtr; + + const auto infoCppAd = centroidalModelInfo_.toCppAd(); + const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd); + auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t &state, + PinocchioInterfaceCppAd &pinocchioInterfaceAd) { + const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd); + updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q); + }; + eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, + footNames, + centroidalModelInfo_.stateDim, + centroidalModelInfo_.inputDim, + velocityUpdateCallback, modelName, + modelSettings_.modelFolderCppAd, + modelSettings_.recompileLibrariesCppAd, + modelSettings_.verboseCppAd)); + + return eeKinematicsPtr; + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getZeroVelocityConstraint( + const EndEffectorKinematics &eeKinematics, + size_t contactPointIndex) { + auto eeZeroVelConConfig = [](scalar_t positionErrorGain) { + EndEffectorLinearConstraint::Config config; + config.b.setZero(3); + config.Av.setIdentity(3, 3); + if (!numerics::almost_eq(positionErrorGain, 0.0)) { + config.Ax.setZero(3, 3); + config.Ax(2, 2) = positionErrorGain; + } + return config; + }; + return std::unique_ptr(new ZeroVelocityConstraintCppAd( + *referenceManagerPtr_, eeKinematics, contactPointIndex, + eeZeroVelConConfig(modelSettings_.positionErrorGain))); + } + + /******************************************************************************************************/ + /******************************************************************************************************/ + /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, + const std::string &taskFile, + const std::string &prefix, + bool verbose) { + std::vector > collisionObjectPairs; + std::vector > collisionLinkPairs; + scalar_t mu = 1e-2; + scalar_t delta = 1e-3; + scalar_t minimumDistance = 0.0; + + boost::property_tree::ptree pt; + boost::property_tree::read_info(taskFile, pt); + if (verbose) { + std::cerr << "\n #### SelfCollision Settings: "; + std::cerr << "\n #### =============================================================================\n"; + } + loadData::loadPtreeValue(pt, mu, prefix + ".mu", verbose); + loadData::loadPtreeValue(pt, delta, prefix + ".delta", verbose); + loadData::loadPtreeValue(pt, minimumDistance, prefix + ".minimumDistance", verbose); + loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose); + loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose); + + geometryInterfacePtr_ = std::make_unique( + pinocchioInterface, collisionLinkPairs, collisionObjectPairs); + if (verbose) { + std::cerr << " #### =============================================================================\n"; + const size_t numCollisionPairs = geometryInterfacePtr_->getNumCollisionPairs(); + std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n"; + } + + std::unique_ptr constraint = std::make_unique( + CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance); + + auto penalty = std::make_unique(RelaxedBarrierPenalty::Config{mu, delta}); + + return std::make_unique(std::move(constraint), std::move(penalty)); + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/interface/LeggedRobotPreComputation.cpp b/controllers/ocs2_quadruped_controller/src/interface/LeggedRobotPreComputation.cpp new file mode 100644 index 0000000..b932cc2 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/LeggedRobotPreComputation.cpp @@ -0,0 +1,109 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h" + +namespace ocs2::legged_robot { + LeggedRobotPreComputation::LeggedRobotPreComputation(PinocchioInterface pinocchioInterface, + CentroidalModelInfo info, + const SwingTrajectoryPlanner &swingTrajectoryPlanner, + ModelSettings settings) + : pinocchioInterface_(std::move(pinocchioInterface)), + info_(std::move(info)), + swingTrajectoryPlannerPtr_(&swingTrajectoryPlanner), + mappingPtr_(new CentroidalModelPinocchioMapping(info_)), + settings_(std::move(settings)) { + eeNormalVelConConfigs_.resize(info_.numThreeDofContacts); + mappingPtr_->setPinocchioInterface(pinocchioInterface_); + } + + + LeggedRobotPreComputation::LeggedRobotPreComputation(const LeggedRobotPreComputation &rhs) + : pinocchioInterface_(rhs.pinocchioInterface_), + info_(rhs.info_), + swingTrajectoryPlannerPtr_(rhs.swingTrajectoryPlannerPtr_), + mappingPtr_(rhs.mappingPtr_->clone()), + settings_(rhs.settings_) { + eeNormalVelConConfigs_.resize(rhs.eeNormalVelConConfigs_.size()); + mappingPtr_->setPinocchioInterface(pinocchioInterface_); + } + + + void LeggedRobotPreComputation::request(RequestSet request, scalar_t t, const vector_t &x, const vector_t &u) { + if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint)) { + return; + } + + // lambda to set config for normal velocity constraints + auto eeNormalVelConConfig = [&](size_t footIndex) { + EndEffectorLinearConstraint::Config config; + config.b = (vector_t(1) << -swingTrajectoryPlannerPtr_->getZvelocityConstraint(footIndex, t)). + finished(); + config.Av = (matrix_t(1, 3) << 0.0, 0.0, 1.0).finished(); + if (!numerics::almost_eq(settings_.positionErrorGain, 0.0)) { + config.b(0) -= settings_.positionErrorGain * swingTrajectoryPlannerPtr_->getZpositionConstraint( + footIndex, t); + config.Ax = (matrix_t(1, 3) << 0.0, 0.0, settings_.positionErrorGain).finished(); + } + return config; + }; + + if (request.contains(Request::Constraint)) { + for (size_t i = 0; i < info_.numThreeDofContacts; i++) { + eeNormalVelConConfigs_[i] = eeNormalVelConConfig(i); + } + } + + const auto &model = pinocchioInterface_.getModel(); + auto &data = pinocchioInterface_.getData(); + vector_t q = mappingPtr_->getPinocchioJointPosition(x); + if (request.contains(Request::Approximation)) { + forwardKinematics(model, data, q); + updateFramePlacements(model, data); + updateGlobalPlacements(model, data); + computeJointJacobians(model, data); + + updateCentroidalDynamics(pinocchioInterface_, info_, q); + vector_t v = mappingPtr_->getPinocchioJointVelocity(x, u); + updateCentroidalDynamicsDerivatives(pinocchioInterface_, info_, q, v); + } else { + forwardKinematics(model, data, q); + updateFramePlacements(model, data); + } + } +} diff --git a/controllers/ocs2_quadruped_controller/src/interface/SwitchedModelReferenceManager.cpp b/controllers/ocs2_quadruped_controller/src/interface/SwitchedModelReferenceManager.cpp new file mode 100644 index 0000000..36d0fda --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/SwitchedModelReferenceManager.cpp @@ -0,0 +1,64 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h" + + +namespace ocs2::legged_robot { + SwitchedModelReferenceManager::SwitchedModelReferenceManager(std::shared_ptr gaitSchedulePtr, + std::shared_ptr + swingTrajectoryPtr) + : ReferenceManager(TargetTrajectories(), ModeSchedule()), + gaitSchedulePtr_(std::move(gaitSchedulePtr)), + swingTrajectoryPtr_(std::move(swingTrajectoryPtr)) { + } + + + void SwitchedModelReferenceManager::setModeSchedule(const ModeSchedule &modeSchedule) { + ReferenceManager::setModeSchedule(modeSchedule); + gaitSchedulePtr_->setModeSchedule(modeSchedule); + } + + + contact_flag_t SwitchedModelReferenceManager::getContactFlags(scalar_t time) const { + return modeNumber2StanceLeg(this->getModeSchedule().modeAtTime(time)); + } + + + void SwitchedModelReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime, + const vector_t &initState, + TargetTrajectories &targetTrajectories, + ModeSchedule &modeSchedule) { + const auto timeHorizon = finalTime - initTime; + modeSchedule = gaitSchedulePtr_->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon); + + const scalar_t terrainHeight = 0.0; + swingTrajectoryPtr_->update(modeSchedule, terrainHeight); + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/EndEffectorLinearConstraint.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/EndEffectorLinearConstraint.cpp new file mode 100644 index 0000000..3fc821f --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/EndEffectorLinearConstraint.cpp @@ -0,0 +1,105 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h" + + +namespace ocs2::legged_robot { + EndEffectorLinearConstraint::EndEffectorLinearConstraint( + const EndEffectorKinematics &endEffectorKinematics, + size_t numConstraints, Config config) + : StateInputConstraint(ConstraintOrder::Linear), + endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + numConstraints_(numConstraints), + config_(std::move(config)) { + if (endEffectorKinematicsPtr_->getIds().size() != 1) { + throw std::runtime_error( + "[EndEffectorLinearConstraint] this class only accepts a single end-effector!"); + } + } + + + EndEffectorLinearConstraint::EndEffectorLinearConstraint(const EndEffectorLinearConstraint &rhs) + : StateInputConstraint(rhs), + endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()), + numConstraints_(rhs.numConstraints_), + config_(rhs.config_) { + } + + + void EndEffectorLinearConstraint::configure(Config &&config) { + assert(config.b.rows() == numConstraints_); + assert(config.Ax.size() > 0 || config.Av.size() > 0); + assert((config.Ax.size() > 0 && config.Ax.rows() == numConstraints_) || config.Ax.size() == 0); + assert((config.Ax.size() > 0 && config.Ax.cols() == 3) || config.Ax.size() == 0); + assert((config.Av.size() > 0 && config.Av.rows() == numConstraints_) || config.Av.size() == 0); + assert((config.Av.size() > 0 && config.Av.cols() == 3) || config.Av.size() == 0); + config_ = std::move(config); + } + + + vector_t EndEffectorLinearConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + vector_t f = config_.b; + if (config_.Ax.size() > 0) { + f.noalias() += config_.Ax * endEffectorKinematicsPtr_->getPosition(state).front(); + } + if (config_.Av.size() > 0) { + f.noalias() += config_.Av * endEffectorKinematicsPtr_->getVelocity(state, input).front(); + } + return f; + } + + + VectorFunctionLinearApproximation EndEffectorLinearConstraint::getLinearApproximation( + scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const { + VectorFunctionLinearApproximation linearApproximation = + VectorFunctionLinearApproximation::Zero(getNumConstraints(time), state.size(), input.size()); + + linearApproximation.f = config_.b; + + if (config_.Ax.size() > 0) { + const auto positionApprox = endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front(); + linearApproximation.f.noalias() += config_.Ax * positionApprox.f; + linearApproximation.dfdx.noalias() += config_.Ax * positionApprox.dfdx; + } + + if (config_.Av.size() > 0) { + const auto velocityApprox = endEffectorKinematicsPtr_->getVelocityLinearApproximation(state, input). + front(); + linearApproximation.f.noalias() += config_.Av * velocityApprox.f; + linearApproximation.dfdx.noalias() += config_.Av * velocityApprox.dfdx; + linearApproximation.dfdu.noalias() += config_.Av * velocityApprox.dfdu; + } + + return linearApproximation; + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/FrictionConeConstraint.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/FrictionConeConstraint.cpp new file mode 100644 index 0000000..9093304 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/FrictionConeConstraint.cpp @@ -0,0 +1,188 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h" + +#include + + +namespace ocs2::legged_robot { + FrictionConeConstraint::FrictionConeConstraint(const SwitchedModelReferenceManager &referenceManager, Config config, + size_t contactPointIndex, CentroidalModelInfo info) + : StateInputConstraint(ConstraintOrder::Quadratic), + referenceManagerPtr_(&referenceManager), + config_(std::move(config)), + contactPointIndex_(contactPointIndex), + info_(std::move(info)) { + } + + + void FrictionConeConstraint::setSurfaceNormalInWorld(const vector3_t &surfaceNormalInWorld) { + t_R_w.setIdentity(); + throw std::runtime_error("[FrictionConeConstraint] setSurfaceNormalInWorld() is not implemented!"); + } + + + bool FrictionConeConstraint::isActive(scalar_t time) const { + return referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; + } + + + vector_t FrictionConeConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + const auto forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_); + const vector3_t localForce = t_R_w * forcesInWorldFrame; + return coneConstraint(localForce); + } + + + VectorFunctionLinearApproximation FrictionConeConstraint::getLinearApproximation( + scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const { + const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_); + const vector3_t localForce = t_R_w * forcesInWorldFrame; + + const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame); + const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce); + const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives); + + VectorFunctionLinearApproximation linearApproximation; + linearApproximation.f = coneConstraint(localForce); + linearApproximation.dfdx = matrix_t::Zero(1, state.size()); + linearApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives); + return linearApproximation; + } + + + VectorFunctionQuadraticApproximation FrictionConeConstraint::getQuadraticApproximation( + scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const { + const vector3_t forcesInWorldFrame = centroidal_model::getContactForces(input, contactPointIndex_, info_); + const vector3_t localForce = t_R_w * forcesInWorldFrame; + + const auto localForceDerivatives = computeLocalForceDerivatives(forcesInWorldFrame); + const auto coneLocalDerivatives = computeConeLocalDerivatives(localForce); + const auto coneDerivatives = computeConeConstraintDerivatives(coneLocalDerivatives, localForceDerivatives); + + VectorFunctionQuadraticApproximation quadraticApproximation; + quadraticApproximation.f = coneConstraint(localForce); + quadraticApproximation.dfdx = matrix_t::Zero(1, state.size()); + quadraticApproximation.dfdu = frictionConeInputDerivative(input.size(), coneDerivatives); + quadraticApproximation.dfdxx.emplace_back(frictionConeSecondDerivativeState(state.size(), coneDerivatives)); + quadraticApproximation.dfduu.emplace_back(frictionConeSecondDerivativeInput(input.size(), coneDerivatives)); + quadraticApproximation.dfdux.emplace_back(matrix_t::Zero(input.size(), state.size())); + return quadraticApproximation; + } + + + FrictionConeConstraint::LocalForceDerivatives FrictionConeConstraint::computeLocalForceDerivatives( + const vector3_t &forcesInWorldFrame) const { + LocalForceDerivatives localForceDerivatives{}; + localForceDerivatives.dF_du = t_R_w; + return localForceDerivatives; + } + + + FrictionConeConstraint::ConeLocalDerivatives FrictionConeConstraint::computeConeLocalDerivatives( + const vector3_t &localForces) const { + const auto F_x_square = localForces.x() * localForces.x(); + const auto F_y_square = localForces.y() * localForces.y(); + const auto F_tangent_square = F_x_square + F_y_square + config_.regularization; + const auto F_tangent_norm = sqrt(F_tangent_square); + const auto F_tangent_square_pow32 = F_tangent_norm * F_tangent_square; // = F_tangent_square ^ (3/2) + + ConeLocalDerivatives coneDerivatives{}; + coneDerivatives.dCone_dF(0) = -localForces.x() / F_tangent_norm; + coneDerivatives.dCone_dF(1) = -localForces.y() / F_tangent_norm; + coneDerivatives.dCone_dF(2) = config_.frictionCoefficient; + + coneDerivatives.d2Cone_dF2(0, 0) = -(F_y_square + config_.regularization) / F_tangent_square_pow32; + coneDerivatives.d2Cone_dF2(0, 1) = localForces.x() * localForces.y() / F_tangent_square_pow32; + coneDerivatives.d2Cone_dF2(0, 2) = 0.0; + coneDerivatives.d2Cone_dF2(1, 0) = coneDerivatives.d2Cone_dF2(0, 1); + coneDerivatives.d2Cone_dF2(1, 1) = -(F_x_square + config_.regularization) / F_tangent_square_pow32; + coneDerivatives.d2Cone_dF2(1, 2) = 0.0; + coneDerivatives.d2Cone_dF2(2, 0) = 0.0; + coneDerivatives.d2Cone_dF2(2, 1) = 0.0; + coneDerivatives.d2Cone_dF2(2, 2) = 0.0; + + return coneDerivatives; + } + + + vector_t FrictionConeConstraint::coneConstraint(const vector3_t &localForces) const { + const auto F_tangent_square = localForces.x() * localForces.x() + localForces.y() * localForces.y() + config_. + regularization; + const auto F_tangent_norm = sqrt(F_tangent_square); + const scalar_t coneConstraint = config_.frictionCoefficient * (localForces.z() + config_.gripperForce) - + F_tangent_norm; + return (vector_t(1) << coneConstraint).finished(); + } + + + FrictionConeConstraint::ConeDerivatives FrictionConeConstraint::computeConeConstraintDerivatives( + const ConeLocalDerivatives &coneLocalDerivatives, const LocalForceDerivatives &localForceDerivatives) const { + ConeDerivatives coneDerivatives; + // First order derivatives + coneDerivatives.dCone_du.noalias() = coneLocalDerivatives.dCone_dF.transpose() * localForceDerivatives.dF_du; + + // Second order derivatives + coneDerivatives.d2Cone_du2.noalias() = + localForceDerivatives.dF_du.transpose() * coneLocalDerivatives.d2Cone_dF2 * localForceDerivatives.dF_du; + + return coneDerivatives; + } + + + matrix_t FrictionConeConstraint::frictionConeInputDerivative(size_t inputDim, + const ConeDerivatives &coneDerivatives) const { + matrix_t dhdu = matrix_t::Zero(1, inputDim); + dhdu.block<1, 3>(0, 3 * contactPointIndex_) = coneDerivatives.dCone_du; + return dhdu; + } + + + matrix_t FrictionConeConstraint::frictionConeSecondDerivativeInput(size_t inputDim, + const ConeDerivatives &coneDerivatives) const { + matrix_t ddhdudu = matrix_t::Zero(inputDim, inputDim); + ddhdudu.block<3, 3>(3 * contactPointIndex_, 3 * contactPointIndex_) = coneDerivatives.d2Cone_du2; + ddhdudu.diagonal().array() -= config_.hessianDiagonalShift; + return ddhdudu; + } + + + matrix_t FrictionConeConstraint::frictionConeSecondDerivativeState(size_t stateDim, + const ConeDerivatives &coneDerivatives) const { + matrix_t ddhdxdx = matrix_t::Zero(stateDim, stateDim); + ddhdxdx.diagonal().array() -= config_.hessianDiagonalShift; + return ddhdxdx; + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/NormalVelocityConstraintCppAd.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/NormalVelocityConstraintCppAd.cpp new file mode 100644 index 0000000..bd7c758 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/NormalVelocityConstraintCppAd.cpp @@ -0,0 +1,77 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h" +#include "ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h" + + +namespace ocs2::legged_robot { + NormalVelocityConstraintCppAd::NormalVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager, + const EndEffectorKinematics & + endEffectorKinematics, + size_t contactPointIndex) + : StateInputConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(&referenceManager), + eeLinearConstraintPtr_(new EndEffectorLinearConstraint(endEffectorKinematics, 1)), + contactPointIndex_(contactPointIndex) { + } + + + NormalVelocityConstraintCppAd::NormalVelocityConstraintCppAd(const NormalVelocityConstraintCppAd &rhs) + : StateInputConstraint(rhs), + referenceManagerPtr_(rhs.referenceManagerPtr_), + eeLinearConstraintPtr_(rhs.eeLinearConstraintPtr_->clone()), + contactPointIndex_(rhs.contactPointIndex_) { + } + + + bool NormalVelocityConstraintCppAd::isActive(scalar_t time) const { + return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; + } + + + vector_t NormalVelocityConstraintCppAd::getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + const auto &preCompLegged = cast(preComp); + eeLinearConstraintPtr_->configure(preCompLegged.getEeNormalVelocityConstraintConfigs()[contactPointIndex_]); + + return eeLinearConstraintPtr_->getValue(time, state, input, preComp); + } + + + VectorFunctionLinearApproximation NormalVelocityConstraintCppAd::getLinearApproximation( + scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const { + const auto &preCompLegged = cast(preComp); + eeLinearConstraintPtr_->configure(preCompLegged.getEeNormalVelocityConstraintConfigs()[contactPointIndex_]); + + return eeLinearConstraintPtr_->getLinearApproximation(time, state, input, preComp); + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/SwingTrajectoryPlanner.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/SwingTrajectoryPlanner.cpp new file mode 100644 index 0000000..c240033 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/SwingTrajectoryPlanner.cpp @@ -0,0 +1,261 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include +#include + +#include "ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h" + +#include +#include +#include + +#include + + +namespace ocs2::legged_robot { + SwingTrajectoryPlanner::SwingTrajectoryPlanner(Config config, size_t numFeet) : config_(std::move(config)), + numFeet_(numFeet) { + } + + + scalar_t SwingTrajectoryPlanner::getZvelocityConstraint(size_t leg, scalar_t time) const { + const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time); + return feetHeightTrajectories_[leg][index].velocity(time); + } + + + scalar_t SwingTrajectoryPlanner::getZpositionConstraint(size_t leg, scalar_t time) const { + const auto index = lookup::findIndexInTimeArray(feetHeightTrajectoriesEvents_[leg], time); + return feetHeightTrajectories_[leg][index].position(time); + } + + + void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, scalar_t terrainHeight) { + const scalar_array_t terrainHeightSequence(modeSchedule.modeSequence.size(), terrainHeight); + feet_array_t liftOffHeightSequence; + liftOffHeightSequence.fill(terrainHeightSequence); + feet_array_t touchDownHeightSequence; + touchDownHeightSequence.fill(terrainHeightSequence); + update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence); + } + + + void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, + const feet_array_t &liftOffHeightSequence, + const feet_array_t &touchDownHeightSequence) { + scalar_array_t heightSequence(modeSchedule.modeSequence.size()); + feet_array_t maxHeightSequence; + for (size_t j = 0; j < numFeet_; j++) { + for (int p = 0; p < modeSchedule.modeSequence.size(); ++p) { + heightSequence[p] = std::max(liftOffHeightSequence[j][p], touchDownHeightSequence[j][p]); + } + maxHeightSequence[j] = heightSequence; + } + update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence, maxHeightSequence); + } + + + void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule, + const feet_array_t &liftOffHeightSequence, + const feet_array_t &touchDownHeightSequence, + const feet_array_t &maxHeightSequence) { + const auto &modeSequence = modeSchedule.modeSequence; + const auto &eventTimes = modeSchedule.eventTimes; + + const auto eesContactFlagStocks = extractContactFlags(modeSequence); + + feet_array_t > startTimesIndices; + feet_array_t > finalTimesIndices; + for (size_t leg = 0; leg < numFeet_; leg++) { + std::tie(startTimesIndices[leg], finalTimesIndices[leg]) = + updateFootSchedule(eesContactFlagStocks[leg]); + } + + for (size_t j = 0; j < numFeet_; j++) { + feetHeightTrajectories_[j].clear(); + feetHeightTrajectories_[j].reserve(modeSequence.size()); + for (int p = 0; p < modeSequence.size(); ++p) { + if (!eesContactFlagStocks[j][p]) { + // for a swing leg + const int swingStartIndex = startTimesIndices[j][p]; + const int swingFinalIndex = finalTimesIndices[j][p]; + checkThatIndicesAreValid(j, p, swingStartIndex, swingFinalIndex, modeSequence); + + const scalar_t swingStartTime = eventTimes[swingStartIndex]; + const scalar_t swingFinalTime = eventTimes[swingFinalIndex]; + + const scalar_t scaling = swingTrajectoryScaling(swingStartTime, swingFinalTime, + config_.swingTimeScale); + + const CubicSpline::Node liftOff{ + swingStartTime, liftOffHeightSequence[j][p], scaling * config_.liftOffVelocity + }; + const CubicSpline::Node touchDown{ + swingFinalTime, touchDownHeightSequence[j][p], scaling * config_.touchDownVelocity + }; + const scalar_t midHeight = maxHeightSequence[j][p] + scaling * config_.swingHeight; + feetHeightTrajectories_[j].emplace_back(liftOff, midHeight, touchDown); + } else { + // for a stance leg + // Note: setting the time here arbitrarily to 0.0 -> 1.0 makes the assert in CubicSpline fail + const CubicSpline::Node liftOff{0.0, liftOffHeightSequence[j][p], 0.0}; + const CubicSpline::Node touchDown{1.0, liftOffHeightSequence[j][p], 0.0}; + feetHeightTrajectories_[j].emplace_back(liftOff, liftOffHeightSequence[j][p], touchDown); + } + } + feetHeightTrajectoriesEvents_[j] = eventTimes; + } + } + + + std::pair, std::vector > SwingTrajectoryPlanner::updateFootSchedule( + const std::vector &contactFlagStock) { + const size_t numPhases = contactFlagStock.size(); + + std::vector startTimeIndexStock(numPhases, 0); + std::vector finalTimeIndexStock(numPhases, 0); + + // find the startTime and finalTime indices for swing feet + for (size_t i = 0; i < numPhases; i++) { + if (!contactFlagStock[i]) { + std::tie(startTimeIndexStock[i], finalTimeIndexStock[i]) = findIndex(i, contactFlagStock); + } + } + return {startTimeIndexStock, finalTimeIndexStock}; + } + + + feet_array_t > SwingTrajectoryPlanner::extractContactFlags( + const std::vector &phaseIDsStock) const { + const size_t numPhases = phaseIDsStock.size(); + + feet_array_t > contactFlagStock; + std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + + for (size_t i = 0; i < numPhases; i++) { + const auto contactFlag = modeNumber2StanceLeg(phaseIDsStock[i]); + for (size_t j = 0; j < numFeet_; j++) { + contactFlagStock[j][i] = contactFlag[j]; + } + } + return contactFlagStock; + } + + + std::pair SwingTrajectoryPlanner::findIndex(size_t index, const std::vector &contactFlagStock) { + const size_t numPhases = contactFlagStock.size(); + + // skip if it is a stance leg + if (contactFlagStock[index]) { + return {0, 0}; + } + + // find the starting time + int startTimesIndex = -1; + for (int ip = index - 1; ip >= 0; ip--) { + if (contactFlagStock[ip]) { + startTimesIndex = ip; + break; + } + } + + // find the final time + int finalTimesIndex = numPhases - 1; + for (size_t ip = index + 1; ip < numPhases; ip++) { + if (contactFlagStock[ip]) { + finalTimesIndex = ip - 1; + break; + } + } + + return {startTimesIndex, finalTimesIndex}; + } + + + void SwingTrajectoryPlanner::checkThatIndicesAreValid(int leg, int index, int startIndex, int finalIndex, + const std::vector &phaseIDsStock) { + const size_t numSubsystems = phaseIDsStock.size(); + if (startIndex < 0) { + std::cerr << "Subsystem: " << index << " out of " << numSubsystems - 1 << std::endl; + for (size_t i = 0; i < numSubsystems; i++) { + std::cerr << "[" << i << "]: " << phaseIDsStock[i] << ", "; + } + std::cerr << std::endl; + + throw std::runtime_error( + "The time of take-off for the first swing of the EE with ID " + std::to_string(leg) + + " is not defined."); + } + if (finalIndex >= numSubsystems - 1) { + std::cerr << "Subsystem: " << index << " out of " << numSubsystems - 1 << std::endl; + for (size_t i = 0; i < numSubsystems; i++) { + std::cerr << "[" << i << "]: " << phaseIDsStock[i] << ", "; + } + std::cerr << std::endl; + + throw std::runtime_error( + "The time of touch-down for the last swing of the EE with ID " + std::to_string(leg) + + " is not defined."); + } + } + + + scalar_t SwingTrajectoryPlanner::swingTrajectoryScaling(scalar_t startTime, scalar_t finalTime, + scalar_t swingTimeScale) { + return std::min(1.0, (finalTime - startTime) / swingTimeScale); + } + + + SwingTrajectoryPlanner::Config loadSwingTrajectorySettings(const std::string &fileName, + const std::string &fieldName, bool verbose) { + boost::property_tree::ptree pt; + read_info(fileName, pt); + + if (verbose) { + std::cerr << "\n #### Swing Trajectory Config:"; + std::cerr << "\n #### =============================================================================\n"; + } + + SwingTrajectoryPlanner::Config config; + const std::string prefix = fieldName + "."; + + loadData::loadPtreeValue(pt, config.liftOffVelocity, prefix + "liftOffVelocity", verbose); + loadData::loadPtreeValue(pt, config.touchDownVelocity, prefix + "touchDownVelocity", verbose); + loadData::loadPtreeValue(pt, config.swingHeight, prefix + "swingHeight", verbose); + loadData::loadPtreeValue(pt, config.swingTimeScale, prefix + "swingTimeScale", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << + std::endl; + } + + return config; + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroForceConstraint.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroForceConstraint.cpp new file mode 100644 index 0000000..477de83 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroForceConstraint.cpp @@ -0,0 +1,67 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h" + +#include + + +namespace ocs2::legged_robot { + ZeroForceConstraint::ZeroForceConstraint(const SwitchedModelReferenceManager &referenceManager, + size_t contactPointIndex, + CentroidalModelInfo info) + : StateInputConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(&referenceManager), + contactPointIndex_(contactPointIndex), + info_(std::move(info)) { + } + + + bool ZeroForceConstraint::isActive(scalar_t time) const { + return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; + } + + + vector_t ZeroForceConstraint::getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + return centroidal_model::getContactForces(input, contactPointIndex_, info_); + } + + + VectorFunctionLinearApproximation ZeroForceConstraint::getLinearApproximation( + scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + VectorFunctionLinearApproximation approx; + approx.f = getValue(time, state, input, preComp); + approx.dfdx = matrix_t::Zero(3, state.size()); + approx.dfdu = matrix_t::Zero(3, input.size()); + approx.dfdu.middleCols<3>(3 * contactPointIndex_).diagonal() = vector_t::Ones(3); + return approx; + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroVelocityConstraintCppAd.cpp b/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroVelocityConstraintCppAd.cpp new file mode 100644 index 0000000..b47f847 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroVelocityConstraintCppAd.cpp @@ -0,0 +1,70 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h" + +namespace ocs2::legged_robot { + ZeroVelocityConstraintCppAd::ZeroVelocityConstraintCppAd(const SwitchedModelReferenceManager &referenceManager, + const EndEffectorKinematics & + endEffectorKinematics, + size_t contactPointIndex, + EndEffectorLinearConstraint::Config config) + : StateInputConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(&referenceManager), + eeLinearConstraintPtr_(new EndEffectorLinearConstraint(endEffectorKinematics, 3, std::move(config))), + contactPointIndex_(contactPointIndex) { + } + + + ZeroVelocityConstraintCppAd::ZeroVelocityConstraintCppAd(const ZeroVelocityConstraintCppAd &rhs) + : StateInputConstraint(rhs), + referenceManagerPtr_(rhs.referenceManagerPtr_), + eeLinearConstraintPtr_(rhs.eeLinearConstraintPtr_->clone()), + contactPointIndex_(rhs.contactPointIndex_) { + } + + + bool ZeroVelocityConstraintCppAd::isActive(scalar_t time) const { + return referenceManagerPtr_->getContactFlags(time)[contactPointIndex_]; + } + + + vector_t ZeroVelocityConstraintCppAd::getValue(scalar_t time, const vector_t &state, const vector_t &input, + const PreComputation &preComp) const { + return eeLinearConstraintPtr_->getValue(time, state, input, preComp); + } + + + VectorFunctionLinearApproximation ZeroVelocityConstraintCppAd::getLinearApproximation( + scalar_t time, const vector_t &state, + const vector_t &input, + const PreComputation &preComp) const { + return eeLinearConstraintPtr_->getLinearApproximation(time, state, input, preComp); + } +} diff --git a/controllers/ocs2_quadruped_controller/src/interface/initialization/LeggedRobotInitializer.cpp b/controllers/ocs2_quadruped_controller/src/interface/initialization/LeggedRobotInitializer.cpp new file mode 100644 index 0000000..e1d76a5 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/interface/initialization/LeggedRobotInitializer.cpp @@ -0,0 +1,58 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include +#include +#include + + +namespace ocs2::legged_robot { + LeggedRobotInitializer::LeggedRobotInitializer(CentroidalModelInfo info, + const SwitchedModelReferenceManager &referenceManager, + bool extendNormalizedMomentum) + : info_(std::move(info)), referenceManagerPtr_(&referenceManager), + extendNormalizedMomentum_(extendNormalizedMomentum) { + } + + + LeggedRobotInitializer *LeggedRobotInitializer::clone() const { + return new LeggedRobotInitializer(*this); + } + + + void LeggedRobotInitializer::compute(scalar_t time, const vector_t &state, scalar_t nextTime, vector_t &input, + vector_t &nextState) { + const auto contactFlags = referenceManagerPtr_->getContactFlags(time); + input = weightCompensatingInput(info_, contactFlags); + nextState = state; + if (!extendNormalizedMomentum_) { + centroidal_model::getNormalizedMomentum(nextState, info_).setZero(); + } + } +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp b/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp new file mode 100644 index 0000000..e484c7f --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp @@ -0,0 +1,22 @@ +// +// Created by qiayuan on 22-12-23. +// + +#include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h" +#include "ocs2_quadruped_controller/wbc/HoQp.h" + +namespace ocs2::legged_robot { + vector_t HierarchicalWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, + const vector_t &rbdStateMeasured, size_t mode, + scalar_t period) { + WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); + + Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + + formulateNoContactMotionTask(); + Task task1 = formulateBaseAccelTask(stateDesired, inputDesired, period) + formulateSwingLegTask(); + Task task2 = formulateContactForceTask(inputDesired); + HoQp hoQp(task2, std::make_shared(task1, std::make_shared(task0))); + + return hoQp.getSolutions(); + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp b/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp new file mode 100644 index 0000000..d5a29df --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp @@ -0,0 +1,162 @@ +// +// Created by qiayuan on 2022/6/28. +// +// Ref: https://github.com/bernhardpg/quadruped_locomotion +// + +#include "ocs2_quadruped_controller/wbc/HoQp.h" + +#include +#include + +namespace ocs2::legged_robot { + HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) { + initVars(); + formulateProblem(); + solveProblem(); + // For next problem + buildZMatrix(); + stackSlackSolutions(); + } + + void HoQp::initVars() { + // Task variables + numSlackVars_ = task_.d_.rows(); + hasEqConstraints_ = task_.a_.rows() > 0; + hasIneqConstraints_ = numSlackVars_ > 0; + + // Pre-Task variables + if (higherProblem_ != nullptr) { + stackedZPrev_ = higherProblem_->getStackedZMatrix(); + stackedTasksPrev_ = higherProblem_->getStackedTasks(); + stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions(); + xPrev_ = higherProblem_->getSolutions(); + numPrevSlackVars_ = higherProblem_->getSlackedNumVars(); + + numDecisionVars_ = stackedZPrev_.cols(); + } else { + numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols()); + + stackedTasksPrev_ = Task(numDecisionVars_); + stackedZPrev_ = matrix_t::Identity(numDecisionVars_, numDecisionVars_); + stackedSlackSolutionsPrev_ = Eigen::VectorXd::Zero(0); + xPrev_ = Eigen::VectorXd::Zero(numDecisionVars_); + numPrevSlackVars_ = 0; + } + + stackedTasks_ = task_ + stackedTasksPrev_; + + // Init convenience matrices + eyeNvNv_ = matrix_t::Identity(numSlackVars_, numSlackVars_); + zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_); + } + + void HoQp::formulateProblem() { + buildHMatrix(); + buildCVector(); + buildDMatrix(); + buildFVector(); + } + + void HoQp::buildHMatrix() { + matrix_t zTaTaz(numDecisionVars_, numDecisionVars_); + + if (hasEqConstraints_) { + // Make sure that all eigenvalues of A_t_A are non-negative, which could arise due to numerical issues + matrix_t aCurrZPrev = task_.a_ * stackedZPrev_; + zTaTaz = aCurrZPrev.transpose() * aCurrZPrev + 1e-12 * matrix_t::Identity( + numDecisionVars_, numDecisionVars_); + // This way of splitting up the multiplication is about twice as fast as multiplying 4 matrices + } else { + zTaTaz.setZero(); + } + + h_ = (matrix_t(numDecisionVars_ + numSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off + << zTaTaz, zeroNvNx_.transpose(), + zeroNvNx_, eyeNvNv_) // clang-format on + .finished(); + } + + void HoQp::buildCVector() { + vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_); + vector_t zeroVec = vector_t::Zero(numSlackVars_); + + vector_t temp(numDecisionVars_); + if (hasEqConstraints_) { + temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_); + } else { + temp.setZero(); + } + + c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished(); + } + + void HoQp::buildDMatrix() { + matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_); + + matrix_t dCurrZ; + if (hasIneqConstraints_) { + dCurrZ = task_.d_ * stackedZPrev_; + } else { + dCurrZ = matrix_t::Zero(0, numDecisionVars_); + } + + // NOTE: This is upside down compared to the paper, + // but more consistent with the rest of the algorithm + d_ = (matrix_t(2 * numSlackVars_ + numPrevSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off + << zeroNvNx_, -eyeNvNv_, + stackedTasksPrev_.d_ * stackedZPrev_, stackedZero, + dCurrZ, -eyeNvNv_) // clang-format on + .finished(); + } + + void HoQp::buildFVector() { + vector_t zeroVec = vector_t::Zero(numSlackVars_); + + vector_t fMinusDXPrev; + if (hasIneqConstraints_) { + fMinusDXPrev = task_.f_ - task_.d_ * xPrev_; + } else { + fMinusDXPrev = vector_t::Zero(0); + } + + f_ = (vector_t(2 * numSlackVars_ + numPrevSlackVars_) << zeroVec, + stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev) + .finished(); + } + + void HoQp::buildZMatrix() { + if (hasEqConstraints_) { + assert((task_.a_.cols() > 0)); + stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel(); + } else { + stackedZ_ = stackedZPrev_; + } + } + + void HoQp::solveProblem() { + auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size()); + qpOASES::Options options; + options.setToMPC(); + options.printLevel = qpOASES::PL_LOW; + qpProblem.setOptions(options); + int nWsr = 20; + + qpProblem.init(h_.data(), c_.data(), d_.data(), nullptr, nullptr, nullptr, f_.data(), nWsr); + vector_t qpSol(numDecisionVars_ + numSlackVars_); + + qpProblem.getPrimalSolution(qpSol.data()); + + decisionVarsSolutions_ = qpSol.head(numDecisionVars_); + slackVarsSolutions_ = qpSol.tail(numSlackVars_); + } + + void HoQp::stackSlackSolutions() { + if (higherProblem_ != nullptr) { + stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(), + slackVarsSolutions_); + } else { + stackedSlackVars_ = slackVarsSolutions_; + } + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp b/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp new file mode 100644 index 0000000..5cac8b5 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp @@ -0,0 +1,280 @@ +// +// Created by qiayuan on 2022/7/1. +// +#include +#include "ocs2_quadruped_controller/wbc/WbcBase.h" +#include +#include +#include // forward declarations must be included first. +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot { + WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &eeKinematics) + : pinocchioInterfaceMeasured_(pinocchioInterface), + pinocchioInterfaceDesired_(pinocchioInterface), + info_(std::move(info)), + mapping_(info_), + inputLast_(vector_t::Zero(info_.inputDim)), + eeKinematics_(eeKinematics.clone()) { + numDecisionVars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum; + qMeasured_ = vector_t(info_.generalizedCoordinatesNum); + vMeasured_ = vector_t(info_.generalizedCoordinatesNum); + } + + vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired, + const vector_t &rbdStateMeasured, size_t mode, + scalar_t /*period*/) { + contactFlag_ = modeNumber2StanceLeg(mode); + numContacts_ = 0; + for (bool flag: contactFlag_) { + if (flag) { + numContacts_++; + } + } + + updateMeasured(rbdStateMeasured); + updateDesired(stateDesired, inputDesired); + + return {}; + } + + void WbcBase::updateMeasured(const vector_t &rbdStateMeasured) { + qMeasured_.head<3>() = rbdStateMeasured.segment<3>(3); + qMeasured_.segment<3>(3) = rbdStateMeasured.head<3>(); + qMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum); + vMeasured_.head<3>() = rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum + 3); + vMeasured_.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity( + qMeasured_.segment<3>(3), rbdStateMeasured.segment<3>(info_.generalizedCoordinatesNum)); + vMeasured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment( + info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum); + + const auto &model = pinocchioInterfaceMeasured_.getModel(); + auto &data = pinocchioInterfaceMeasured_.getData(); + + // For floating base EoM task + forwardKinematics(model, data, qMeasured_, vMeasured_); + computeJointJacobians(model, data); + updateFramePlacements(model, data); + crba(model, data, qMeasured_); + data.M.triangularView() = data.M.transpose().triangularView(); + nonLinearEffects(model, data, qMeasured_, vMeasured_); + j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + Eigen::Matrix jac; + jac.setZero(6, info_.generalizedCoordinatesNum); + getFrameJacobian(model, data, info_.endEffectorFrameIndices[i], pinocchio::LOCAL_WORLD_ALIGNED, + jac); + j_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) = jac.template topRows<3>(); + } + + // For not contact motion task + computeJointJacobiansTimeVariation(model, data, qMeasured_, vMeasured_); + dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + Eigen::Matrix jac; + jac.setZero(6, info_.generalizedCoordinatesNum); + getFrameJacobianTimeVariation(model, data, info_.endEffectorFrameIndices[i], + pinocchio::LOCAL_WORLD_ALIGNED, jac); + dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) = jac.template topRows<3>(); + } + } + + void WbcBase::updateDesired(const vector_t &stateDesired, const vector_t &inputDesired) { + const auto &model = pinocchioInterfaceDesired_.getModel(); + auto &data = pinocchioInterfaceDesired_.getData(); + + mapping_.setPinocchioInterface(pinocchioInterfaceDesired_); + const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); + forwardKinematics(model, data, qDesired); + computeJointJacobians(model, data, qDesired); + updateFramePlacements(model, data); + updateCentroidalDynamics(pinocchioInterfaceDesired_, info_, qDesired); + const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired); + forwardKinematics(model, data, qDesired, vDesired); + } + + Task WbcBase::formulateFloatingBaseEomTask() { + auto &data = pinocchioInterfaceMeasured_.getData(); + + matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum); + s.block(0, 0, info_.actuatedDofNum, 6).setZero(); + s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity(); + + matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, numDecisionVars_) << data.M, -j_.transpose(), -s. + transpose()).finished(); + vector_t b = -data.nle; + + return {a, b, matrix_t(), vector_t()}; + } + + Task WbcBase::formulateTorqueLimitsTask() { + matrix_t d(2 * info_.actuatedDofNum, numDecisionVars_); + d.setZero(); + matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum); + d.block(0, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts, info_.actuatedDofNum, + info_.actuatedDofNum) = i; + d.block(info_.actuatedDofNum, info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts, + info_.actuatedDofNum, + info_.actuatedDofNum) = -i; + vector_t f(2 * info_.actuatedDofNum); + for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) { + f.segment<3>(3 * l) = torqueLimits_; + } + + return {matrix_t(), vector_t(), d, f}; + } + + Task WbcBase::formulateNoContactMotionTask() { + matrix_t a(3 * numContacts_, numDecisionVars_); + vector_t b(a.rows()); + a.setZero(); + b.setZero(); + size_t j = 0; + for (size_t i = 0; i < info_.numThreeDofContacts; i++) { + if (contactFlag_[i]) { + a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block( + 3 * i, 0, 3, info_.generalizedCoordinatesNum); + b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_; + j++; + } + } + + return {a, b, matrix_t(), vector_t()}; + } + + Task WbcBase::formulateFrictionConeTask() { + matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_); + a.setZero(); + size_t j = 0; + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + if (!contactFlag_[i]) { + a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3); + } + } + vector_t b(a.rows()); + b.setZero(); + + matrix_t frictionPyramic(5, 3); // clang-format off + frictionPyramic << 0, 0, -1, + 1, 0, -frictionCoeff_, + -1, 0, -frictionCoeff_, + 0, 1, -frictionCoeff_, + 0, -1, -frictionCoeff_; // clang-format on + + matrix_t d(5 * numContacts_ + 3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_); + d.setZero(); + j = 0; + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + if (contactFlag_[i]) { + d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic; + } + } + vector_t f = Eigen::VectorXd::Zero(d.rows()); + + return {a, b, d, f}; + } + + Task WbcBase::formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period) { + matrix_t a(6, numDecisionVars_); + a.setZero(); + a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6); + + vector_t jointAccel = centroidal_model::getJointVelocities(inputDesired - inputLast_, info_) / period; + inputLast_ = inputDesired; + mapping_.setPinocchioInterface(pinocchioInterfaceDesired_); + + const auto &model = pinocchioInterfaceDesired_.getModel(); + auto &data = pinocchioInterfaceDesired_.getData(); + const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); + const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired); + + const auto &A = getCentroidalMomentumMatrix(pinocchioInterfaceDesired_); + const Matrix6 Ab = A.template leftCols<6>(); + const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab); + const auto Aj = A.rightCols(info_.actuatedDofNum); + const auto ADot = dccrba(model, data, qDesired, vDesired); + Vector6 centroidalMomentumRate = info_.robotMass * getNormalizedCentroidalMomentumRate( + pinocchioInterfaceDesired_, info_, inputDesired); + centroidalMomentumRate.noalias() -= ADot * vDesired; + centroidalMomentumRate.noalias() -= Aj * jointAccel; + + Vector6 b = AbInv * centroidalMomentumRate; + + return {a, b, matrix_t(), vector_t()}; + } + + Task WbcBase::formulateSwingLegTask() { + eeKinematics_->setPinocchioInterface(pinocchioInterfaceMeasured_); + std::vector posMeasured = eeKinematics_->getPosition(vector_t()); + std::vector velMeasured = eeKinematics_->getVelocity(vector_t(), vector_t()); + eeKinematics_->setPinocchioInterface(pinocchioInterfaceDesired_); + std::vector posDesired = eeKinematics_->getPosition(vector_t()); + std::vector velDesired = eeKinematics_->getVelocity(vector_t(), vector_t()); + + matrix_t a(3 * (info_.numThreeDofContacts - numContacts_), numDecisionVars_); + vector_t b(a.rows()); + a.setZero(); + b.setZero(); + size_t j = 0; + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + if (!contactFlag_[i]) { + vector3_t accel = swingKp_ * (posDesired[i] - posMeasured[i]) + swingKd_ * ( + velDesired[i] - velMeasured[i]); + a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block( + 3 * i, 0, 3, info_.generalizedCoordinatesNum); + b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * vMeasured_; + j++; + } + } + + return {a, b, matrix_t(), vector_t()}; + } + + Task WbcBase::formulateContactForceTask(const vector_t &inputDesired) const { + matrix_t a(3 * info_.numThreeDofContacts, numDecisionVars_); + vector_t b(a.rows()); + a.setZero(); + + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + a.block(3 * i, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3); + } + b = inputDesired.head(a.rows()); + + return {a, b, matrix_t(), vector_t()}; + } + + void WbcBase::loadTasksSetting(const std::string &taskFile, bool verbose) { + // Load task file + torqueLimits_ = vector_t(info_.actuatedDofNum / 4); + loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torqueLimits_); + if (verbose) { + std::cerr << "\n #### Torque Limits Task:"; + std::cerr << "\n #### =============================================================================\n"; + std::cerr << "\n #### HAA HFE KFE: " << torqueLimits_.transpose() << "\n"; + std::cerr << " #### =============================================================================\n"; + } + boost::property_tree::ptree pt; + boost::property_tree::read_info(taskFile, pt); + std::string prefix = "frictionConeTask."; + if (verbose) { + std::cerr << "\n #### Friction Cone Task:"; + std::cerr << "\n #### =============================================================================\n"; + } + loadData::loadPtreeValue(pt, frictionCoeff_, prefix + "frictionCoefficient", verbose); + if (verbose) { + std::cerr << " #### =============================================================================\n"; + } + prefix = "swingLegTask."; + if (verbose) { + std::cerr << "\n #### Swing Leg Task:"; + std::cerr << "\n #### =============================================================================\n"; + } + loadData::loadPtreeValue(pt, swingKp_, prefix + "kp", verbose); + loadData::loadPtreeValue(pt, swingKd_, prefix + "kd", verbose); + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp b/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp new file mode 100644 index 0000000..6ea9634 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp @@ -0,0 +1,81 @@ +// +// Created by qiayuan on 22-12-23. +// + +#include "ocs2_quadruped_controller/wbc/WeightedWbc.h" + +#include + +#include +#include +#include + +namespace ocs2::legged_robot { + vector_t WeightedWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, + const vector_t &rbdStateMeasured, size_t mode, + scalar_t period) { + WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); + + // Constraints + Task constraints = formulateConstraints(); + size_t numConstraints = constraints.b_.size() + constraints.f_.size(); + + Eigen::Matrix A(numConstraints, getNumDecisionVars()); + vector_t lbA(numConstraints), ubA(numConstraints); // clang-format off + A << constraints.a_, + constraints.d_; + + lbA << constraints.b_, + -qpOASES::INFTY * vector_t::Ones(constraints.f_.size()); + ubA << constraints.b_, + constraints.f_; // clang-format on + + // Cost + Task weighedTask = formulateWeightedTasks(stateDesired, inputDesired, period); + Eigen::Matrix H = + weighedTask.a_.transpose() * weighedTask.a_; + vector_t g = -weighedTask.a_.transpose() * weighedTask.b_; + + // Solve + auto qpProblem = qpOASES::QProblem(getNumDecisionVars(), numConstraints); + qpOASES::Options options; + options.setToMPC(); + options.printLevel = qpOASES::PL_LOW; + options.enableEqualities = qpOASES::BT_TRUE; + qpProblem.setOptions(options); + int nWsr = 20; + + qpProblem.init(H.data(), g.data(), A.data(), nullptr, nullptr, lbA.data(), ubA.data(), nWsr); + vector_t qpSol(getNumDecisionVars()); + + qpProblem.getPrimalSolution(qpSol.data()); + return qpSol; + } + + Task WeightedWbc::formulateConstraints() { + return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + + formulateNoContactMotionTask(); + } + + Task WeightedWbc::formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, + scalar_t period) { + return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) * + weightBaseAccel_ + + formulateContactForceTask(inputDesired) * weightContactForce_; + } + + void WeightedWbc::loadTasksSetting(const std::string &taskFile, bool verbose) { + WbcBase::loadTasksSetting(taskFile, verbose); + + boost::property_tree::ptree pt; + read_info(taskFile, pt); + const std::string prefix = "weight."; + if (verbose) { + std::cerr << "\n #### WBC weight:"; + std::cerr << "\n #### =============================================================================\n"; + } + loadData::loadPtreeValue(pt, weightSwingLeg_, prefix + "swingLeg", verbose); + loadData::loadPtreeValue(pt, weightBaseAccel_, prefix + "baseAccel", verbose); + loadData::loadPtreeValue(pt, weightContactForce_, prefix + "contactForce", verbose); + } +} // namespace legged diff --git a/submodules/qpOASES b/submodules/qpOASES new file mode 160000 index 0000000..0b86dbf --- /dev/null +++ b/submodules/qpOASES @@ -0,0 +1 @@ +Subproject commit 0b86dbf00c7fce34420bedc5914f71b176fe79d3