EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ocs2 controller init

This commit is contained in:
Huang Zhenbiao 2024-09-24 19:48:14 +08:00
parent 5f7702bdb9
commit ad680af0f1
46 changed files with 4228 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored Normal file
View File

@ -0,0 +1,3 @@
[submodule "submodules/qpOASES"]
path = submodules/qpOASES
url = https://github.com/coin-or/qpOASES

76
controllers/ocs2_quadruped_controller/CMakeLists.txt Normal file
View File

@ -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}
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
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()

202
controllers/ocs2_quadruped_controller/LICENSE Normal file
View File

@ -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.

7
controllers/ocs2_quadruped_controller/README.md Normal file
View File

@ -0,0 +1,7 @@
# OCS2 Quadruped Controller
## 2. Build
```bash
cd ~/ros2_ws
colcon build --packages-up-to ocs2_quadruped_controller
```

33
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h Normal file
View File

@ -0,0 +1,33 @@
//
// Created by qiayuan on 2022/7/24.
//
#pragma once
#include "StateEstimateBase.h"
#include <realtime_tools/realtime_tools/realtime_buffer.h>
#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<scalar_t> &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<nav_msgs::Odometry> buffer_;
};
} // namespace legged

60
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h Normal file
View File

@ -0,0 +1,60 @@
//
// Created by qiayuan on 2022/7/24.
//
#pragma once
#include "StateEstimateBase.h"
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <realtime_tools/realtime_tools/realtime_buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
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<nav_msgs::Odometry> buffer_;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener tfListener_;
tf2::Transform world2odom_;
std::string frameOdom_, frameGuess_;
bool topicUpdated_;
};
} // namespace legged

81
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h Normal file
View File

@ -0,0 +1,81 @@
//
// Created by qiayuan on 2021/11/15.
//
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <realtime_tools/realtime_tools/realtime_publisher.h>
#include <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_legged_robot/common/ModelSettings.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
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<scalar_t> &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<PinocchioEndEffectorKinematics> eeKinematics_;
vector3_t zyxOffset_ = vector3_t::Zero();
vector_t rbdState_;
contact_flag_t contactFlag_{};
Eigen::Quaternion<scalar_t> quat_;
vector3_t angularVelLocal_, linearAccelLocal_;
matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr posePub_;
rclcpp::Time lastPub_;
rclcpp::Node::SharedPtr node_;
};
template<typename T>
T square(T a) {
return a * a;
}
template<typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T> &q) {
Eigen::Matrix<SCALAR_T, 3, 1> 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

127
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h Normal file
View File

@ -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 <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/Types.h>
#include <ocs2_core/initialization/Initializer.h>
#include <ocs2_core/penalties/Penalties.h>
#include <ocs2_ddp/DDP_Settings.h>
#include <ocs2_ipm/IpmSettings.h>
#include <ocs2_legged_robot/common/ModelSettings.h>
#include <ocs2_mpc/MPC_Settings.h>
#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
#include <ocs2_robotic_tools/common/RobotInterface.h>
#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
#include <ocs2_self_collision/PinocchioGeometryInterface.h>
#include <ocs2_sqp/SqpSettings.h>
#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<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const {
return referenceManagerPtr_;
}
const Initializer &getInitializer() const override { return *initializerPtr_; }
std::shared_ptr<ReferenceManagerInterface> 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<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;
std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile,
const CentroidalModelInfo &info, bool verbose);
matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info);
static std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(
const std::string &taskFile, bool verbose);
std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient);
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config &
barrierPenaltyConfig);
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &footNames,
const std::string &modelName);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &eeKinematics,
size_t contactPointIndex);
std::unique_ptr<StateCost> 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<PinocchioInterface> pinocchioInterfacePtr_;
CentroidalModelInfo centroidalModelInfo_;
std::unique_ptr<PinocchioGeometryInterface> geometryInterfacePtr_;
std::unique_ptr<OptimalControlProblem> problemPtr_;
std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_;
rollout::Settings rolloutSettings_;
std::unique_ptr<RolloutBase> rolloutPtr_;
std::unique_ptr<Initializer> initializerPtr_;
vector_t initialState_;
};
} // namespace legged
#pragma clang diagnostic pop

76
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h Normal file
View File

