robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

add moveit config and update scene

Browse source
This commit is contained in:
Ilya Uraev 2025-03-07 12:49:02 +03:00
parent d1553e2a9e
commit eac8f088d3
23 changed files with 1131 additions and 269 deletions
Download patch file Download diff file Expand all files Collapse all files

42
rbs_mill_assist/CMakeLists.txt
View file

@ -7,51 +7,13 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_srvs REQUIRED)
find_package(gz-cmake3 REQUIRED)
find_package(gz-plugin2 REQUIRED COMPONENTS register)
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
find_package(gz-common5 REQUIRED COMPONENTS profiler)
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
# Harmonic
if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
find_package(gz-sim8 REQUIRED)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Harmonic")
# Default to Garden
else()
find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden")
endif()
add_library(VacuumGripper
SHARED
src/VacuumGripper.cpp
)
include_directories(include)
target_link_libraries(VacuumGripper
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
ament_target_dependencies(VacuumGripper
rclcpp
std_srvs
)
install(
TARGETS VacuumGripper
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config
install(DIRECTORY world urdf meshes moveit srdf launch config assets bt/xmls bt/config
DESTINATION share/${PROJECT_NAME})
add_subdirectory(src)
add_subdirectory(scripts)
add_subdirectory(bt)

233
rbs_mill_assist/launch/moveit.launch.py Normal file
View file

@ -0,0 +1,233 @@
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch_ros.substitutions import FindPackage, FindPackageShare
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from rbs_launch_utils.launch_common import (
get_package_share_directory,
load_yaml,
load_yaml_abs,
)
import os
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_description",
default_value="''",
description="robot description param",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_description_semantic",
default_value="''",
description="robot description semantic param",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_description_kinematics",
default_value="''",
description="robot description kinematics param",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="rbs_arm",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="rbs_arm.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"tf_prefix",
default_value="''",
description="tf_prefix for robot links",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"namespace",
default_value="",
description="Namespace for move_group_node",
)
)
return LaunchDescription(
declared_arguments + [OpaqueFunction(function=launch_setup)]
)
def launch_setup(context, *args, **kwargs):
use_sim_time = LaunchConfiguration("use_sim_time")
robot_description_content = LaunchConfiguration("robot_description").perform(
context
)
robot_description_semantic_content = LaunchConfiguration(
"robot_description_semantic"
).perform(context)
robot_description_kinematics_filepath = LaunchConfiguration(
"robot_description_kinematics"
).perform(context)
namespace = LaunchConfiguration("namespace").perform(context)
moveit_config_package = LaunchConfiguration("moveit_config_package").perform(
context
)
robot_description = {"robot_description": robot_description_content}
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content
}
robot_description_kinematics = {
"robot_description_kinematics": load_yaml_abs(
robot_description_kinematics_filepath
)
}
use_sim_time = {"use_sim_time": use_sim_time}
# Planning Configuration
ompl_planning_pipeline_config = {
"default_planning_pipeline": "ompl",
"planning_pipelines": ["ompl"],
"ompl": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
},
}
ompl_planning_yaml = load_yaml(moveit_config_package, "moveit/ompl_planning.yaml")
ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
moveit_controllers = load_yaml(
moveit_config_package, "moveit/moveit_controllers.yaml"
)
# moveit_controllers = {
# "moveit_simple_controller_manager":
# controllers_yaml,
# "moveit_controller_manager":
# "moveit_simple_controller_manager/MoveItSimpleControllerManager",
# }
moveit_config = (
MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
.robot_description(file_path="urdf/rbs_arm.xacro")
.trajectory_execution(file_path="moveit/moveit_controllers.yaml")
.joint_limits("moveit/joint_limits.yaml")
.pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml")
.robot_description_kinematics("moveit/kinematics.yaml")
.robot_description_semantic("srdf/rbs_arm.srdf")
.moveit_cpp("moveit/moveit_cpp.yaml")
.planning_scene_monitor(
publish_geometry_updates=True,
publish_planning_scene=True,
publish_robot_description=True,
publish_robot_description_semantic=True,
)
.to_moveit_configs()
)
# trajectory_execution = {
# "moveit_manage_controllers": False,
# "trajectory_execution.allowed_execution_duration_scaling": 1.2,
# "trajectory_execution.allowed_goal_duration_margin": 0.5,
# "trajectory_execution.allowed_start_tolerance": 0.01,
# }
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
robot_description_planning = {
"robot_description_planning": load_yaml(
moveit_config_package,
os.path.join("moveit", "joint_limits.yaml"),
)
}
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
ompl_planning_pipeline_config,
# trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
use_sim_time,
robot_description_planning,
# moveit_config.to_dict()
],
)
planning_scene_publisher = Node(
package="rbs_mill_assist",
executable="planning_scene_publisher",
parameters=[moveit_config.to_dict(), {"use_sim_time": True}],
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
# arguments=["-d", rviz_full_config],
parameters=[
# robot_description,
# robot_description_semantic,
# robot_description_kinematics,
# robot_description_planning,
# ompl_planning_pipeline_config,
# # trajectory_execution,
# moveit_controllers,
# planning_scene_monitor_parameters,
{"use_sim_time": True},
# moveit_config.robot_description,
# moveit_config.robot_description_semantic,
# moveit_config.robot_description_kinematics,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.planning_scene_monitor,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
],
)
nodes_to_start = [
move_group_node,
planning_scene_publisher,
rviz_node,
]
return nodes_to_start

