From ee4118b4e55039933d92ed35fe94fb6906d9017a Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Fri, 27 Sep 2024 17:40:22 +0800 Subject: [PATCH] current problem is from wbc --- .../estimator/LinearKalmanFilter.h | 4 +- .../estimator/StateEstimateBase.h | 12 +++--- .../wbc/HierarchicalWbc.h | 2 +- .../ocs2_quadruped_controller/wbc/Task.h | 1 - .../src/Ocs2QuadrupedController.cpp | 40 ++++++++++--------- .../src/Ocs2QuadrupedController.h | 2 + .../src/control/GaitManager.cpp | 1 - .../src/estimator/LinearKalmanFilter.cpp | 26 ++++++------ .../src/estimator/StateEstimateBase.cpp | 24 +++++------ 9 files changed, 57 insertions(+), 55 deletions(-) diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h index d951298..bce9cfa 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h @@ -16,8 +16,8 @@ namespace ocs2::legged_robot { class KalmanFilterEstimate final : public StateEstimateBase { public: - KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics, + KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &ee_kinematics, CtrlComponent &ctrl_component, const rclcpp_lifecycle::LifecycleNode::SharedPtr &node); diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index 619291a..33eb4bd 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -23,8 +23,8 @@ namespace ocs2::legged_robot { public: virtual ~StateEstimateBase() = default; - StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics, + StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &ee_kinematics, CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node); @@ -47,12 +47,12 @@ namespace ocs2::legged_robot { CtrlComponent &ctrl_component_; - PinocchioInterface pinocchioInterface_; + PinocchioInterface pinocchio_interface_; CentroidalModelInfo info_; - std::unique_ptr eeKinematics_; + std::unique_ptr ee_kinematics_; - vector3_t zyxOffset_ = vector3_t::Zero(); - vector_t rbdState_; + vector3_t zyx_offset_ = vector3_t::Zero(); + vector_t rbd_state_; contact_flag_t contact_flag_{}; Eigen::Quaternion quat_; vector3_t angular_vel_local_, linear_accel_local_; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h index e09c7ce..2da0308 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h @@ -6,7 +6,7 @@ #include "WbcBase.h" namespace ocs2::legged_robot { - class HierarchicalWbc : public WbcBase { + class HierarchicalWbc final : public WbcBase { public: using WbcBase::WbcBase; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h index 615b587..4080018 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h @@ -12,7 +12,6 @@ #include namespace ocs2::legged_robot { - using namespace ocs2; class Task { public: diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 703b33a..4cdf878 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -64,42 +65,43 @@ namespace ocs2::legged_robot { mpc_mrt_interface_->updatePolicy(); // Evaluate the current policy - vector_t optimizedState, optimizedInput; - size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at. - mpc_mrt_interface_->evaluatePolicy(current_observation_.time, current_observation_.state, optimizedState, - optimizedInput, plannedMode); + vector_t optimized_state, optimized_input; + size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at. + mpc_mrt_interface_->evaluatePolicy(current_observation_.time, current_observation_.state, optimized_state, + optimized_input, planned_mode); // Whole body control - current_observation_.input = optimizedInput; + current_observation_.input = optimized_input; wbc_timer_.startTimer(); - vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds()); + vector_t x = wbc_->update(optimized_state, optimized_input, measuredRbdState_, planned_mode, period.seconds()); wbc_timer_.endTimer(); vector_t torque = x.tail(12); - vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo()); - vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, + vector_t pos_des = centroidal_model::getJointAngles(optimized_state, legged_interface_->getCentroidalModelInfo()); + vector_t vel_des = centroidal_model::getJointVelocities(optimized_input, legged_interface_->getCentroidalModelInfo()); // Safety check, if failed, stop the controller - if (!safety_checker_->check(current_observation_, optimizedState, optimizedInput)) { + 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(torque(i)); - ctrl_comp_.joint_position_command_interface_[i].get().set_value(posDes(i)); - ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(velDes(i)); + ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i)); + ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i)); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0); - ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.0); + ctrl_comp_.joint_kd_command_interface_[i].get().set_value(1.0); } + observation_publisher_->publish(ros_msg_conversions::createObservationMsg(current_observation_)); + return controller_interface::return_type::OK; } controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { - // Initialize OCS2 urdf_file_ = auto_declare("urdf_file", urdf_file_); task_file_ = auto_declare("task_file", task_file_); @@ -162,6 +164,9 @@ namespace ocs2::legged_robot { ctrl_comp_.control_inputs_.ry = msg->ry; }); + observation_publisher_ = get_node()->create_publisher( + "legged_robot_mpc_observation", 10); + get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_); @@ -209,9 +214,7 @@ namespace ocs2::legged_robot { mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories); RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ..."); while (!mpc_mrt_interface_->initialPolicyReceived()) { - std::cout<<"Waiting for the initial policy ..."<advanceMpc(); - std::cout<<"Advance MPC"<mpcSettings().mrtDesiredFrequency_).sleep(); } RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received."); @@ -253,7 +256,7 @@ namespace ocs2::legged_robot { legged_interface_->getOptimalControlProblem(), legged_interface_->getInitializer()); rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo()); + legged_interface_->getCentroidalModelInfo()); const std::string robotName = "legged_robot"; @@ -268,10 +271,9 @@ namespace ocs2::legged_robot { // ROS ReferenceManager const auto rosReferenceManagerPtr = std::make_shared( robotName, legged_interface_->getReferenceManagerPtr()); - // rosReferenceManagerPtr->subscribe(this->get_node()); + rosReferenceManagerPtr->subscribe(get_node()); mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); - // observationPublisher_ = nh.advertise(robotName + "_mpc_observation", 1); } void Ocs2QuadrupedController::setupMrt() { @@ -316,7 +318,7 @@ namespace ocs2::legged_robot { const scalar_t yaw_last = current_observation_.state(9); current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measuredRbdState_); current_observation_.state(9) = yaw_last + angles::shortest_angular_distance( - yaw_last, current_observation_.state(9)); + yaw_last, current_observation_.state(9)); current_observation_.mode = ctrl_comp_.estimator_->getMode(); } } diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index a73fe33..11eb0bb 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -12,6 +12,7 @@ #include #include #include +#include #include "SafetyChecker.h" #include "ocs2_quadruped_controller/control/CtrlComponent.h" @@ -100,6 +101,7 @@ namespace ocs2::legged_robot { std::vector foot_force_interface_types_; rclcpp::Subscription::SharedPtr control_input_subscription_; + rclcpp::Publisher::SharedPtr observation_publisher_; SystemObservation current_observation_; diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index b5e49eb..2f97b7f 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -41,7 +41,6 @@ namespace ocs2::legged_robot { } void GaitManager::getTargetGait() { - std::cout << "ctrl_component_.control_inputs_.command: " << ctrl_component_.control_inputs_.command << std::endl; if (ctrl_component_.control_inputs_.command == 0) return; target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1]; RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s", diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index ebde51c..9420114 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -13,11 +13,11 @@ #include namespace ocs2::legged_robot { - KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics, + KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &ee_kinematics, CtrlComponent &ctrl_component, const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) - : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, ctrl_component, + : StateEstimateBase(std::move(pinocchio_interface), std::move(info), ee_kinematics, ctrl_component, node), numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), dimContacts_(3 * numContacts_), @@ -45,7 +45,7 @@ namespace ocs2::legged_robot { r_.setIdentity(numObserve_, numObserve_); feetHeights_.setZero(numContacts_); - eeKinematics_->setPinocchioInterface(pinocchioInterface_); + ee_kinematics_->setPinocchioInterface(pinocchio_interface_); } vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) { @@ -61,28 +61,28 @@ namespace ocs2::legged_robot { 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(); + const auto &model = pinocchio_interface_.getModel(); + auto &data = pinocchio_interface_.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); + qPino.segment<3>(3) = rbd_state_.head<3>(); // Only set orientation, let position in origin. + qPino.tail(actuatedDofNum) = rbd_state_.segment(6, actuatedDofNum); vPino.setZero(); vPino.segment<3>(3) = getEulerAnglesZyxDerivativesFromGlobalAngularVelocity( qPino.segment<3>(3), - rbdState_.segment<3>(info_.generalizedCoordinatesNum)); + rbd_state_.segment<3>(info_.generalizedCoordinatesNum)); // Only set angular velocity, let linear velocity be zero - vPino.tail(actuatedDofNum) = rbdState_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum); + vPino.tail(actuatedDofNum) = rbd_state_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum); forwardKinematics(model, data, qPino, vPino); updateFramePlacements(model, data); - const auto eePos = eeKinematics_->getPosition(vector_t()); - const auto eeVel = eeKinematics_->getVelocity(vector_t(), vector_t()); + const auto eePos = ee_kinematics_->getPosition(vector_t()); + const auto eeVel = ee_kinematics_->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_; @@ -154,7 +154,7 @@ namespace ocs2::legged_robot { odom.child_frame_id = "base"; publishMsgs(odom); - return rbdState_; + return rbd_state_; } nav_msgs::msg::Odometry KalmanFilterEstimate::getOdomMsg() { diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 62ac0f0..d792928 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -13,15 +13,15 @@ namespace ocs2::legged_robot { - StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics, + StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics &ee_kinematics, CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node) : ctrl_component_(ctrl_component), - pinocchioInterface_(std::move(pinocchioInterface)), + pinocchio_interface_(std::move(pinocchio_interface)), info_(std::move(info)), - eeKinematics_(eeKinematics.clone()), - rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { + ee_kinematics_(ee_kinematics.clone()), + rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { odom_pub_ = node_->create_publisher("odom", 10); pose_pub_ = node_->create_publisher("pose", 10); } @@ -35,8 +35,8 @@ namespace ocs2::legged_robot { joint_vel(i) = ctrl_component_.joint_velocity_state_interface_[i].get().get_value(); } - rbdState_.segment(6, info_.actuatedDofNum) = joint_pos; - rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel; + rbd_state_.segment(6, info_.actuatedDofNum) = joint_pos; + rbd_state_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel; } void StateEstimateBase::updateContact() { @@ -70,20 +70,20 @@ namespace ocs2::legged_robot { // angularVelCovariance_ = angularVelCovariance; // linearAccelCovariance_ = linearAccelCovariance; - const vector3_t zyx = quatToZyx(quat_) - zyxOffset_; + const vector3_t zyx = quatToZyx(quat_) - zyx_offset_; const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat_), angular_vel_local_)); 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; + rbd_state_.segment<3>(0) = zyx; + rbd_state_.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; + rbd_state_.segment<3>(3) = pos; + rbd_state_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; } void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) const {