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

able to work on wbc

This commit is contained in:
Huang Zhenbiao 2024-09-29 17:09:06 +08:00
parent 00b6dbaeeb
commit 66c20b4050
7 changed files with 51 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
README.md
View File

@ -14,6 +14,7 @@ Todo List:
- [x] Contact Sensor
- [ ] OCS2 Legged Control
[![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/)
## 1. Build
* rosdep

19
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp
View File

@ -74,7 +74,7 @@ namespace ocs2::legged_robot {
current_observation_.input = optimized_input;
wbc_timer_.startTimer();
vector_t x = wbc_->update(optimized_state, optimized_input, measuredRbdState_, planned_mode, period.seconds());
vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode, period.seconds());
wbc_timer_.endTimer();
vector_t torque = x.tail(12);
@ -86,6 +86,13 @@ namespace ocs2::legged_robot {
// Safety check, if failed, stop the controller
if (!safety_checker_->check(current_observation_, optimized_state, optimized_input)) {
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
for (int i = 0; i < joint_names_.size(); i++) {
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.35);
}
return controller_interface::return_type::ERROR;
}
@ -109,6 +116,7 @@ namespace ocs2::legged_robot {
reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
// Load verbose parameter from the task file
verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
@ -129,9 +137,9 @@ namespace ocs2::legged_robot {
setupMrt();
// Visualization
CentroidalModelPinocchioMapping pinocchioMapping(legged_interface_->getCentroidalModelInfo());
CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
legged_interface_->getPinocchioInterface(), pinocchioMapping,
legged_interface_->getPinocchioInterface(), pinocchio_mapping,
legged_interface_->modelSettings().contactNames3DoF);
// robotVisualizer_ = std::make_shared<LeggedRobotVisualizer>(leggedInterface_->getPinocchioInterface(),
// leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh);
@ -182,6 +190,7 @@ namespace ocs2::legged_robot {
// assign command interfaces
for (auto &interface: command_interfaces_) {
std::string interface_name = interface.get_interface_name();
std::cout << "interface_name: " << interface.get_prefix_name() << std::endl;
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else {
@ -314,10 +323,10 @@ namespace ocs2::legged_robot {
}
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
measuredRbdState_ = ctrl_comp_.estimator_->update(time, period);
measured_rbd_state_ = ctrl_comp_.estimator_->update(time, period);
current_observation_.time += period.seconds();
const scalar_t yaw_last = current_observation_.state(9);
current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measuredRbdState_);
current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
current_observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, current_observation_.state(9));
current_observation_.mode = ctrl_comp_.estimator_->getMode();

2
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h
View File

@ -127,7 +127,7 @@ namespace ocs2::legged_robot {
std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
private:
vector_t measuredRbdState_;
vector_t measured_rbd_state_;
std::thread mpc_thread_;
std::atomic_bool controller_running_{}, mpc_running_{};
benchmark::RepeatedTimer mpc_timer_;

42
controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp
View File

@ -2,13 +2,24 @@
// Created by qiayuan on 2022/7/16.
//
#include <memory>
#include <pinocchio/fwd.hpp> // forward declarations must be included first.
#include "ocs2_quadruped_controller/interface/LeggedInterface.h"
#include <memory>
#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_legged_robot/dynamics/LeggedRobotDynamicsAD.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
#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"
@ -18,25 +29,13 @@
#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 &task_file, const std::string &urdf_file,
LeggedInterface::LeggedInterface(const std::string &task_file,
const std::string &urdf_file,
const std::string &reference_file,
const bool use_hard_friction_cone_constraint)
: use_hard_friction_cone_constraint_(use_hard_friction_cone_constraint) {
@ -85,7 +84,8 @@ namespace ocs2::legged_robot {
}
void LeggedInterface::setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file,
void LeggedInterface::setupOptimalControlProblem(const std::string &task_file,
const std::string &urdf_file,
const std::string &reference_file,
const bool verbose) {
setupModel(task_file, urdf_file, reference_file);
@ -147,9 +147,9 @@ namespace ocs2::legged_robot {
rollout_ptr_ = std::make_unique<TimeTriggeredRollout>(*problem_ptr_->dynamicsPtr, rollout_settings_);
// Initialization
constexpr bool extendNormalizedNomentum = true;
constexpr bool extend_normalized_momentum = true;
initializer_ptr_ = std::make_unique<LeggedRobotInitializer>(centroidal_model_info_, *reference_manager_ptr_,
extendNormalizedNomentum);
extend_normalized_momentum);
}
@ -171,7 +171,7 @@ namespace ocs2::legged_robot {
void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile,
bool verbose) {
const bool verbose) {
auto swingTrajectoryPlanner =
std::make_unique<SwingTrajectoryPlanner>(
loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);

10
controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp
View File

@ -1,16 +1,18 @@
//
// Created by qiayuan on 2022/7/1.
//
#include <utility>
#include "ocs2_quadruped_controller/wbc/WbcBase.h"
#include <utility>
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_centroidal_model/ModelHelperFunctions.h>
#include <ocs2_core/misc/LoadData.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,
@ -31,7 +33,7 @@ namespace ocs2::legged_robot {
scalar_t /*period*/) {
contact_flag_ = modeNumber2StanceLeg(mode);
num_contacts_ = 0;
for (bool flag: contact_flag_) {
for (const bool flag: contact_flag_) {
if (flag) {
num_contacts_++;
}
@ -99,7 +101,7 @@ namespace ocs2::legged_robot {
}
Task WbcBase::formulateFloatingBaseEomTask() {
auto &data = pinocchio_interface_measured_.getData();
const auto &data = pinocchio_interface_measured_.getData();
matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
s.block(0, 0, info_.actuatedDofNum, 6).setZero();

8
descriptions/go1_description/config/robot_control.yaml
View File

@ -79,12 +79,12 @@ ocs2_quadruped_controller:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
@ -103,8 +103,8 @@ ocs2_quadruped_controller:
feet_names:
- FL_foot
- RL_foot
- FR_foot
- RL_foot
- RR_foot

9
descriptions/go2_description/config/robot_control.yaml
View File

@ -79,12 +79,12 @@ ocs2_quadruped_controller:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
@ -103,11 +103,10 @@ ocs2_quadruped_controller:
feet_names:
- FL_foot
- RL_foot
- FR_foot
- RL_foot
- RR_foot
imu_name: "imu_sensor"
base_name: "base"