58
rbs_mill_assist/launch/simulation.launch.py
View file

@ -1,3 +1,4 @@
from ntpath import isfile
import os
import xacro
@ -64,23 +65,20 @@ def launch_setup(context, *args, **kwargs):
robot_description_semantic_content = ""
if use_moveit.perform(context) == "true":
srdf_config_file = os.path.join(
get_package_share_directory(moveit_config_package.perform(context)),
"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=" ")
)
package_dir = get_package_share_directory(moveit_config_package.perform(context))
srdf_file = os.path.join(package_dir, "srdf", moveit_config_file.perform(context))
if srdf_file.endswith(".xacro"):
srdf_config_file = os.path.join(package_dir, "srdf", "xacro_args.yaml")
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=" ")
elif srdf_file.endswith(".srdf"):
with open(srdf_file, "r") as file:
robot_description_semantic_content = file.read()
control_space = "joint"
control_strategy = "position"
interactive = "false"
@ -175,14 +173,28 @@ def launch_setup(context, *args, **kwargs):
executable="get_key_pose_frame.py"
)
get_workspace = Node(
package="rbs_mill_assist",
executable="get_workspace",
parameters=[
{
"urdf_path": os.path.join(get_package_share_directory("rbs_mill_assist"), "urdf", "current.urdf"),
"ee_link": ee_link_name,
"use_sim_time": True,
"robot_position": [0.0, 0.0, 0.0]
}
]
)
nodes_to_start = [
rbs_robot_setup,
rviz_node,
# rviz_node,
gazebo,
gazebo_spawn_robot,
gz_bridge,
grasping_service,
get_named_pose_service
get_named_pose_service,
# get_workspace
]
return nodes_to_start
@ -232,7 +244,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="rbs_arm",
default_value="rbs_mill_assist",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
)
@ -240,7 +252,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="rbs_arm.srdf.xacro",
default_value="rbs_arm.srdf",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
@ -259,7 +271,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"use_moveit", default_value="false", description="Launch moveit?"
"use_moveit", default_value="true", description="Launch moveit?"
)
)
declared_arguments.append(
@ -314,7 +326,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"interactive",
default_value="false",
default_value="true",
description="Wheter to run the motion_control_handle controller",
),
)

BIN
rbs_mill_assist/meshes/Link6.STL
View file

Binary file not shown.

BIN
rbs_mill_assist/meshes/Link7.STL
View file

Binary file not shown.

BIN
rbs_mill_assist/meshes/Link8.STL
View file

Binary file not shown.

9
rbs_mill_assist/moveit/initial_positions.yaml Normal file
View file

@ -0,0 +1,9 @@
# Default initial positions for rbs_arm's ros2_control fake system
initial_positions:
ax1: 0
ax2: 0
ax3: 0
ax4: 0
ax5: 0
ax6: 0

40
rbs_mill_assist/moveit/joint_limits.yaml Normal file
View file

@ -0,0 +1,40 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
ax1:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0
ax2:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0
ax3:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0
ax4:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0
ax5:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0
ax6:
has_velocity_limits: true
max_velocity: 10.0
has_acceleration_limits: true
max_acceleration: 10.0

4
rbs_mill_assist/moveit/kinematics.yaml Normal file
View file

@ -0,0 +1,4 @@
arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001

22
rbs_mill_assist/moveit/moveit_controllers.yaml Normal file
View file

@ -0,0 +1,22 @@
trajectory_execution:
"moveit_manage_controllers": False,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- joint_trajectory_controller
joint_trajectory_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
joints:
- ax1
- ax2
- ax3
- ax4
- ax5
- ax6

43
rbs_mill_assist/moveit/moveit_cpp.yaml Normal file
View file

@ -0,0 +1,43 @@
planning_scene_monitor_options:
name: "planning_scene_monitor"
robot_description: "robot_description"
joint_state_topic: "/joint_states"
attached_collision_object_topic: "/planning_scene_monitor"
publish_planning_scene_topic: "/publish_planning_scene"
monitored_planning_scene_topic: "/monitored_planning_scene"
wait_for_initial_state_timeout: 10.0
planning_pipelines:
pipeline_names: ["ompl", "chomp"]
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
ompl_rrtc: # Namespace for individual plan request
plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem
planning_pipeline: ompl # Name of the pipeline that is being used
planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline
max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
pilz_lin:
plan_request_params:
planning_attempts: 1
planning_pipeline: pilz_industrial_motion_planner
planner_id: "PTP"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 0.8
chomp_planner:
plan_request_params:
planning_attempts: 1
planning_pipeline: chomp
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 1.5

94
rbs_mill_assist/moveit/ompl_planning.yaml Normal file
View file

