diff --git a/.gitignore b/.gitignore index 333df5bf..848e7995 100644 --- a/.gitignore +++ b/.gitignore @@ -231,4 +231,5 @@ CATKIN_IGNORE /**/local_config.py -*.graphml \ No newline at end of file +*.graphml +unilabos/device_mesh/view_robot.rviz diff --git a/test/experiments/plr_test_converted.json b/test/experiments/plr_test_converted.json index 9e502e10..d2b3397c 100644 --- a/test/experiments/plr_test_converted.json +++ b/test/experiments/plr_test_converted.json @@ -163,7 +163,7 @@ "type": "plate", "class": "opentrons_96_filtertiprack_1000ul", "position": { - "x": 0, + "x": 265.0, "y": 0, "z": 69 }, @@ -5762,8 +5762,8 @@ "type": "plate", "class": "nest_96_wellplate_2ml_deep", "position": { - "x": 265.0, - "y": 0, + "x": 0, + "y": 90.5, "z": 69 }, "config": { @@ -9623,6 +9623,65 @@ "pending_liquids": [], "liquid_history": [] } + }, + { + "id": "benyao", + "name": "benyao", + "children": [ + ], + "parent": null, + "type": "device", + "class": "moveit.benyao_arm", + "position": { + "x": -500, + "y": 1000, + "z": -100 + }, + "config": { + "moveit_type": "benyao_arm", + "joint_poses": { + "arm": { + "hotel_1": [1.05, 0.568, -1.0821, 0.0, 1.0821], + "home": [0.865, 0.09, 0.8727, 0.0, -0.8727] + } + }, + "rotation": { + "x": 0, + "y": 0, + "z": -1.5708, + "type": "Rotation" + }, + "device_config": { + } + }, + "data": { + } + }, + { + "id": "hotel", + "name": "hotel", + "children": [ + ], + "parent": null, + "type": "device", + "class": "hotel.thermo_orbitor_rs2_hotel", + "position": { + "x": 0, + "y": -700, + "z": -10 + }, + "config": { + "rotation": { + "x": 0, + "y": 0, + "z": 0, + "type": "Rotation" + }, + "device_config": { + } + }, + "data": { + } } ], "links": [] diff --git a/test/experiments/test_moveit.json b/test/experiments/test_moveit.json new file mode 100644 index 00000000..2859dc3a --- /dev/null +++ b/test/experiments/test_moveit.json @@ -0,0 +1,35 @@ +{ + "nodes": [ + + { + "id": "benyao", + "name": "benyao", + "children": [ + ], + "parent": null, + "type": "device", + "class": "moveit.benyao_arm", + "position": { + "x": 0, + "y": 0, + "z": 0 + }, + "config": { + "moveit_type": "benyao_arm", + "joint_poses": { + "arm": { + "home": [0.0, 0.2, 0.0, 0.0, 0.0], + "pick": [1.2, 0.0, 0.0, 0.0, 0.0] + } + }, + "device_config": { + } + }, + "data": { + } + } + ], + "links": [ + + ] +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml b/unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml new file mode 100644 index 00000000..94fb9f55 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml @@ -0,0 +1,9 @@ +# Default initial positions for full_dev's ros2_control fake system + +initial_positions: + arm_base_joint: 0 + arm_link_1_joint: 0 + arm_link_2_joint: 0 + arm_link_3_joint: 0 + gripper_base_joint: 0 + gripper_right_joint: 0.03 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml b/unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml new file mode 100644 index 00000000..d4dffc37 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml @@ -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: + arm_base_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + arm_link_1_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + arm_link_2_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + arm_link_3_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + gripper_base_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + gripper_right_joint: + has_velocity_limits: true + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml b/unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml new file mode 100644 index 00000000..c9a5d608 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 diff --git a/unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro b/unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro new file mode 100644 index 00000000..70f8634f --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro @@ -0,0 +1,56 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['arm_base_joint']} + + + + + + + ${initial_positions['arm_link_1_joint']} + + + + + + + ${initial_positions['arm_link_2_joint']} + + + + + + + ${initial_positions['arm_link_3_joint']} + + + + + + + ${initial_positions['gripper_base_joint']} + + + + + + + ${initial_positions['gripper_right_joint']} + + + + + + + diff --git a/unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro b/unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro new file mode 100644 index 00000000..aeb56777 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/benyao_arm/config/move_group.json b/unilabos/device_mesh/devices/benyao_arm/config/move_group.json new file mode 100644 index 00000000..5be9ad34 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/move_group.json @@ -0,0 +1,14 @@ +{ + "arm": + { + "joint_names": [ + "arm_base_joint", + "arm_link_1_joint", + "arm_link_2_joint", + "arm_link_3_joint", + "gripper_base_joint" + ], + "base_link_name": "device_link", + "end_effector_name": "gripper_base" + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml b/unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml new file mode 100644 index 00000000..70a1b55d --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml @@ -0,0 +1,29 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - arm_controller + - gripper_controller + + arm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - arm_base_joint + - arm_link_1_joint + - arm_link_2_joint + - arm_link_3_joint + - gripper_base_joint + action_ns: follow_joint_trajectory + default: true + gripper_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - gripper_right_joint + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml b/unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml new file mode 100644 index 00000000..8560e1cb --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml @@ -0,0 +1,2 @@ +planner_configs: + - ompl_interface/OMPLPlanner \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml b/unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml @@ -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 diff --git a/unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml b/unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml new file mode 100644 index 00000000..9c68cbf1 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml @@ -0,0 +1,39 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +arm_controller: + ros__parameters: + joints: + - arm_base_joint + - arm_link_1_joint + - arm_link_2_joint + - arm_link_3_joint + - gripper_base_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + +gripper_controller: + ros__parameters: + joints: + - gripper_right_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml b/unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml new file mode 100644 index 00000000..b1412620 --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml @@ -0,0 +1,44 @@ +joint_limits: + + arm_base_joint: + effort: 50 + velocity: 1.0 + lower: 0 + upper: 1.5 + + arm_link_1_joint: + effort: 50 + velocity: 1.0 + lower: 0 + upper: 0.6 + + arm_link_2_joint: + effort: 50 + velocity: 1.0 + lower: !degrees -95 + upper: !degrees 95 + + arm_link_3_joint: + effort: 50 + velocity: 1.0 + lower: !degrees -195 + upper: !degrees 195 + + gripper_base_joint: + effort: 50 + velocity: 1.0 + lower: !degrees -95 + upper: !degrees 95 + + + gripper_right_joint: + effort: 50 + velocity: 1.0 + lower: 0 + upper: 0.03 + + gripper_left_joint: + effort: 50 + velocity: 1.0 + lower: 0 + upper: 0.03 diff --git a/unilabos/device_mesh/devices/benyao_arm/macro_device.xacro b/unilabos/device_mesh/devices/benyao_arm/macro_device.xacro new file mode 100644 index 00000000..137c916d --- /dev/null +++ b/unilabos/device_mesh/devices/benyao_arm/macro_device.xacro @@ -0,0 +1,293 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL new file mode 100644 index 00000000..804e1c69 Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL new file mode 100644 index 00000000..dd1d7c14 Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL new file mode 100644 index 00000000..6042fa0f Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL new file mode 100644 index 00000000..e8510fbc Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL new file mode 100644 index 00000000..65737ab5 Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL new file mode 100644 index 00000000..5de88d0c Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL new file mode 100644 index 00000000..0a5fd526 Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL differ diff --git a/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL new file mode 100644 index 00000000..0c5ac69f Binary files /dev/null and b/unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro index 4e660557..f76f654f 100644 --- a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro +++ b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro @@ -3,11 +3,10 @@ - + params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0 mesh_path:=''"> - + diff --git a/unilabos/device_mesh/devices/slide_w140/macro_device.xacro b/unilabos/device_mesh/devices/slide_w140/macro_device.xacro index 7e43242e..d6d93f96 100644 --- a/unilabos/device_mesh/devices/slide_w140/macro_device.xacro +++ b/unilabos/device_mesh/devices/slide_w140/macro_device.xacro @@ -8,11 +8,11 @@ For more information, please see http://wiki.ros.org/sw_urdf_exporter --> - + - + diff --git a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png new file mode 100644 index 00000000..daecc639 Binary files /dev/null and b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png differ diff --git a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro new file mode 100644 index 00000000..52c9536e --- /dev/null +++ b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb new file mode 100644 index 00000000..724b85f7 Binary files /dev/null and b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb differ diff --git a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl new file mode 100644 index 00000000..59cb9084 Binary files /dev/null and b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl differ diff --git a/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json new file mode 100644 index 00000000..b8f35cb5 --- /dev/null +++ b/unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json @@ -0,0 +1,10 @@ +{ + "fileName": "thermo_orbitor_rs2_hotel", + "related": [ + "thermo_skyline_stacker", + "thermo_cytomat2c_stacker_15", + "thermo_fisher_cytomat_stacker_16", + "thermo_cytomat2c_stacker_21", + "thermo_orbitor_rs2_hotel" + ] +} diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro b/unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro new file mode 100644 index 00000000..29bd48de --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml new file mode 100644 index 00000000..d44ccee9 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml @@ -0,0 +1,6 @@ +# Default initial positions for full_dev's ros2_control fake system + +initial_positions: + slider1_joint: 0 + slider2_joint: 0 + slider3_joint: 0 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml new file mode 100644 index 00000000..251b26c1 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml @@ -0,0 +1,25 @@ +# 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: + slider1_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + slider2_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + slider3_joint: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml new file mode 100644 index 00000000..d11fa7bf --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml @@ -0,0 +1,4 @@ +toyo_xyz: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro b/unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro new file mode 100644 index 00000000..5bc3404d --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro @@ -0,0 +1,35 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['slider1_joint']} + + + + + + + ${initial_positions['slider2_joint']} + + + + + + + ${initial_positions['slider3_joint']} + + + + + + + diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro b/unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro new file mode 100644 index 00000000..753382fe --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/move_group.json b/unilabos/device_mesh/devices/toyo_xyz/config/move_group.json new file mode 100644 index 00000000..d9006744 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/move_group.json @@ -0,0 +1,12 @@ +{ + "toyo_xyz": + { + "joint_names": [ + "slider1_joint", + "slider2_joint", + "slider3_joint" + ], + "base_link_name": "device_link", + "end_effector_name": "slider3_link" + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml new file mode 100644 index 00000000..0fcac512 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml @@ -0,0 +1,16 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - toyo_xyz_controller + + toyo_xyz_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - slider1_joint + - slider2_joint + - slider3_joint \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml @@ -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 diff --git a/unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml b/unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml new file mode 100644 index 00000000..1d1a552e --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml @@ -0,0 +1,34 @@ +# This config file is used by ros2_control +controller_manager: + + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + # action_ns: $(var device_id) + toyo_xyz_controller: + type: joint_trajectory_controller/JointTrajectoryController + +# joint_state_broadcaster: +# ros__parameters: {} + +toyo_xyz_controller: + ros__parameters: + joints: + - slider1_joint + - slider2_joint + - slider3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + allow_partial_joints_goal: false + open_loop_control: true + allow_integration_in_goal_trajectories: true + action_monitor_rate: 20.0 + # goal_time: 0.0 + # constraints: + # stopped_velocity_tolerance: 0.01 + # goal_time: 0.0 \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/joint_config.json b/unilabos/device_mesh/devices/toyo_xyz/joint_config.json new file mode 100644 index 00000000..26bda262 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/joint_config.json @@ -0,0 +1,14 @@ +{ + "slider1_joint": { + "child":"slider1_link", + "axis" : "x" + }, + "slider2_joint": { + "child":"slider2_link", + "axis" : "x" + }, + "slider3_joint": { + "child":"slider3_link", + "axis" : "x" + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro b/unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro new file mode 100644 index 00000000..5694c353 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro @@ -0,0 +1,465 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL new file mode 100755 index 00000000..ded0641d Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx new file mode 100644 index 00000000..a278cc67 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL new file mode 100755 index 00000000..d1dba510 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx new file mode 100644 index 00000000..8c28d394 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL new file mode 100755 index 00000000..24205e50 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx new file mode 100644 index 00000000..5a243395 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL new file mode 100755 index 00000000..fcbfdce5 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx new file mode 100644 index 00000000..8bd35dbf Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL new file mode 100755 index 00000000..1b72d5db Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx new file mode 100644 index 00000000..01079615 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL new file mode 100755 index 00000000..ce0dd9f1 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx new file mode 100644 index 00000000..f0e55b56 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL new file mode 100755 index 00000000..6f5e5044 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx new file mode 100644 index 00000000..4ba363c7 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL new file mode 100755 index 00000000..7a064ccf Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx new file mode 100644 index 00000000..ea46d658 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL new file mode 100755 index 00000000..9eb85cdc Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx new file mode 100644 index 00000000..5375fcec Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL new file mode 100755 index 00000000..3ffad0f2 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx new file mode 100644 index 00000000..f94c5a6d Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL new file mode 100755 index 00000000..1afcdd2c Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx new file mode 100644 index 00000000..a812b4c5 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL new file mode 100755 index 00000000..9ef91e28 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx new file mode 100644 index 00000000..e2a7c861 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL new file mode 100755 index 00000000..abd8c568 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx new file mode 100644 index 00000000..a46df101 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL new file mode 100755 index 00000000..a0bfb168 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx new file mode 100644 index 00000000..6cec9641 Binary files /dev/null and b/unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx differ diff --git a/unilabos/device_mesh/devices/toyo_xyz/param_config.json b/unilabos/device_mesh/devices/toyo_xyz/param_config.json new file mode 100644 index 00000000..457332b6 --- /dev/null +++ b/unilabos/device_mesh/devices/toyo_xyz/param_config.json @@ -0,0 +1,20 @@ +{ + "private_param": + { + "min_d1": 0.01 , + "max_d1": 0.01 , + "slider_d1": 0.135, + "min_d2": 0.01 , + "max_d2": 0.01 , + "slider_d2": 0.116, + "min_d3": 0.01 , + "max_d3": 0.01 , + "slider_d3": 0.09 + }, + "public_param": + { + "length1" :0.5, + "length2" :0.2, + "length3" :0.2 + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py index b840789f..dab18f18 100644 --- a/unilabos/device_mesh/resource_visalization.py +++ b/unilabos/device_mesh/resource_visalization.py @@ -1,13 +1,38 @@ +import json import os from pathlib import Path +import re + +import yaml from launch import LaunchService from launch import LaunchDescription from launch_ros.actions import Node as nd import xacro from lxml import etree - +from launch_param_builder import load_yaml +from launch_ros.parameter_descriptions import ParameterFile from unilabos.registry.registry import lab_registry +from ament_index_python.packages import get_package_share_directory +def get_pattern_matches(folder, pattern): + """Given all the files in the folder, find those that match the pattern. + + If there are groups defined, the groups are returned. Otherwise the path to the matches are returned. + """ + matches = [] + if not folder.exists(): + return matches + for child in folder.iterdir(): + if not child.is_file(): + continue + m = pattern.search(child.name) + if m: + groups = m.groups() + if groups: + matches.append(groups[0]) + else: + matches.append(child) + return matches class ResourceVisualization: def __init__(self, device: dict, resource: dict, enable_rviz: bool = True): @@ -31,9 +56,8 @@ class ResourceVisualization: self.enable_rviz = enable_rviz registry = lab_registry - self.srdf_str = ''' - - + self.srdf_str = ''' + ''' @@ -43,23 +67,46 @@ class ResourceVisualization: ''' self.root = etree.fromstring(self.robot_state_str) + self.root_srdf = etree.fromstring(self.srdf_str) xacro_uri = self.root.nsmap["xacro"] + self.moveit_nodes = {} + self.moveit_nodes_kinematics = {} + self.moveit_controllers_yaml = { + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + "moveit_simple_controller_manager": { + "controller_names": [] + } + } + self.ros2_controllers_yaml = { + "controller_manager": { + "ros__parameters": { + "update_rate": 100, + "joint_state_broadcaster": { + "type": "joint_state_broadcaster/JointStateBroadcaster", + } + } + } + } + # 遍历设备节点 for node in device.values(): - if node['type'] == 'device' and node['class'] != '': - device_class = node['class'] - # 检查设备类型是否在注册表中 - if device_class not in registry.device_type_registry.keys(): - raise ValueError(f"设备类型 {device_class} 未在注册表中注册") - elif node['type'] in self.resource_type: - # print(registry.resource_type_registry) - resource_class = node['class'] - if resource_class not in registry.resource_type_registry.keys(): - raise ValueError(f"资源类型 {resource_class} 未在注册表中注册") - elif "model" in registry.resource_type_registry[resource_class].keys(): - model_config = registry.resource_type_registry[resource_class]['model'] + if node['type'] in self.resource_type or (node['type'] == 'device' and node['class'] != ''): + model_config = {} + if node['type'] in self.resource_type: + resource_class = node['class'] + if resource_class not in registry.resource_type_registry.keys(): + raise ValueError(f"资源类型 {resource_class} 未在注册表中注册") + elif "model" in registry.resource_type_registry[resource_class].keys(): + model_config = registry.resource_type_registry[resource_class]['model'] + elif node['type'] == 'device' and node['class'] != '': + device_class = node['class'] + if device_class not in registry.device_type_registry.keys(): + raise ValueError(f"设备类型 {device_class} 未在注册表中注册") + elif "model" in registry.device_type_registry[device_class].keys(): + model_config = registry.device_type_registry[device_class]['model'] + if model_config: if model_config['type'] == 'resource': self.resource_model[node['id']] = { 'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}", @@ -71,18 +118,40 @@ class ResourceVisualization: 'mesh_tf': model_config['children_mesh_tf'] } elif model_config['type'] == 'device': + new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include") new_include.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/macro_device.xacro") new_dev = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}") new_dev.set("parent_link", "world") new_dev.set("mesh_path", str(self.mesh_path)) new_dev.set("device_name", node["id"]+"_") - new_dev.set("station_name", node["parent"]+'_') + if node["parent"] is not None: + new_dev.set("station_name", node["parent"]+'_') new_dev.set("x",str(float(node["position"]["x"])/1000)) new_dev.set("y",str(float(node["position"]["y"])/1000)) new_dev.set("z",str(float(node["position"]["z"])/1000)) if "rotation" in node["config"]: - new_dev.set("r",str(float(node["config"]["rotation"]["z"])/1000)) + new_dev.set("rx",str(float(node["config"]["rotation"]["x"]))) + new_dev.set("ry",str(float(node["config"]["rotation"]["y"]))) + new_dev.set("r",str(float(node["config"]["rotation"]["z"]))) + if "device_config" in node["config"]: + for key, value in node["config"]["device_config"].items(): + new_dev.set(key, str(float(value))) + + # 添加ros2_controller + if node['class'].startswith('moveit.'): + new_include_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}include") + new_include_controller.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.ros2_control.xacro") + new_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}_ros2_control") + new_controller.set("device_name", node["id"]+"_") + new_controller.set("mesh_path", str(self.mesh_path)) + + # 添加moveit的srdf + new_include_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}include") + new_include_srdf.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.srdf.xacro") + new_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}{model_config['mesh']}_srdf") + new_srdf.set("device_name", node["id"]+"_") + self.moveit_nodes[node["id"]] = model_config['mesh'] else: print("错误的注册表类型！") re = etree.tostring(self.root, encoding="unicode") @@ -90,8 +159,37 @@ class ResourceVisualization: xacro.process_doc(doc) self.urdf_str = doc.toxml() + re_srdf = etree.tostring(self.root_srdf, encoding="unicode") + doc_srdf = xacro.parse(re_srdf) + xacro.process_doc(doc_srdf) + self.urdf_str_srdf = doc_srdf.toxml() + if self.moveit_nodes: + self.moveit_init() - def create_launch_description(self, urdf_str: str) -> LaunchDescription: + def moveit_init(self): + + for name, config in self.moveit_nodes.items(): + controller_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/ros2_controllers.yaml", "r")) + moveit_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/moveit_controllers.yaml", "r")) + kinematics_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/kinematics.yaml", "r")) + + for key_kinematics, value_kinematics in kinematics_dict.items(): + self.moveit_nodes_kinematics[f'{name}_{key_kinematics}'] = value_kinematics + + for key, value in controller_dict['controller_manager']['ros__parameters'].items(): + if key == 'update_rate' or key == 'joint_state_broadcaster': + continue + self.ros2_controllers_yaml['controller_manager']['ros__parameters'][f"{name}_{key}"] = value + controller_dict[key]['ros__parameters']['joints'] = [f"{name}_{joint}" for joint in controller_dict[key]['ros__parameters']['joints']] + self.ros2_controllers_yaml[f"{name}_{key}"] = controller_dict[key] + + for controller_name in moveit_dict['moveit_simple_controller_manager']['controller_names']: + self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names'].append(f"{name}_{controller_name}") + moveit_dict['moveit_simple_controller_manager'][controller_name]['joints'] = [f"{name}_{joint}" for joint in moveit_dict['moveit_simple_controller_manager'][controller_name]['joints']] + self.moveit_controllers_yaml['moveit_simple_controller_manager'][f"{name}_{controller_name}"] = moveit_dict['moveit_simple_controller_manager'][controller_name] + + + def create_launch_description(self) -> LaunchDescription: """ 创建launch描述，包含robot_state_publisher和move_group节点 @@ -101,10 +199,93 @@ class ResourceVisualization: Returns: LaunchDescription: launch描述对象 """ + moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils")) + default_folder = moveit_configs_utils_path / "default_configs" + planning_pattern = re.compile("^(.*)_planning.yaml$") + pipelines = [] - + for pipeline in get_pattern_matches(default_folder, planning_pattern): + if pipeline not in pipelines: + pipelines.append(pipeline) + + if "ompl" in pipelines: + default_planning_pipeline = "ompl" + else: + default_planning_pipeline = pipelines[0] + + planning_pipelines = { + "planning_pipelines": pipelines, + "default_planning_pipeline": default_planning_pipeline, + } + + for pipeline in pipelines: + planning_pipelines[pipeline] = load_yaml( + default_folder / f"{pipeline}_planning.yaml" + ) + + if "ompl" in planning_pipelines: + ompl_config = planning_pipelines["ompl"] + if "planner_configs" not in ompl_config: + ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml")) + + yaml.safe_dump(self.ros2_controllers_yaml, open(f"{str(self.mesh_path)}/ros2_controllers.yaml", "w")) + + robot_description_planning = { + "default_velocity_scaling_factor": 0.1, + "default_acceleration_scaling_factor": 0.1, + "cartesian_limits": { + "max_trans_vel": 1.0, + "max_trans_acc": 2.25, + "max_trans_dec": -5.0, + "max_rot_vel": 1.57 + } + } # 解析URDF文件 - robot_description = urdf_str + robot_description = self.urdf_str + urdf_str_srdf = self.urdf_str_srdf + + kinematics_dict = self.moveit_nodes_kinematics + + if self.moveit_nodes: + controllers = [] + ros2_controllers = ParameterFile(f"{str(self.mesh_path)}/ros2_controllers.yaml", allow_substs=True) + + controllers.append( + nd( + package="controller_manager", + executable="ros2_control_node", + output='screen', + parameters=[ + {"robot_description": robot_description}, + ros2_controllers, + ] + ) + ) + for controller in self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names']: + controllers.append( + nd( + package="controller_manager", + executable="spawner", + arguments=[f"{controller}", "--controller-manager", f"controller_manager"], + output="screen", + ) + ) + controllers.append( + nd( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", f"controller_manager"], + output="screen", + ) + ) + for i in controllers: + self.launch_description.add_action(i) + else: + ros2_controllers = None + + + + # 创建robot_state_publisher节点 robot_state_publisher = nd( @@ -115,23 +296,21 @@ class ResourceVisualization: parameters=[{ 'robot_description': robot_description, 'use_sim_time': False - }] + }, + # kinematics_dict + ] ) - # joint_state_publisher_node = nd( - # package='joint_state_publisher_gui', # 或 joint_state_publisher - # executable='joint_state_publisher_gui', - # name='joint_state_publisher', - # output='screen' - # ) # 创建move_group节点 move_group = nd( package='moveit_ros_move_group', executable='move_group', output='screen', parameters=[{ + 'allow_trajectory_execution': True, 'robot_description': robot_description, - 'robot_description_semantic': self.srdf_str, + 'robot_description_semantic': urdf_str_srdf, + 'robot_description_kinematics': kinematics_dict, 'capabilities': '', 'disable_capabilities': '', 'monitor_dynamics': False, @@ -141,7 +320,13 @@ class ResourceVisualization: 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, - }] + # 'robot_description_planning': robot_description_planning, + }, + self.moveit_controllers_yaml, + # ompl_planning_pipeline_config, + robot_description_planning, + planning_pipelines, + ] ) # 将节点添加到launch描述中 @@ -156,7 +341,14 @@ class ResourceVisualization: executable='rviz2', name='rviz2', arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"], - output='screen' + output='screen', + parameters=[ + {'robot_description_kinematics': kinematics_dict, + }, + robot_description_planning, + planning_pipelines, + + ] ) self.launch_description.add_action(rviz_node) @@ -169,6 +361,11 @@ class ResourceVisualization: Args: urdf_str: URDF文件路径 """ - launch_description = self.create_launch_description(self.urdf_str) + launch_description = self.create_launch_description() + # print('--------------------------------') + # print(self.moveit_controllers_yaml) + # print('--------------------------------') + # print(self.urdf_str) + # print('--------------------------------') self.launch_service.include_launch_description(launch_description) self.launch_service.run() \ No newline at end of file diff --git a/unilabos/device_mesh/ros2_controllers.yaml b/unilabos/device_mesh/ros2_controllers.yaml new file mode 100644 index 00000000..e54c64c3 --- /dev/null +++ b/unilabos/device_mesh/ros2_controllers.yaml @@ -0,0 +1,31 @@ +benyao_arm_controller: + ros__parameters: + command_interfaces: + - position + joints: + - benyao_arm_base_joint + - benyao_arm_link_1_joint + - benyao_arm_link_2_joint + - benyao_arm_link_3_joint + - benyao_gripper_base_joint + state_interfaces: + - position + - velocity +benyao_gripper_controller: + ros__parameters: + command_interfaces: + - position + joints: + - benyao_gripper_right_joint + state_interfaces: + - position + - velocity +controller_manager: + ros__parameters: + benyao_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + benyao_gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + update_rate: 100 diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz index 07bf15df..f042cc27 100644 --- a/unilabos/device_mesh/view_robot.rviz +++ b/unilabos/device_mesh/view_robot.rviz @@ -1,23 +1,18 @@ Panels: - Class: rviz_common/Displays - Help Height: 138 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - - /TF1 + - /TF1/Frames1 - /TF1/Tree1 - - /RobotModel1 - - /PlanningScene1 - /PlanningScene1/Scene Geometry1 - - /RobotState1 - - /RobotState1/Links1 - /MotionPlanning1 - /MotionPlanning1/Scene Geometry1 - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - Splitter Ratio: 0.5 - Tree Height: 345 + Splitter Ratio: 0.4940796494483948 + Tree Height: 602 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -61,7 +56,7 @@ Visualization Manager: Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: {} Update Interval: 0 @@ -203,6 +198,63 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + benyao_arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_slideway: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + benyao_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hotel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hotel_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + hotel_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false world: Alpha: 1 Show Axes: false @@ -230,7 +282,7 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: "" + Planning Group: benyao_arm Query Goal State: false Query Start State: false Show Workspace: false @@ -290,6 +342,63 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + benyao_arm_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_arm_slideway: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + benyao_gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + benyao_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hotel_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hotel_device_link: + Alpha: 1 + Show Axes: false + Show Trail: false + hotel_socketTypeGenericSbsFootprint: + Alpha: 1 + Show Axes: false + Show Trail: false world: Alpha: 1 Show Axes: false @@ -345,43 +454,43 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.595012903213501 + Distance: 2.622864246368408 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.29730814695358276 - Y: 0.21228469908237457 - Z: 0.20008830726146698 + X: -0.2880733013153076 + Y: -0.16004444658756256 + Z: -0.16730672121047974 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.38979560136795044 + Pitch: 0.634795606136322 Target Frame: Value: Orbit (rviz) - Yaw: 0.05074193701148033 + Yaw: 5.590744495391846 Saved: ~ Window Geometry: Displays: - collapsed: false - Height: 2160 - Hide Left Dock: false + collapsed: true + Height: 1897 + Hide Left Dock: true Hide Right Dock: true MotionPlanning: - collapsed: false + collapsed: true MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000010000000087000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a300000714fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000027000002c80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002fb00000440000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000ddb0000071400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 4096 - X: 0 - Y: 0 + Width: 3547 + X: -857 + Y: 149 diff --git a/unilabos/devices/resource_container/__init__.py b/unilabos/devices/resource_container/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/unilabos/devices/resource_container/container.py b/unilabos/devices/resource_container/container.py new file mode 100644 index 00000000..38b9a806 --- /dev/null +++ b/unilabos/devices/resource_container/container.py @@ -0,0 +1,9 @@ + +class HotelContainer: + def __init__(self, rotation: dict, device_config: dict): + self.rotation = rotation + self.device_config = device_config + self.status = 'idle' + + def get_rotation(self): + return self.rotation diff --git a/unilabos/devices/ros_dev/moveit_interface.py b/unilabos/devices/ros_dev/moveit_interface.py new file mode 100644 index 00000000..4f9daad0 --- /dev/null +++ b/unilabos/devices/ros_dev/moveit_interface.py @@ -0,0 +1,435 @@ +from pathlib import Path +from threading import Thread +import time + +import rclpy +import rclpy.duration +import rclpy.time +from unilabos_msgs.action import SendCmd +from rclpy.action import ActionServer, ActionClient +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import ReentrantCallbackGroup +from moveit_msgs.msg import JointConstraint, Constraints +from copy import deepcopy + +from pymoveit2 import MoveIt2, GripperInterface + +import numpy as np +from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode + +# from tf_transformations import quaternion_from_euler +from tf2_ros import Buffer, TransformListener +import json + + +class MoveitInterface(BaseROS2DeviceNode): + def __init__(self, device_id, moveit_type, joint_poses, resource_tracker, rotation = None, device_config = None): + super().__init__( + driver_instance=self, + device_id=device_id, + status_types={}, + action_value_mappings={}, + hardware_interface={}, + print_publish=False, + resource_tracker=resource_tracker, + ) + self.callback_group = ReentrantCallbackGroup() + data_config = json.load(open(f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json", encoding="utf-8")) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.arm_move_flag = False + + self.move_option = ['pick', 'place', 'side_pick', 'side_place'] + self.joint_poses = joint_poses + self.cartesian_flag = False + + self.mesh_group = ['reactor','sample','beaker'] + + self.moveit2 = {} + + + self.resource_action = None + self.resource_client = None + self.resource_action_ok = False + + + + for move_group, config in data_config.items(): + + base_link_name = f"{device_id}_{config['base_link_name']}" + end_effector_name = f"{device_id}_{config['end_effector_name']}" + joint_names = [f"{device_id}_{name}" for name in config['joint_names']] + + self.moveit2[f"{move_group}"] = MoveIt2( + node=self, + joint_names=joint_names, + base_link_name=base_link_name, + end_effector_name=end_effector_name, + group_name=f"{device_id}_{move_group}", + callback_group=self.callback_group, + use_move_group_action= True, + ignore_new_calls_while_executing = True + ) + self.moveit2[f"{move_group}"].allowed_planning_time = 3.0 + + + self.moveit_pose_server = ActionServer( + self, + SendCmd, + f'set_position', + execute_callback=self.callback, + callback_group=self.callback_group, + result_timeout=5000 + ) + + self.moveit_pick_server = ActionServer( + self, + SendCmd, + f'pick_and_place', + execute_callback=self.pick_and_place_callback, + callback_group=self.callback_group, + result_timeout=5000 + ) + + + self.moveit_joint_server = ActionServer( + self, + SendCmd, + f'set_status', + execute_callback=self.set_status_callback, + callback_group=self.callback_group, + result_timeout=5000 + ) + + self.create_timer(1, self.wait_for_resource_action,callback_group=ReentrantCallbackGroup()) + + + def wait_for_resource_action(self): + if not self.resource_action_ok: + + while self.resource_action is None: + self.resource_action = self.check_tf_update_actions() + time.sleep(1) + self.resource_client = ActionClient(self, SendCmd, self.resource_action) + self.resource_action_ok = True + while not self.resource_client.wait_for_server(timeout_sec=5.0): + self.get_logger().info('等待 TfUpdate 服务器...') + + + def check_tf_update_actions(self): + topics = self.get_topic_names_and_types() + + + for topic_item in topics: + + topic_name, topic_types = topic_item + + if 'action_msgs/msg/GoalStatusArray' in topic_types: + # 删除 /_action/status 部分 + + base_name = topic_name.replace('/_action/status', '') + # 检查最后一个部分是否为 tf_update + parts = base_name.split('/') + if parts and parts[-1] == 'tf_update': + return base_name + + return None + + def callback(self,goal_handle: ServerGoalHandle): + + """使用moveit 移动到指定点 + Args: + command: A JSON-formatted string that includes quaternion, speed, position + + position (list) : 点的位置 [x,y,z] + quaternion (list) : 点的姿态(四元数) [x,y,z,w] + move_group (string) : The move group moveit will plan + speed (float) : The speed of the movement, speed > 0 + retry (int) : Retry times when moveit plan fails + + Returns: + None + """ + + result = SendCmd.Result() + + try: + cmd_str = str(goal_handle.request.command).replace('\'','\"') + cmd_dict = json.loads(cmd_str) + + self.moveit_task(**cmd_dict) + goal_handle.succeed() + result.success=True + + except Exception as e: + print(e) + goal_handle.abort() + result.success=False + + return result + + def moveit_task(self,move_group,position,quaternion,speed=1,retry=10,cartesian=False, target_link = None ,offsets =[0,0,0]): + + speed_ = float(max(0.1, min(speed, 1))) + + self.moveit2[move_group].max_velocity = speed_ + self.moveit2[move_group].max_acceleration = speed_ + + re_ = False + + pose_result =[x + y for x, y in zip(position, offsets)] + # print(pose_result) + + while retry > -1 and not re_ : + + self.moveit2[move_group].move_to_pose( + target_link=target_link, + position=pose_result, + quat_xyzw=quaternion, + cartesian=cartesian, + # cartesian_fraction_threshold=0.0, + cartesian_max_step=0.01, + weight_position=1.0 + ) + re_ = self.moveit2[move_group].wait_until_executed() + retry += -1 + + return re_ + + def moveit_joint_task(self, move_group, joint_positions, joint_names = None,speed=1,retry=10): + + re_ = False + + joint_positions_ = [float(x) for x in joint_positions] + + speed_ = float(max(0.1, min(speed, 1))) + + self.moveit2[move_group].max_velocity = speed_ + self.moveit2[move_group].max_acceleration = speed_ + + while retry > -1 and not re_ : + + self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_,joint_names=joint_names) + re_ = self.moveit2[move_group].wait_until_executed() + + retry += -1 + print(self.moveit2[move_group].compute_fk(joint_positions)) + return re_ + + + def resource_manager(self,resource, parent_link): + goal_msg = SendCmd.Goal() + str_dict = {} + str_dict[resource] = parent_link + + goal_msg.command = json.dumps(str_dict) + self.resource_client.send_goal(goal_msg) + + return True + + + + def pick_and_place_callback(self,goal_handle: ServerGoalHandle): + + """ + Using MoveIt to make the robotic arm pick or place materials to a target point. + + Args: + command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height + + *option (string) : Action type: pick/place/side_pick/side_place + *move_group (string): The move group moveit will plan + *status(string) : Target pose + resource(string) : The target resource + x_distance (float) : The distance to the target in x direction(meters) + y_distance (float) : The distance to the target in y direction(meters) + lift_height (float) : The height at which the material should be lifted(meters) + retry (float) : Retry times when moveit plan fails + speed (float) : The speed of the movement, speed > 0 + Returns: + None + """ + result = SendCmd.Result() + + try: + cmd_str = str(goal_handle.request.command).replace('\'','\"') + cmd_dict = json.loads(cmd_str) + + if cmd_dict["option"] in self.move_option: + option_index = self.move_option.index(cmd_dict["option"]) + place_flag = option_index % 2 + + config = {} + function_list = [] + + status = cmd_dict["status"] + joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status] + + config.update({k: cmd_dict[k] for k in ["speed", "retry", 'move_group'] if k in cmd_dict}) + + + # 夹取 + if not place_flag: + if 'target' in cmd_dict.keys(): + function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"])) + else: + function_list.append(lambda: self.resource_manager(cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name)) + else: + function_list.append(lambda: self.resource_manager(cmd_dict["resource"], 'world')) + + constraints = [] + if "constraints" in cmd_dict.keys(): + + for i in range(len(cmd_dict["constraints"])): + v = float(cmd_dict["constraints"][i]) + if v > 0: + constraints.append(JointConstraint( + joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i], + position=joint_positions_[i], + tolerance_above=v, + tolerance_below=v, + weight=1.0 + )) + + + if "lift_height" in cmd_dict.keys(): + + retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_) + pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z] + quaternion = [retval.pose.orientation.x, retval.pose.orientation.y, retval.pose.orientation.z, retval.pose.orientation.w] + + function_list = [lambda: self.moveit_task(position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z], quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list + + pose[2] += float(cmd_dict["lift_height"]) + function_list.append(lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)) + end_pose = pose + + + if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys(): + if "x_distance" in cmd_dict.keys(): + deep_pose = deepcopy(pose) + deep_pose[0] += float(cmd_dict["x_distance"]) + elif "y_distance" in cmd_dict.keys(): + deep_pose = deepcopy(pose) + deep_pose[1] += float(cmd_dict["y_distance"]) + + function_list = [lambda: self.moveit_task(position=pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)] + function_list + function_list.append(lambda: self.moveit_task(position=deep_pose, quaternion=quaternion,**config, cartesian=self.cartesian_flag)) + end_pose = deep_pose + + retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik( + position=end_pose, + quat_xyzw=quaternion, + constraints=Constraints(joint_constraints=constraints)) + + + position_ = [retval_ik.position[retval_ik.name.index(i)] for i in self.moveit2[cmd_dict["move_group"]].joint_names] + + function_list = [lambda: self.moveit_joint_task( + joint_positions=position_, + joint_names=self.moveit2[cmd_dict["move_group"]].joint_names, + **config, + )] + function_list + else: + + function_list = [lambda: self.moveit_joint_task(**config,joint_positions=joint_positions_)] + function_list + + for i in range(len(function_list)): + if i == 0: + self.cartesian_flag = False + else: + self.cartesian_flag = True + + re = function_list[i]() + + if not re: + + print(i,re) + goal_handle.abort() + result.success=False + + return result + + goal_handle.succeed() + result.success=True + + except Exception as e: + print(e) + goal_handle.abort() + self.cartesian_flag = False + result.success=False + + return result + + def set_status_callback(self,goal_handle: ServerGoalHandle): + + """ + Goto home position + + Args: + command: A JSON-formatted string that includes speed + *status (string) : The joint status moveit will plan + *move_group (string): The move group moveit will plan + separate (list) : The joint index to be separated + lift_height (float) : The height at which the material should be lifted(meters) + x_distance (float) : The distance to the target in x direction(meters) + y_distance (float) : The distance to the target in y direction(meters) + speed (float) : The speed of the movement, speed > 0 + retry (float) : Retry times when moveit plan fails + + Returns: + None + """ + + result = SendCmd.Result() + + try: + cmd_str = str(goal_handle.request.command).replace('\'','\"') + cmd_dict = json.loads(cmd_str) + + config = {} + config["move_group"] = cmd_dict["move_group"] + if "speed" in cmd_dict.keys(): + config["speed"] = cmd_dict["speed"] + if "retry" in cmd_dict.keys(): + config["retry"] = cmd_dict["retry"] + + + + if "separate" in cmd_dict.keys(): + separate = cmd_dict["separate"] + else: + separate = None + + status = cmd_dict["status"] + joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status] + + if separate is not None : + + separate_j = [joint_positions_[i] for i in separate] + separate_n = [self.joint_names[i] for i in separate] + + re = self.moveit_joint_task(**config, joint_positions=separate_j,joint_names=separate_n) + if not re: + goal_handle.abort() + result.success=False + return result + + re = self.moveit_joint_task(**config,joint_positions=joint_positions_) + + if not re: + goal_handle.abort() + result.success=False + return result + + goal_handle.succeed() + result.success=True + + except Exception as e: + print(e) + goal_handle.abort() + result.success=False + + return result \ No newline at end of file diff --git a/unilabos/registry/devices/hotel.yaml b/unilabos/registry/devices/hotel.yaml new file mode 100644 index 00000000..0f925118 --- /dev/null +++ b/unilabos/registry/devices/hotel.yaml @@ -0,0 +1,9 @@ +hotel.thermo_orbitor_rs2_hotel: + description: Thermo Orbitor RS2 Hotel + class: + module: unilabos.devices.resource_container.container:HotelContainer + type: python + model: + type: device + mesh: thermo_orbitor_rs2_hotel + diff --git a/unilabos/registry/devices/moveit_config.yaml b/unilabos/registry/devices/moveit_config.yaml new file mode 100644 index 00000000..c30ffba8 --- /dev/null +++ b/unilabos/registry/devices/moveit_config.yaml @@ -0,0 +1,17 @@ +moveit.toyo_xyz: + description: Toyo XYZ + class: + module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface + type: ros2 + model: + type: device + mesh: toyo_xyz + +moveit.benyao_arm: + description: Benyao Arm + class: + module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface + type: ros2 + model: + type: device + mesh: benyao_arm \ No newline at end of file diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index afa90568..d49f733d 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -124,6 +124,8 @@ class ResourceMeshManager(BaseROS2DeviceNode): """检查move_group节点是否已初始化完成""" # 获取当前可用的节点列表 + if len(self.resource_tf_dict) == 0: + return tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2)) # if tf_ready: @@ -397,7 +399,8 @@ class ResourceMeshManager(BaseROS2DeviceNode): transform = self.tf_buffer.lookup_transform( parent_id, resource_id, - rclpy.time.Time(seconds=0) + self.get_clock().now(), + timeout=rclpy.duration.Duration(seconds=10) ) # 提取转换中的位置和旋转信息