@ -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 <memory>
#include <ocs2_core/PreComputation.h>
#include <ocs2_pinocchio_interface/PinocchioInterface.h>
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_legged_robot/common/ModelSettings.h>
#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<EndEffectorLinearConstraint::Config> &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<CentroidalModelPinocchioMapping> mappingPtr_;
const ModelSettings settings_;
std::vector<EndEffectorLinearConstraint::Config> eeNormalVelConConfigs_;
};
}

67
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h Normal file
View File

@ -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 <ocs2_core/thread_support/Synchronized.h>
#include <ocs2_oc/synchronized_module/ReferenceManager.h>
#include <ocs2_legged_robot/gait/GaitSchedule.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#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<GaitSchedule> gaitSchedulePtr,
std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr);
~SwitchedModelReferenceManager() override = default;
void setModeSchedule(const ModeSchedule &modeSchedule) override;
contact_flag_t getContactFlags(scalar_t time) const;
const std::shared_ptr<GaitSchedule> &getGaitSchedule() { return gaitSchedulePtr_; }
const std::shared_ptr<SwingTrajectoryPlanner> &getSwingTrajectoryPlanner() { return swingTrajectoryPtr_; }
protected:
void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t &initState,
TargetTrajectories &targetTrajectories,
ModeSchedule &modeSchedule) override;
std::shared_ptr<GaitSchedule> gaitSchedulePtr_;
std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr_;
};
}

95
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/EndEffectorLinearConstraint.h Normal file
View File

@ -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 <memory>
#include <ocs2_core/constraint/StateInputConstraint.h>
#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
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<scalar_t> &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<scalar_t> &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<EndEffectorKinematics<scalar_t> > endEffectorKinematicsPtr_;
const size_t numConstraints_;
Config config_;
};
}

160
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/FrictionConeConstraint.h Normal file
View File

@ -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 <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/constraint/StateInputConstraint.h>
#include <ocs2_legged_robot/common/Types.h>
#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();
};
}

30
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/LeggedSelfCollisionConstraint.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by qiayuan on 23-1-29.
//
#pragma once
#include <ocs2_self_collision/SelfCollisionConstraint.h>
#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<LeggedRobotPreComputation>(preComputation).getPinocchioInterface();
}
};
} // namespace legged

78
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/NormalVelocityConstraintCppAd.h Normal file
View File

@ -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 <ocs2_core/constraint/StateInputConstraint.h>
#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<scalar_t> &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<EndEffectorLinearConstraint> eeLinearConstraintPtr_;
const size_t contactPointIndex_;
};
}

118
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h Normal file
View File

@ -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 <ocs2_core/reference/ModeSchedule.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_legged_robot/foot_planner/SplineCpg.h>
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<scalar_array_t> &liftOffHeightSequence,
const feet_array_t<scalar_array_t> &touchDownHeightSequence);
void update(const ModeSchedule &modeSchedule, const feet_array_t<scalar_array_t> &liftOffHeightSequence,
const feet_array_t<scalar_array_t> &touchDownHeightSequence,
const feet_array_t<scalar_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<std::vector<bool> > extractContactFlags(const std::vector<size_t> &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<int, int> findIndex(size_t index, const std::vector<bool> &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<int>, std::vector<int> > updateFootSchedule(
const std::vector<bool> &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<size_t> &phaseIDsStock);
static scalar_t swingTrajectoryScaling(scalar_t startTime, scalar_t finalTime, scalar_t swingTimeScale);
const Config config_;
const size_t numFeet_;
feet_array_t<std::vector<SplineCpg> > feetHeightTrajectories_;
feet_array_t<std::vector<scalar_t> > feetHeightTrajectoriesEvents_;
};
SwingTrajectoryPlanner::Config loadSwingTrajectorySettings(const std::string &fileName,
const std::string &fieldName = "swing_trajectory_config",
bool verbose = true);
}

71
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroForceConstraint.h Normal file
View File

@ -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 <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/constraint/StateInputConstraint.h>
#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_;
};
}

80
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/constraint/ZeroVelocityConstraintCppAd.h Normal file
View File

@ -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 <ocs2_core/constraint/StateInputConstraint.h>
#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<scalar_t> &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<EndEffectorLinearConstraint> eeLinearConstraintPtr_;
const size_t contactPointIndex_;
};
}

101
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/cost/LeggedRobotQuadraticTrackingCost.h Normal file
View File