@ -0,0 +1,94 @@
planning_plugins:
- ompl_interface/OMPLPlanner
request_adapters:
- default_planning_request_adapters/ResolveConstraintFrames
- default_planning_request_adapters/ValidateWorkspaceBounds
- default_planning_request_adapters/CheckStartStateBounds
- default_planning_request_adapters/CheckStartStateCollision
response_adapters:
- default_planning_response_adapters/AddTimeOptimalParameterization
- default_planning_response_adapters/ValidateSolution
- default_planning_response_adapters/DisplayMotionPath
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
ESTkConfigDefault:
type: geometric::EST
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
LBKPIECEkConfigDefault:
type: geometric::LBKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
BKPIECEkConfigDefault:
type: geometric::BKPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
KPIECEkConfigDefault:
type: geometric::KPIECE
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
RRTkConfigDefault:
type: geometric::RRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
RRTConnectkConfigDefault:
type: geometric::RRTConnect
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
RRTstarkConfigDefault:
type: geometric::RRTstar
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
TRRTkConfigDefault:
type: geometric::TRRT
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
max_states_failed: 10 # when to start increasing temp. default: 10
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
init_temperature: 10e-6 # initial temperature. default: 10e-6
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
PRMkConfigDefault:
type: geometric::PRM
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
PRMstarkConfigDefault:
type: geometric::PRMstar
ar_manipulator:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
#projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
longest_valid_segment_fraction: 0.01
rbs_arm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault

6
rbs_mill_assist/moveit/pilz_cartesian_limits.yaml Normal file
View file

@ -0,0 +1,6 @@
# Limits for the Pilz planner
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
max_trans_dec: -5.0
max_rot_vel: 1.57

56
rbs_mill_assist/moveit/rbs_arm.ros2_control.xacro Normal file
View file

@ -0,0 +1,56 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rbs_arm_ros2_control" params="name initial_positions_file">
<xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="ax1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="ax2">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax2']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="ax3">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax3']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="ax4">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax4']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="ax5">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax5']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="ax6">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['ax6']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>

14
rbs_mill_assist/moveit/rbs_arm.urdf.xacro Normal file
View file

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rbs_arm">
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
<!-- Import rbs_arm urdf file -->
<xacro:include filename="$(find rbs_mill_assist)/urdf/current.urdf" />
<!-- Import control_xacro -->
<xacro:include filename="rbs_arm.ros2_control.xacro" />
<xacro:rbs_arm_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
</robot>

11
rbs_mill_assist/moveit/sensors_3d.yaml Normal file
View file

@ -0,0 +1,11 @@
sensors:
- default_sensor
default_sensor:
filtered_cloud_topic: ""
max_range: ""
max_update_rate: ""
padding_offset: ""
padding_scale: ""
point_cloud_topic: ""
point_subsample: ""
sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

70
rbs_mill_assist/src/CMakeLists.txt Normal file
View file

@ -0,0 +1,70 @@
# Gazebo VacuumGripper plugin
find_package(rclcpp REQUIRED)
find_package(gz-cmake3 REQUIRED)
find_package(gz-plugin2 REQUIRED COMPONENTS register)
find_package(std_srvs REQUIRED)
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
find_package(gz-common5 REQUIRED COMPONENTS profiler)
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
# Harmonic
if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
find_package(gz-sim8 REQUIRED)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Harmonic")
# Default to Garden
else()
find_package(gz-sim7 REQUIRED)
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
message(STATUS "Compiling against Gazebo Garden")
endif()
add_library(VacuumGripper
SHARED
VacuumGripper.cpp
)
target_link_libraries(VacuumGripper
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
)
ament_target_dependencies(VacuumGripper
rclcpp
std_srvs
)
install(
TARGETS VacuumGripper
DESTINATION lib/${PROJECT_NAME}
)
# GetWorkspace Node
find_package(sensor_msgs REQUIRED)
find_package(pinocchio REQUIRED)
find_package(Eigen3 REQUIRED)
add_executable(get_workspace get_workspace.cpp)
ament_target_dependencies(get_workspace rclcpp sensor_msgs pinocchio Eigen3)
target_include_directories(get_workspace PRIVATE ${Eigen3_INCLUDE_DIRS})
target_link_libraries(get_workspace Eigen3::Eigen pinocchio::pinocchio)
install(
TARGETS get_workspace
DESTINATION lib/${PROJECT_NAME}
)
# planning_scene_publisher
find_package(moveit_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(shape_msgs REQUIRED)
add_executable(planning_scene_publisher planning_scene_publisher.cpp)
ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
install(
TARGETS planning_scene_publisher
DESTINATION lib/${PROJECT_NAME}
)

112
rbs_mill_assist/src/planning_scene_publisher.cpp Normal file
View file

@ -0,0 +1,112 @@
#include "moveit_msgs/msg/planning_scene.hpp"
#include "shape_msgs/msg/mesh.hpp"
#include <chrono>
#include <functional>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <moveit_msgs/msg/planning_scene.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <shape_msgs/msg/mesh.hpp>
#include <geometric_shapes/shape_operations.h>
class PlanningScenePublisher : public rclcpp::Node {
public:
PlanningScenePublisher() : Node("planning_scene_publisher"), object_published_(false) {
planning_scene_publisher_ =
this->create_publisher<moveit_msgs::msg::PlanningScene>(
"planning_scene", 10);
timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&PlanningScenePublisher::publish_scene, this));
}
private:
rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
bool object_published_;
void publish_scene() {
if (object_published_) {
return; // Избегаем повторной публикации объекта
}
moveit_msgs::msg::PlanningScene planning_scene;
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "world";
collision_object.id = "table";
// Определяем геометрию объекта (стола)
shape_msgs::msg::SolidPrimitive table;
table.type = shape_msgs::msg::SolidPrimitive::BOX;
table.dimensions = {1.2, 0.7, 0.8}; // Длина, ширина, высота
// Задаем позу объекта
geometry_msgs::msg::Pose table_pose;
table_pose.position.x = 0.0;
table_pose.position.y = 0.0;
table_pose.position.z = -0.4;
collision_object.primitives.push_back(table);
collision_object.primitive_poses.push_back(table_pose);
collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
planning_scene.world.collision_objects.push_back(collision_object);
// Добавляем объект меша
moveit_msgs::msg::CollisionObject mesh_object;
mesh_object.header.frame_id = "world";
mesh_object.id = "mesh_object";
shape_msgs::msg::Mesh mesh;
shapes::Mesh* m = shapes::createMeshFromResource("package://rbs_mill_assist/assets/laser/meshes/laser.dae");
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(m, mesh_msg);
mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);
//0.30 0.0 0 0 0 3.14159
geometry_msgs::msg::Pose mesh_pose;
mesh_pose.position.x = 0.30;
mesh_pose.position.y = 0.0;
mesh_pose.position.z = 0.0;
mesh_pose.orientation.x = 0.0;
mesh_pose.orientation.y = 0.0;
mesh_pose.orientation.z = 1.0;
mesh_pose.orientation.w = 0.0;
moveit_msgs::msg::ObjectColor color;
color.id = mesh_object.id;
color.color.r = 0.0;
color.color.g = 1.0;
color.color.b = 0.0;
color.color.a = 1.0;
mesh_object.meshes.push_back(mesh);
mesh_object.mesh_poses.push_back(mesh_pose);
mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
// planning_scene.object_colors.push_back(color);
// planning_scene.object_colors.clear();
planning_scene.world.collision_objects.push_back(mesh_object);
planning_scene.is_diff = true;
planning_scene_publisher_->publish(planning_scene);
RCLCPP_INFO(this->get_logger(), "Published collision objects: table and mesh");
object_published_ = true; // Устанавливаем флаг, чтобы больше не публиковать объект
}
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<PlanningScenePublisher>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

61
rbs_mill_assist/srdf/rbs_arm.srdf Normal file
View file

@ -0,0 +1,61 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot name="rbs_arm">
<group name="arm">
<chain base_link="base_link" tip_link="grasp_point"/>
</group>
<group_state name="zero" group="arm">
<joint name="ax1" value="0"/>
<joint name="ax2" value="0"/>
<joint name="ax3" value="0"/>
<joint name="ax4" value="0"/>
<joint name="ax5" value="0"/>
<joint name="ax6" value="0"/>
</group_state>
<group_state name="home" group="arm">
<joint name="ax1" value="1.57"/>
<joint name="ax2" value="0.6586"/>
<joint name="ax3" value="0.8582"/>
<joint name="ax4" value="0"/>
<joint name="ax5" value="1.5215"/>
<joint name="ax6" value="0"/>
</group_state>
<disable_collisions link1="Link1" link2="Link2" reason="Adjacent"/>
<disable_collisions link1="Link1" link2="base_link" reason="Adjacent"/>
<disable_collisions link1="Link1" link2="rgbd_camera" reason="Adjacent"/>
<disable_collisions link1="Link2" link2="Link3" reason="Adjacent"/>
<disable_collisions link1="Link2" link2="Link5" reason="Never"/>
<disable_collisions link1="Link2" link2="base_link" reason="Never"/>
<disable_collisions link1="Link2" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="Link3" link2="Link4" reason="Adjacent"/>
<disable_collisions link1="Link3" link2="Link5" reason="Never"/>
<disable_collisions link1="Link3" link2="left_contact_panel" reason="Never"/>
<disable_collisions link1="Link3" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="Link4" link2="Link5" reason="Adjacent"/>
<disable_collisions link1="Link4" link2="Link6" reason="Never"/>
<disable_collisions link1="Link4" link2="Link7" reason="Never"/>
<disable_collisions link1="Link4" link2="Link8" reason="Never"/>
<disable_collisions link1="Link4" link2="left_contact_panel" reason="Never"/>
<disable_collisions link1="Link4" link2="right_contact_panel" reason="Never"/>
<disable_collisions link1="Link5" link2="Link6" reason="Adjacent"/>
<disable_collisions link1="Link5" link2="Link7" reason="Never"/>
<disable_collisions link1="Link5" link2="Link8" reason="Never"/>
<disable_collisions link1="Link5" link2="left_contact_panel" reason="Never"/>
<disable_collisions link1="Link5" link2="right_contact_panel" reason="Never"/>
<disable_collisions link1="Link6" link2="Link7" reason="Adjacent"/>
<disable_collisions link1="Link6" link2="Link8" reason="Adjacent"/>
<disable_collisions link1="Link6" link2="left_contact_panel" reason="Never"/>
<disable_collisions link1="Link6" link2="right_contact_panel" reason="Never"/>
<disable_collisions link1="Link7" link2="Link8" reason="Never"/>
<disable_collisions link1="Link7" link2="left_contact_panel" reason="Adjacent"/>
<disable_collisions link1="Link7" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="Link7" link2="right_contact_panel" reason="Never"/>
<disable_collisions link1="Link8" link2="left_contact_panel" reason="Never"/>
<disable_collisions link1="Link8" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="Link8" link2="right_contact_panel" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="left_contact_panel" link2="rgbd_camera" reason="Never"/>
<disable_collisions link1="left_contact_panel" link2="right_contact_panel" reason="Never"/>
<disable_collisions link1="rgbd_camera" link2="right_contact_panel" reason="Never"/>
</robot>

228
rbs_mill_assist/urdf/current.urdf
View file

@ -5,32 +5,8 @@
<!-- =================================================================================== -->
<robot name="rbs_arm">
<link name="world"/>
<link name="machine_link">
<visual>
<geometry>
<box size="0.4 0.5 0.115"/>
</geometry>
<material name="gray">
<color rgba="0 1 0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.4 0.5 0.115"/>
</geometry>
</collision>
<inertial>
<mass value="40.0"/>
<inertia ixx="0.6580625" ixy="0" ixz="0" iyy="0.4330625" iyz="0" izz="1.0250000000000001"/>
</inertial>
</link>
<joint name="machine_joint" type="fixed">
<parent link="world"/>
<child link="machine_link"/>
<origin rpy="0 0 0" xyz="0.25 0 0.0575"/>
</joint>
<joint name="parent_connection" type="fixed">
<origin rpy="0.0 0.0 0.0" xyz="-0.10 0.0 0.03"/>
<origin rpy="0.0 0.0 0.0" xyz="-0.20 0.0 0.0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
@ -83,7 +59,7 @@
<parent link="base_link"/>
<child link="Link1"/>
<axis xyz="0 0 1"/>
<limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
<limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
</joint>
<link name="Link2">
<inertial>
@ -111,8 +87,8 @@
<origin rpy="0 0 0" xyz="0 0 0.15465"/>
<parent link="Link1"/>
<child link="Link2"/>
<axis xyz="-1 0 0"/>
<limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
<axis xyz="1 0 0"/>
<limit effort="30" lower="-2.2689" upper="1.7453" velocity="10"/>
</joint>
<link name="Link3">
<inertial>
@ -141,7 +117,7 @@
<parent link="Link2"/>
<child link="Link3"/>
<axis xyz="1 0 0"/>
<limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
<limit effort="30" lower="-2.2689" upper="1.7453" velocity="10"/>
</joint>
<link name="Link4">
<inertial>
@ -169,8 +145,8 @@
<origin rpy="0 0 0" xyz="0 0 0.098"/>
<parent link="Link3"/>
<child link="Link4"/>
<axis xyz="0 0 -1"/>
<limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
<axis xyz="0 0 1"/>
<limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
</joint>
<link name="Link5">
<inertial>
@ -199,13 +175,13 @@
<parent link="Link4"/>
<child link="Link5"/>
<axis xyz="1 0 0"/>
<limit effort="30" lower="-1.57" upper="1.57" velocity="10"/>
<limit effort="30" lower="-1.7453" upper="2.0944" velocity="10"/>
</joint>
<link name="Link6">
<inertial>
<origin rpy="0 0 0" xyz="2.3821E-11 -0.054292 0.018171"/>
<mass value="0.41222"/>
<inertia ixx="0.00057892" ixy="-5.0591E-13" ixz="1.1462E-13" iyy="8.7903E-05" iyz="5.3383E-05" izz="0.00063814"/>
<origin rpy="0 0 0" xyz="1.11237115621161E-06 0.105319882001022 0.0151262038950002"/>
<mass value="0.280654829111196"/>
<inertia ixx="0.00166005184865464" ixy="-2.80959481690827E-08" ixz="-7.84445697318557E-10" iyy="2.87495064989592E-05" iyz="-7.18334236975057E-05" izz="0.00166929050771312"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
@ -213,7 +189,7 @@
<mesh filename="file:///home/narenmak/rbs_ws/install/rbs_mill_assist/share/rbs_mill_assist/meshes/Link6.STL"/>
</geometry>
<material name="">
<color rgba="0.75294 0.75294 0.75294 1"/>
<color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
</material>
</visual>
<collision>
@ -228,13 +204,13 @@
<parent link="Link5"/>
<child link="Link6"/>
<axis xyz="0 0 1"/>
<limit effort="30" lower="-3.14159" upper="3.14159" velocity="10"/>
<limit effort="30" lower="-6.2832" upper="6.2832" velocity="10"/>
</joint>
<link name="Link7">
<inertial>
<origin rpy="0 0 0" xyz="3.46944695195361E-18 -5.55111512312578E-17 -0.00360613836994461"/>
<origin rpy="0 0 0" xyz="8.67361737988404E-18 0 -0.0036061383699445"/>
<mass value="0.0198935071603267"/>
<inertia ixx="1.01905343575873E-06" ixy="3.76158192263132E-37" ixz="-3.1449831265618E-23" iyy="1.01905343575873E-06" iyz="6.29843212132371E-23" izz="1.43041044988815E-06"/>
<inertia ixx="1.01905343575873E-06" ixy="1.40555551021685E-36" ixz="-7.39766847562968E-22" iyy="1.01905343575873E-06" iyz="-2.63361925044231E-22" izz="1.43041044988815E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
@ -253,16 +229,16 @@
</collision>
</link>
<joint name="Right_point" type="fixed">
<origin rpy="0 0 0" xyz="0.015 -0.095 0.05"/>
<origin rpy="0 0 0" xyz="0.015 0.195 0.05"/>
<parent link="Link6"/>
<child link="Link7"/>
<axis xyz="0 0 0"/>
</joint>
<link name="Link8">
<inertial>
<origin rpy="0 0 0" xyz="0 -2.77555756156289E-17 -0.00360613836994461"/>
<origin rpy="0 0 0" xyz="1.04083408558608E-17 0 -0.0036061383699445"/>
<mass value="0.0198935071603267"/>
<inertia ixx="1.01905343575873E-06" ixy="0" ixz="-2.89418945570099E-23" iyy="1.01905343575872E-06" iyz="-1.78775100697742E-23" izz="1.43041044988815E-06"/>
<inertia ixx="1.01905343575873E-06" ixy="1.10608362843633E-36" ixz="-7.54785908468325E-22" iyy="1.01905343575873E-06" iyz="-2.63361925044231E-22" izz="1.43041044988815E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
@ -281,11 +257,84 @@
</collision>
</link>
<joint name="Left_point" type="fixed">
<origin rpy="0 0 0" xyz="-0.015 -0.095 0.05"/>
<origin rpy="0 0 0" xyz="-0.015 0.195 0.05"/>
<parent link="Link6"/>
<child link="Link8"/>
<axis xyz="0 0 0"/>
</joint>
<link name="grasp_point"/>
<joint name="grasp_point_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.195 0.05"/>
<parent link="Link6"/>
<child link="grasp_point"/>
<axis xyz="0 0 0"/>
</joint>
<link name="right_contact_panel">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0198935071603267"/>
<inertia ixx="1.674370185994164e-07" ixy="0" ixz="0" iyy="1.674370185994164e-07" iyz="0" izz="3.315584526721117e-07"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.001"/>
</geometry>
<material name="">
<color rgba="1 1 1 0.4"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.001"/>
</geometry>
</collision>
</link>
<link name="left_contact_panel">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0198935071603267"/>
<inertia ixx="1.674370185994164e-07" ixy="0" ixz="0" iyy="1.674370185994164e-07" iyz="0" izz="3.315584526721117e-07"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.001"/>
</geometry>
<material name="">
<color rgba="1 1 1 0.4"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.001"/>
</geometry>
</collision>
</link>
<joint name="right_contact_panel_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="Link8"/>
<child link="right_contact_panel"/>
</joint>
<joint name="left_contact_panel_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="Link7"/>
<child link="left_contact_panel"/>
</joint>
<gazebo reference="Right_point">
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
<gazebo reference="Left_point">
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
<gazebo reference="right_contact_panel_joint">
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
<gazebo reference="left_contact_panel_joint">
<preserveFixedJoint>true</preserveFixedJoint>
</gazebo>
<ros2_control name="ax1_hardware_interface" type="actuator">
<hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
@ -293,10 +342,6 @@
<joint name="ax1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
@ -312,13 +357,9 @@
<joint name="ax2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
<param name="initial_value">0.85</param>
</state_interface>
<state_interface name="velocity"/>
<!-- <state_interface name="acceleration"/> -->
@ -331,13 +372,9 @@
<joint name="ax3">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
<param name="initial_value">1.0</param>
</state_interface>
<state_interface name="velocity"/>
<!-- <state_interface name="acceleration"/> -->
@ -350,10 +387,6 @@
<joint name="ax4">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
@ -369,13 +402,9 @@
<joint name="ax5">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
<param name="initial_value">1.0</param>
</state_interface>
<state_interface name="velocity"/>
<!-- <state_interface name="acceleration"/> -->
@ -388,10 +417,6 @@
<joint name="ax6">
<command_interface name="position"/>
<command_interface name="velocity"/>
<!-- WARN When this active a robot falls down -->
<!-- <command_interface name="effort"/> -->
<!-- <param name="p">${p}</param> -->
<!-- <param name="d">${d}</param> -->
<!-- <command_interface name="effort"/> -->
<state_interface name="position">
<param name="initial_value">0.0</param>
@ -401,6 +426,54 @@
</joint>
</ros2_control>
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
<link name="rgbd_camera">
<inertial>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<mass value="0.1"/>
<inertia ixx="0.000166667" ixy="0.0" ixz="0.0" iyy="0.000166667" iyz="0.0" izz="0.000166667"/>
</inertial>
<visual name="">
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<material name="">
<color rgba="0.0 0.0 0.0 1.0"/>
<texture filename=""/>
</material>
</visual>
<collision>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<gazebo reference="rgbd_camera">
<sensor name="rgbd_camera" type="rgbd_camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>1920</width>
<height>1080</height>
</image>
<clip>
<near>0.1</near>
<far>3</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<topic>rgbd_camera</topic>
<enable_metrics>true</enable_metrics>
</sensor>
</gazebo>
<joint name="rgbd_camera_to_parent" type="fixed">
<parent link="Link1"/>
<child link="rgbd_camera"/>
<origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"/>
</joint>
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters></parameters>
@ -409,13 +482,20 @@
</ros>
</plugin>
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
<link_name>Link6</link_name>
<link_name>Link8</link_name>
</plugin>
</gazebo>
<gazebo reference="Link6">
<sensor name="sensor_contact" type="contact">
<gazebo reference="left_contact_panel">
<sensor name="left_sensor_contact" type="contact">
<contact>
<collision>Link6_collision</collision>
<collision>left_contact_panel_collision</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="right_contact_panel">
<sensor name="right_sensor_contact" type="contact">
<contact>
<collision>right_contact_panel_collision</collision>
</contact>
</sensor>
</gazebo>

2
rbs_mill_assist/urdf/rbs_arm.xacro
View file

@ -6,7 +6,7 @@
<xacro:arg name="tf_prefix" default="" />
<xacro:arg name="namespace" default="arm0"/>
<xacro:arg name="x" default="-0.10" />
<xacro:arg name="x" default="-0.20" />
<xacro:arg name="y" default="0.0" />
<xacro:arg name="z" default="0.0" />

198
rbs_mill_assist/urdf/rbs_arm_macro.xacro
View file

@ -342,45 +342,45 @@
velocity="10" />
</joint>
<link
name="Link6">
<inertial>
<origin
xyz="2.3821E-11 -0.054292 0.018171"
rpy="0 0 0" />
<mass
value="0.41222" />
<inertia
ixx="0.00057892"
ixy="-5.0591E-13"
ixz="1.1462E-13"
iyy="8.7903E-05"
iyz="5.3383E-05"
izz="0.00063814" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
</geometry>
<material
name="">
<color
rgba="0.75294 0.75294 0.75294 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
</geometry>
</collision>
</link>
name="Link6">
<inertial>
<origin
xyz="1.11237115621161E-06 0.105319882001022 0.0151262038950002"
rpy="0 0 0" />
<mass
value="0.280654829111196" />
<inertia
ixx="0.00166005184865464"
ixy="-2.80959481690827E-08"
ixz="-7.84445697318557E-10"
iyy="2.87495064989592E-05"
iyz="-7.18334236975057E-05"
izz="0.00166929050771312" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link6.STL" />
</geometry>
</collision>
</link>
<joint
name="ax6"
type="revolute">
@ -403,16 +403,16 @@
name="Link7">
<inertial>
<origin
xyz="3.46944695195361E-18 -5.55111512312578E-17 -0.00360613836994461"
xyz="8.67361737988404E-18 0 -0.0036061383699445"
rpy="0 0 0" />
<mass
value="0.0198935071603267" />
<inertia
ixx="1.01905343575873E-06"
ixy="3.76158192263132E-37"
ixz="-3.1449831265618E-23"
ixy="1.40555551021685E-36"
ixz="-7.39766847562968E-22"
iyy="1.01905343575873E-06"
iyz="6.29843212132371E-23"
iyz="-2.63361925044231E-22"
izz="1.43041044988815E-06" />
</inertial>
<visual>
@ -443,7 +443,7 @@
name="Right_point"
type="fixed">
<origin
xyz="0.015 -0.095 0.05"
xyz="0.015 0.195 0.05"
rpy="0 0 0" />
<parent
link="Link6" />
@ -453,64 +453,64 @@
xyz="0 0 0" />
</joint>
<link
name="Link8">
<inertial>
<origin
xyz="0 -2.77555756156289E-17 -0.00360613836994461"
rpy="0 0 0" />
<mass
value="0.0198935071603267" />
<inertia
ixx="1.01905343575873E-06"
ixy="0"
ixz="-2.89418945570099E-23"
iyy="1.01905343575872E-06"
iyz="-1.78775100697742E-23"
izz="1.43041044988815E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 0.4" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
</geometry>
</collision>
</link>
<joint
name="Left_point"
type="fixed">
name="Link8">
<inertial>
<origin
xyz="-0.015 -0.095 0.05"
xyz="1.04083408558608E-17 0 -0.0036061383699445"
rpy="0 0 0" />
<parent
link="Link6" />
<child
link="Link8" />
<axis
xyz="0 0 0" />
</joint>
<mass
value="0.0198935071603267" />
<inertia
ixx="1.01905343575873E-06"
ixy="1.10608362843633E-36"
ixz="-7.54785908468325E-22"
iyy="1.01905343575873E-06"
iyz="-2.63361925044231E-22"
izz="1.43041044988815E-06" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 0.4" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="file://$(find rbs_mill_assist)/meshes/Link8.STL" />
</geometry>
</collision>
</link>
<joint
name="Left_point"
type="fixed">
<origin
xyz="-0.015 0.195 0.05"
rpy="0 0 0" />
<parent
link="Link6" />
<child
link="Link8" />
<axis
xyz="0 0 0" />
</joint>
<link name="grasp_point" />
<joint
name="grasp_point_joint"
type="fixed">
<origin
xyz="0.0 -0.095 0.05"
xyz="0.0 0.195 0.05"
rpy="0 0 0" />
<parent
link="Link6" />
@ -616,15 +616,15 @@
<xacro:joint_hardware joint_name="ax3" hardware="${hardware}" initial_joint_position="1.0" />
<xacro:joint_hardware joint_name="ax4" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:joint_hardware joint_name="ax5" hardware="${hardware}" initial_joint_position="1.0" />
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="3.14159" />
<xacro:joint_hardware joint_name="ax6" hardware="${hardware}" initial_joint_position="0.0" />
<xacro:if value="${hardware=='gazebo'}">
<!-- <xacro:fts link="tool0" name="fts_sensor" tf_prefix="${tf_prefix}"></xacro:fts> -->
<xacro:rgbd parent="Link1" tf_prefix="${tf_prefix}">
<origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin>
</xacro:rgbd>
<!-- <xacro:rgbd parent="world" tf_prefix="${tf_prefix}"> -->
<!-- <origin rpy="0.0 0.0 -1.57" xyz="0.0 -0.035 0.03"></origin> -->
<!-- </xacro:rgbd> -->
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(arg simulation_controllers)</parameters>

97
rbs_mill_assist/world/default.sdf
View file

@ -56,33 +56,33 @@
<include>
<name>shildik_0</name>
<pose>0.0 -0.28 0.0 0 0 3.14159</pose>
<pose>-0.5 0.0 0.0 0 0 1.57</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_1</name>
<pose>0.0 -0.28 0.01 0 0 3.14159</pose>
<pose>-0.5 0.0 0.001 0 0 1.57</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_2</name>
<pose>0.0 -0.28 0.02 0 0 3.14159</pose>
<pose>-0.5 0.0 0.002 0 0 1.57</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_3</name>
<pose>0.0 -0.28 0.03 0 0 3.14159</pose>
<pose>-0.5 0.0 0.003 0 0 1.57</pose>
<uri>model://shildik</uri>
</include>
<include>
<name>shildik_4</name>
<pose>0.0 -0.28 0.04 0 0 3.14159</pose>
<pose>-0.5 0.0 0.004 0 0 1.57</pose>
<uri>model://shildik</uri>
</include>
<include>
<static>true</static>
<pose>0.30 0.0 0 0 0 -1.57</pose>
<pose>0.30 0.0 0 0 0 3.14159</pose>
<uri>model://laser</uri>
</include>
@ -92,32 +92,63 @@
<!-- <uri>model://bunker</uri> -->
<!-- </include> -->
<!-- <model name="ground_plane"> -->
<!-- <static>true</static> -->
<!-- <link name="link"> -->
<!-- <collision name="collision"> -->
<!-- <geometry> -->
<!-- <plane> -->
<!-- <normal>0 0 1</normal> -->
<!-- <size>1 1</size> -->
<!-- </plane> -->
<!-- </geometry> -->
<!-- </collision> -->
<!-- <visual name="visual"> -->
<!-- <geometry> -->
<!-- <plane> -->
<!-- <normal>0 0 1</normal> -->
<!-- <size>1 1</size> -->
<!-- </plane> -->
<!-- </geometry> -->
<!-- <material> -->
<!-- <ambient>0.8 0.8 0.8 1</ambient> -->
<!-- <diffuse>0.8 0.8 0.8 1</diffuse> -->
<!-- <specular>0.8 0.8 0.8 1</specular> -->
<!-- </material> -->
<!-- </visual> -->
<!-- </link> -->
<!-- </model> -->
<model name="rgbd_camera">
<pose>0 0 0 0 0 0</pose>
<static>1</static>
<link name="rgbd_camera">
<!-- Visual -->
<visual name="visual">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
<material>
<ambient>0 0 0 1</ambient>
<diffuse>0 0 0 1</diffuse>
<specular>0 0 0 1</specular>
</material>
</visual>
<!-- Collision -->
<collision name="collision">
<pose>0 0 0 0 0 0</pose>
<geometry>
<box>
<size>0.01 0.01 0.01</size>
</box>
</geometry>
</collision>
<!-- RGBD Sensor -->
<sensor name="rgbd_camera" type="camera">
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>1920</width>
<height>1080</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>3</far>
</clip>
<depth_camera>
<output>depth</output>
</depth_camera>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<topic>/rgbd_camera/image</topic>
<enable_metrics>true</enable_metrics>
</sensor>
</link>
</model>
<model name="table">
<static>1</static>
<link name='table_link'>
@ -166,5 +197,7 @@
</model>
</world>
</sdf>