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

Motion planner (MoveIt) and Task planner (Plansys) integration

Browse source
This commit is contained in:
Roman Andrianov 2023-04-30 11:46:52 +00:00 • committed by Igor Brylyov
parent 0a735b87c9
commit 000ddb4831
43 changed files with 1402 additions and 379 deletions
Download patch file Download diff file Expand all files Collapse all files

35
rbs_task_planner/README.md Normal file
View file

@ -0,0 +1,35 @@
### Usage
change `rbs_simulation.laucnh.py` file by providing `${PATH}` and `${PREFIX}` to assemble files
```python
assemble_state = Node(
package="rbs_skill_servers",
executable="assemble_state_service_server",
output="screen",
emulate_tty=True,
parameters=[
{"assemble_prefix": "ASSEMBLE_"} # ${PREFIX}
{"assemble_package_dir": ${COLCON_WS}/src/robossembler-ros2/rbs_task_planner/exemples/sdf_models}, # ${PATH}
]
)
```
launch simulation proccess
```bash
ros2 launch rbs_simulation rbs_simulation.launch.py
```
launch task_planner process
```bash
ros2 launch rbs_task_planner task_planner.launch.py
```
start env_manager process
```bash
./${COLCON_WS}/build/env_manager/main
```
you can send `problem` throw plansys2 service with `${PROBLEM_FILE_CONTENT}`
```bash
ros2 service call /problem_expert/add_problem plansys2_msgs/srv/AddProblem '{problem: ${PROBLEM_FILE_CONTENT}}'
```
or you can use `plansys2_terminal`
```bash
ros2 run plansys2_terminal plansys2_terminal
```
after that use `plansys2_terminal` to call `run` command and start executing process

10
rbs_task_planner/config/params.yaml
View file

@ -0,0 +1,10 @@
assemble:
ros__parameters:
plugins:
- rbs_skill_move_topose_bt_action_client
- rbs_skill_get_pick_place_pose_service_client
- rbs_skill_gripper_move_bt_action_client
- rbs_skill_move_joint_state
- rbs_assemble_process_state
- rbs_detect_object

11
rbs_task_planner/domain/atomic_domain.pddl
View file

@ -1,7 +1,7 @@
(define (domain atomic_domain)
(:requirements :strips :typing :adl :fluents :durative-actions)
(:types
workspace - zone
zone
part
arm
assembly
@ -19,16 +19,17 @@
:parameters (?p - part ?prev ?next - assembly ?z - zone ?a - arm)
:duration ( = ?duration 1)
:condition (and
(at start (assembly_order ?prev ?next))
(at start (assembled ?prev))
(at start (assembly_at ?prev ?z))
(at start (part_of ?p ?next))
(at start (arm_available ?a))
(at start (assembly_order ?prev ?next))
(at start (part_of ?p ?next))
(at start (assembly_at ?prev ?z))
(at start (assembled ?prev))
)
:effect (and
(at start (not (arm_available ?a)))
(at end (assembly_at ?next ?z))
(at end (assembled ?next))
(at end (arm_available ?a))
)
)
)

19
rbs_task_planner/example/sdf_models/asm1.sdf Normal file
View file

@ -0,0 +1,19 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

28
rbs_task_planner/example/sdf_models/asm2.sdf Normal file
View file

@ -0,0 +1,28 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box2" type="fixed">
<parent>base_link</parent>
<child>box2</child>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

38
rbs_task_planner/example/sdf_models/asm3.sdf Normal file
View file

@ -0,0 +1,38 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box2" type="fixed">
<parent>base_link</parent>
<child>box2</child>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box3" type="fixed">
<parent>base_link</parent>
<child>box3</child>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

48
rbs_task_planner/example/sdf_models/asm4.sdf Normal file
View file

@ -0,0 +1,48 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box2" type="fixed">
<parent>base_link</parent>
<child>box2</child>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box3" type="fixed">
<parent>base_link</parent>
<child>box3</child>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box4" type="fixed">
<parent>base_link</parent>
<child>box4</child>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

58
rbs_task_planner/example/sdf_models/asm5.sdf Normal file
View file

@ -0,0 +1,58 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box2" type="fixed">
<parent>base_link</parent>
<child>box2</child>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box3" type="fixed">
<parent>base_link</parent>
<child>box3</child>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box4" type="fixed">
<parent>base_link</parent>
<child>box4</child>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box5" type="fixed">
<parent>base_link</parent>
<child>box5</child>
<pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

68
rbs_task_planner/example/sdf_models/asm6.sdf Normal file
View file

@ -0,0 +1,68 @@
<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
<model name="assembly">
<link name="base_link">
<pose>0 0 0 0 0 0</pose>
</link>
<include>
<uri>model://box1</uri>
<name>box1</name>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box2</name>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box3</name>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box4</name>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box5</name>
<pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose>
</include>
<include>
<uri>model://box1</uri>
<name>box6</name>
<pose>-0.45 0.0 0.56 -3.1415927 0.0 0.0</pose>
</include>
<joint name="box1" type="fixed">
<parent>base_link</parent>
<child>box1</child>
<pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box2" type="fixed">
<parent>base_link</parent>
<child>box2</child>
<pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box3" type="fixed">
<parent>base_link</parent>
<child>box3</child>
<pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box4" type="fixed">
<parent>base_link</parent>
<child>box4</child>
<pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box5" type="fixed">
<parent>base_link</parent>
<child>box5</child>
<pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose>
</joint>
<joint name="box6" type="fixed">
<parent>base_link</parent>
<child>box6</child>
<pose>-0.45 0.0 0.56 -3.1415927 0.0 0.0</pose>
</joint>
</model>
</sdf>

1
rbs_task_planner/example/sdf_models/cat_problem Normal file
View file

@ -0,0 +1 @@
(define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm world - zone box1 box2 box3 box4 box5 box6 - part asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly)(:init (part_of box1 asm1) (part_of box2 asm2) (part_of box3 asm3) (part_of box4 asm4) (part_of box5 asm5) (part_of box6 asm6) (assembly_order asm0 asm1) (assembly_order asm1 asm2) (assembly_order asm2 asm3) (assembly_order asm3 asm4) (assembly_order asm4 asm5) (assembly_order asm5 asm6) (arm_available ur_manipulator_gripper) (assembly_at asm0 world) (assembled asm0) )(:goal (and (assembled asm6))))

26
rbs_task_planner/example/sdf_models/problem.pddl Normal file
View file

@ -0,0 +1,26 @@
(define (problem box_problem)
(:domain atomic_domain)
(:objects
ur_manipulator_gripper - arm
workspace1 - zone
box1 box2 box3 - part
asm0 asm1 asm2 asm3 - assembly
)
(:init
(part_of box1 asm1)
(part_of box2 asm2)
(part_of box3 asm3)
(assembly_order asm0 asm1)
(assembly_order asm1 asm2)
(assembly_order asm2 asm3)
(arm_available ur_manipulator_gripper)
(assembly_at asm0 workspace1)
(assembled asm0)
)
(:goal (and
(assembled asm3)
))
)

14
rbs_task_planner/launch/task_planner.launch.py
View file

@ -6,7 +6,7 @@ from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
# from launch_ros.actions import Node
from launch_ros.actions import Node
def load_file(package_name, file_path):
@ -36,6 +36,7 @@ def load_yaml(package_name, file_path):
def generate_launch_description():
pkg_dir = get_package_share_directory('rbs_task_planner')
bt_exec_dir = get_package_share_directory('rbs_bt_executor')
namespace = LaunchConfiguration('namespace')
declare_namespace_cmd = DeclareLaunchArgument(
@ -52,7 +53,6 @@ def generate_launch_description():
'model_file': pkg_dir + '/domain/atomic_domain.pddl',
'namespace': namespace
}.items())
"""
assemble = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
@ -61,20 +61,16 @@ def generate_launch_description():
output='screen',
parameters=[
pkg_dir + '/config/params.yaml',
robot_description,
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'assemble',
# 'publisher_port': 1666,
# 'server_port': 1667,
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml',
}
])
"""
ld = LaunchDescription()
ld.add_action(declare_namespace_cmd)
# Declare the launch options
ld.add_action(plansys2_cmd)
ld.add_action(assemble)
return ld

3
rbs_task_planner/package.xml
View file

@ -18,9 +18,12 @@
<depend>plansys2_planner</depend>
<depend>plansys2_executor</depend>
<depend>plansys2_bt_actions</depend>
<depend>plansys2_terminal</depend>
<depend>behaviortree_cpp_v3</depend>
<depend>behavior_tree</depend>
<exec_depend>popf</exec_depend>
<exec_depend>plansys2_bringup</exec_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>

32
rbs_task_planner/problems/example_problem.pddl Normal file
View file

@ -0,0 +1,32 @@
(define (problem box_problem)
(:domain atomic_domain)
(:objects
ur_manipulator_gripper - arm
world - zone
box1 box2 box3 box4 box5 box6 - part
asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly
)
(:init
(part_of box1 asm1)
(part_of box2 asm2)
(part_of box3 asm3)
(part_of box4 asm4)
(part_of box5 asm5)
(part_of box6 asm6)
(assembly_order asm0 asm1)
(assembly_order asm1 asm2)
(assembly_order asm2 asm3)
(assembly_order asm3 asm4)
(assembly_order asm4 asm5)
(assembly_order asm5 asm6)
(arm_available ur_manipulator_gripper)
(assembly_at asm0 world)
(assembled asm0)
)
(:goal (and
(assembled asm6)
))
)

43
rbs_task_planner/problems/problem.pddl
View file

@ -1,30 +1,31 @@
(define (problem cube3-problem)
(define (problem Cube3-problem)
(:domain atomic_domain)
(:objects
ws1 - zone
rbsarm - arm
cube3 cube1 cube2 cube004 cube005 cube006 cube007 - part
zeroasm subasm0 subasm1 subasm2 subasm3 subasm4 subasm5 subasm6 - assembly
ur_manipulator_gripper - arm
ws - zone
cube cube001 cube002 cube004 cube005 cube006 - part
asm1 asm2 asm3 asm4 asm5 asm6 - assembly
)
(:init
(part_of cube3 subasm0)
(part_of cube1 subasm1)
(part_of cube2 subasm2)
(part_of cube004 subasm3)
(part_of cube005 subasm4)
(part_of cube006 subasm5)
(part_of cube007 subasm6)
(assembly_order zeroasm subasm0)
(assembly_order subasm0 subasm1)
(assembly_order subasm1 subasm2)
(assembly_order subasm3 subasm4)
(assembly_order subasm5 subasm6)
(part_of cube asm1)
(part_of cube001 asm2)
(part_of cube002 asm3)
(part_of cube004 asm4)
(part_of cube005 asm5)
(part_of cube006 asm6)
(assembly_order asm1 asm2)
(assembly_order asm2 asm3)
(assembly_order asm3 asm4)
(assembly_order asm4 asm5)
(assembly_order asm5 asm6)
(arm_available rbsarm)
(assembly_at zeroasm ws1)
(assembled zeroasm)
(arm_available ur_manipulator_gripper)
(assembly_at asm1 ws)
(assembled asm1)
)
(:goal (and (assembled subasm6)))
(:goal (and
(assembled asm6)
))
)