@ -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 <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/cost/QuadraticStateCost.h>
#include <ocs2_core/cost/QuadraticStateInputCost.h>
#include <ocs2_legged_robot/common/utils.h>
#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<vector_t, vector_t> 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_;
};
}

63
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h Normal file
View File

@ -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 <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/initialization/Initializer.h>
#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_;
};
}

17
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h Normal file
View File

@ -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

76
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h Normal file
View File

@ -0,0 +1,76 @@
//
// Created by qiayuan on 2022/6/28.
//
//
// Ref: https://github.com/bernhardpg/quadruped_locomotion
//
#pragma once
#include "Task.h"
#include <memory>
namespace ocs2::legged_robot{
// Hierarchical Optimization Quadratic Program
class HoQp {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using HoQpPtr = std::shared_ptr<HoQp>;
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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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

78
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h Normal file
View File

@ -0,0 +1,78 @@
//
// Created by qiayuan on 2022/6/28.
//
//
// Ref: https://github.com/bernhardpg/quadruped_locomotion
//
#pragma once
#include <ocs2_core/Types.h>
#include <utility>
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

68
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h Normal file
View File

@ -0,0 +1,68 @@
//
// Created by qiayuan on 2022/7/1.
//
#pragma once
#include "Task.h"
#include <ocs2_centroidal_model/PinocchioCentroidalDynamics.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
namespace ocs2::legged_robot {
// Decision Variables: x = [\dot u^T, F^T, \tau^T]^T
class WbcBase {
using Vector6 = Eigen::Matrix<scalar_t, 6, 1>;
using Matrix6 = Eigen::Matrix<scalar_t, 6, 6>;
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<PinocchioEndEffectorKinematics> 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

28
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h Normal file
View File

@ -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

0
controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml Normal file
View File

25
controllers/ocs2_quadruped_controller/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ocs2_quadruped_controller</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="biao876990970@hotmail.com">tlab-uav</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>backward_ros</depend>
<depend>controller_interface</depend>
<depend>pluginlib</depend>
<depend>control_input_msgs</depend>
<depend>qpOASES</depend>
<depend>ocs2_self_collision</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

5
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp Normal file
View File

@ -0,0 +1,5 @@
//
// Created by tlab-uav on 24-9-24.
//
#include "Ocs2QuadrupedController.h"

57
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h Normal file
View File

@ -0,0 +1,57 @@
//
// Created by tlab-uav on 24-9-24.
//
#ifndef OCS2QUADRUPEDCONTROLLER_H
#define OCS2QUADRUPEDCONTROLLER_H
#include <controller_interface/controller_interface.hpp>
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

36
controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp Normal file
View File

@ -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<nav_msgs::Odometry>("/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<scalar_t>(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z)),
Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.angular.x, odom.twist.twist.angular.y,
odom.twist.twist.angular.z));
updateLinear(Eigen::Matrix<scalar_t, 3, 1>(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z),
Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.linear.x, odom.twist.twist.linear.y,
odom.twist.twist.linear.z));
publishMsgs(odom);
return rbdState_;
}
} // namespace legged

277
controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp Normal file
View File

@ -0,0 +1,277 @@
//
// Created by qiayuan on 2022/7/24.
//
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>
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<nav_msgs::msg::Odometry>("/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<scalar_t>(
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

74
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp Normal file
View File

@ -0,0 +1,74 @@
//
// Created by qiayuan on 2021/11/15.
//
#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
#include <ocs2_centroidal_model/FactoryFunctions.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
#include <memory>
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<nav_msgs::msg::Odometry>("odom", 10);
posePub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("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<scalar_t> &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<scalar_t>(
zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity<scalar_t>(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

414
controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp Normal file
View File

@ -0,0 +1,414 @@
//
// Created by qiayuan on 2022/7/16.
//
#include <pinocchio/fwd.hpp> // forward declarations must be included first.
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#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 <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_centroidal_model/FactoryFunctions.h>
#include <ocs2_centroidal_model/ModelHelperFunctions.h>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/misc/LoadStdVectorOfPair.h>
#include <ocs2_core/soft_constraint/StateInputSoftConstraint.h>
#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
#include <ocs2_legged_robot/dynamics/LeggedRobotDynamicsAD.h>
// Boost
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
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<OptimalControlProblem>();
// Dynamics
std::unique_ptr<SystemDynamicsBase> dynamicsPtr;
dynamicsPtr = std::make_unique<LeggedRobotDynamicsAD>(*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<EndEffectorKinematics<scalar_t> > 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<StateInputConstraint>(
new ZeroForceConstraint(
*referenceManagerPtr_, i, centroidalModelInfo_)));
problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
getZeroVelocityConstraint(*eeKinematicsPtr, i));
problemPtr_->equalityConstraintPtr->add(
footName + "_normalVelocity",
std::unique_ptr<StateInputConstraint>(
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<TimeTriggeredRollout>(*problemPtr_->dynamicsPtr, rolloutSettings_);
// Initialization
constexpr bool extendNormalizedNomentum = true;
initializerPtr_ = std::make_unique<LeggedRobotInitializer>(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<PinocchioInterface>(
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<SwingTrajectoryPlanner>(
loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
referenceManagerPtr_ =
std::make_shared<SwitchedModelReferenceManager>(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<LeggedRobotPreComputation>(
*pinocchioInterfacePtr_, centroidalModelInfo_, *referenceManagerPtr_->getSwingTrajectoryPlanner(),
modelSettings_);
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::shared_ptr<GaitSchedule> 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<GaitSchedule>(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<StateInputCost> 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<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info,
*referenceManagerPtr_);
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::pair<scalar_t, RelaxedBarrierPenalty::Config> 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<StateInputConstraint> LeggedInterface::getFrictionConeConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient) {
FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
return std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, frictionConeConConfig, contactPointIndex,
centroidalModelInfo_);
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateInputCost> LeggedInterface::getFrictionConeSoftConstraint(
size_t contactPointIndex, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) {
return std::make_unique<StateInputSoftConstraint>(
getFrictionConeConstraint(contactPointIndex, frictionCoefficient),
std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<EndEffectorKinematics<scalar_t> > LeggedInterface::getEeKinematicsPtr(
const std::vector<std::string> &footNames,
const std::string &modelName) {
std::unique_ptr<EndEffectorKinematics<scalar_t> > 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<StateInputConstraint> LeggedInterface::getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &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<StateInputConstraint>(new ZeroVelocityConstraintCppAd(
*referenceManagerPtr_, eeKinematics, contactPointIndex,
eeZeroVelConConfig(modelSettings_.positionErrorGain)));
}
/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
const std::string &taskFile,
const std::string &prefix,
bool verbose) {
std::vector<std::pair<size_t, size_t> > collisionObjectPairs;
std::vector<std::pair<std::string, std::string> > 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<PinocchioGeometryInterface>(
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<StateConstraint> constraint = std::make_unique<LeggedSelfCollisionConstraint>(
CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance);
auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta});
return std::make_unique<StateSoftConstraint>(std::move(constraint), std::move(penalty));
}
} // namespace legged

109
controllers/ocs2_quadruped_controller/src/interface/LeggedRobotPreComputation.cpp Normal file
View File

@ -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 <pinocchio/fwd.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <ocs2_centroidal_model/ModelHelperFunctions.h>
#include <ocs2_core/misc/Numerics.h>
#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);
}
}
}

64
controllers/ocs2_quadruped_controller/src/interface/SwitchedModelReferenceManager.cpp Normal file
View File

@ -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<GaitSchedule> gaitSchedulePtr,
std::shared_ptr<SwingTrajectoryPlanner>
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

105
controllers/ocs2_quadruped_controller/src/interface/constraint/EndEffectorLinearConstraint.cpp Normal file
View File

@ -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<scalar_t> &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

188
controllers/ocs2_quadruped_controller/src/interface/constraint/FrictionConeConstraint.cpp Normal file
View File

@ -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 <ocs2_centroidal_model/AccessHelperFunctions.h>
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

77
controllers/ocs2_quadruped_controller/src/interface/constraint/NormalVelocityConstraintCppAd.cpp Normal file
View File

@ -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<scalar_t> &
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<LeggedRobotPreComputation>(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<LeggedRobotPreComputation>(preComp);
eeLinearConstraintPtr_->configure(preCompLegged.getEeNormalVelocityConstraintConfigs()[contactPointIndex_]);
return eeLinearConstraintPtr_->getLinearApproximation(time, state, input, preComp);
}
} // namespace ocs2::legged_robot

261
controllers/ocs2_quadruped_controller/src/interface/constraint/SwingTrajectoryPlanner.cpp Normal file
View File

@ -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 <boost/property_tree/info_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include "ocs2_quadruped_controller/interface/constraint/SwingTrajectoryPlanner.h"
#include <iostream>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/misc/Lookup.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
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<scalar_array_t> liftOffHeightSequence;
liftOffHeightSequence.fill(terrainHeightSequence);
feet_array_t<scalar_array_t> touchDownHeightSequence;
touchDownHeightSequence.fill(terrainHeightSequence);
update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence);
}
void SwingTrajectoryPlanner::update(const ModeSchedule &modeSchedule,
const feet_array_t<scalar_array_t> &liftOffHeightSequence,
const feet_array_t<scalar_array_t> &touchDownHeightSequence) {
scalar_array_t heightSequence(modeSchedule.modeSequence.size());
feet_array_t<scalar_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<scalar_array_t> &liftOffHeightSequence,
const feet_array_t<scalar_array_t> &touchDownHeightSequence,
const feet_array_t<scalar_array_t> &maxHeightSequence) {
const auto &modeSequence = modeSchedule.modeSequence;
const auto &eventTimes = modeSchedule.eventTimes;
const auto eesContactFlagStocks = extractContactFlags(modeSequence);
feet_array_t<std::vector<int> > startTimesIndices;
feet_array_t<std::vector<int> > 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<int>, std::vector<int> > SwingTrajectoryPlanner::updateFootSchedule(
const std::vector<bool> &contactFlagStock) {
const size_t numPhases = contactFlagStock.size();
std::vector<int> startTimeIndexStock(numPhases, 0);
std::vector<int> 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<std::vector<bool> > SwingTrajectoryPlanner::extractContactFlags(
const std::vector<size_t> &phaseIDsStock) const {
const size_t numPhases = phaseIDsStock.size();
feet_array_t<std::vector<bool> > contactFlagStock;
std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector<bool>(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<int, int> SwingTrajectoryPlanner::findIndex(size_t index, const std::vector<bool> &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<size_t> &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

67
controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroForceConstraint.cpp Normal file
View File

@ -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 <ocs2_centroidal_model/AccessHelperFunctions.h>
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

70
controllers/ocs2_quadruped_controller/src/interface/constraint/ZeroVelocityConstraintCppAd.cpp Normal file
View File

@ -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<scalar_t> &
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);
}
}

58
controllers/ocs2_quadruped_controller/src/interface/initialization/LeggedRobotInitializer.cpp Normal file
View File

@ -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 <ocs2_quadruped_controller/interface/initialization/LeggedRobotInitializer.h>
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_legged_robot/common/utils.h>
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

22
controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp Normal file
View File

@ -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<HoQp>(task1, std::make_shared<HoQp>(task0)));
return hoQp.getSolutions();
}
} // namespace legged

