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

Merge branch 'main' into 109-nix-jazzy

Browse source
This commit is contained in:
Игорь Брылёв 2024-12-01 16:05:45 +03:00
commit 2d361242a4
19 changed files with 905 additions and 309 deletions
Download patch file Download diff file Expand all files Collapse all files

7
doc/ru/installation.md
View file

@ -2,7 +2,7 @@
Первым делом необходимо установить [ROS2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html). Рекомендуется минимальная установка.
Вторым делом надо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников из этого [форка](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers) стоит отметить, что также существует альтернативная установка с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool) который поставляется с базовыми пакетами ROS2.
Вторым шагом надо собрать [`ros2_control`](https://github.com/ros-controls/ros2_control) из исходников из этого [форка](https://github.com/solid-sinusoid/ros2_control/tree/gz-ros2-cartesian-controllers) стоит отметить, что также существует альтернативная установка с использованием [`vsctool`](https://github.com/dirk-thomas/vcstool) который поставляется с базовыми пакетами ROS2.
Если устанавливать через `vcstool` тогда необходимые пакеты будут клонированы в тоже рабочее пространство, что и сам фреймворк. Сама команда будет выглядеть так
```sh
@ -14,6 +14,11 @@ vcs import . < robossembler-ros2/repos/all-deps.repos
pip insatll -r robossembler-ros2/repos/requirements.txt
```
> [!IMPORTANT]
> Стоит отметить, что для того, чтобы выполнить следующую команду вам необходимо
> установить `git lfs` так как в `requirements.txt` есть модуль `rbs_assets_library` который содержит
> в себе тяжелые файлы, но при этом устанавливается как модуль python.
При этом команду надо выполнять в директории `{robossembler_ws}/src/`
Более четкая последовательность команд кому лень:

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

@ -8,6 +8,7 @@ from .objects import (
ObjectData,
PlaneObjectData,
SphereObjectData,
ObjectRandomizerData,
)
from .robot import RobotData
from .terrain import TerrainData

137
env_manager/env_manager/env_manager/models/terrains/random_ground.py Normal file
View file

@ -0,0 +1,137 @@
import os
import numpy as np
from gym_gz.scenario import model_wrapper
from gym_gz.utils.scenario import get_unique_model_name
from scenario import core as scenario_core
from scenario import gazebo as scenario_gazebo
from rbs_assets_library import get_textures_path
class RandomGround(model_wrapper.ModelWrapper):
def __init__(
self,
world: scenario_gazebo.World,
name: str = "random_ground",
position: tuple[float, float, float] = (0, 0, 0),
orientation: tuple[float, float, float, float] = (1, 0, 0, 0),
size: tuple[float, float] = (1.0, 1.0),
friction: float = 5.0,
texture_dir: str | None = None,
**kwargs,
):
np_random = np.random.default_rng()
# Get a unique model name
model_name = get_unique_model_name(world, name)
# Initial pose
initial_pose = scenario_core.Pose(position, orientation)
# Get textures from ENV variable if not directly specified
if not texture_dir:
texture_dir = os.environ.get("TEXTURE_DIRS", default="")
if not texture_dir:
texture_dir = get_textures_path()
# Find random PBR texture
albedo_map = None
normal_map = None
roughness_map = None
metalness_map = None
if texture_dir:
if ":" in texture_dir:
textures = []
for d in texture_dir.split(":"):
textures.extend([os.path.join(d, f) for f in os.listdir(d)])
else:
# Get list of the available textures
textures = os.listdir(texture_dir)
# Choose a random texture from these
random_texture_dir = str(np_random.choice(textures))
random_texture_dir = texture_dir + random_texture_dir
# List all files
texture_files = os.listdir(random_texture_dir)
# Extract the appropriate files
for texture in texture_files:
texture_lower = texture.lower()
if "color" in texture_lower or "albedo" in texture_lower:
albedo_map = os.path.join(random_texture_dir, texture)
elif "normal" in texture_lower:
normal_map = os.path.join(random_texture_dir, texture)
elif "roughness" in texture_lower:
roughness_map = os.path.join(random_texture_dir, texture)
elif "specular" in texture_lower or "metalness" in texture_lower:
metalness_map = os.path.join(random_texture_dir, texture)
# Create SDF string for the model
sdf = f"""<sdf version="1.9">
<model name="{model_name}">
<static>true</static>
<link name="{model_name}_link">
<collision name="{model_name}_collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>{friction}</mu>
<mu2>{friction}</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="{model_name}_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>{size[0]} {size[1]}</size>
</plane>
</geometry>
<material>
<ambient>1 1 1 1</ambient>
<diffuse>1 1 1 1</diffuse>
<specular>1 1 1 1</specular>
<pbr>
<metal>
{"<albedo_map>%s</albedo_map>"
% albedo_map if albedo_map is not None else ""}
{"<normal_map>%s</normal_map>"
% normal_map if normal_map is not None else ""}
{"<roughness_map>%s</roughness_map>"
% roughness_map if roughness_map is not None else ""}
{"<metalness_map>%s</metalness_map>"
% metalness_map if metalness_map is not None else ""}
</metal>
</pbr>
</material>
</visual>
</link>
</model>
</sdf>"""
# Insert the model
ok_model = world.to_gazebo().insert_model_from_string(
sdf, initial_pose, model_name
)
if not ok_model:
raise RuntimeError("Failed to insert " + model_name)
# Get the model
model = world.get_model(model_name)
# Initialize base class
model_wrapper.ModelWrapper.__init__(self, model=model)

236
env_manager/env_manager/env_manager/scene/scene.py
View file

@ -1,23 +1,34 @@
import asyncio
from dataclasses import asdict
from gym_gz.scenario.model_wrapper import ModelWrapper
from rbs_assets_library import get_model_meshes_info
import rclpy
import time
from threading import Thread
import numpy as np
from rcl_interfaces.srv import GetParameters
from rclpy.node import Node, Parameter
from scenario.bindings.gazebo import GazeboSimulator, PhysicsEngine_dart, World
from scipy.spatial import distance
from scipy.spatial.transform import Rotation
from yaml import Mark
from env_manager.models.terrains.random_ground import RandomGround
from ..models import Box, Camera, Cylinder, Ground, Mesh, Model, Plane, Sphere, Sun
from ..models.configs import CameraData, ObjectData, SceneData
from ..models.robots import get_robot_model_class
from ..utils import Tf2Broadcaster
from ..utils import Tf2Broadcaster, Tf2DynamicBroadcaster
from ..utils.conversions import quat_to_wxyz
from ..utils.gazebo import (
transform_move_to_model_pose,
transform_move_to_model_position,
)
from ..utils.types import Point, Pose
from visualization_msgs.msg import MarkerArray, Marker
from geometry_msgs.msg import Point as RosPoint
from geometry_msgs.msg import Quaternion as RosQuat
# TODO: Split scene randomizer and scene
@ -71,6 +82,7 @@ class Scene:
self.world: World = gazebo.get_world()
self.__objects_unique_names = {}
self.object_models: list[ModelWrapper] = []
self.__object_positions: dict[str, Point] = {}
self.__objects_workspace_centre: Point = (0.0, 0.0, 0.0)
@ -88,6 +100,7 @@ class Scene:
scene.robot.urdf_string = robot_urdf_string
self.tf2_broadcaster = Tf2Broadcaster(node=node)
self.tf2_broadcaster_dyn = Tf2DynamicBroadcaster(node=node)
self.__workspace_center = (0.0, 0.0, 0.0)
self.__workspace_dimensions = (1.5, 1.5, 1.5)
@ -107,6 +120,9 @@ class Scene:
self.__workspace_center[2] + half_depth,
)
# self.marker_array = MarkerArray()
async def get_parameter_async(self):
"""
Asynchronously retrieves the robot_description parameter from the robot state publisher.
@ -165,6 +181,7 @@ class Scene:
This method must be called after the Scene object has been initialized
and before running the simulation.
"""
self.gazebo_paused_run()
self.init_world_plugins()
@ -494,16 +511,199 @@ class Scene:
self.__objects_unique_names[obj.name].append(model_name)
self.__object_positions[model_name] = obj.position
self.object_models.append(object_wrapper.model)
# Enable contact detection
for link_name in object_wrapper.link_names():
link = object_wrapper.to_gazebo().get_link(link_name=link_name)
link.enable_contact_detection(True)
self.node.get_logger().info(
f"Add object with name [{obj.name}], relative_to [{obj.relative_to}]"
)
self.node.get_logger().info(f"On position: {obj.position}")
self.node.get_logger().info(f"On orientation: {obj.orientation}")
publish_tf = True
publish_marker = True
if publish_tf:
if not hasattr(self, "tf_broadcaster_and_markers"):
self.tf_broadcaster_and_markers = self.node.create_timer(0.1, self.publish_marker)
if publish_marker:
if not hasattr(self, "marker_array"):
self.marker_array = MarkerArray()
self.marker_publisher = self.node.create_publisher(MarkerArray, "/markers", 10)
if not hasattr(self, "tf_broadcaster_and_markers"):
self.tf_broadcaster_and_markers = self.node.create_timer(0.1, self.publish_marker)
marker = self.create_marker(obj, object_wrapper, len(self.object_models))
self.marker_array.markers.append(marker)
except Exception as ex:
self.node.get_logger().warn(f"Model could not be inserted. Reason: {ex}")
self.gazebo_paused_run
def create_marker(self, object: ObjectData, model: ModelWrapper, id):
"""
Create a single marker.
"""
marker = Marker()
marker.header.frame_id = object.relative_to
marker.header.stamp = self.node.get_clock().now().to_msg()
marker.ns = "assembly"
marker.id = id
if object.type == "mesh" or object.type == "model":
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = f"file://{get_model_meshes_info(object.name)['visual']}"
marker.mesh_use_embedded_materials = True
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 0.5
marker.color.b = 1.0
marker.color.a = 1.0
self.node.get_logger().info(f"Created mesh marker for {object.name}")
elif object.type == "sphere":
marker.type = Marker.SPHERE
marker.scale.x = object.radius
marker.scale.y = object.radius
marker.scale.z = object.radius
marker.color.r = 0.0
marker.color.g = 0.5
marker.color.b = 1.0
marker.color.a = 1.0
self.node.get_logger().info(f"Created sphere marker for {object.name}")
elif object.type == "box":
marker.type = Marker.CUBE
marker.scale.x = object.size[0]
marker.scale.y = object.size[1]
marker.scale.z = object.size[2]
marker.color.r = 0.0
marker.color.g = 0.5
marker.color.b = 1.0
marker.color.a = 1.0
self.node.get_logger().info(f"Created box marker for {object.name}")
elif object.type == "cylinder":
marker.type = Marker.CYLINDER
marker.scale.x = object.radius * 2
marker.scale.y = object.radius * 2
marker.scale.z = object.length
marker.color.r = 0.0
marker.color.g = 0.5
marker.color.b = 1.0
marker.color.a = 1.0
self.node.get_logger().info(f"Created cylinder marker for {object.name}")
else:
self.node.get_logger().fatal(f"Unsupported object type: {object.type}")
return None
marker.action = Marker.ADD
model_position, model_orientation = self.rotate_around_parent(model.base_position(), model.base_orientation(), (0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0))
position = RosPoint()
position.x = model_position[0]
position.y = model_position[1]
position.z = model_position[2]
orientation = RosQuat()
orientation.x = model_orientation[3]
orientation.y = model_orientation[1]
orientation.z = model_orientation[2]
orientation.w = model_orientation[0]
marker.pose.position = position
marker.pose.orientation = orientation
return marker
def publish_marker(self):
for idx, model in enumerate(self.object_models):
# model_position, model_orientation = self.rotate_around_parent(model.base_position(), model.base_orientation(), (0.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0))
model_position, model_orientation = (model.base_position(), model.base_orientation())
self.marker_array.markers[idx].pose.position.x = float(model_position[0])
self.marker_array.markers[idx].pose.position.y = float(model_position[1])
self.marker_array.markers[idx].pose.position.z = float(model_position[2])
self.marker_array.markers[idx].pose.orientation.w = float(model_orientation[0])
self.marker_array.markers[idx].pose.orientation.x = float(model_orientation[1])
self.marker_array.markers[idx].pose.orientation.y = float(model_orientation[2])
self.marker_array.markers[idx].pose.orientation.z = float(model_orientation[3])
self.tf2_broadcaster_dyn.broadcast_tf(
parent_frame_id="world",
child_frame_id=model.base_frame(),
translation=model_position,
rotation=model_orientation,
xyzw=False,
)
self.marker_publisher.publish(self.marker_array)
def rotate_around_parent(self, position, orientation, parent_position, parent_orientation):
"""
Apply a 180-degree rotation around the parent's Z-axis to the object's pose.
Args:
position: Tuple (x, y, z) - the object's position relative to the parent.
orientation: Tuple (w, x, y, z) - the object's orientation relative to the parent.
parent_position: Tuple (x, y, z) - the parent's position.
parent_orientation: Tuple (w, x, y, z) - the parent's orientation.
Returns:
A tuple containing the updated position and orientation:
(new_position, new_orientation)
"""
# Define a quaternion for 180-degree rotation around Z-axis (w, x, y, z)
rotation_180_z = (0.0, 0.0, 0.0, 1.0)
w1, x1, y1, z1 = orientation
# Кватернион для 180° поворота вокруг Z
w2, x2, y2, z2 = (0, 0, 0, 1)
# Умножение кватернионов
w3 = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x3 = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y3 = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
z3 = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
new_orientation = (w3, x3, y3, z3)
# Compute the new orientation: parent_orientation * rotation_180_z * orientation
# new_orientation = (
# parent_orientation[0] * rotation_180_z[0] - parent_orientation[1] * rotation_180_z[1] -
# parent_orientation[2] * rotation_180_z[2] - parent_orientation[3] * rotation_180_z[3],
# parent_orientation[0] * rotation_180_z[1] + parent_orientation[1] * rotation_180_z[0] +
# parent_orientation[2] * rotation_180_z[3] - parent_orientation[3] * rotation_180_z[2],
# parent_orientation[0] * rotation_180_z[2] - parent_orientation[1] * rotation_180_z[3] +
# parent_orientation[2] * rotation_180_z[0] + parent_orientation[3] * rotation_180_z[1],
# parent_orientation[0] * rotation_180_z[3] + parent_orientation[1] * rotation_180_z[2] -
# parent_orientation[2] * rotation_180_z[1] + parent_orientation[3] * rotation_180_z[0],
# )
# Apply rotation to the position
rotated_position = (
-position[0], # Rotate 180 degrees around Z-axis: x -> -x
-position[1], # y -> -y
position[2] # z remains unchanged
)
# Add parent position
new_position = (
parent_position[0] + rotated_position[0],
parent_position[1] + rotated_position[1],
parent_position[2] + rotated_position[2],
)
return new_position, new_orientation
def add_light(self):
"""
Configure and insert a default light source into the simulation.
@ -566,17 +766,29 @@ class Scene:
changes are reflected in the environment.
"""
# 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,
ambient=self.terrain.ambient,
diffuse=self.terrain.diffuse,
specular=self.terrain.specular,
)
if self.terrain.type == "flat":
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,
ambient=self.terrain.ambient,
diffuse=self.terrain.diffuse,
specular=self.terrain.specular,
)
elif self.terrain.type == "random_flat":
self.terrain_wrapper = RandomGround(
self.world,
name=self.terrain.name,
position=self.terrain.spawn_position,
orientation=quat_to_wxyz(self.terrain.spawn_quat_xyzw),
size=self.terrain.size,
)
else:
raise ValueError(
"Type of ground is not supported, supported type is: [ground, random_ground]"
)
# Enable contact detection
for link_name in self.terrain_wrapper.link_names():

1
env_manager/env_manager/env_manager/utils/__init__.py
View file

@ -1,3 +1,4 @@
from . import conversions, gazebo, logging, math, types
from .tf2_broadcaster import Tf2Broadcaster, Tf2BroadcasterStandalone
from .tf2_dynamic_broadcaster import Tf2DynamicBroadcaster
from .tf2_listener import Tf2Listener, Tf2ListenerStandalone

74
env_manager/env_manager/env_manager/utils/tf2_dynamic_broadcaster.py Normal file
View file

@ -0,0 +1,74 @@
import sys
from typing import Tuple
import rclpy
from geometry_msgs.msg import TransformStamped
from rclpy.node import Node
from rclpy.parameter import Parameter
from tf2_ros import TransformBroadcaster
class Tf2DynamicBroadcaster:
def __init__(
self,
node: Node,
):
self._node = node
self.__tf2_broadcaster = TransformBroadcaster(node=self._node)
self._transform_stamped = TransformStamped()
def broadcast_tf(
self,
parent_frame_id: str,
child_frame_id: str,
translation: Tuple[float, float, float],
rotation: Tuple[float, float, float, float],
xyzw: bool = True,
):
"""
Broadcast transformation of the camera
"""
self._transform_stamped.header.frame_id = parent_frame_id
self._transform_stamped.child_frame_id = child_frame_id
self._transform_stamped.header.stamp = self._node.get_clock().now().to_msg()
self._transform_stamped.transform.translation.x = float(translation[0])
self._transform_stamped.transform.translation.y = float(translation[1])
self._transform_stamped.transform.translation.z = float(translation[2])
if xyzw:
self._transform_stamped.transform.rotation.x = float(rotation[0])
self._transform_stamped.transform.rotation.y = float(rotation[1])
self._transform_stamped.transform.rotation.z = float(rotation[2])
self._transform_stamped.transform.rotation.w = float(rotation[3])
else:
self._transform_stamped.transform.rotation.w = float(rotation[0])
self._transform_stamped.transform.rotation.x = float(rotation[1])
self._transform_stamped.transform.rotation.y = float(rotation[2])
self._transform_stamped.transform.rotation.z = float(rotation[3])
self.__tf2_broadcaster.sendTransform(self._transform_stamped)
class Tf2BroadcasterStandalone(Node, Tf2DynamicBroadcaster):
def __init__(
self,
node_name: str = "env_manager_tf_broadcaster",
use_sim_time: bool = True,
):
try:
rclpy.init()
except Exception as e:
if not rclpy.ok():
sys.exit(f"ROS 2 context could not be initialised: {e}")
Node.__init__(self, node_name)
self.set_parameters(
[Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time)]
)
Tf2DynamicBroadcaster.__init__(self, node=self)

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

@ -15,6 +15,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import yaml
def launch_setup(context, *args, **kwargs):
@ -22,25 +23,21 @@ def launch_setup(context, *args, **kwargs):
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
use_moveit = LaunchConfiguration("use_moveit")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gripper_name = LaunchConfiguration("gripper_name")
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
base_link_name = LaunchConfiguration("base_link_name").perform(context)
control_space = LaunchConfiguration("control_space").perform(context)
control_strategy = LaunchConfiguration("control_strategy").perform(context)
interactive = LaunchConfiguration("interactive").perform(context)
# training arguments
env = LaunchConfiguration("env")
@ -48,28 +45,72 @@ def launch_setup(context, *args, **kwargs):
log_level = LaunchConfiguration("log_level")
env_kwargs = LaunchConfiguration("env_kwargs")
sim_gazebo = LaunchConfiguration("sim_gazebo")
launch_simulation = LaunchConfiguration("launch_sim")
initial_joint_controllers_file_path = os.path.join(
get_package_share_directory("rbs_arm"), "config", "controllers.yaml"
description_package_abs_path = get_package_share_directory(
description_package.perform(context)
)
simulation_controllers = os.path.join(
description_package_abs_path, "config", "controllers.yaml"
)
xacro_file = os.path.join(
get_package_share_directory(description_package.perform(context)),
description_package_abs_path,
"urdf",
description_file.perform(context),
)
robot_description_doc = xacro.process_file(
xacro_file,
mappings={
"gripper_name": gripper_name.perform(context),
"hardware": hardware.perform(context),
"simulation_controllers": initial_joint_controllers_file_path,
"namespace": "",
},
)
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
# TODO: hide this to another place
# Load xacro_args
def param_constructor(loader, node, local_vars):
value = loader.construct_scalar(node)
return LaunchConfiguration(value).perform(
local_vars.get("context", "Launch context if not defined")
)
def variable_constructor(loader, node, local_vars):
value = loader.construct_scalar(node)
return local_vars.get(value, f"Variable '{value}' not found")
def load_xacro_args(yaml_file, local_vars):
# Get valut from ros2 argument
yaml.add_constructor(
"!param", lambda loader, node: param_constructor(loader, node, local_vars)
)
# Get value from local variable in this code
# The local variable should be initialized before the loader was called
yaml.add_constructor(
"!variable",
lambda loader, node: variable_constructor(loader, node, local_vars),
)
with open(yaml_file, "r") as file:
return yaml.load(file, Loader=yaml.FullLoader)
mappings_data = load_xacro_args(xacro_config_file, locals())
robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
robot_description_semantic_content = ""
if use_moveit.perform(context) == "true":
srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
srdf_file = os.path.join(
get_package_share_directory(moveit_config_package.perform(context)),
"srdf",
moveit_config_file.perform(context),
)
srdf_mappings = load_xacro_args(srdf_config_file, locals())
robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
control_space = "joint"
control_strategy = "position"
interactive = "false"
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
robot_description = {"robot_description": robot_description_content}
@ -83,28 +124,25 @@ def launch_setup(context, *args, **kwargs):
]
),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": initial_joint_controllers_file_path,
"controllers_file": simulation_controllers,
"robot_type": robot_type,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_type,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"use_moveit": "false",
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": "true",
"use_sim_time": "true",
"use_skills": "false",
"use_controllers": "true",
"robot_description": robot_description_content,
"robot_description_semantic": robot_description_semantic_content,
"base_link_name": base_link_name,
"ee_link_name": ee_link_name,
"control_space": control_space,
"control_strategy": "effort",
"interactive_control": "false",
}.items(),
)
@ -137,7 +175,7 @@ def launch_setup(context, *args, **kwargs):
nodes_to_start = [
# env,
rl_task,
# rl_task,
clock_bridge,
delay_robot_control_stack,
]
@ -150,18 +188,20 @@ def generate_launch_description():
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
choices=[
"rbs_arm",
"ar4",
"ur3",
"ur3e",
"ur5",
"ur5e",
"ur10",
"ur10e",
"ur16e",
],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
@ -184,20 +224,6 @@ def generate_launch_description():
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
@ -221,14 +247,6 @@ def generate_launch_description():
This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"with_gripper", default_value="true", description="With gripper or not?"
@ -236,25 +254,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"sim_gazebo", default_value="true", description="Gazebo Simulation"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"env_manager", default_value="false", description="Launch env_manager?"
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_moveit", default_value="false", description="Launch moveit?"
"use_moveit", default_value="false", description="Launch moveit?"
)
)
declared_arguments.append(
@ -264,39 +264,55 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_task_planner",
default_value="false",
description="Launch task_planner?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"launch_controllers",
"use_controllers",
default_value="true",
description="Launch controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gazebo_gui", default_value="true", description="Launch gazebo with gui?"
"scene_config_file",
default_value="",
description="Path to a scene configuration file",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"ee_link_name",
default_value="",
description="End effector name of robot arm",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"base_link_name",
default_value="",
description="Base link name if robot arm",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"control_space",
default_value="task",
choices=["task", "joint"],
description="Specify the control space for the robot (e.g., task space).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"control_strategy",
default_value="position",
choices=["position", "velocity", "effort"],
description="Specify the control strategy (e.g., position control).",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"interactive",
default_value="true",
description="Wheter to run the motion_control_handle controller",
),
)
# training arguments
declared_arguments.append(
DeclareLaunchArgument(

268
env_manager/rbs_gym/launch/train.launch.py
View file

@ -13,31 +13,29 @@ from launch_ros.actions import Node
import os
from os import cpu_count
from ament_index_python.packages import get_package_share_directory
import yaml
import xacro
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
use_moveit = LaunchConfiguration("use_moveit")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gripper_name = LaunchConfiguration("gripper_name")
scene_config_file = LaunchConfiguration("scene_config_file").perform(context)
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
base_link_name = LaunchConfiguration("base_link_name").perform(context)
control_space = LaunchConfiguration("control_space").perform(context)
control_strategy = LaunchConfiguration("control_strategy").perform(context)
interactive = LaunchConfiguration("interactive").perform(context)
# training arguments
env = LaunchConfiguration("env")
@ -64,47 +62,104 @@ def launch_setup(context, *args, **kwargs):
track = LaunchConfiguration("track")
sim_gazebo = LaunchConfiguration("sim_gazebo")
launch_simulation = LaunchConfiguration("launch_sim")
initial_joint_controllers_file_path = os.path.join(
get_package_share_directory('rbs_arm'), 'config', 'controllers.yaml'
description_package_abs_path = get_package_share_directory(
description_package.perform(context)
)
simulation_controllers = os.path.join(
description_package_abs_path, "config", "controllers.yaml"
)
xacro_file = os.path.join(
description_package_abs_path,
"urdf",
description_file.perform(context),
)
xacro_config_file = f"{description_package_abs_path}/config/xacro_args.yaml"
# TODO: hide this to another place
# Load xacro_args
def param_constructor(loader, node, local_vars):
value = loader.construct_scalar(node)
return LaunchConfiguration(value).perform(
local_vars.get("context", "Launch context if not defined")
)
def variable_constructor(loader, node, local_vars):
value = loader.construct_scalar(node)
return local_vars.get(value, f"Variable '{value}' not found")
def load_xacro_args(yaml_file, local_vars):
# Get valut from ros2 argument
yaml.add_constructor(
"!param", lambda loader, node: param_constructor(loader, node, local_vars)
)
# Get value from local variable in this code
# The local variable should be initialized before the loader was called
yaml.add_constructor(
"!variable",
lambda loader, node: variable_constructor(loader, node, local_vars),
)
with open(yaml_file, "r") as file:
return yaml.load(file, Loader=yaml.FullLoader)
mappings_data = load_xacro_args(xacro_config_file, locals())
robot_description_doc = xacro.process_file(xacro_file, mappings=mappings_data)
robot_description_semantic_content = ""
if use_moveit.perform(context) == "true":
srdf_config_file = f"{description_package_abs_path}/config/srdf_xacro_args.yaml"
srdf_file = os.path.join(
get_package_share_directory(moveit_config_package.perform(context)),
"srdf",
moveit_config_file.perform(context),
)
srdf_mappings = load_xacro_args(srdf_config_file, locals())
robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings)
robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ")
control_space = "joint"
control_strategy = "position"
interactive = "false"
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
robot_description = {"robot_description": robot_description_content}
single_robot_setup = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "launch", "rbs_robot.launch.py"]
)
]
),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"controllers_file": simulation_controllers,
"robot_type": robot_type,
"controllers_file": initial_joint_controllers_file_path,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_name,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"robot_name": robot_type,
"use_moveit": use_moveit,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": launch_controllers,
# "gazebo_gui": gazebo_gui
}.items()
"use_controllers": "true",
"robot_description": robot_description_content,
"robot_description_semantic": robot_description_semantic_content,
"base_link_name": base_link_name,
"ee_link_name": ee_link_name,
"control_space": control_space,
"control_strategy": control_strategy,
"interactive_control": interactive,
}.items(),
)
args = [
@ -188,18 +243,20 @@ def generate_launch_description():
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
choices=[
"rbs_arm",
"ar4",
"ur3",
"ur3e",
"ur5",
"ur5e",
"ur10",
"ur10e",
"ur16e",
],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
@ -222,20 +279,6 @@ def generate_launch_description():
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
@ -261,69 +304,69 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
"with_gripper", default_value="true", description="With gripper or not?"
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
DeclareLaunchArgument(
"use_moveit", default_value="false", description="Launch moveit?"
)
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
DeclareLaunchArgument(
"launch_perception", default_value="false", description="Launch perception?"
)
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
DeclareLaunchArgument(
"use_controllers",
default_value="true",
description="Launch controllers?",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
DeclareLaunchArgument(
"scene_config_file",
default_value="",
description="Path to a scene configuration file",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit",
default_value="false",
description="Launch moveit?")
DeclareLaunchArgument(
"ee_link_name",
default_value="",
description="End effector name of robot arm",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
DeclareLaunchArgument(
"base_link_name",
default_value="",
description="Base link name if robot arm",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
DeclareLaunchArgument(
"control_space",
default_value="task",
choices=["task", "joint"],
description="Specify the control space for the robot (e.g., task space).",
)
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?")
DeclareLaunchArgument(
"control_strategy",
default_value="position",
choices=["position", "velocity", "effort"],
description="Specify the control strategy (e.g., position control).",
)
)
declared_arguments.append(
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
DeclareLaunchArgument(
"interactive",
default_value="true",
description="Wheter to run the motion_control_handle controller",
),
)
# training arguments
declared_arguments.append(
@ -448,13 +491,12 @@ def generate_launch_description():
default_value="1",
description="Verbose mode (0: no output, 1: INFO).",
))
# 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(
"log_level",

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

@ -105,14 +105,14 @@ SCENE_CONFIGURATION: SceneData = SceneData(
name="rbs_arm",
joint_positions=[0.0, 0.5, 3.14159, 1.5, 0.0, 1.4, 0.0],
with_gripper=True,
gripper_joint_positions=0.064,
gripper_joint_positions=0.00,
randomizer=RobotRandomizerData(joint_positions=True),
),
objects=[
SphereObjectData(
name="sphere",
relative_to="base_link",
position=(0.0, 0.4, 1.0),
position=(0.0, 0.3, 0.5),
static=True,
collision=False,
color=(0.0, 1.0, 0.0, 0.8),

16
env_manager/rbs_runtime/launch/runtime.launch.py
View file

@ -41,6 +41,8 @@ def launch_setup(context, *args, **kwargs):
interactive = LaunchConfiguration("interactive").perform(context)
real_robot = LaunchConfiguration("real_robot").perform(context)
if not scene_config_file == "":
config_file = {"config_file": scene_config_file}
else:
@ -153,9 +155,15 @@ def launch_setup(context, *args, **kwargs):
}.items(),
)
if real_robot == "true":
return [rbs_robot_setup]
rbs_runtime = Node(
package="rbs_runtime",
executable="runtime",
output='log',
emulate_tty=True,
# arguments=[('log_level:=debug')],
parameters=[robot_description, config_file, {"use_sim_time": True}],
)
@ -307,6 +315,14 @@ def generate_launch_description():
description="Wheter to run the motion_control_handle controller",
),
)
declared_arguments.append(
DeclareLaunchArgument(
"real_robot",
default_value="false",
description="Wheter to run on the real robot",
),
)
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]

17
env_manager/rbs_runtime/rbs_runtime/__init__.py
View file

@ -6,22 +6,25 @@ from env_manager.models.configs import SceneData
from typing import Dict, Any
from env_manager.models.configs import (
BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData
BoxObjectData, CylinderObjectData, MeshData, ModelData, ObjectData, ObjectRandomizerData
)
def object_factory(obj_data: Dict[str, Any]) -> ObjectData:
obj_type = obj_data.get('type', '')
if "randomize" in obj_data and isinstance(obj_data["randomize"], dict):
obj_data["randomize"] = from_dict(data_class=ObjectRandomizerData, data=obj_data["randomize"])
if obj_type == 'box':
return BoxObjectData(**obj_data)
return from_dict(data_class=BoxObjectData, data=obj_data)
elif obj_type == 'cylinder':
return CylinderObjectData(**obj_data)
return from_dict(data_class=CylinderObjectData, data=obj_data)
elif obj_type == 'mesh':
return MeshData(**obj_data)
return from_dict(data_class=MeshData, data=obj_data)
elif obj_type == 'model':
return ModelData(**obj_data)
return from_dict(data_class=ModelData, data=obj_data)
else:
return ObjectData(**obj_data)
return from_dict(data_class=ObjectData, data=obj_data)
def scene_config_loader(file: str | Path) -> SceneData:
def tuple_constructor(loader, node):
@ -35,6 +38,4 @@ def scene_config_loader(file: str | Path) -> SceneData:
scene_data.objects = [object_factory(obj) for obj in scene_cfg.get('objects', [])]
print(scene_data)
return scene_data

2
rbs_bringup/launch/rbs_bringup.launch.py
View file

@ -25,7 +25,7 @@ def launch_setup(context, *args, **kwargs):
"description_package": "rbs_arm",
"description_file": "rbs_arm_modular.xacro",
"robot_name": "rbs_arm",
"use_moveit": "true",
"use_moveit": "false",
"moveit_config_package": "rbs_arm",
"moveit_config_file": "rbs_arm.srdf.xacro",
"use_sim_time": "true",

12
rbs_bringup/launch/rbs_robot.launch.py
View file

@ -2,7 +2,7 @@ import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch import LaunchDescription, condition
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
@ -30,6 +30,7 @@ def launch_setup(context, *args, **kwargs):
moveit_config_package = LaunchConfiguration("moveit_config_package")
use_sim_time = LaunchConfiguration("use_sim_time")
use_controllers = LaunchConfiguration("use_controllers")
use_skills = LaunchConfiguration("use_skills")
namespace = LaunchConfiguration("namespace")
multi_robot = LaunchConfiguration("multi_robot")
robot_name = robot_name.perform(context)
@ -144,6 +145,7 @@ def launch_setup(context, *args, **kwargs):
"ee_link_name": ee_link_name,
"base_link_name": base_link_name,
}.items(),
condition=IfCondition(use_skills),
)
nodes_to_start = [
@ -247,6 +249,14 @@ def generate_launch_description():
description="Specify if MoveIt should be launched.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_skills",
default_value="true",
description="Specify if skills should be launched.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_controllers",

5
rbs_skill_servers/launch/skills.launch.py
View file

@ -55,6 +55,8 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
{"base_link": base_link_name},
{"ee_link": ee_link_name},
robot_description,
],
),
@ -66,7 +68,7 @@ def launch_setup(context, *args, **kwargs):
{"use_sim_time": use_sim_time},
{"robot_name": namespace},
{"base_link": base_link_name},
{"robot_ee_link": ee_link_name},
{"ee_link": ee_link_name},
robot_description,
],
),
@ -101,6 +103,7 @@ def launch_setup(context, *args, **kwargs):
package="rbs_utils",
executable="assembly_config_service.py",
namespace=namespace,
parameters=[{"use_sim_time": use_sim_time}],
output="screen",
)

12
rbs_skill_servers/src/mtjs_jtc.cpp
View file

@ -60,8 +60,16 @@ protected:
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
trajectory_goal.trajectory.points = generate_trajectory(

36
rbs_skill_servers/src/mtp_jtc.cpp
View file

@ -57,6 +57,12 @@ public:
std::placeholders::_1),
s_options);
this->declare_parameter("robot_description", "");
this->declare_parameter("base_link", "");
this->declare_parameter("ee_link", "");
m_robot_description = this->get_parameter("robot_description").as_string();
m_base_link = this->get_parameter("base_link").as_string();
m_ee_link = this->get_parameter("ee_link").as_string();
}
protected:
@ -75,8 +81,16 @@ protected:
trajectory_goal.trajectory.joint_names = m_joint_names;
// TODO: Better sync solution
while (m_current_joint_positions.empty()) {
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
std::vector<double> target_joint_values;
@ -86,6 +100,7 @@ protected:
generate_trajectory(m_current_joint_positions, target_joint_values,
m_current_goal->duration);
// RCLCPP_INFO(this->get_logger(), "Starting executing action goal");
auto send_goal_options =
rclcpp_action::Client<FollowJointTrajectory>::SendGoalOptions();
send_goal_options.result_callback =
@ -109,7 +124,7 @@ private:
if (m_joint_names.empty()) {
return;
}
RCLCPP_WARN(this->get_logger(), "Called joint positions");
// RCLCPP_WARN(this->get_logger(), "Called joint positions");
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
@ -140,10 +155,12 @@ private:
const std::vector<double> &target_joint_values,
const double duration) {
RCLCPP_INFO(this->get_logger(), "Starting generate_trajectory");
const int num_points = 100;
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
for (int i = 0; i <= num_points; ++i) {
trajectory_msgs::msg::JointTrajectoryPoint point;
double t = static_cast<double>(i) / num_points;
for (size_t j = 0; j < target_joint_values.size(); ++j) {
double position = start_joint_values[j] +
@ -157,6 +174,8 @@ private:
}
void solveIK(std::vector<double> &out) {
RCLCPP_INFO(this->get_logger(), "Starting solveIK");
KDL::JntArray q_in(m_joint_names.size());
for (size_t i = 0; i < m_joint_names.size(); ++i) {
q_in(i) = m_current_joint_positions[i];
@ -175,14 +194,8 @@ private:
target_pose.translation().y(),
target_pose.translation().z()));
const std::string m_base_link = "base_link";
const std::string m_ee_link = "gripper_grasp_point";
auto robot_description =
this->get_parameter("robot_description").as_string();
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromString(robot_description, kdl_tree)) {
if (!kdl_parser::treeFromString(m_robot_description, kdl_tree)) {
RCLCPP_ERROR(this->get_logger(),
"Failed to parse KDL tree from robot description.");
return;
@ -216,6 +229,9 @@ private:
std::vector<double> m_current_joint_positions;
std::unordered_map<std::string, size_t> m_joint_mame_to_index;
std::vector<std::string> m_joint_names;
std::string m_base_link;
std::string m_ee_link;
std::string m_robot_description;
};
} // namespace rbs_skill_actions

4
rbs_skill_servers/src/mtp_jtc_cart.cpp
View file

@ -60,7 +60,7 @@ public:
s_options);
this->declare_parameter("robot_description", "");
this->declare_parameter("base_link", "");
this->declare_parameter("robot_ee_link", "");
this->declare_parameter("ee_link", "");
auto robot_description =
this->get_parameter("robot_description").as_string();
@ -73,7 +73,7 @@ public:
}
auto base_link = this->get_parameter("base_link").as_string();
auto robot_ee_link = this->get_parameter("robot_ee_link").as_string();
auto robot_ee_link = this->get_parameter("ee_link").as_string();
if (base_link.empty() or robot_ee_link.empty()) {
RCLCPP_ERROR(this->get_logger(),
"Describe robot end-effector link and base link to continue");

118
rbs_utils/rbs_utils/scripts/assembly_config_service.py
View file

@ -4,24 +4,55 @@ import os
import rclpy
from rclpy.node import Node
import yaml
from geometry_msgs.msg import Point, Pose, Quaternion, PoseArray
from geometry_msgs.msg import Point, Pose, Quaternion
from visualization_msgs.msg import Marker, MarkerArray
from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
from tf2_ros import TransformListener, Buffer, TransformException
from rbs_assets_library import get_model_meshes_info
from env_manager.utils import Tf2Broadcaster
class AssemblyConfigService(Node):
def __init__(self, node_name="assembly_config"):
super().__init__(node_name)
# Initialize parameters
self.declare_parameter("db_folder", "asp-example")
db_folder = self.get_parameter("db_folder").get_parameter_value().string_value
# Parse the YAML file
yaml_file = os.path.join(os.getenv('RBS_ASSEMBLY_DIR', ''), db_folder,'rbs_db.yaml')
yaml_file = os.path.join(
os.getenv("RBS_ASSEMBLY_DIR", ""), db_folder, "rbs_db.yaml"
)
self.assembly_config = parse_yaml(yaml_file)
# Create services
self.workspace_service = self.create_service(GetWorkspace, "get_workspace", self.get_workspace_callback)
self.grasp_pose_service = self.create_service(GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback)
tf2_broadcaster = Tf2Broadcaster(self)
for relative_part in self.assembly_config.relative_part:
tf2_broadcaster.broadcast_tf(
relative_part.relative_at,
relative_part.name,
translation=(
relative_part.pose.position.x,
relative_part.pose.position.y,
relative_part.pose.position.z,
),
rotation=(
relative_part.pose.orientation.x,
relative_part.pose.orientation.y,
relative_part.pose.orientation.z,
relative_part.pose.orientation.w
)
)
# Services
self.workspace_service = self.create_service(
GetWorkspace, "get_workspace", self.get_workspace_callback
)
self.grasp_pose_service = self.create_service(
GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback
)
def get_workspace_callback(self, request, response):
self.get_logger().info("Workspace request received")
@ -35,52 +66,73 @@ class AssemblyConfigService(Node):
return response
def parse_yaml(yaml_file):
with open(yaml_file, 'r') as file:
with open(yaml_file, "r") as file:
data = yaml.safe_load(file)
assets_db = data.get('assets_db', '')
workspace = [Point(**point) for point in data.get('workspace', [])]
assets_db = data.get("assets_db", "")
workspace = [Point(**point) for point in data.get("workspace", [])]
absolute_part = []
for part in data.get('absolute_part', []):
position = part['pose']['position']
orientation = part['pose']['orientation']
for part in data.get("absolute_part", []):
position = part["pose"]["position"]
orientation = part["pose"].get(
"orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
)
pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
absolute_part.append(NamedPose(name=part['name'], pose=pose))
absolute_part.append(NamedPose(name=part["name"], pose=pose))
relative_part = []
for part in data.get('relative_part', []):
position = part['pose']['position']
orientation = part['pose']['orientation']
for part in data.get("relative_part", []):
position = part["pose"]["position"]
orientation = part["pose"].get(
"orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
)
pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
relative_part.append(RelativeNamedPose(name=part['name'], relative_at=part['relative_at'], pose=pose))
relative_part.append(
RelativeNamedPose(
name=part["name"], relative_at=part["relative_at"], pose=pose
)
)
grasp_pose = []
for pose in data.get('grasp_pose', []):
position = pose['pose']['position']
orientation = pose['pose']['orientation']
pose_obj = Pose(position=Point(**position), orientation=Quaternion(**orientation))
grasp_pose.append(RelativeNamedPose(name=pose['name'], relative_at=pose['relative_at'], pose=pose_obj))
for pose in data.get("grasp_pose", []):
position = pose["pose"]["position"]
orientation = pose["pose"].get(
"orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
)
pose_obj = Pose(
position=Point(**position), orientation=Quaternion(**orientation)
)
grasp_pose.append(
RelativeNamedPose(
name=pose["name"], relative_at=pose["relative_at"], pose=pose_obj
)
)
extra_poses = []
assembly_config = AssemblyConfig(
assets_db=assets_db,
workspace=workspace,
absolute_part=absolute_part,
relative_part=relative_part,
grasp_pose=grasp_pose,
extra_poses=extra_poses
extra_poses=extra_poses,
)
return assembly_config
def main(args=None):
rclpy.init(args=args)
node = AssemblyConfigService()
rclpy.spin(node)
rclpy.shutdown()
def main():
rclpy.init()
if __name__ == '__main__':
executor = rclpy.executors.SingleThreadedExecutor()
node = AssemblyConfigService()
executor.add_node(node)
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
node.destroy_node()
if __name__ == "__main__":
main()

10
repos/requirements.txt
View file

@ -23,7 +23,9 @@ trimesh>=4.4.9
wandb>=0.18.6
# Packages from devpi custom repository
scenario @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/63e/86b0583c22e52/scenario-1.4.0-cp310-cp310-linux_x86_64.whl#sha256=63e86b0583c22e52c299b9a25e74a26178fc7dbc423f661bf34e4dd26543d11f
gym-gz @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/539/1f448c1391486/gym_gz-1.4.0-py3-none-any.whl#sha256=5391f448c13914860e2a65904f6d0fac7eba71daaa3327d8399d7ae516471a68
gym-gz-models @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/f42/4784934183e88/gym_gz_models-1.2.0-cp310-cp310-linux_x86_64.whl#sha256=f424784934183e88541c703c789315ba6118661bd221a4bf9135c6a0ee012e96
rbs_asstss_library @ https://devpi.solid-sinusoid.duckdns.org/narmak/dev/+f/165/dd2476d9dc455/rbs_assets_library-0.1.0-py2.py3-none-any.whl#sha256=165dd2476d9dc455b425a8c8d3e09a9a9541d54c1f900a3a0e63526c6118de6d
scenario @ https://devpi.narmak.xyz/narmak/dev/+f/63e/86b0583c22e52/scenario-1.4.0-cp310-cp310-linux_x86_64.whl#sha256=63e86b0583c22e52c299b9a25e74a26178fc7dbc423f661bf34e4dd26543d11f
gym-gz @ https://devpi.narmak.xyz/narmak/dev/+f/539/1f448c1391486/gym_gz-1.4.0-py3-none-any.whl#sha256=5391f448c13914860e2a65904f6d0fac7eba71daaa3327d8399d7ae516471a68
gym-gz-models @ https://devpi.narmak.xyz/narmak/dev/+f/f42/4784934183e88/gym_gz_models-1.2.0-cp310-cp310-linux_x86_64.whl#sha256=f424784934183e88541c703c789315ba6118661bd221a4bf9135c6a0ee012e96
# rbs_assets_library
git+https://github.com/solid-sinusoid/rbs_assets_library.git@master