robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

add external robot_description parameter for rbs_robot.launch.py

Browse source 
feat(robot): add support for handling scalar joint positions

- Updated `RobotData` and `RbsArm` to allow joint positions to be passed
as either a list or a scalar value.
- Refactored logic to dynamically handle scalar values and repeat them
across all joints.
- Added new `initial_arm_joint_positions` and
`initial_gripper_joint_positions` properties in `RbsArm`.
- Improved joint position initialization in the `Scene` class to
retrieve robot description asynchronously if not provided.
- Enhanced the launch script to configure environment variables based on
available CPU cores.
- Minor code cleanups and bug fixes, including terrain naming and
corrected joint defaultsd.

git commit --amend
This commit is contained in:
Ilya Uraev 2024-09-25 12:11:17 +03:00
parent bc8745abe5
commit e4e3e4e3af
18 changed files with 686 additions and 342 deletions
Download patch file Download diff file Expand all files Collapse all files

3
env_manager/env_manager/env_manager/models/configs/camera.py
View file

@ -1,9 +1,10 @@
from dataclasses import dataclass, field
import numpy as np
@dataclass
class CameraData:
name: str = field(default_factory=str)
name: str = field(default_factory=str)
enable: bool = field(default=True)
type: str = field(default="rgbd_camera")
relative_to: str = field(default="base_link")

1
env_manager/env_manager/env_manager/models/configs/light.py
View file

@ -1,5 +1,6 @@
from dataclasses import dataclass, field
@dataclass
class LightData:
type: str = field(default="sun")

5
env_manager/env_manager/env_manager/models/configs/robot.py
View file

@ -1,5 +1,6 @@
from dataclasses import dataclass, field
@dataclass
class RobotRandomizerData:
pose: bool = field(default=False)
@ -19,7 +20,7 @@ class RobotData:
spawn_quat_xyzw: tuple[float, float, float, float] = field(
default=(0.0, 0.0, 0.0, 1.0)
)
joint_positioins: list[float] = field(default_factory=list)
joint_positions: list[float] = field(default_factory=list)
with_gripper: bool = field(default=False)
gripper_jont_positions: list[float] = field(default_factory=list)
gripper_joint_positions: list[float] | float = field(default_factory=list)
randomizer: RobotRandomizerData = field(default_factory=RobotRandomizerData)

7
env_manager/env_manager/env_manager/models/configs/scene.py
View file

@ -1,4 +1,3 @@
from dataclasses import dataclass, field
from .objects import ObjectData
from .robot import RobotData
@ -6,6 +5,7 @@ from .terrain import TerrainData
from .light import LightData
from .camera import CameraData
@dataclass
class PluginsData:
scene_broadcaster: bool = field(default_factory=bool)
@ -13,6 +13,7 @@ class PluginsData:
fts_broadcaster: bool = field(default_factory=bool)
sensors_render_engine: str = field(default="ogre2")
@dataclass
class SceneData:
physics_rollouts_num: int = field(default=0)
@ -24,7 +25,3 @@ class SceneData:
objects: list[ObjectData] = field(default_factory=list)
camera: list[CameraData] = field(default_factory=list)
plugins: PluginsData = field(default_factory=PluginsData)

2
env_manager/env_manager/env_manager/models/configs/terrain.py
View file

@ -1,9 +1,9 @@
from dataclasses import dataclass, field
@dataclass
class TerrainData:
name: str = field(default="ground")
type: str = field(default="flat")
spawn_position: tuple[float, float, float] = field(default=(0, 0, 0))
spawn_quat_xyzw: tuple[float, float, float, float] = field(default=(0, 0, 0, 1))

50
env_manager/env_manager/env_manager/models/robots/rbs_arm_b.py
View file

@ -13,6 +13,7 @@ class RbsArm(RobotWrapper):
"""
Experimental class with `robot_builder`
"""
DEFAULT_ARM_JOINT_POSITIONS: list[float] = [
0.0,
0.5,
@ -41,12 +42,10 @@ class RbsArm(RobotWrapper):
name: str = "rbs_arm",
position: tuple[float, float, float] = (0.0, 0.0, 0.0),
orientation: tuple[float, float, float, float] = (1.0, 0, 0, 0),
initial_arm_joint_positions: list[float] = DEFAULT_ARM_JOINT_POSITIONS,
initial_gripper_joint_positions: list[float] = DEFAULT_GRIPPER_JOINT_POSITIONS,
initial_arm_joint_positions: list[float] | float = DEFAULT_ARM_JOINT_POSITIONS,
initial_gripper_joint_positions: list[float]
| float = DEFAULT_GRIPPER_JOINT_POSITIONS,
):
self.__initial_arm_joint_positions = initial_arm_joint_positions
self.__initial_gripper_joint_positions = initial_gripper_joint_positions
# Get a unique model name
model_name = get_unique_model_name(world, name)
@ -63,11 +62,19 @@ class RbsArm(RobotWrapper):
# Parse robot to get metadata
self._robot: Robot = URDF_parser.load_string(urdf_string)
node.get_logger().info("{}".format(self._robot.actuated_joint_names))
node.get_logger().info("{}".format(self._robot.get_gripper_mimic_joint_name))
node.get_logger().info("{}".format(self._robot.gripper_actuated_joint_names))
node.get_logger().info("{}".format(model.joint_names()))
self.__initial_gripper_joint_positions = (
[float(initial_gripper_joint_positions)]
* len(self._robot.gripper_actuated_joint_names)
if isinstance(initial_gripper_joint_positions, (int, float))
else initial_gripper_joint_positions
)
self.__initial_arm_joint_positions = (
[float(initial_arm_joint_positions)] * len(self._robot.actuated_joint_names)
if isinstance(initial_arm_joint_positions, (int, float))
else initial_arm_joint_positions
)
# Set initial joint configuration
self.set_initial_joint_positions(model)
@ -77,14 +84,23 @@ class RbsArm(RobotWrapper):
def robot(self) -> Robot:
return self._robot
@property
def initial_arm_joint_positions(self) -> list[float]:
return self.__initial_arm_joint_positions
@property
def initial_gripper_joint_positions(self) -> list[float]:
return self.__initial_gripper_joint_positions
def set_initial_joint_positions(self, model):
model = model.to_gazebo()
if not model.reset_joint_positions(
self.__initial_arm_joint_positions, self._robot.actuated_joint_names
):
raise RuntimeError("Failed to set initial positions of arm's joints")
if not model.reset_joint_positions(
self.__initial_gripper_joint_positions, [self._robot.get_gripper_mimic_joint_name]
):
raise RuntimeError("Failed to set initial positions of gripper's joints")
joint_position_data = [
(self.__initial_arm_joint_positions, self._robot.actuated_joint_names),
(self.__initial_gripper_joint_positions, self._robot.gripper_actuated_joint_names),
]
for positions, joint_names in joint_position_data:
print(f"Setting joint positions for {joint_names}: {positions}")
if not model.reset_joint_positions(positions, joint_names):
raise RuntimeError(f"Failed to set initial positions of {joint_names[0].split('_')[0]}'s joints")

15
env_manager/env_manager/env_manager/models/robots/robot.py
View file

@ -12,8 +12,8 @@ class RobotWrapper(model_wrapper.ModelWrapper):
name: str | None = None,
position: tuple[float, float, float] | None = None,
orientation: tuple[float, float, float, float] | None = None,
initial_arm_joint_positions: list[float] = [],
initial_gripper_joint_positions: list[float] = [],
initial_arm_joint_positions: list[float] | float = [],
initial_gripper_joint_positions: list[float] | float = [],
model: model_wrapper.ModelWrapper | None = None,
**kwargs,
):
@ -27,6 +27,17 @@ class RobotWrapper(model_wrapper.ModelWrapper):
def robot(self) -> Robot:
pass
@property
@abstractmethod
def initial_gripper_joint_positions(self) -> list[float]:
pass
@property
@abstractmethod
def initial_arm_joint_positions(self) -> list[float]:
pass
@abstractmethod
def set_initial_joint_positions(self, model):
pass

55
env_manager/env_manager/env_manager/scene/scene.py
View file