162
controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp Normal file
View File

@ -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 <qpOASES.hpp>
#include <utility>
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

280
controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp Normal file
View File

@ -0,0 +1,280 @@
//
// Created by qiayuan on 2022/7/1.
//
#include <utility>
#include "ocs2_quadruped_controller/wbc/WbcBase.h"
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_centroidal_model/ModelHelperFunctions.h>
#include <pinocchio/fwd.hpp> // forward declarations must be included first.
#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <ocs2_core/misc/LoadData.h>
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<scalar_t>(
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<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
nonLinearEffects(model, data, qMeasured_, vMeasured_);
j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum);
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) {
Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> 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<scalar_t, 6, Eigen::Dynamic> 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<vector3_t> posMeasured = eeKinematics_->getPosition(vector_t());
std::vector<vector3_t> velMeasured = eeKinematics_->getVelocity(vector_t(), vector_t());
eeKinematics_->setPinocchioInterface(pinocchioInterfaceDesired_);
std::vector<vector3_t> posDesired = eeKinematics_->getPosition(vector_t());
std::vector<vector3_t> 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

81
controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp Normal file
View File

@ -0,0 +1,81 @@
//
// Created by qiayuan on 22-12-23.
//
#include "ocs2_quadruped_controller/wbc/WeightedWbc.h"
#include <qpOASES.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <ocs2_core/misc/LoadData.h>
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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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

1
submodules/qpOASES Submodule

@ -0,0 +1 @@
Subproject commit 0b86dbf00c7fce34420bedc5914f71b176fe79d3