diff --git a/README.md b/README.md index 8936db3..aba5799 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ Todo List: - [x] Unitree Guide Controller - [x] Gazebo Simulation - [x] Leg PD Controller -- [ ] Contact Sensor +- [x] Contact Sensor - [ ] OCS2 Legged Control diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index a207689..be07f08 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -17,6 +17,7 @@ set(CONTROLLER_INCLUDE_DEPENDS ocs2_legged_robot_ros ocs2_self_collision control_input_msgs + angles nav_msgs qpOASES ) @@ -55,12 +56,32 @@ target_include_directories(${PROJECT_NAME} PUBLIC ${qpOASES_INCLUDE_DIR} "$" - "$") + "$" + PRIVATE + src) ament_target_dependencies( ${PROJECT_NAME} PUBLIC ${CONTROLLER_INCLUDE_DEPENDS} ) +pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin +) + +ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + if (BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h new file mode 100644 index 0000000..e1c5fc7 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h @@ -0,0 +1,65 @@ +// +// Created by biao on 24-9-10. +// + +#ifndef INTERFACE_H +#define INTERFACE_H + +#include +#include +#include +#include + +#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h" + +struct CtrlComponent { + std::vector > + joint_torque_command_interface_; + std::vector > + joint_position_command_interface_; + std::vector > + joint_velocity_command_interface_; + std::vector > + joint_kp_command_interface_; + std::vector > + joint_kd_command_interface_; + + + std::vector > + joint_effort_state_interface_; + std::vector > + joint_position_state_interface_; + std::vector > + joint_velocity_state_interface_; + + std::vector > + imu_state_interface_; + + std::vector > + foot_force_state_interface_; + + control_input_msgs::msg::Inputs control_inputs_; + int frequency_{}; + + std::shared_ptr estimator_; + + CtrlComponent() { + } + + void clear() { + joint_torque_command_interface_.clear(); + joint_position_command_interface_.clear(); + joint_velocity_command_interface_.clear(); + joint_kd_command_interface_.clear(); + joint_kp_command_interface_.clear(); + + joint_effort_state_interface_.clear(); + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + + imu_state_interface_.clear(); + foot_force_state_interface_.clear(); + } +}; + +#endif //INTERFACE_H 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 60576c6..d951298 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 @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -19,6 +18,7 @@ namespace ocs2::legged_robot { public: KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, + CtrlComponent &ctrl_component, const rclcpp_lifecycle::LifecycleNode::SharedPtr &node); vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override; @@ -26,10 +26,6 @@ namespace ocs2::legged_robot { 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_; @@ -48,10 +44,5 @@ namespace ocs2::legged_robot { matrix_t a_, b_, c_, q_, p_, r_; vector_t xHat_, ps_, vs_; - - // Topic - tf2::Transform world2odom_; - std::string frameOdom_, frameGuess_; - bool topicUpdated_; }; } // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index 3b1cbe3..619291a 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 @@ -16,6 +16,8 @@ #include #include +struct CtrlComponent; + namespace ocs2::legged_robot { class StateEstimateBase { public: @@ -23,27 +25,27 @@ namespace ocs2::legged_robot { StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, + CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node); - virtual void updateJointStates(const vector_t &jointPos, const vector_t &jointVel); + virtual void updateJointStates(); - virtual void updateContact(contact_flag_t contactFlag) { contactFlag_ = contactFlag; } + virtual void updateContact(); - virtual void updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, - const vector3_t &linearAccelLocal, - const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance, - const matrix3_t &linearAccelCovariance); + virtual void updateImu(); virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0; - size_t getMode() { return stanceLeg2ModeNumber(contactFlag_); } + size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); } 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); + void publishMsgs(const nav_msgs::msg::Odometry &odom) const; + + CtrlComponent &ctrl_component_; PinocchioInterface pinocchioInterface_; CentroidalModelInfo info_; @@ -51,15 +53,13 @@ namespace ocs2::legged_robot { vector3_t zyxOffset_ = vector3_t::Zero(); vector_t rbdState_; - contact_flag_t contactFlag_{}; + contact_flag_t contact_flag_{}; Eigen::Quaternion quat_; - vector3_t angularVelLocal_, linearAccelLocal_; + vector3_t angular_vel_local_, linear_accel_local_; matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; - rclcpp::Publisher::SharedPtr odomPub_; - rclcpp::Publisher::SharedPtr posePub_; - - rclcpp::Time lastPub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; rclcpp_lifecycle::LifecycleNode::SharedPtr node_; }; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h index 2c2b13c..7b704ae 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h @@ -27,43 +27,43 @@ 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(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file, + bool use_hard_friction_cone_constraint = false); ~LeggedInterface() override = default; - virtual void setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, + virtual void setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file, + const std::string &reference_file, bool verbose); - const OptimalControlProblem &getOptimalControlProblem() const override { return *problemPtr_; } + const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; } - 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 ModelSettings &modelSettings() const { return model_settings_; } + const ddp::Settings &ddpSettings() const { return ddp_settings_; } + const mpc::Settings &mpcSettings() const { return mpc_settings_; } + const rollout::Settings &rolloutSettings() const { return rollout_settings_; } + const sqp::Settings &sqpSettings() { return sqp_settings_; } + const ipm::Settings &ipmSettings() { return ipm_settings_; } - 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_; } + const vector_t &getInitialState() const { return initial_state_; } + const RolloutBase &getRollout() const { return *rollout_ptr_; } + PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; } + const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; } + PinocchioGeometryInterface &getGeometryInterface() { return *geometry_interface_ptr_; } std::shared_ptr getSwitchedModelReferenceManagerPtr() const { - return referenceManagerPtr_; + return reference_manager_ptr_; } - const Initializer &getInitializer() const override { return *initializerPtr_; } + const Initializer &getInitializer() const override { return *initializer_ptr_; } std::shared_ptr getReferenceManagerPtr() const override { - return referenceManagerPtr_; + return reference_manager_ptr_; } protected: - virtual void setupModel(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, bool verbose); + virtual void setupModel(const std::string &task_file, const std::string &urdf_file, + const std::string &reference_file); virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile, @@ -76,7 +76,7 @@ namespace ocs2::legged_robot { std::shared_ptr loadGaitSchedule(const std::string &file, bool verbose) const; std::unique_ptr getBaseTrackingCost(const std::string &taskFile, - const CentroidalModelInfo &info, bool verbose); + const CentroidalModelInfo &info, bool verbose); matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info); @@ -84,43 +84,43 @@ namespace ocs2::legged_robot { const std::string &taskFile, bool verbose); std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, - scalar_t frictionCoefficient); + scalar_t frictionCoefficient); std::unique_ptr getFrictionConeSoftConstraint(size_t contactPointIndex, - scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config & - barrierPenaltyConfig); + scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config & + barrierPenaltyConfig); - std::unique_ptr > getEeKinematicsPtr(const std::vector &footNames, - const std::string &modelName); + std::unique_ptr > getEeKinematicsPtr(const std::vector &foot_names, + const std::string &model_name); std::unique_ptr getZeroVelocityConstraint( - const EndEffectorKinematics &eeKinematics, - size_t contactPointIndex); + const EndEffectorKinematics &end_effector_kinematics, + size_t contact_point_index); std::unique_ptr getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, const std::string &taskFile, const std::string &prefix, bool verbose); - ModelSettings modelSettings_; - mpc::Settings mpcSettings_; - ddp::Settings ddpSettings_; - sqp::Settings sqpSettings_; - ipm::Settings ipmSettings_; - const bool useHardFrictionConeConstraint_; + ModelSettings model_settings_; + mpc::Settings mpc_settings_; + ddp::Settings ddp_settings_; + sqp::Settings sqp_settings_; + ipm::Settings ipm_settings_; + const bool use_hard_friction_cone_constraint_; - std::unique_ptr pinocchioInterfacePtr_; - CentroidalModelInfo centroidalModelInfo_; - std::unique_ptr geometryInterfacePtr_; + std::unique_ptr pinocchio_interface_ptr_; + CentroidalModelInfo centroidal_model_info_; + std::unique_ptr geometry_interface_ptr_; - std::unique_ptr problemPtr_; - std::shared_ptr referenceManagerPtr_; + std::unique_ptr problem_ptr_; + std::shared_ptr reference_manager_ptr_; - rollout::Settings rolloutSettings_; - std::unique_ptr rolloutPtr_; - std::unique_ptr initializerPtr_; + rollout::Settings rollout_settings_; + std::unique_ptr rollout_ptr_; + std::unique_ptr initializer_ptr_; - vector_t initialState_; + vector_t initial_state_; }; } // namespace legged diff --git a/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml b/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml index e69de29..e5d3a67 100644 --- a/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml +++ b/controllers/ocs2_quadruped_controller/ocs2_quadruped_controller.xml @@ -0,0 +1,9 @@ + + + + Quadruped Controller based on OCS2 Legged Control. + + + diff --git a/controllers/ocs2_quadruped_controller/package.xml b/controllers/ocs2_quadruped_controller/package.xml index 16771f7..85f3c1b 100644 --- a/controllers/ocs2_quadruped_controller/package.xml +++ b/controllers/ocs2_quadruped_controller/package.xml @@ -15,6 +15,7 @@ control_input_msgs qpOASES ocs2_self_collision + angles ament_lint_auto ament_lint_common diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 98763df..c04c235 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -12,13 +12,116 @@ #include #include #include +#include namespace ocs2::legged_robot { + using config_type = controller_interface::interface_configuration_type; + + controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto &joint_name: joint_names_) { + for (const auto &interface_type: command_interface_types_) { + conf.names.push_back(joint_name + "/" += interface_type); + } + } + + return conf; + } + + controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto &joint_name: joint_names_) { + for (const auto &interface_type: state_interface_types_) { + conf.names.push_back(joint_name + "/" += interface_type); + } + } + + for (const auto &interface_type: imu_interface_types_) { + conf.names.push_back(imu_name_ + "/" += interface_type); + } + + for (const auto &interface_type: foot_force_interface_types_) { + conf.names.push_back(foot_force_name_ + "/" += interface_type); + } + + return conf; + } + + controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, + const rclcpp::Duration &period) { + // State Estimate + const vector_t rbd_state = ctrl_comp_.estimator_->update(time, period); + currentObservation_.time += period.seconds(); + const scalar_t yaw_last = currentObservation_.state(9); + currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(rbd_state); + currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(yaw_last, currentObservation_.state(9)); + currentObservation_.mode = stateEstimate_->getMode(); + + // Update the current state of the system + mpcMrtInterface_->setCurrentObservation(currentObservation_); + + // Load the latest MPC policy + mpcMrtInterface_->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. + mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, optimizedInput, plannedMode); + + // Whole body control + currentObservation_.input = optimizedInput; + + wbcTimer_.startTimer(); + vector_t x = wbc_->update(optimizedState, optimizedInput, rbd_state, plannedMode, period.seconds()); + wbcTimer_.endTimer(); + + vector_t torque = x.tail(12); + + vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo()); + vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, legged_interface_->getCentroidalModelInfo()); + + // Safety check, if failed, stop the controller + if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) { + 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_kp_command_interface_[i].get().set_value(0.0); + ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.0); + } + + 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_); + reference_file_ = auto_declare("reference_file", reference_file_); + verbose_ = false; loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_); - setUpLeggedInterface(); + // Hardware Parameters + joint_names_ = auto_declare >("joints", joint_names_); + command_interface_types_ = + auto_declare >("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare >("state_interfaces", state_interface_types_); + imu_name_ = auto_declare("imu_name", imu_name_); + imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); + foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); + foot_force_interface_types_ = + auto_declare >("foot_force_interfaces", state_interface_types_); + + setupLeggedInterface(); setupMpc(); setupMrt(); @@ -47,7 +150,75 @@ namespace ocs2::legged_robot { return CallbackReturn::SUCCESS; } - void Ocs2QuadrupedController::setUpLeggedInterface() { + controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + control_input_subscription_ = get_node()->create_subscription( + "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { + // Handle message + ctrl_comp_.control_inputs_.command = msg->command; + ctrl_comp_.control_inputs_.lx = msg->lx; + ctrl_comp_.control_inputs_.ly = msg->ly; + ctrl_comp_.control_inputs_.rx = msg->rx; + ctrl_comp_.control_inputs_.ry = msg->ry; + }); + + get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); + RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_); + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + // clear out vectors in case of restart + ctrl_comp_.clear(); + + // assign command interfaces + for (auto &interface: command_interfaces_) { + std::string interface_name = interface.get_interface_name(); + if (const size_t pos = interface_name.find('/'); pos != std::string::npos) { + command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface); + } else { + command_interface_map_[interface_name]->push_back(interface); + } + } + + // assign state interfaces + for (auto &interface: state_interfaces_) { + if (interface.get_prefix_name() == imu_name_) { + ctrl_comp_.imu_state_interface_.emplace_back(interface); + } else if (interface.get_prefix_name() == foot_force_name_) { + ctrl_comp_.foot_force_state_interface_.emplace_back(interface); + } else { + state_interface_map_[interface.get_interface_name()]->push_back(interface); + } + } + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + release_interfaces(); + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) { + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown( + const rclcpp_lifecycle::State & /*previous_state*/) { + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn Ocs2QuadrupedController::on_error( + const rclcpp_lifecycle::State & /*previous_state*/) { + return CallbackReturn::SUCCESS; + } + + void Ocs2QuadrupedController::setupLeggedInterface() { legged_interface_ = std::make_shared(task_file_, urdf_file_, reference_file_); legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); } @@ -60,18 +231,21 @@ namespace ocs2::legged_robot { legged_interface_->getCentroidalModelInfo()); const std::string robotName = "legged_robot"; + + // Todo Handle Gait Receive. // Gait receiver - auto gaitReceiverPtr = - std::make_shared(this->get_node(), - legged_interface_->getSwitchedModelReferenceManagerPtr()-> - getGaitSchedule(), robotName); + // auto gaitReceiverPtr = + // std::make_shared(this->get_node(), + // legged_interface_->getSwitchedModelReferenceManagerPtr()-> + // getGaitSchedule(), robotName); + // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared( - robotName, legged_interface_->getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(this->get_node()); - mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); - observationPublisher_ = nh.advertise(robotName + "_mpc_observation", 1); + // auto rosReferenceManagerPtr = std::make_shared( + // robotName, legged_interface_->getReferenceManagerPtr()); + // rosReferenceManagerPtr->subscribe(this->get_node()); + // mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + // observationPublisher_ = nh.advertise(robotName + "_mpc_observation", 1); } void Ocs2QuadrupedController::setupMrt() { @@ -80,11 +254,11 @@ namespace ocs2::legged_robot { mpcTimer_.reset(); controllerRunning_ = true; - mpcThread_ = std::thread([&]() { + mpcThread_ = std::thread([&] { while (controllerRunning_) { try { executeAndSleep( - [&]() { + [&] { if (mpcRunning_) { mpcTimer_.startTimer(); mpcMrtInterface_->advanceMpc(); @@ -102,10 +276,13 @@ namespace ocs2::legged_robot { } void Ocs2QuadrupedController::setupStateEstimate() { - stateEstimate_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_, this->get_node()); - dynamic_cast(*stateEstimate_).loadSettings(task_file_, verbose_); + ctrl_comp_.estimator_ = std::make_shared(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *eeKinematicsPtr_, ctrl_comp_, this->get_node()); + dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); currentObservation_.time = 0; } } + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface); \ No newline at end of file diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index ebb1315..0ff4a67 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -4,13 +4,16 @@ #ifndef OCS2QUADRUPEDCONTROLLER_H #define OCS2QUADRUPEDCONTROLLER_H -#include "SafetyChecker.h" + #include -#include -#include +#include #include +#include #include +#include #include +#include "SafetyChecker.h" +#include "ocs2_quadruped_controller/control/CtrlComponent.h" namespace ocs2::legged_robot { class Ocs2QuadrupedController final : public controller_interface::ControllerInterface { @@ -56,7 +59,7 @@ namespace ocs2::legged_robot { const rclcpp_lifecycle::State &previous_state) override; protected: - void setUpLeggedInterface(); + void setupLeggedInterface(); void setupMpc(); @@ -64,6 +67,38 @@ namespace ocs2::legged_robot { void setupStateEstimate(); + CtrlComponent ctrl_comp_; + std::vector joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + std::unordered_map< + std::string, std::vector > *> + command_interface_map_ = { + {"effort", &ctrl_comp_.joint_torque_command_interface_}, + {"position", &ctrl_comp_.joint_position_command_interface_}, + {"velocity", &ctrl_comp_.joint_velocity_command_interface_}, + {"kp", &ctrl_comp_.joint_kp_command_interface_}, + {"kd", &ctrl_comp_.joint_kd_command_interface_} + }; + std::unordered_map< + std::string, std::vector > *> + state_interface_map_ = { + {"position", &ctrl_comp_.joint_position_state_interface_}, + {"effort", &ctrl_comp_.joint_effort_state_interface_}, + {"velocity", &ctrl_comp_.joint_velocity_state_interface_} + }; + + // IMU Sensor + std::string imu_name_; + std::vector imu_interface_types_; + // Foot Force Sensor + std::string foot_force_name_; + std::vector foot_force_interface_types_; + + rclcpp::Subscription::SharedPtr control_input_subscription_; + + SystemObservation currentObservation_; std::string task_file_; diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index 4fb5e80..ebde51c 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -15,18 +15,20 @@ namespace ocs2::legged_robot { KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, + CtrlComponent &ctrl_component, const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) - : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, std::move(node)), + : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, ctrl_component, + node), numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), dimContacts_(3 * numContacts_), numState_(6 + dimContacts_), - numObserve_(2 * dimContacts_ + numContacts_), - topicUpdated_(false) { + numObserve_(2 * dimContacts_ + numContacts_) { 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(); @@ -44,11 +46,13 @@ namespace ocs2::legged_robot { feetHeights_.setZero(numContacts_); eeKinematics_->setPinocchioInterface(pinocchioInterface_); - - world2odom_.setRotation(tf2::Quaternion::getIdentity()); } vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) { + updateJointStates(); + updateContact(); + updateImu(); + scalar_t dt = period.seconds(); a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity(); b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity(); @@ -101,7 +105,7 @@ namespace ocs2::legged_robot { int rIndex1 = i1; int rIndex2 = dimContacts_ + i1; int rIndex3 = 2 * dimContacts_ + i; - bool isContact = contactFlag_[i]; + bool isContact = contact_flag_[i]; scalar_t high_suspect_number(100); q.block(qIndex, qIndex, 3, 3) = (isContact ? 1. : high_suspect_number) * q.block(qIndex, qIndex, 3, 3); @@ -115,7 +119,7 @@ namespace ocs2::legged_robot { } vector3_t g(0, 0, -9.81); - vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linearAccelLocal_ + g; + vector3_t accel = getRotationMatrixFromZyxEulerAngles(quatToZyx(quat_)) * linear_accel_local_ + g; vector_t y(numObserve_); y << ps_, vs_, feetHeights_; @@ -142,11 +146,6 @@ namespace ocs2::legged_robot { // p_.block(0, 0, 2, 2) /= 10.; // } - if (topicUpdated_) { - updateFromTopic(); - topicUpdated_ = false; - } - updateLinear(xHat_.segment<3>(0), xHat_.segment<3>(3)); auto odom = getOdomMsg(); @@ -179,9 +178,9 @@ namespace ocs2::legged_robot { 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(); + odom.twist.twist.angular.x = angular_vel_local_.x(); + odom.twist.twist.angular.y = angular_vel_local_.y(); + odom.twist.twist.angular.z = angular_vel_local_.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); diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 8a8bd82..83ace89 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -9,39 +9,71 @@ #include #include +#include "ocs2_quadruped_controller/control/CtrlComponent.h" namespace ocs2::legged_robot { using namespace legged_robot; StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, + CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node) - : pinocchioInterface_(std::move(pinocchioInterface)), + : ctrl_component_(ctrl_component), + pinocchioInterface_(std::move(pinocchioInterface)), info_(std::move(info)), eeKinematics_(eeKinematics.clone()), rbdState_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { - odomPub_ = node_->create_publisher("odom", 10); - posePub_ = node_->create_publisher("pose", 10); + odom_pub_ = node_->create_publisher("odom", 10); + pose_pub_ = node_->create_publisher("pose", 10); } - void StateEstimateBase::updateJointStates(const vector_t &jointPos, const vector_t &jointVel) { - rbdState_.segment(6, info_.actuatedDofNum) = jointPos; - rbdState_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = jointVel; + void StateEstimateBase::updateJointStates() { + const size_t size = ctrl_component_.joint_effort_state_interface_.size(); + vector_t joint_pos(size), joint_vel(size); + + for (int i = 0; i < size; i++) { + joint_pos(i) = ctrl_component_.joint_position_state_interface_[i].get().get_value(); + 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; } - void StateEstimateBase::updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, - const vector3_t &linearAccelLocal, const matrix3_t &orientationCovariance, - const matrix3_t &angularVelCovariance, const matrix3_t &linearAccelCovariance) { - quat_ = quat; - angularVelLocal_ = angularVelLocal; - linearAccelLocal_ = linearAccelLocal; - orientationCovariance_ = orientationCovariance; - angularVelCovariance_ = angularVelCovariance; - linearAccelCovariance_ = linearAccelCovariance; + void StateEstimateBase::updateContact() { + const size_t size = ctrl_component_.foot_force_state_interface_.size(); + for (int i = 0; i < size; i++) { + contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 1; + } + } - vector3_t zyx = quatToZyx(quat) - zyxOffset_; - vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( - zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat), angularVelLocal)); + void StateEstimateBase::updateImu() { + quat_ = { + ctrl_component_.imu_state_interface_[0].get().get_value(), + ctrl_component_.imu_state_interface_[1].get().get_value(), + ctrl_component_.imu_state_interface_[2].get().get_value(), + ctrl_component_.imu_state_interface_[3].get().get_value() + }; + + angular_vel_local_ = { + ctrl_component_.imu_state_interface_[4].get().get_value(), + ctrl_component_.imu_state_interface_[5].get().get_value(), + ctrl_component_.imu_state_interface_[6].get().get_value() + }; + + linear_accel_local_ = { + ctrl_component_.imu_state_interface_[7].get().get_value(), + ctrl_component_.imu_state_interface_[8].get().get_value(), + ctrl_component_.imu_state_interface_[9].get().get_value() + }; + + // orientationCovariance_ = orientationCovariance; + // angularVelCovariance_ = angularVelCovariance; + // linearAccelCovariance_ = linearAccelCovariance; + + const vector3_t zyx = quatToZyx(quat_) - zyxOffset_; + const vector3_t angularVelGlobal = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives( + zyx, getEulerAnglesZyxDerivativesFromLocalAngularVelocity(quatToZyx(quat_), angular_vel_local_)); updateAngular(zyx, angularVelGlobal); } @@ -55,20 +87,13 @@ namespace ocs2::legged_robot { rbdState_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; } - void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) { + void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) const { 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(); - // } - // } + odom_pub_->publish(odom); + + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header = odom.header; + pose.pose.pose = odom.pose.pose; + pose_pub_->publish(pose); } } // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp index d634c0a..2cab118 100644 --- a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp +++ b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp @@ -2,6 +2,7 @@ // Created by qiayuan on 2022/7/16. // +#include #include // forward declarations must be included first. #include @@ -35,166 +36,160 @@ #include namespace ocs2::legged_robot { - LeggedInterface::LeggedInterface(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, - bool useHardFrictionConeConstraint) - : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { + 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) { // check that task file exists - boost::filesystem::path taskFilePath(taskFile); - if (boost::filesystem::exists(taskFilePath)) { - std::cerr << "[LeggedInterface] Loading task file: " << taskFilePath << std::endl; + if (const boost::filesystem::path task_file_path(task_file); exists(task_file_path)) { + std::cerr << "[LeggedInterface] Loading task file: " << task_file_path << std::endl; } else { - throw std::invalid_argument("[LeggedInterface] Task file not found: " + taskFilePath.string()); + throw std::invalid_argument("[LeggedInterface] Task file not found: " + task_file_path.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; + if (const boost::filesystem::path urdf_file_path(urdf_file); exists(urdf_file_path)) { + std::cerr << "[LeggedInterface] Loading Pinocchio model from: " << urdf_file_path << std::endl; } else { - throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdfFilePath.string()); + throw std::invalid_argument("[LeggedInterface] URDF file not found: " + urdf_file_path.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; + if (const boost::filesystem::path reference_file_path(reference_file); exists(reference_file_path)) { + std::cerr << "[LeggedInterface] Loading target command settings from: " << reference_file_path << std::endl; } else { throw std::invalid_argument( - "[LeggedInterface] targetCommand file not found: " + referenceFilePath.string()); + "[LeggedInterface] targetCommand file not found: " + reference_file_path.string()); } bool verbose = false; - loadData::loadCppDataType(taskFile, "legged_robot_interface.verbose", verbose); + loadData::loadCppDataType(task_file, "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); + model_settings_ = loadModelSettings(task_file, "model_settings", verbose); + + // Todo : load settings from ros param + model_settings_.jointNames = { + "FF_hip_joint", "FL_thigh_joint", "FL_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" + }; + model_settings_.contactNames3DoF = {"FL_foot", "FR_foot", "RL_foot", "RR_foot"}; + + mpc_settings_ = mpc::loadSettings(task_file, "mpc", verbose); + ddp_settings_ = ddp::loadSettings(task_file, "ddp", verbose); + sqp_settings_ = sqp::loadSettings(task_file, "sqp", verbose); + ipm_settings_ = ipm::loadSettings(task_file, "ipm", verbose); + rollout_settings_ = rollout::loadSettings(task_file, "rollout", verbose); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ - void LeggedInterface::setupOptimalControlProblem(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, - bool verbose) { - setupModel(taskFile, urdfFile, referenceFile, verbose); + + 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); // Initial state - initialState_.setZero(centroidalModelInfo_.stateDim); - loadData::loadEigenMatrix(taskFile, "initialState", initialState_); + initial_state_.setZero(centroidal_model_info_.stateDim); + loadData::loadEigenMatrix(task_file, "initialState", initial_state_); - setupReferenceManager(taskFile, urdfFile, referenceFile, verbose); + setupReferenceManager(task_file, urdf_file, reference_file, verbose); // Optimal control problem - problemPtr_ = std::make_unique(); + problem_ptr_ = std::make_unique(); // Dynamics - std::unique_ptr dynamicsPtr; - dynamicsPtr = std::make_unique(*pinocchioInterfacePtr_, centroidalModelInfo_, "dynamics", - modelSettings_); - problemPtr_->dynamicsPtr = std::move(dynamicsPtr); + std::unique_ptr dynamicsPtr = std::make_unique( + *pinocchio_interface_ptr_, centroidal_model_info_, "dynamics", + model_settings_); + problem_ptr_->dynamicsPtr = std::move(dynamicsPtr); // Cost terms - problemPtr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(taskFile, centroidalModelInfo_, verbose)); + problem_ptr_->costPtr->add("baseTrackingCost", getBaseTrackingCost(task_file, centroidal_model_info_, verbose)); // Constraint terms // friction cone settings scalar_t frictionCoefficient = 0.7; RelaxedBarrierPenalty::Config barrierPenaltyConfig; - std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(taskFile, verbose); + std::tie(frictionCoefficient, barrierPenaltyConfig) = loadFrictionConeSettings(task_file, verbose); - for (size_t i = 0; i < centroidalModelInfo_.numThreeDofContacts; i++) { - const std::string &footName = modelSettings_.contactNames3DoF[i]; + for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++) { + const std::string &footName = model_settings_.contactNames3DoF[i]; std::unique_ptr > eeKinematicsPtr = getEeKinematicsPtr({footName}, footName); - if (useHardFrictionConeConstraint_) { - problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", - getFrictionConeConstraint(i, frictionCoefficient)); + if (use_hard_friction_cone_constraint_) { + problem_ptr_->inequalityConstraintPtr->add(footName + "_frictionCone", + getFrictionConeConstraint(i, frictionCoefficient)); } else { - problemPtr_->softConstraintPtr->add(footName + "_frictionCone", - getFrictionConeSoftConstraint( - i, frictionCoefficient, barrierPenaltyConfig)); + problem_ptr_->softConstraintPtr->add(footName + "_frictionCone", + getFrictionConeSoftConstraint( + i, frictionCoefficient, barrierPenaltyConfig)); } - problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", std::unique_ptr( - new ZeroForceConstraint( - *referenceManagerPtr_, i, centroidalModelInfo_))); - problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", - getZeroVelocityConstraint(*eeKinematicsPtr, i)); - problemPtr_->equalityConstraintPtr->add( + problem_ptr_->equalityConstraintPtr->add(footName + "_zeroForce", std::make_unique( + *reference_manager_ptr_, i, centroidal_model_info_)); + problem_ptr_->equalityConstraintPtr->add(footName + "_zeroVelocity", + getZeroVelocityConstraint(*eeKinematicsPtr, i)); + problem_ptr_->equalityConstraintPtr->add( footName + "_normalVelocity", - std::unique_ptr( - new NormalVelocityConstraintCppAd(*referenceManagerPtr_, *eeKinematicsPtr, i))); + std::make_unique(*reference_manager_ptr_, *eeKinematicsPtr, i)); } // Self-collision avoidance constraint - problemPtr_->stateSoftConstraintPtr->add("selfCollision", - getSelfCollisionConstraint( - *pinocchioInterfacePtr_, taskFile, "selfCollision", verbose)); + problem_ptr_->stateSoftConstraintPtr->add("selfCollision", + getSelfCollisionConstraint( + *pinocchio_interface_ptr_, task_file, "selfCollision", verbose)); - setupPreComputation(taskFile, urdfFile, referenceFile, verbose); + setupPreComputation(task_file, urdf_file, reference_file, verbose); // Rollout - rolloutPtr_ = std::make_unique(*problemPtr_->dynamicsPtr, rolloutSettings_); + rollout_ptr_ = std::make_unique(*problem_ptr_->dynamicsPtr, rollout_settings_); // Initialization constexpr bool extendNormalizedNomentum = true; - initializerPtr_ = std::make_unique(centroidalModelInfo_, *referenceManagerPtr_, - extendNormalizedNomentum); + initializer_ptr_ = std::make_unique(centroidal_model_info_, *reference_manager_ptr_, + extendNormalizedNomentum); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ - void LeggedInterface::setupModel(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, - bool /*verbose*/) { + + void LeggedInterface::setupModel(const std::string &task_file, const std::string &urdf_file, + const std::string &reference_file) { // PinocchioInterface - pinocchioInterfacePtr_ = + pinocchio_interface_ptr_ = std::make_unique( - centroidal_model::createPinocchioInterface(urdfFile, modelSettings_.jointNames)); + centroidal_model::createPinocchioInterface(urdf_file, model_settings_.jointNames)); // CentroidalModelInfo - centroidalModelInfo_ = centroidal_model::createCentroidalModelInfo( - *pinocchioInterfacePtr_, centroidal_model::loadCentroidalType(taskFile), - centroidal_model::loadDefaultJointState(pinocchioInterfacePtr_->getModel().nq - 6, referenceFile), - modelSettings_.contactNames3DoF, - modelSettings_.contactNames6DoF); + centroidal_model_info_ = centroidal_model::createCentroidalModelInfo( + *pinocchio_interface_ptr_, centroidal_model::loadCentroidalType(task_file), + centroidal_model::loadDefaultJointState(pinocchio_interface_ptr_->getModel().nq - 6, reference_file), + model_settings_.contactNames3DoF, + model_settings_.contactNames6DoF); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + void LeggedInterface::setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile, bool verbose) { auto swingTrajectoryPlanner = std::make_unique( loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); - referenceManagerPtr_ = + reference_manager_ptr_ = std::make_shared(loadGaitSchedule(referenceFile, verbose), std::move(swingTrajectoryPlanner)); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + void LeggedInterface::setupPreComputation(const std::string &taskFile, const std::string &urdfFile, const std::string &referenceFile, bool verbose) { - problemPtr_->preComputationPtr = std::make_unique( - *pinocchioInterfacePtr_, centroidalModelInfo_, *referenceManagerPtr_->getSwingTrajectoryPlanner(), - modelSettings_); + problem_ptr_->preComputationPtr = std::make_unique( + *pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(), + model_settings_); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::shared_ptr LeggedInterface::loadGaitSchedule(const std::string &file, bool verbose) const { const auto initModeSchedule = loadModeSchedule(file, "initialModeSchedule", false); const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(file, "defaultModeSequenceTemplate", false); @@ -221,26 +216,24 @@ namespace ocs2::legged_robot { } return std::make_shared(initModeSchedule, defaultModeSequenceTemplate, - modelSettings_.phaseTransitionStanceTime); + model_settings_.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); + const auto &model = pinocchio_interface_ptr_->getModel(); + auto &data = pinocchio_interface_ptr_->getData(); + const auto q = centroidal_model::getGeneralizedCoordinates(initial_state_, centroidal_model_info_); + computeJointJacobians(model, data, q); + 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); + getFrameJacobian(model, data, model.getBodyId(model_settings_.contactNames3DoF[i]), + pinocchio::LOCAL_WORLD_ALIGNED, jac); base2feetJac.block(3 * i, 0, 3, info.actuatedDofNum) = jac.block(0, 6, 3, info.actuatedDofNum); } @@ -255,9 +248,7 @@ namespace ocs2::legged_robot { return r; } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getBaseTrackingCost( const std::string &taskFile, const CentroidalModelInfo &info, bool verbose) { @@ -274,16 +265,14 @@ namespace ocs2::legged_robot { } return std::make_unique(std::move(Q), std::move(R), info, - *referenceManagerPtr_); + *reference_manager_ptr_); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::pair LeggedInterface::loadFrictionConeSettings( const std::string &taskFile, bool verbose) { boost::property_tree::ptree pt; - boost::property_tree::read_info(taskFile, pt); + read_info(taskFile, pt); const std::string prefix = "frictionConeSoftConstraint."; scalar_t frictionCoefficient = 1.0; @@ -302,19 +291,16 @@ namespace ocs2::legged_robot { return {frictionCoefficient, barrierPenaltyConfig}; } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getFrictionConeConstraint( size_t contactPointIndex, scalar_t frictionCoefficient) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); - return std::make_unique(*referenceManagerPtr_, frictionConeConConfig, contactPointIndex, - centroidalModelInfo_); + return std::make_unique(*reference_manager_ptr_, frictionConeConConfig, + contactPointIndex, + centroidal_model_info_); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getFrictionConeSoftConstraint( size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config &barrierPenaltyConfig) { @@ -323,39 +309,35 @@ namespace ocs2::legged_robot { std::make_unique(barrierPenaltyConfig)); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ - std::unique_ptr > LeggedInterface::getEeKinematicsPtr( - const std::vector &footNames, - const std::string &modelName) { - std::unique_ptr > eeKinematicsPtr; - const auto infoCppAd = centroidalModelInfo_.toCppAd(); + std::unique_ptr > LeggedInterface::getEeKinematicsPtr( + const std::vector &foot_names, + const std::string &model_name) { + const auto infoCppAd = centroidal_model_info_.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)); + std::unique_ptr > end_effector_kinematics = std::make_unique< + PinocchioEndEffectorKinematicsCppAd>( + *pinocchio_interface_ptr_, pinocchioMappingCppAd, + foot_names, + centroidal_model_info_.stateDim, + centroidal_model_info_.inputDim, + velocityUpdateCallback, model_name, + model_settings_.modelFolderCppAd, + model_settings_.recompileLibrariesCppAd, + model_settings_.verboseCppAd); - return eeKinematicsPtr; + return end_effector_kinematics; } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getZeroVelocityConstraint( - const EndEffectorKinematics &eeKinematics, - size_t contactPointIndex) { + const EndEffectorKinematics &end_effector_kinematics, + const size_t contact_point_index) { auto eeZeroVelConConfig = [](scalar_t positionErrorGain) { EndEffectorLinearConstraint::Config config; config.b.setZero(3); @@ -366,14 +348,12 @@ namespace ocs2::legged_robot { } return config; }; - return std::unique_ptr(new ZeroVelocityConstraintCppAd( - *referenceManagerPtr_, eeKinematics, contactPointIndex, - eeZeroVelConConfig(modelSettings_.positionErrorGain))); + return std::make_unique( + *reference_manager_ptr_, end_effector_kinematics, contact_point_index, + eeZeroVelConConfig(model_settings_.positionErrorGain)); } - /******************************************************************************************************/ - /******************************************************************************************************/ - /******************************************************************************************************/ + std::unique_ptr LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, const std::string &taskFile, const std::string &prefix, @@ -385,7 +365,7 @@ namespace ocs2::legged_robot { scalar_t minimumDistance = 0.0; boost::property_tree::ptree pt; - boost::property_tree::read_info(taskFile, pt); + read_info(taskFile, pt); if (verbose) { std::cerr << "\n #### SelfCollision Settings: "; std::cerr << "\n #### =============================================================================\n"; @@ -396,16 +376,16 @@ namespace ocs2::legged_robot { loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose); loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose); - geometryInterfacePtr_ = std::make_unique( + geometry_interface_ptr_ = std::make_unique( pinocchioInterface, collisionLinkPairs, collisionObjectPairs); if (verbose) { std::cerr << " #### =============================================================================\n"; - const size_t numCollisionPairs = geometryInterfacePtr_->getNumCollisionPairs(); + const size_t numCollisionPairs = geometry_interface_ptr_->getNumCollisionPairs(); std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n"; } std::unique_ptr constraint = std::make_unique( - CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance); + CentroidalModelPinocchioMapping(centroidal_model_info_), *geometry_interface_ptr_, minimumDistance); auto penalty = std::make_unique(RelaxedBarrierPenalty::Config{mu, delta}); diff --git a/descriptions/go1_description/config/robot_control.yaml b/descriptions/go1_description/config/robot_control.yaml index 76fe2a1..c42be8f 100644 --- a/descriptions/go1_description/config/robot_control.yaml +++ b/descriptions/go1_description/config/robot_control.yaml @@ -13,12 +13,65 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController + ocs2_quadruped_controller: + type: ocs2_quadruped_controller/Ocs2QuadrupedController + imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" frame_id: "imu_link" unitree_guide_controller: + ros__parameters: + update_rate: 500 # Hz + joints: + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - FR_foot + - FL_foot + - RR_foot + - RL_foot + + imu_name: "imu_sensor" + base_name: "base" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z + +ocs2_quadruped_controller: ros__parameters: update_rate: 500 # Hz joints: diff --git a/descriptions/go2_description/CMakeLists.txt b/descriptions/go2_description/CMakeLists.txt index 07e0194..2c29009 100644 --- a/descriptions/go2_description/CMakeLists.txt +++ b/descriptions/go2_description/CMakeLists.txt @@ -4,7 +4,7 @@ project(go2_description) find_package(ament_cmake REQUIRED) install( - DIRECTORY meshes xacro launch config + DIRECTORY meshes xacro launch config urdf DESTINATION share/${PROJECT_NAME}/ ) diff --git a/descriptions/go2_description/README.md b/descriptions/go2_description/README.md index bd35bff..f14ce12 100644 --- a/descriptions/go2_description/README.md +++ b/descriptions/go2_description/README.md @@ -20,11 +20,17 @@ source ~/ros2_ws/install/setup.bash ros2 launch go2_description visualize.launch.py ``` -## Launch Hardware Interface -```bash -source ~/ros2_ws/install/setup.bash -ros2 launch go2_description hardware.launch.py -``` +## Launch ROS2 Control +* Unitree Guide Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch go2_description hardware.launch.py + ``` +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch go2_description ocs2_control.launch.py + ``` ## When used for isaac gym or other similiar engine diff --git a/descriptions/go2_description/config/ocs2/reference.info b/descriptions/go2_description/config/ocs2/reference.info new file mode 100644 index 0000000..b36ce8e --- /dev/null +++ b/descriptions/go2_description/config/ocs2/reference.info @@ -0,0 +1,46 @@ +targetDisplacementVelocity 0.5; +targetRotationVelocity 1.57; + +comHeight 0.3 + +defaultJointState +{ + (0,0) -0.20 ; FL_hip_joint + (1,0) 0.72 ; FL_thigh_joint + (2,0) -1.44 ; FL_calf_joint + (3,0) -0.20 ; RL_hip_joint + (4,0) 0.72 ; RL_thigh_joint + (5,0) -1.44 ; RL_calf_joint + (6,0) 0.20 ; FR_hip_joint + (7,0) 0.72 ; FR_thigh_joint + (8,0) -1.44 ; FR_calf_joint + (9,0) 0.20 ; RR_hip_joint + (10,0) 0.72 ; RR_thigh_joint + (11,0) -1.44 ; RR_calf_joint +} + +initialModeSchedule +{ + modeSequence + { + [0] STANCE + [1] STANCE + } + eventTimes + { + [0] 0.5 + } +} + +defaultModeSequenceTemplate +{ + modeSequence + { + [0] STANCE + } + switchingTimes + { + [0] 0.0 + [1] 1.0 + } +} diff --git a/descriptions/go2_description/config/ocs2/task.info b/descriptions/go2_description/config/ocs2/task.info new file mode 100644 index 0000000..f30e3f0 --- /dev/null +++ b/descriptions/go2_description/config/ocs2/task.info @@ -0,0 +1,319 @@ +centroidalModelType 0 // 0: FullCentroidalDynamics, 1: Single Rigid Body Dynamics + +legged_robot_interface +{ + verbose false // show the loaded parameters +} + +model_settings +{ + positionErrorGain 0.0 + phaseTransitionStanceTime 0.1 + + verboseCppAd true + recompileLibrariesCppAd false + modelFolderCppAd /tmp/ocs2_quadruped_controller/go2 +} + +swing_trajectory_config +{ + liftOffVelocity 0.05 + touchDownVelocity -0.1 + swingHeight 0.08 + swingTimeScale 0.15 +} + +; DDP settings +ddp +{ + algorithm SLQ + + nThreads 3 + threadPriority 50 + + maxNumIterations 1 + minRelCost 1e-1 + constraintTolerance 5e-3 + + displayInfo false + displayShortSummary false + checkNumericalStability false + debugPrintRollout false + debugCaching false + + AbsTolODE 1e-5 + RelTolODE 1e-3 + maxNumStepsPerSecond 10000 + timeStep 0.015 + backwardPassIntegratorType ODE45 + + constraintPenaltyInitialValue 20.0 + constraintPenaltyIncreaseRate 2.0 + + preComputeRiccatiTerms true + + useFeedbackPolicy false + + strategy LINE_SEARCH + lineSearch + { + minStepLength 1e-2 + maxStepLength 1.0 + hessianCorrectionStrategy DIAGONAL_SHIFT + hessianCorrectionMultiple 1e-5 + } +} + + +; Multiple_Shooting SQP settings +sqp +{ + nThreads 3 + dt 0.015 + sqpIteration 1 + deltaTol 1e-4 + g_max 1e-2 + g_min 1e-6 + inequalityConstraintMu 0.1 + inequalityConstraintDelta 5.0 + projectStateInputEqualityConstraints true + printSolverStatistics true + printSolverStatus false + printLinesearch false + useFeedbackPolicy false + integratorType RK2 + threadPriority 50 +} + +; Multiple_Shooting IPM settings +ipm +{ + nThreads 3 + dt 0.015 + ipmIteration 1 + deltaTol 1e-4 + g_max 10.0 + g_min 1e-6 + computeLagrangeMultipliers true + printSolverStatistics true + printSolverStatus false + printLinesearch false + useFeedbackPolicy false + integratorType RK2 + threadPriority 50 + + initialBarrierParameter 1e-4 + targetBarrierParameter 1e-4 + barrierLinearDecreaseFactor 0.2 + barrierSuperlinearDecreasePower 1.5 + barrierReductionCostTol 1e-3 + barrierReductionConstraintTol 1e-3 + + fractionToBoundaryMargin 0.995 + usePrimalStepSizeForDual false + + initialSlackLowerBound 1e-4 + initialDualLowerBound 1e-4 + initialSlackMarginRate 1e-2 + initialDualMarginRate 1e-2 +} + +; Rollout settings +rollout +{ + AbsTolODE 1e-5 + RelTolODE 1e-3 + timeStep 0.015 + integratorType ODE45 + maxNumStepsPerSecond 10000 + checkNumericalStability false +} + +mpc +{ + timeHorizon 1.0 ; [s] + solutionTimeWindow -1 ; maximum [s] + coldStart false + + debugPrint false + + mpcDesiredFrequency 100 ; [Hz] + mrtDesiredFrequency 1000 ; [Hz] Useless +} + +initialState +{ + ;; Normalized Centroidal Momentum: [linear, angular] ;; + (0,0) 0.0 ; vcom_x + (1,0) 0.0 ; vcom_y + (2,0) 0.0 ; vcom_z + (3,0) 0.0 ; L_x / robotMass + (4,0) 0.0 ; L_y / robotMass + (5,0) 0.0 ; L_z / robotMass + + ;; Base Pose: [position, orientation] ;; + (6,0) 0.0 ; p_base_x + (7,0) 0.0 ; p_base_y + (8,0) 0.3 ; p_base_z + (9,0) 0.0 ; theta_base_z + (10,0) 0.0 ; theta_base_y + (11,0) 0.0 ; theta_base_x + + ;; Leg Joint Positions: [FL, RL, FR, RR] ;; + (12,0) -0.20 ; FL_hip_joint + (13,0) 0.72 ; FL_thigh_joint + (14,0) -1.44 ; FL_calf_joint + (15,0) -0.20 ; RL_hip_joint + (16,0) 0.72 ; RL_thigh_joint + (17,0) -1.44 ; RL_calf_joint + (18,0) 0.20 ; FR_hip_joint + (19,0) 0.72 ; FR_thigh_joint + (20,0) -1.44 ; FR_calf_joint + (21,0) 0.20 ; RR_hip_joint + (22,0) 0.72 ; RR_thigh_joint + (23,0) -1.44 ; RR_calf_joint +} + +; standard state weight matrix +Q +{ + scaling 1e+0 + + ;; Normalized Centroidal Momentum: [linear, angular] ;; + (0,0) 15.0 ; vcom_x + (1,1) 15.0 ; vcom_y + (2,2) 100.0 ; vcom_z + (3,3) 10.0 ; L_x / robotMass + (4,4) 30.0 ; L_y / robotMass + (5,5) 30.0 ; L_z / robotMass + + ;; Base Pose: [position, orientation] ;; + (6,6) 1000.0 ; p_base_x + (7,7) 1000.0 ; p_base_y + (8,8) 1500.0 ; p_base_z + (9,9) 100.0 ; theta_base_z + (10,10) 300.0 ; theta_base_y + (11,11) 300.0 ; theta_base_x + + ;; Leg Joint Positions: [FL, RL, FR, RR] ;; + (12,12) 5.0 ; FL_hip_joint + (13,13) 5.0 ; FL_thigh_joint + (14,14) 2.5 ; FL_calf_joint + (15,15) 5.0 ; RL_hip_joint + (16,16) 5.0 ; RL_thigh_joint + (17,17) 2.5 ; RL_calf_joint + (18,18) 5.0 ; FR_hip_joint + (19,19) 5.0 ; FR_thigh_joint + (20,20) 2.5 ; FR_calf_joint + (21,21) 5.0 ; RR_hip_joint + (22,22) 5.0 ; RR_thigh_joint + (23,23) 2.5 ; RR_calf_joint +} + +; control weight matrix +R +{ + scaling 1e-3 + + ;; Feet Contact Forces: [FL, FR, RL, RR] ;; + (0,0) 1.0 ; front_left_force + (1,1) 1.0 ; front_left_force + (2,2) 1.0 ; front_left_force + (3,3) 1.0 ; front_right_force + (4,4) 1.0 ; front_right_force + (5,5) 1.0 ; front_right_force + (6,6) 1.0 ; rear_left_force + (7,7) 1.0 ; rear_left_force + (8,8) 1.0 ; rear_left_force + (9,9) 1.0 ; rear_right_force + (10,10) 1.0 ; rear_right_force + (11,11) 1.0 ; rear_right_force + + ;; foot velocity relative to base: [FL, RL, FR, RR] (uses the Jacobian at nominal configuration) ;; + (12,12) 5000.0 ; x + (13,13) 5000.0 ; y + (14,14) 5000.0 ; z + (15,15) 5000.0 ; x + (16,16) 5000.0 ; y + (17,17) 5000.0 ; z + (18,18) 5000.0 ; x + (19,19) 5000.0 ; y + (20,20) 5000.0 ; z + (21,21) 5000.0 ; x + (22,22) 5000.0 ; y + (23,23) 5000.0 ; z +} + +frictionConeSoftConstraint +{ + frictionCoefficient 0.3 + + ; relaxed log barrier parameters + mu 0.1 + delta 5.0 +} + +selfCollision +{ + ; Self Collision raw object pairs + collisionObjectPairs + { + } + + ; Self Collision pairs + collisionLinkPairs + { + [0] "FL_calf, FR_calf" + [1] "RL_calf, RR_calf" + [2] "FL_calf, RL_calf" + [3] "FR_calf, RR_calf" + [4] "FL_foot, FR_foot" + [5] "RL_foot, RR_foot" + [6] "FL_foot, RL_foot" + [7] "FR_foot, RR_foot" + } + + minimumDistance 0.05 + + ; relaxed log barrier parameters + mu 1e-2 + delta 1e-3 +} + +; Whole body control +torqueLimitsTask +{ + (0,0) 33.5 ; HAA + (1,0) 33.5 ; HFE + (2,0) 33.5 ; KFE +} + +frictionConeTask +{ + frictionCoefficient 0.3 +} + +swingLegTask +{ + kp 350 + kd 37 +} + +weight +{ + swingLeg 100 + baseAccel 1 + contactForce 0.01 +} + +; State Estimation +kalmanFilter +{ + footRadius 0.02 + imuProcessNoisePosition 0.02 + imuProcessNoiseVelocity 0.02 + footProcessNoisePosition 0.002 + footSensorNoisePosition 0.005 + footSensorNoiseVelocity 0.1 + footHeightSensorNoise 0.01 +} diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index 76fe2a1..1a2efbc 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -13,6 +13,9 @@ controller_manager: unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController + ocs2_quadruped_controller: + type: ocs2_quadruped_controller/Ocs2QuadrupedController + imu_sensor_broadcaster: ros__parameters: sensor_name: "imu_sensor" @@ -66,4 +69,67 @@ unitree_guide_controller: - angular_velocity.z - linear_acceleration.x - linear_acceleration.y - - linear_acceleration.z \ No newline at end of file + - linear_acceleration.z + +ocs2_quadruped_controller: + ros__parameters: + update_rate: 500 # Hz + + # urdf_file: "$(find legged_controllers)/config/$(arg robot_type)/robot.urdf" + # task_file: "$(find go2_description)/config/ocs2/task.info" + # reference_file: "$(find go2_description)/config/ocs2/reference.info" + + joints: + - 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 + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + + command_interfaces: + - effort + - position + - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet_names: + - FL_foot + - RL_foot + - FR_foot + - RR_foot + + + imu_name: "imu_sensor" + base_name: "base" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z + + foot_force_name: "foot_force" + foot_force_interfaces: + - FL + - RL + - FR + - RR \ No newline at end of file diff --git a/descriptions/go2_description/launch/ocs2_control.launch.py b/descriptions/go2_description/launch/ocs2_control.launch.py new file mode 100644 index 0000000..4bcf455 --- /dev/null +++ b/descriptions/go2_description/launch/ocs2_control.launch.py @@ -0,0 +1,120 @@ +import os + +import xacro +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from sympy.physics.vector.printing import params + +package_description = "go2_description" + + +def process_xacro(context): + robot_type_value = context.launch_configurations['robot_type'] + pkg_path = os.path.join(get_package_share_directory(package_description)) + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description_config = xacro.process_file(xacro_file, mappings={'robot_type': robot_type_value}) + return (robot_description_config.toxml(), robot_type_value) + + +def launch_setup(context, *args, **kwargs): + (robot_description, robot_type) = process_xacro(context) + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare(package_description), + "config", + "robot_control.yaml", + ] + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[ + { + 'publish_frequency': 20.0, + 'use_tf_static': True, + 'robot_description': robot_description, + 'ignore_timestamp': True + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers, + { + 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + 'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2', + 'task.info'), + 'reference_file': os.path.join(get_package_share_directory(package_description), 'config', + 'ocs2', 'reference.info') + }], + output="both", + ) + + joint_state_publisher = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + imu_sensor_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + unitree_guide_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] + ) + + return [ + robot_state_publisher, + controller_manager, + joint_state_publisher, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=imu_sensor_broadcaster, + on_exit=[unitree_guide_controller], + ) + ), + ] + + +def generate_launch_description(): + robot_type_arg = DeclareLaunchArgument( + 'robot_type', + default_value='go2', + description='Type of the robot' + ) + + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + + return LaunchDescription([ + robot_type_arg, + OpaqueFunction(function=launch_setup), + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + ]) diff --git a/descriptions/go2_description/urdf/robot.urdf b/descriptions/go2_description/urdf/robot.urdf new file mode 100644 index 0000000..4c34838 --- /dev/null +++ b/descriptions/go2_description/urdf/robot.urdf @@ -0,0 +1,798 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + hardware_unitree_mujoco/HardwareUnitree + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + +