ocs2 controller init
This commit is contained in:
Huang Zhenbiao
parent
5f7702bdb9
commit
ad680af0f1
|
|
@ -0,0 +1,3 @@
|
|||
[submodule "submodules/qpOASES"]
|
||||
path = submodules/qpOASES
|
||||
url = https://github.com/coin-or/qpOASES
|
||||
|
|
@ -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()
|
||||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
# OCS2 Quadruped Controller
|
||||
|
||||
## 2. Build
|
||||
```bash
|
||||
cd ~/ros2_ws
|
||||
colcon build --packages-up-to ocs2_quadruped_controller
|
||||
```
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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();
|
||||
};
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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,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>
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-24.
|
||||
//
|
||||
|
||||
#include "Ocs2QuadrupedController.h"
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 0b86dbf00c7fce34420bedc5914f71b176fe79d3
|
||||
Loading…
Reference in New Issue