@ -1,7 +1,7 @@
from dataclasses import asdict
import numpy as np
from rclpy.node import Node
from rclpy.node import Node, Parameter
from scenario.bindings.gazebo import GazeboSimulator, World, PhysicsEngine_dart
from scipy.spatial.transform import Rotation
from scipy.spatial import distance
@ -16,6 +16,8 @@ from ..utils.gazebo import (
transform_move_to_model_position,
)
from ..utils.types import Point, Pose
from rcl_interfaces.srv import GetParameters
import asyncio
# TODO: Split scene randomizer and scene
@ -52,7 +54,15 @@ class Scene:
self.__objects_unique_names = {}
self.__object_positions: dict[str, Point] = {}
self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0)
scene.robot.urdf_string = robot_urdf_string
if not robot_urdf_string:
self.param_client = node.create_client(
GetParameters, "/robot_state_publisher/get_parameters"
)
asyncio.run(self.get_parameter_async())
scene.robot.urdf_string = node.get_parameter("robot_description").get_parameter_value().string_value
else:
scene.robot.urdf_string = robot_urdf_string
self.tf2_broadcaster = Tf2Broadcaster(node=node)
@ -74,6 +84,32 @@ class Scene:
self.__workspace_center[2] + half_depth,
)
async def get_parameter_async(self):
while not self.param_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
"Waiting for parameter service [robot_state_publisher]..."
)
self.node.get_logger().info("Sending request for parameter...")
future = self.param_client.call_async(self.create_get_parameters_request())
response = await future
match response:
case None:
raise RuntimeError("robot_description parameter not found in robot_state_publisher")
case GetParameters.Response(values=[value]) if value.string_value:
self.node.get_logger().info(f"Got parameter: {value.string_value}")
param = Parameter(
"robot_description", Parameter.Type.STRING, value.string_value
)
self.node.set_parameters([param])
case _:
raise RuntimeError(f"I don't know what but the type of robot_description parameter is {type(response)}")
def create_get_parameters_request(self) -> GetParameters.Request:
return GetParameters.Request(names=["robot_description"])
def init_scene(self):
self.gazebo_paused_run()
@ -176,8 +212,8 @@ class Scene:
position=self.robot.spawn_position,
orientation=quat_to_wxyz(self.robot.spawn_quat_xyzw),
urdf_string=self.robot.urdf_string,
initial_arm_joint_positions=self.robot.joint_positioins,
initial_gripper_joint_positions=self.robot.gripper_jont_positions,
initial_arm_joint_positions=self.robot.joint_positions,
initial_gripper_joint_positions=self.robot.gripper_joint_positions,
)
# The desired name is passed as arg on creation, however, a different name might be selected to be unique
@ -361,6 +397,7 @@ class Scene:
# TODO: add another terrains
self.terrain_wrapper = Ground(
self.world,
name=self.terrain.name,
position=self.terrain.spawn_position,
orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw),
size=self.terrain.size,
@ -390,7 +427,7 @@ class Scene:
self.gazebo_paused_run()
def randomize_camera_pose(self, camera: CameraData):
# Get random camera pose, centred at object position (or centre of object spawn box)
# Get random camera pose, centered at object position (or center of object spawn box)
if "orbit" == camera.random_pose_mode:
camera_position, camera_quat_xyzw = self.get_random_camera_pose_orbit(
camera
@ -526,7 +563,7 @@ class Scene:
def get_random_camera_pose_sample_process(
self, camera: CameraData, position: Point
) -> Pose:
# Determine orientation such that camera faces the centre
# Determine orientation such that camera faces the center
rpy = [
0.0,
np.arctan2(
@ -584,7 +621,7 @@ class Scene:
def reset_robot_joint_positions(self):
gazebo_robot = self.robot_wrapper.to_gazebo()
arm_joint_positions = self.robot.joint_positioins
arm_joint_positions = self.robot_wrapper.initial_arm_joint_positions
# Add normal noise if desired
if self.robot.randomizer.joint_positions:
@ -610,7 +647,7 @@ class Scene:
and self.robot_wrapper.robot.gripper_actuated_joint_names
):
if not gazebo_robot.reset_joint_positions(
self.robot.gripper_jont_positions,
self.robot_wrapper.initial_gripper_joint_positions,
self.robot_wrapper.robot.gripper_actuated_joint_names,
):
raise RuntimeError("Failed to reset gripper joint positions")
@ -627,7 +664,7 @@ class Scene:
# if self.robot.with_gripper:
# if (
# self.robot_wrapper.CLOSED_GRIPPER_JOINT_POSITIONS
# == self.robot.gripper_jont_positions
# == self.robot.gripper_joint_positions
# ):
# self.gripper_controller.close()
# else:

BIN
env_manager/rbs_gym/img_color.png
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 62 KiB

28
env_manager/rbs_gym/launch/test_env.launch.py
View file

@ -440,14 +440,12 @@ def generate_launch_description():
)
)
# HER specifics
(
declared_arguments.append(
DeclareLaunchArgument(
"truncate_last_trajectory",
default_value="True",
description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.",
)
),
declared_arguments.append(
DeclareLaunchArgument(
"truncate_last_trajectory",
default_value="True",
description="When using HER with online sampling the last trajectory in the replay buffer will be truncated after) reloading the replay buffer.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
@ -456,11 +454,17 @@ def generate_launch_description():
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
)
)
cpu_c = cpu_count()
if cpu_c is not None:
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_c // 2)),
]
else:
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
]
env_variables = [
SetEnvironmentVariable(name="OMP_DYNAMIC", value="TRUE"),
SetEnvironmentVariable(name="OMP_NUM_THREADS", value=str(cpu_count() // 2)),
]
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)] + env_variables

4
env_manager/rbs_gym/rbs_gym/envs/__init__.py
View file

@ -103,9 +103,9 @@ SCENE_CONFIGURATION: SceneData = SceneData(
physics_rollouts_num=0,
robot=RobotData(
name="rbs_arm",
joint_positioins=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
joint_positions=[0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0],
with_gripper=True,
gripper_jont_positions=[0],
gripper_joint_positions=0.064,
randomizer=RobotRandomizerData(joint_positions=True),
),
objects=[

51
env_manager/rbs_gym/rbs_gym/envs/tasks/manipulation.py
View file

@ -183,7 +183,6 @@ class Manipulation(Task, Node, abc.ABC):
# )
# Names of important models (in addition to robot model)
self.terrain_name = "terrain"
# Setup listener and broadcaster of transforms via tf2
self.tf2_listener = Tf2Listener(node=self)
@ -228,7 +227,8 @@ class Manipulation(Task, Node, abc.ABC):
raise NotImplementedError()
def reset_task(self):
self.__consume_parameter_overrides()
# self.__consume_parameter_overrides()
pass
# # Helper functions #
# def get_relative_ee_position(
@ -596,7 +596,8 @@ class Manipulation(Task, Node, abc.ABC):
Returns true if robot links are in collision with the ground.
"""
robot_name_len = len(self.robot.name)
terrain_model = self.world.get_model(self.terrain_name)
model_names = self.world.model_names()
terrain_model = self.world.get_model(self.scene.terrain.name)
for contact in terrain_model.contacts():
body_b = contact.body_b
@ -644,30 +645,30 @@ class Manipulation(Task, Node, abc.ABC):
# or object_position[2] > self.workspace_max_bound[2]
# )
def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self.add_task_parameter_overrides(parameter_overrides)
self.add_randomizer_parameter_overrides(parameter_overrides)
# def add_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self.add_task_parameter_overrides(parameter_overrides)
# self.add_randomizer_parameter_overrides(parameter_overrides)
# #
# def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self.__task_parameter_overrides.update(parameter_overrides)
# #
# def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
# self._randomizer_parameter_overrides.update(parameter_overrides)
#
def add_task_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self.__task_parameter_overrides.update(parameter_overrides)
# def __consume_parameter_overrides(self):
# for key, value in self.__task_parameter_overrides.items():
# if hasattr(self, key):
# setattr(self, key, value)
# elif hasattr(self, f"_{key}"):
# setattr(self, f"_{key}", value)
# elif hasattr(self, f"__{key}"):
# setattr(self, f"__{key}", value)
# else:
# self.get_logger().error(
# f"Override '{key}' is not supperted by the task."
# )
#
def add_randomizer_parameter_overrides(self, parameter_overrides: Dict[str, Any]):
self._randomizer_parameter_overrides.update(parameter_overrides)
def __consume_parameter_overrides(self):
for key, value in self.__task_parameter_overrides.items():
if hasattr(self, key):
setattr(self, key, value)
elif hasattr(self, f"_{key}"):
setattr(self, f"_{key}", value)
elif hasattr(self, f"__{key}"):
setattr(self, f"__{key}", value)
else:
self.get_logger().error(
f"Override '{key}' is not supperted by the task."
)
self.__task_parameter_overrides.clear()
# self.__task_parameter_overrides.clear()
@property
def robot(self) -> Robot:

9
env_manager/rbs_gym/rbs_gym/envs/tasks/reach/reach.py
View file

@ -53,7 +53,7 @@ class Reach(Manipulation, abc.ABC):
self._previous_distance: float = 0.0
# self._collision_reward: float = collision_reward
# self.initial_gripper_joint_positions = self.robot_data.gripper_jont_positions
# self.initial_gripper_joint_positions = self.robot_data.gripper_joint_positions
self.twist = TwistSubscriber(
self,
topic="/cartesian_force_controller/current_twist",
@ -122,10 +122,11 @@ class Reach(Manipulation, abc.ABC):
twist.twist.angular.z,
)
observation: Observation = np.concatenate([ee_position, target_position, twt], dtype=np.float32)
# Create the observation
observation = Observation(
np.concatenate([ee_position, target_position, twt], dtype=np.float32)
)
# observation = Observation(
# np.concatenate([ee_position, target_position, twt], dtype=np.float32)
# )
self.get_logger().debug(f"\nobservation: {observation}")

17
env_manager/rbs_runtime/scripts/asdict.py Normal file
View file

@ -0,0 +1,17 @@
from env_manager.models.configs import SceneData, RobotData, MeshObjectData, CameraData
from dataclasses import asdict
import json
scene: SceneData = SceneData(
robot=RobotData(
name="rbs_arm",
with_gripper=True,
joint_positions=[0, 0, 0, 0, 0, 0, 0],
gripper_joint_positions=[0],
),
objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))],
camera=[CameraData("robot_camera")],
)
with open("scene_config.json", "w") as file:
json.dump(asdict(scene), file)

4
env_manager/rbs_runtime/scripts/runtime.py
View file

@ -22,8 +22,8 @@ DEFAULT_SCENE: SceneData = SceneData(
robot=RobotData(
name="rbs_arm",
with_gripper=True,
joint_positioins=[0, 0, 0, 0, 0, 0, 0],
gripper_jont_positions=[0],
joint_positions=[0, 0, 0, 0, 0, 0, 0],
gripper_joint_positions=[0],
),
objects=[MeshObjectData("bishop", position=(0.0, 1.0, 0.3))],
camera=[CameraData("robot_camera")],

186
env_manager/scene_config.json Normal file
View file

@ -0,0 +1,186 @@
{
"physics_rollouts_num": 0,
"gravity": [
0.0,
0.0,
-9.80665
],
"gravity_std": [
0.0,
0.0,
0.0232
],
"robot": {
"name": "rbs_arm",
"urdf_string": "",
"spawn_position": [
0.0,
0.0,
0.0
],
"spawn_quat_xyzw": [
0.0,
0.0,
0.0,
1.0
],
"joint_positioins": [
0,
0,
0,
0,
0,
0,
0
],
"with_gripper": true,
"gripper_jont_positions": [
0
],
"randomizer": {
"pose": false,
"spawn_volume": [
1.0,
1.0,
0.0
],
"joint_positions": false,
"joint_positions_std": 0.1,
"joint_positions_above_object_spawn": false,
"joint_positions_above_object_spawn_elevation": 0.2,
"joint_positions_above_object_spawn_xy_randomness": 0.2
}
},
"terrain": {
"name": "ground",
"type": "flat",
"spawn_position": [
0,
0,
0
],
"spawn_quat_xyzw": [
0,
0,
0,
1
],
"size": [
1.5,
1.5
],
"model_rollouts_num": 1
},
"light": {
"type": "sun",
"direction": [
0.5,
-0.25,
-0.75
],
"random_minmax_elevation": [
-0.15,
-0.65
],
"color": [
1.0,
1.0,
1.0,
1.0
],
"distance": 1000.0,
"visual": true,
"radius": 25.0,
"model_rollouts_num": 1
},
"objects": [
{
"name": "bishop",
"type": "",
"relative_to": "world",
"position": [
0.0,
1.0,
0.3
],
"orientation": [
1.0,
0.0,
0.0,
0.0
],
"static": false,
"randomize": {
"count": 0,
"random_pose": false,
"random_position": false,
"random_orientation": false,
"random_model": false,
"random_spawn_position_segments": [],
"random_spawn_position_update_workspace_centre": false,
"random_spawn_volume": [
0.5,
0.5,
0.5
],
"models_rollouts_num": 0
},
"texture": []
}
],
"camera": [
{
"name": "robot_camera",
"enable": true,
"type": "rgbd_camera",
"relative_to": "base_link",
"width": 128,
"height": 128,
"image_format": "R8G8B8",
"update_rate": 10,
"horizontal_fov": 1.0471975511965976,
"vertical_fov": 1.0471975511965976,
"clip_color": [
0.01,
1000.0
],
"clip_depth": [
0.05,
10.0
],
"noise_mean": null,
"noise_stddev": null,
"publish_color": false,
"publish_depth": false,
"publish_points": false,
"spawn_position": [
0,
0,
1
],
"spawn_quat_xyzw": [
0,
0.70710678118,
0,
0.70710678118
],
"random_pose_rollouts_num": 1,
"random_pose_mode": "orbit",
"random_pose_orbit_distance": 1.0,
"random_pose_orbit_height_range": [
0.1,
0.7
],
"random_pose_orbit_ignore_arc_behind_robot": 0.39269908169872414,
"random_pose_select_position_options": [],
"random_pose_focal_point_z_offset": 0.0,
"random_pose_rollout_counter": 0.0
}
],
"plugins": {
"scene_broadcaster": false,
"user_commands": false,
"fts_broadcaster": false,
"sensors_render_engine": "ogre2"
}
}