From 979e3228b3438b08214e7d7260b9eb9cff734945 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Wed, 2 Oct 2024 13:45:21 +0800 Subject: [PATCH] load joint names and feet names from config file --- .../interface/LeggedInterface.h | 36 +++++++------ .../src/Ocs2QuadrupedController.cpp | 13 +++-- .../src/Ocs2QuadrupedController.h | 2 + .../src/interface/LeggedInterface.cpp | 15 +++--- .../a1_description/config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ .../config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ .../b2_description/config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ .../go1_description/config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ .../go2_description/config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ .../config/robot_control.yaml | 2 +- .../launch/ocs2_control.launch.py | 53 +++++++------------ 16 files changed, 162 insertions(+), 234 deletions(-) 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 7b704ae..12194f6 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 @@ -25,31 +25,33 @@ #include "SwitchedModelReferenceManager.h" namespace ocs2::legged_robot { - class LeggedInterface : public RobotInterface { + class LeggedInterface final : public RobotInterface { public: - LeggedInterface(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file, + 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 &task_file, const std::string &urdf_file, - const std::string &reference_file, - bool verbose); + void setupJointNames(const std::vector &joint_names, + const std::vector &foot_names); + + 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 *problem_ptr_; } 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 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 reference_manager_ptr_; @@ -62,16 +64,16 @@ namespace ocs2::legged_robot { } protected: - virtual void setupModel(const std::string &task_file, const std::string &urdf_file, - const std::string &reference_file); + 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, - bool verbose); + void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose); - virtual void setupPreComputation(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, - bool verbose); + void setupPreComputation(const std::string &taskFile, const std::string &urdfFile, + const std::string &referenceFile, + bool verbose); std::shared_ptr loadGaitSchedule(const std::string &file, bool verbose) const; diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 5520378..4d802ea 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -131,6 +131,7 @@ namespace ocs2::legged_robot { // Hardware Parameters joint_names_ = auto_declare >("joints", joint_names_); + feet_names_ = auto_declare >("feet", feet_names_); command_interface_types_ = auto_declare >("command_interfaces", command_interface_types_); state_interface_types_ = @@ -226,12 +227,15 @@ namespace ocs2::legged_robot { if (mpc_running_ == false) { // Initial state - ctrl_comp_.observation_.state.setZero(static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); + ctrl_comp_.observation_.state.setZero( + static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000)); - ctrl_comp_.observation_.input.setZero(static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); + ctrl_comp_.observation_.input.setZero( + static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); ctrl_comp_.observation_.mode = STANCE; - const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time}, {ctrl_comp_.observation_.state}, + const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time}, + {ctrl_comp_.observation_.state}, {ctrl_comp_.observation_.input}); // Set the first observation and command and wait for optimization to finish @@ -273,6 +277,7 @@ namespace ocs2::legged_robot { void Ocs2QuadrupedController::setupLeggedInterface() { legged_interface_ = std::make_shared(task_file_, urdf_file_, reference_file_); + legged_interface_->setupJointNames(joint_names_, feet_names_); legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); } @@ -338,7 +343,7 @@ namespace ocs2::legged_robot { const scalar_t yaw_last = ctrl_comp_.observation_.state(9); ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_); ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance( - yaw_last, ctrl_comp_.observation_.state(9)); + yaw_last, ctrl_comp_.observation_.state(9)); ctrl_comp_.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 117d771..a501ca6 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -73,6 +73,7 @@ namespace ocs2::legged_robot { CtrlComponent ctrl_comp_; std::vector joint_names_; + std::vector feet_names_; std::vector command_interface_types_; std::vector state_interface_types_; @@ -99,6 +100,7 @@ namespace ocs2::legged_robot { // Foot Force Sensor std::string foot_force_name_; std::vector foot_force_interface_types_; + double default_kp_ = 0; double default_kd_ = 6; diff --git a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp index 32fd167..d95383a 100644 --- a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp +++ b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp @@ -67,15 +67,6 @@ namespace ocs2::legged_robot { // load setting from loading file model_settings_ = loadModelSettings(task_file, "model_settings", verbose); - // Todo : load settings from ros param - model_settings_.jointNames = { - "FL_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); @@ -84,6 +75,12 @@ namespace ocs2::legged_robot { } + void LeggedInterface::setupJointNames(const std::vector &joint_names, + const std::vector &foot_names) { + model_settings_.jointNames = joint_names; + model_settings_.contactNames3DoF = foot_names; + } + void LeggedInterface::setupOptimalControlProblem(const std::string &task_file, const std::string &urdf_file, const std::string &reference_file, diff --git a/descriptions/unitree/a1_description/config/robot_control.yaml b/descriptions/unitree/a1_description/config/robot_control.yaml index ae2cfb6..0ebfd6b 100644 --- a/descriptions/unitree/a1_description/config/robot_control.yaml +++ b/descriptions/unitree/a1_description/config/robot_control.yaml @@ -101,7 +101,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/unitree/a1_description/launch/ocs2_control.launch.py b/descriptions/unitree/a1_description/launch/ocs2_control.launch.py index a4a2eae..dde8f81 100644 --- a/descriptions/unitree/a1_description/launch/ocs2_control.launch.py +++ b/descriptions/unitree/a1_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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 = "a1_description" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='a1', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.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/unitree/aliengo_description/config/robot_control.yaml b/descriptions/unitree/aliengo_description/config/robot_control.yaml index a749fb3..178e124 100644 --- a/descriptions/unitree/aliengo_description/config/robot_control.yaml +++ b/descriptions/unitree/aliengo_description/config/robot_control.yaml @@ -102,7 +102,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py b/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py index 95ffd3e..88ab2ea 100644 --- a/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py +++ b/descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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 = "aliengo_description" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='aliengo', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.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/unitree/b2_description/config/robot_control.yaml b/descriptions/unitree/b2_description/config/robot_control.yaml index 62dc870..f825fbd 100644 --- a/descriptions/unitree/b2_description/config/robot_control.yaml +++ b/descriptions/unitree/b2_description/config/robot_control.yaml @@ -103,7 +103,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/unitree/b2_description/launch/ocs2_control.launch.py b/descriptions/unitree/b2_description/launch/ocs2_control.launch.py index 3a70d43..e151a14 100644 --- a/descriptions/unitree/b2_description/launch/ocs2_control.launch.py +++ b/descriptions/unitree/b2_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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 = "b2_description" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='b2', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.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/unitree/go1_description/config/robot_control.yaml b/descriptions/unitree/go1_description/config/robot_control.yaml index ae2cfb6..0ebfd6b 100644 --- a/descriptions/unitree/go1_description/config/robot_control.yaml +++ b/descriptions/unitree/go1_description/config/robot_control.yaml @@ -101,7 +101,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/unitree/go1_description/launch/ocs2_control.launch.py b/descriptions/unitree/go1_description/launch/ocs2_control.launch.py index bcbd6c8..8ecf4f1 100644 --- a/descriptions/unitree/go1_description/launch/ocs2_control.launch.py +++ b/descriptions/unitree/go1_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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 = "go1_description" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='go1', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.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/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 8ab4aa7..4d51433 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -101,7 +101,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/unitree/go2_description/launch/ocs2_control.launch.py b/descriptions/unitree/go2_description/launch/ocs2_control.launch.py index 10b7197..dea1799 100644 --- a/descriptions/unitree/go2_description/launch/ocs2_control.launch.py +++ b/descriptions/unitree/go2_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_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_controller), "config", "visualize_ocs2.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/xiaomi/cyberdog_description/config/robot_control.yaml b/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml index ab6782a..ac7029d 100644 --- a/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml +++ b/descriptions/xiaomi/cyberdog_description/config/robot_control.yaml @@ -129,7 +129,7 @@ ocs2_quadruped_controller: - position - velocity - feet_names: + feet: - FL_foot - FR_foot - RL_foot diff --git a/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py b/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py index b35e736..9fed3d6 100644 --- a/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py +++ b/descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py @@ -3,26 +3,26 @@ 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.actions import OpaqueFunction, 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 = "cyberdog_description" package_controller = "ocs2_quadruped_controller" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] + +def process_xacro(): 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) + robot_description_config = xacro.process_file(xacro_file) + return robot_description_config.toxml() +def generate_launch_description(): + rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.rviz") -def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + robot_description = process_xacro() robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -50,13 +50,14 @@ def launch_setup(context, *args, **kwargs): executable="ros2_control_node", parameters=[robot_controllers, { - 'urdf_file': os.path.join(get_package_share_directory(package_description), 'urdf', 'robot.urdf'), + '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'), 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'gait.info') + 'ocs2', 'gait.info') }], output="both", ) @@ -81,7 +82,15 @@ def launch_setup(context, *args, **kwargs): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) - return [ + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ), + robot_state_publisher, controller_manager, joint_state_publisher, @@ -97,26 +106,4 @@ def launch_setup(context, *args, **kwargs): on_exit=[ocs2_controller], ) ), - ] - - -def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='go1', - description='Type of the robot' - ) - - rviz_config_file = os.path.join(get_package_share_directory(package_controller), "config", "visualize_ocs2.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] - ) ])