Added rbs_gym package for RL & multi-robot launch setup
This commit is contained in:
parent
f92670cd0d
commit
b58307dea1
103 changed files with 15170 additions and 653 deletions
|
|
@ -4,7 +4,7 @@ scene_config:
|
|||
pose:
|
||||
position:
|
||||
x: -0.45
|
||||
y: -2.0
|
||||
y: 0.0
|
||||
z: 1.6
|
||||
orientation:
|
||||
x: 3.14159
|
||||
|
|
@ -15,7 +15,7 @@ scene_config:
|
|||
pose:
|
||||
position:
|
||||
x: 0.45
|
||||
y: -2.0
|
||||
y: 0.0
|
||||
z: 1.6
|
||||
orientation:
|
||||
x: 3.14159
|
||||
|
|
|
|||
|
|
@ -285,7 +285,7 @@ def generate_launch_description():
|
|||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("robots_config_file",
|
||||
default_value="robot_scene",
|
||||
default_value="roboclone.yaml",
|
||||
description="Filename for config file with robots in scene")
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -14,12 +14,14 @@ from launch_ros.substitutions import FindPackageShare
|
|||
import xacro
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from rbs_launch_utils.launch_common import load_yaml
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Initialize Arguments
|
||||
robot_type = LaunchConfiguration("robot_type")
|
||||
# General arguments
|
||||
launch_rviz = LaunchConfiguration("launch_rviz")
|
||||
with_gripper_condition = LaunchConfiguration("with_gripper")
|
||||
controllers_file = LaunchConfiguration("controllers_file")
|
||||
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
|
||||
|
|
@ -44,14 +46,20 @@ def launch_setup(context, *args, **kwargs):
|
|||
pitch = LaunchConfiguration("pitch")
|
||||
yaw = LaunchConfiguration("yaw")
|
||||
namespace = LaunchConfiguration("namespace")
|
||||
multi_robot = LaunchConfiguration("multi_robot")
|
||||
robot_name = robot_name.perform(context)
|
||||
namespace = namespace.perform(context)
|
||||
robot_type = robot_type.perform(context)
|
||||
description_package = description_package.perform(context)
|
||||
description_file = description_file.perform(context)
|
||||
controllers_file = controllers_file.perform(context)
|
||||
multi_robot = multi_robot.perform(context)
|
||||
|
||||
# remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
|
||||
remappings = []
|
||||
if multi_robot == "true":
|
||||
remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
|
||||
|
||||
controllers_file = os.path.join(get_package_share_directory(description_package), "config", controllers_file)
|
||||
|
||||
xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file)
|
||||
robot_description_doc = xacro.process_file(
|
||||
|
|
@ -67,7 +75,6 @@ def launch_setup(context, *args, **kwargs):
|
|||
"roll": roll.perform(context),
|
||||
"pitch": pitch.perform(context),
|
||||
"yaw": yaw.perform(context)
|
||||
#TODO: add rotation and add probably via dict
|
||||
}
|
||||
)
|
||||
|
||||
|
|
@ -83,7 +90,7 @@ def launch_setup(context, *args, **kwargs):
|
|||
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
|
||||
),
|
||||
" ",
|
||||
"name:=",robot_name," ",
|
||||
"name:=",robot_type," ",
|
||||
"with_gripper:=",with_gripper_condition, " ",
|
||||
"gripper_name:=", gripper_name, " ",
|
||||
]
|
||||
|
|
@ -93,12 +100,19 @@ def launch_setup(context, *args, **kwargs):
|
|||
robot_description_kinematics = PathJoinSubstitution(
|
||||
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
|
||||
)
|
||||
|
||||
# kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
|
||||
|
||||
robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics}
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
|
||||
)
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
namespace=namespace,
|
||||
output="both",
|
||||
# remappings=remappings,
|
||||
remappings=remappings,
|
||||
parameters=[{"use_sim_time": use_sim_time}, robot_description],
|
||||
)
|
||||
|
||||
|
|
@ -121,6 +135,19 @@ def launch_setup(context, *args, **kwargs):
|
|||
]
|
||||
)
|
||||
|
||||
rviz = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_kinematics
|
||||
],
|
||||
condition=IfCondition(launch_rviz))
|
||||
|
||||
control = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
|
|
@ -209,9 +236,9 @@ def launch_setup(context, *args, **kwargs):
|
|||
control,
|
||||
moveit,
|
||||
skills,
|
||||
task_planner,
|
||||
perception,
|
||||
# rviz
|
||||
# task_planner,
|
||||
# perception,
|
||||
rviz
|
||||
]
|
||||
return nodes_to_start
|
||||
|
||||
|
|
@ -230,7 +257,7 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"controllers_file",
|
||||
default_value="rbs_arm_controllers_gazebosim.yaml",
|
||||
default_value="rbs_arm0_controllers.yaml",
|
||||
description="YAML file with the controllers configuration.",
|
||||
)
|
||||
)
|
||||
|
|
@ -310,7 +337,7 @@ def generate_launch_description():
|
|||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_rviz",
|
||||
default_value="true",
|
||||
default_value="false",
|
||||
description="Launch RViz?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
|
|
@ -401,6 +428,29 @@ def generate_launch_description():
|
|||
default_value="0.0",
|
||||
description="Position of robot in world by Z")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("roll",
|
||||
default_value="0.0",
|
||||
description="Position of robot in world by Z")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("pitch",
|
||||
default_value="0.0",
|
||||
description="Position of robot in world by Z")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("yaw",
|
||||
default_value="0.0",
|
||||
description="Position of robot in world by Z")
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"multi_robot",
|
||||
default_value="false",
|
||||
description="Flag if you use multi robot setup"
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
|
||||
|
|
|
|||
|
|
@ -76,6 +76,9 @@ def launch_setup(context, *args, **kwargs):
|
|||
)
|
||||
|
||||
# controller_paramfile = configured_params.perform(context)
|
||||
# controller_paramfile = PathJoinSubstitution([
|
||||
# FindPackageShare(robot_type), "config", "rbs_arm0_controllers.yaml"
|
||||
# ])
|
||||
# namespace = "/" + robot_name.perform(context)
|
||||
namespace = ""
|
||||
|
||||
|
|
@ -102,7 +105,7 @@ def launch_setup(context, *args, **kwargs):
|
|||
"gripper_name": gripper_name,
|
||||
"controllers_file": controllers_file,
|
||||
"robot_type": robot_type,
|
||||
"controllers_file": initial_joint_controllers_file_path,
|
||||
# "controllers_file": controller_paramfile,
|
||||
"cartesian_controllers": cartesian_controllers,
|
||||
"description_package": description_package,
|
||||
"description_file": description_file,
|
||||
|
|
@ -148,7 +151,7 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"controllers_file",
|
||||
default_value="rbs_arm_controllers_gazebosim.yaml",
|
||||
default_value="rbs_arm0_controllers.yaml",
|
||||
description="YAML file with the controllers configuration.",
|
||||
)
|
||||
)
|
||||
|
|
@ -221,7 +224,7 @@ def generate_launch_description():
|
|||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("with_gripper",
|
||||
default_value="true",
|
||||
default_value="false",
|
||||
description="With gripper or not?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ import math
|
|||
import os
|
||||
import sys
|
||||
import yaml
|
||||
from typing import Dict
|
||||
from typing import Any, Dict
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
|
@ -24,7 +24,7 @@ def construct_angle_degrees(loader, node) -> float:
|
|||
"""Utility function for converting degrees into radians from yaml."""
|
||||
return math.radians(construct_angle_radians(loader, node))
|
||||
|
||||
def load_yaml(package_name: str, file_path: str) -> Dict:
|
||||
def load_yaml(package_name: str, file_path: str) -> dict[str, Any]:
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue