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

load joint names and feet names from config file

This commit is contained in:
Huang Zhenbiao 2024-10-02 13:45:21 +08:00
parent 86bbcf75e6
commit 979e3228b3
16 changed files with 162 additions and 234 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h
View File

@ -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<std::string> &joint_names,
const std::vector<std::string> &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<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,
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<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const;

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

@ -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();
}
}

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

@ -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;

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

@ -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,

2
descriptions/unitree/a1_description/config/robot_control.yaml
View File

@ -101,7 +101,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/unitree/a1_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])

2
descriptions/unitree/aliengo_description/config/robot_control.yaml
View File

@ -102,7 +102,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/unitree/aliengo_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])

2
descriptions/unitree/b2_description/config/robot_control.yaml
View File

@ -103,7 +103,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/unitree/b2_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])

2
descriptions/unitree/go1_description/config/robot_control.yaml
View File

@ -101,7 +101,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/unitree/go1_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])

2
descriptions/unitree/go2_description/config/robot_control.yaml
View File

@ -101,7 +101,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/unitree/go2_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])

2
descriptions/xiaomi/cyberdog_description/config/robot_control.yaml
View File

@ -129,7 +129,7 @@ ocs2_quadruped_controller:
- position
- velocity
feet_names:
feet:
- FL_foot
- FR_foot
- RL_foot

53
descriptions/xiaomi/cyberdog_description/launch/ocs2_control.launch.py
View File

@ -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]
)
])