load joint names and feet names from config file

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,
+        void setupJointNames(const std::vector<std::string> &joint_names,
-                                                const std::string &reference_file,
+                             const std::vector<std::string> &foot_names);
-                                                bool verbose);
+
+        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<SwitchedModelReferenceManager> 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,
+        void setupModel(const std::string &task_file, const std::string &urdf_file,
-                                const std::string &reference_file);
+                        const std::string &reference_file);
 
-        virtual void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
+        void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
-                                           const std::string &referenceFile,
+                                   const std::string &referenceFile,
-                                           bool verbose);
+                                   bool verbose);
 
-        virtual void setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
+        void setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
-                                         const std::string &referenceFile,
+                                 const std::string &referenceFile,
-                                         bool verbose);
+                                 bool verbose);
 
         std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;
 

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -131,6 +131,7 @@ namespace ocs2::legged_robot {
 
         // Hardware Parameters
         joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
+        feet_names_ = auto_declare<std::vector<std::string> >("feet", feet_names_);
         command_interface_types_ =
                 auto_declare<std::vector<std::string> >("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<long>(legged_interface_->getCentroidalModelInfo().stateDim));
+            ctrl_comp_.observation_.state.setZero(
+                static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
             updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
-            ctrl_comp_.observation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
+            ctrl_comp_.observation_.input.setZero(
+                static_cast<long>(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<LeggedInterface>(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();
     }
 }

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -73,6 +73,7 @@ namespace ocs2::legged_robot {
 
         CtrlComponent ctrl_comp_;
         std::vector<std::string> joint_names_;
+        std::vector<std::string> feet_names_;
         std::vector<std::string> command_interface_types_;
         std::vector<std::string> state_interface_types_;
 
@@ -99,6 +100,7 @@ namespace ocs2::legged_robot {
         // Foot Force Sensor
         std::string foot_force_name_;
         std::vector<std::string> foot_force_interface_types_;
+
         double default_kp_ = 0;
         double default_kd_ = 6;
 

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<std::string> &joint_names,
+                                          const std::vector<std::string> &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,

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])

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

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})
+    robot_description_config = xacro.process_file(xacro_file)
-    return (robot_description_config.toxml(), robot_type_value)
+    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 = process_xacro()
-    (robot_description, robot_type) = process_xacro(context)
     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]
-        )
     ])