From 97f6ca664734bf65569a0671719ad676d24fade9 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Sun, 30 Oct 2022 14:44:39 +0100
Subject: [PATCH] integration of christophs pkg
---
.../profiles/default/devel_collisions.txt | 2 +-
src/.vscode/settings.json | 3 +-
src/ceti_double/config/fake_controllers.yaml | 4 +-
src/ceti_double/launch/demo.launch | 5 +-
src/mtc/CMakeLists.txt | 1 +
src/mtc/launch/cell_routine.launch | 10 +-
src/mtc/mtc_task_file/-1406488393.yaml | 208 ++++++++++++++++
src/mtc/mtc_task_file/-854508939.yaml | 208 ++++++++++++++++
src/mtc/mtc_task_file/dummy.yaml | 226 +++++++++---------
src/mtc/src/cell_routine.cpp | 16 +-
src/mtc/src/impl/map_loader.cpp | 10 +-
src/mtc/src/impl/moveit_mediator.cpp | 38 ++-
12 files changed, 601 insertions(+), 130 deletions(-)
create mode 100644 src/mtc/mtc_task_file/-1406488393.yaml
create mode 100644 src/mtc/mtc_task_file/-854508939.yaml
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index f49f83e..935b46d 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 12194
+/home/matteo/reachability/devel/./cmake.lock 12632
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/.vscode/settings.json b/src/.vscode/settings.json
index 299c8d4..dbabd7d 100644
--- a/src/.vscode/settings.json
+++ b/src/.vscode/settings.json
@@ -78,6 +78,7 @@
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
- "variant": "cpp"
+ "variant": "cpp",
+ "any": "cpp"
}
}
\ No newline at end of file
diff --git a/src/ceti_double/config/fake_controllers.yaml b/src/ceti_double/config/fake_controllers.yaml
index 9ec7514..9440678 100644
--- a/src/ceti_double/config/fake_controllers.yaml
+++ b/src/ceti_double/config/fake_controllers.yaml
@@ -31,6 +31,6 @@ controller_list:
- panda_2_finger_joint2
initial: # Define initial robot poses per group
- group: panda_arm1
- pose: home1
+ pose: ready
- group: panda_arm2
- pose: home2
\ No newline at end of file
+ pose: ready
\ No newline at end of file
diff --git a/src/ceti_double/launch/demo.launch b/src/ceti_double/launch/demo.launch
index e9c08c8..2d39189 100644
--- a/src/ceti_double/launch/demo.launch
+++ b/src/ceti_double/launch/demo.launch
@@ -56,11 +56,12 @@
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>
- <!-- Run Rviz and load the default config to see the state of the move_group node -->
+
+ <!-- Run Rviz and load the default config to see the state of the move_group node
<include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
<arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
<arg name="debug" value="$(arg debug)"/>
- </include>
+ </include> -->
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 6442dfa..7ae8bc5 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -88,6 +88,7 @@ src/impl/collision_helper.cpp
)
add_executable(cell_routine src/cell_routine.cpp
+src/impl/map_loader.cpp
src/impl/abstract_mediator.cpp
src/impl/moveit_mediator.cpp
src/impl/wing_moveit_decorator.cpp
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index 60298dd..b6b2eaf 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -2,9 +2,15 @@
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<rosparam command="load" file="$(find mtc)/results/-671106318.yaml"/>
<rosparam command="load" file="$(find mtc)/mtc_task_file/dummy.yaml" />
+ <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
+ <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
- <!--<include file="$(find ceti_double)/launch/moveit_rviz.launch"> -->
- <!--</include> -->
+ <include file="$(find ceti_double)/launch/demo.launch">
+ </include>
+
+ <include file="$(find ceti_double)/launch/moveit_rviz.launch">
+ <arg name="rviz_config" value="$(find mtc)/launch/rviz/cell_routine.rviz" />
+ </include>
<node pkg="mtc" type="cell_routine" name="cell_routine" output="screen" required="true" />
</launch>
diff --git a/src/mtc/mtc_task_file/-1406488393.yaml b/src/mtc/mtc_task_file/-1406488393.yaml
new file mode 100644
index 0000000..83a3b79
--- /dev/null
+++ b/src/mtc/mtc_task_file/-1406488393.yaml
@@ -0,0 +1,208 @@
+planners:
+ - id: cartesian
+ type: CartesianPath
+ - id: sampling
+ type: PipelinePlanner
+ properties:
+ step_size: 0.005
+ goal_joint_tolerance: 1e-05
+ - id: interpolation
+ type: JointInterpolationPlanner
+task:
+ name: Pick and Place test
+ properties:
+ group: Robot_arm1
+ eef: hand_1
+ hand_grasping_frame: panda_1_link8
+ ik_frame: panda_1_link8
+ hand: hand_1
+ stages:
+ - name: current
+ type: CurrentState
+ - name: move to ready
+ type: MoveTo
+ id: ready
+ planner: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values:
+ - group
+ set:
+ goal: ready
+ - type: MoveTo
+ planner: sampling
+ id: hand_open
+ properties:
+ group: hand_1
+ set:
+ goal: open
+ - type: Connect
+ group_planner_vector:
+ panda_arm: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ - type: SerialContainer
+ name: grasp
+ properties_exposeTo:
+ source: task
+ values: [eef, hand, group, ik_frame]
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [eef, hand, group, ik_frame]
+ stages:
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.07
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
+ - type: ComputeIK
+ properties: {max_ik_solutions: 5}
+ set:
+ ik_frame:
+ isometry:
+ translation: {x: 0.1, y: 0.0, z: 0.0}
+ quaternion: {r: 1.571, p: 0.785, y: 1.571}
+ link: panda_1_link8
+ propertiesConfigureInitFrom:
+ - source: PARENT
+ values: [eef, group]
+ - source: INTERFACE
+ values: [target_pose]
+ stage:
+ type: GenerateGraspPose
+ propertiesConfigureInitFrom:
+ source: PARENT
+ properties:
+ object: bottle
+ angle_delta: 1.571
+ pregrasp: open
+ set:
+ monitored_stage: ready
+ - type: ModifyPlanningScene
+ set:
+ allow_collisions:
+ first: bottle
+ second:
+ joint_model_group_name: hand_1
+ allow: true
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: close
+ - type: ModifyPlanningScene
+ set:
+ attach_object:
+ object: bottle
+ link: panda_1_link8
+ - type: MoveRelative
+ planner: cartesian
+ id: pick_up
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ properties:
+ min_distance: 0.1
+ max_distance: 0.2
+ set:
+ ik_frame:
+ link: panda_1_link8
+ direction:
+ vector: {x: 0.0, y: 0.0, z: 1.0}
+ - type: Connect
+ group_planner_vector:
+ panda_arm: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ - type: SerialContainer
+ name: place
+ properties_exposeTo:
+ source: task
+ values: [eef, hand, group, ik_frame]
+ propertiesConfigureInitFrom:
+ source: PARENT
+ stages:
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.1
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: -1.0}
+ - type: ComputeIK
+ properties: {max_ik_solutions: 5}
+ set:
+ ik_frame:
+ isometry:
+ translation: {x: 0.1, y: 0.0, z: 0.0}
+ quaternion: {r: 1.571, p: 0.785, y: 1.571}
+ link: panda_1_link8
+ propertiesConfigureInitFrom:
+ - source: PARENT
+ values: [eef, group]
+ - source: INTERFACE
+ values: [target_pose]
+ stage:
+ type: GeneratePose
+ set:
+ monitored_stage: pick_up
+ pose:
+ point: {x: 0.100000, 0.700000: ~, z: 0.9305f}
+ orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: open
+ - type: ModifyPlanningScene
+ set:
+ detach_object:
+ object: bottle
+ link: panda_1_link8
+ allow_collisions:
+ first: bottle
+ second:
+ joint_model_group_name: hand_1
+ allow: false
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.07
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: close
+ - name: move to ready
+ type: MoveTo
+ planner: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ value:
+ - group
+ set:
+ goal: ready
+max_planning_solutions: 10
\ No newline at end of file
diff --git a/src/mtc/mtc_task_file/-854508939.yaml b/src/mtc/mtc_task_file/-854508939.yaml
new file mode 100644
index 0000000..83a3b79
--- /dev/null
+++ b/src/mtc/mtc_task_file/-854508939.yaml
@@ -0,0 +1,208 @@
+planners:
+ - id: cartesian
+ type: CartesianPath
+ - id: sampling
+ type: PipelinePlanner
+ properties:
+ step_size: 0.005
+ goal_joint_tolerance: 1e-05
+ - id: interpolation
+ type: JointInterpolationPlanner
+task:
+ name: Pick and Place test
+ properties:
+ group: Robot_arm1
+ eef: hand_1
+ hand_grasping_frame: panda_1_link8
+ ik_frame: panda_1_link8
+ hand: hand_1
+ stages:
+ - name: current
+ type: CurrentState
+ - name: move to ready
+ type: MoveTo
+ id: ready
+ planner: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values:
+ - group
+ set:
+ goal: ready
+ - type: MoveTo
+ planner: sampling
+ id: hand_open
+ properties:
+ group: hand_1
+ set:
+ goal: open
+ - type: Connect
+ group_planner_vector:
+ panda_arm: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ - type: SerialContainer
+ name: grasp
+ properties_exposeTo:
+ source: task
+ values: [eef, hand, group, ik_frame]
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [eef, hand, group, ik_frame]
+ stages:
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.07
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
+ - type: ComputeIK
+ properties: {max_ik_solutions: 5}
+ set:
+ ik_frame:
+ isometry:
+ translation: {x: 0.1, y: 0.0, z: 0.0}
+ quaternion: {r: 1.571, p: 0.785, y: 1.571}
+ link: panda_1_link8
+ propertiesConfigureInitFrom:
+ - source: PARENT
+ values: [eef, group]
+ - source: INTERFACE
+ values: [target_pose]
+ stage:
+ type: GenerateGraspPose
+ propertiesConfigureInitFrom:
+ source: PARENT
+ properties:
+ object: bottle
+ angle_delta: 1.571
+ pregrasp: open
+ set:
+ monitored_stage: ready
+ - type: ModifyPlanningScene
+ set:
+ allow_collisions:
+ first: bottle
+ second:
+ joint_model_group_name: hand_1
+ allow: true
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: close
+ - type: ModifyPlanningScene
+ set:
+ attach_object:
+ object: bottle
+ link: panda_1_link8
+ - type: MoveRelative
+ planner: cartesian
+ id: pick_up
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ properties:
+ min_distance: 0.1
+ max_distance: 0.2
+ set:
+ ik_frame:
+ link: panda_1_link8
+ direction:
+ vector: {x: 0.0, y: 0.0, z: 1.0}
+ - type: Connect
+ group_planner_vector:
+ panda_arm: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ - type: SerialContainer
+ name: place
+ properties_exposeTo:
+ source: task
+ values: [eef, hand, group, ik_frame]
+ propertiesConfigureInitFrom:
+ source: PARENT
+ stages:
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.1
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: -1.0}
+ - type: ComputeIK
+ properties: {max_ik_solutions: 5}
+ set:
+ ik_frame:
+ isometry:
+ translation: {x: 0.1, y: 0.0, z: 0.0}
+ quaternion: {r: 1.571, p: 0.785, y: 1.571}
+ link: panda_1_link8
+ propertiesConfigureInitFrom:
+ - source: PARENT
+ values: [eef, group]
+ - source: INTERFACE
+ values: [target_pose]
+ stage:
+ type: GeneratePose
+ set:
+ monitored_stage: pick_up
+ pose:
+ point: {x: 0.100000, 0.700000: ~, z: 0.9305f}
+ orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: open
+ - type: ModifyPlanningScene
+ set:
+ detach_object:
+ object: bottle
+ link: panda_1_link8
+ allow_collisions:
+ first: bottle
+ second:
+ joint_model_group_name: hand_1
+ allow: false
+ - type: MoveRelative
+ planner: cartesian
+ properties:
+ link: panda_1_link8
+ min_distance: 0.07
+ max_distance: 0.2
+ propertiesConfigureInitFrom:
+ source: PARENT
+ values: [group]
+ set:
+ direction:
+ vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
+ - type: MoveTo
+ planner: sampling
+ properties:
+ group: hand_1
+ set:
+ goal: close
+ - name: move to ready
+ type: MoveTo
+ planner: sampling
+ propertiesConfigureInitFrom:
+ source: PARENT
+ value:
+ - group
+ set:
+ goal: ready
+max_planning_solutions: 10
\ No newline at end of file
diff --git a/src/mtc/mtc_task_file/dummy.yaml b/src/mtc/mtc_task_file/dummy.yaml
index ddf5b49..8bd37d8 100644
--- a/src/mtc/mtc_task_file/dummy.yaml
+++ b/src/mtc/mtc_task_file/dummy.yaml
@@ -1,208 +1,208 @@
planners:
- - id: cartesian
- type: CartesianPath
- - id: sampling
- type: PipelinePlanner
+ - id: "cartesian"
+ type: "CartesianPath"
+ - id: "sampling"
+ type: "PipelinePlanner"
properties:
step_size: 0.005
- goal_joint_tolerance: 1e-05
- - id: interpolation
- type: JointInterpolationPlanner
+ goal_joint_tolerance: 0.00001
+ - id: "interpolation"
+ type: "JointInterpolationPlanner"
task:
- name: Pick and Place test
+ name: "Pick and Place test"
properties:
- group: Robot_arm1
- eef: hand_1
- hand_grasping_frame: panda_1_link8
- ik_frame: panda_1_link8
- hand: hand_1
+ group: "panda_arm1"
+ eef: "hand_1"
+ hand_grasping_frame: "panda_1_link8"
+ ik_frame: "panda_1_link8"
+ hand: "hand_1"
stages:
- - name: current
- type: CurrentState
- - name: move to ready
- type: MoveTo
- id: ready
- planner: sampling
+ - name: "current"
+ type: "CurrentState"
+ - name: "move to ready"
+ type: "MoveTo"
+ id: "ready"
+ planner: "sampling"
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
values:
- - group
+ - "group"
set:
- goal: ready
- - type: MoveTo
- planner: sampling
- id: hand_open
+ goal: "ready"
+ - type: "MoveTo"
+ planner: "sampling"
+ id: "hand_open"
properties:
- group: hand_1
+ group: "hand_1"
set:
- goal: open
- - type: Connect
+ goal: "open"
+ - type: "Connect"
group_planner_vector:
- panda_arm: sampling
+ panda_arm1: "sampling"
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
- type: SerialContainer
- name: grasp
+ name: "grasp"
properties_exposeTo:
- source: task
- values: [eef, hand, group, ik_frame]
+ source: "task"
+ values: ["eef", "hand", "group", "ik_frame"]
propertiesConfigureInitFrom:
- source: PARENT
- values: [eef, hand, group, ik_frame]
+ source: "PARENT"
+ values: ["eef", "hand", "group", "ik_frame"]
stages:
- - type: MoveRelative
- planner: cartesian
+ - type: "MoveRelative"
+ planner: "cartesian"
properties:
- link: panda_1_link8
+ link: "panda_1_link8"
min_distance: 0.07
max_distance: 0.2
propertiesConfigureInitFrom:
- source: PARENT
- values: [group]
+ source: "PARENT"
+ values: ["group"]
set:
direction:
- vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: panda_1_link8}}
- - type: ComputeIK
+ vector: {x: 0.0, y: 0.0, z: 1.0, header: {frame_id: "panda_1_link8"}}
+ - type: "ComputeIK"
properties: {max_ik_solutions: 5}
set:
ik_frame:
isometry:
translation: {x: 0.1, y: 0.0, z: 0.0}
quaternion: {r: 1.571, p: 0.785, y: 1.571}
- link: panda_1_link8
+ link: "panda_1_link8"
propertiesConfigureInitFrom:
- - source: PARENT
- values: [eef, group]
- - source: INTERFACE
- values: [target_pose]
+ - source: "PARENT"
+ values: ["eef", "group"]
+ - source: "INTERFACE"
+ values: ["target_pose"]
stage:
- type: GenerateGraspPose
+ type: "GenerateGraspPose"
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
properties:
- object: bottle/body
+ object: "bottle"
angle_delta: 1.571
- pregrasp: open
+ pregrasp: "open"
set:
- monitored_stage: ready
- - type: ModifyPlanningScene
+ monitored_stage: "ready"
+ - type: "ModifyPlanningScene"
set:
allow_collisions:
- first: bottle
+ first: "bottle"
second:
- joint_model_group_name: hand_1
+ joint_model_group_name: "hand_1"
allow: true
- - type: MoveTo
- planner: sampling
+ - type: "MoveTo"
+ planner: "sampling"
properties:
- group: hand_1
+ group: "hand_1"
set:
- goal: close
- - type: ModifyPlanningScene
+ goal: "close"
+ - type: "ModifyPlanningScene"
set:
attach_object:
- object: bottle
- link: panda_1_link8
- - type: MoveRelative
- planner: cartesian
- id: pick_up
+ object: "bottle"
+ link: "panda_1_link8"
+ - type: "MoveRelative"
+ planner: "cartesian"
+ id: "pick_up"
propertiesConfigureInitFrom:
- source: PARENT
- values: [group]
+ source: "PARENT"
+ values: ["group"]
properties:
min_distance: 0.1
max_distance: 0.2
set:
ik_frame:
- link: panda_1_link8
+ link: "panda_1_link8"
direction:
vector: {x: 0.0, y: 0.0, z: 1.0}
- - type: Connect
+ - type: "Connect"
group_planner_vector:
- panda_arm: sampling
+ panda_arm1: "sampling"
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
- type: SerialContainer
- name: place
+ name: "place"
properties_exposeTo:
- source: task
- values: [eef, hand, group, ik_frame]
+ source: "task"
+ values: ["eef", "hand", "group", "ik_frame"]
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
stages:
- - type: MoveRelative
- planner: cartesian
+ - type: "MoveRelative"
+ planner: "cartesian"
properties:
- link: panda_1_link8
+ link: "panda_1_link8"
min_distance: 0.1
max_distance: 0.2
propertiesConfigureInitFrom:
- source: PARENT
- values: [group]
+ source: "PARENT"
+ values: ["group"]
set:
direction:
vector: {x: 0.0, y: 0.0, z: -1.0}
- - type: ComputeIK
+ - type: "ComputeIK"
properties: {max_ik_solutions: 5}
set:
ik_frame:
isometry:
translation: {x: 0.1, y: 0.0, z: 0.0}
quaternion: {r: 1.571, p: 0.785, y: 1.571}
- link: panda_1_link8
+ link: "panda_1_link8"
propertiesConfigureInitFrom:
- - source: PARENT
- values: [eef, group]
- - source: INTERFACE
- values: [target_pose]
+ - source: "PARENT"
+ values: ["eef", "group"]
+ - source: "INTERFACE"
+ values: ["target_pose"]
stage:
- type: GeneratePose
+ type: "GeneratePose"
set:
- monitored_stage: pick_up
+ monitored_stage: "pick_up"
pose:
- point: {x: 0.4f, y: 0.5f, z: 0.1}
+ point: {x: 0.1, y: 0.7, z: 0.9305}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
- - type: MoveTo
- planner: sampling
+ - type: "MoveTo"
+ planner: "sampling"
properties:
- group: hand_1
+ group: "hand_1"
set:
- goal: open
- - type: ModifyPlanningScene
+ goal: "open"
+ - type: "ModifyPlanningScene"
set:
detach_object:
- object: bottle
- link: panda_1_link8
+ object: "bottle"
+ link: "panda_1_link8"
allow_collisions:
- first: bottle
+ first: "bottle"
second:
- joint_model_group_name: hand_1
+ joint_model_group_name: "hand_1"
allow: false
- - type: MoveRelative
- planner: cartesian
+ - type: "MoveRelative"
+ planner: "cartesian"
properties:
- link: panda_1_link8
+ link: "panda_1_link8"
min_distance: 0.07
max_distance: 0.2
propertiesConfigureInitFrom:
- source: PARENT
- values: [group]
+ source: "PARENT"
+ values: ["group"]
set:
direction:
vector: {x: 0.0, y: 0.0, z: -1.0, header: {frame_id: panda_1_link8}}
- - type: MoveTo
- planner: sampling
+ - type: "MoveTo"
+ planner: "sampling"
properties:
- group: hand_1
+ group: "hand_1"
set:
- goal: close
- - name: move to ready
- type: MoveTo
- planner: sampling
+ goal: "close"
+ - name: "move to ready"
+ type: "MoveTo"
+ planner: "sampling"
propertiesConfigureInitFrom:
- source: PARENT
+ source: "PARENT"
value:
- - group
+ - "group"
set:
- goal: ready
+ goal: "ready"
max_planning_solutions: 10
\ No newline at end of file
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index c8d41fa..1579095 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -1,4 +1,6 @@
#include "impl/abstract_robot.h"
+#include "impl/abstract_map_loader.h"
+#include "impl/map_loader.h"
#include "impl/abstract_robot_element.h"
#include "impl/abstract_robot_element_decorator.h"
#include "impl/abstract_mediator.h"
@@ -15,6 +17,14 @@ int main(int argc, char **argv){
ros::AsyncSpinner spinner(1);
spinner.start();
+ XmlRpc::XmlRpcValue map, task;
+
+ nh.getParam("/data",map);
+ nh.getParam("/task/groups",task);
+
+
+ Abstract_map_loader* map_loader = new Map_loader(map, task);
+
float x1,y1,z1,qx1, qy1, qz1, qw1;
float x2,y2,z2,qx2, qy2, qz2, qw2;
int a,b;
@@ -37,10 +47,9 @@ int main(int argc, char **argv){
nh.getParam("/Robot_arm2/wings",b);
- std::vector<std::vector<tf2::Transform>> objects; //in progress
ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
- Abstract_mediator* mediator = new Moveit_mediator(objects, pub, nh);
+ Abstract_mediator* mediator = new Moveit_mediator(map_loader->task_grasps(), pub, nh);
Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
moveit_mediator->load_robot_description();
@@ -73,7 +82,8 @@ int main(int argc, char **argv){
ceti1->notify();
ceti2->notify();
moveit_mediator->publish_tables();
- moveit_mediator->check_collision(1);
+ moveit_mediator->mediate();
+ //moveit_mediator->check_collision(1);
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
index 3c2ae07..b1c2e40 100644
--- a/src/mtc/src/impl/map_loader.cpp
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -139,9 +139,11 @@ std::vector<pcl::PointXYZ> Abstract_map_loader::create_pcl_box(){
void Map_loader::write_task(){
- std::ofstream o(ros::package::getPath("mtc") + "/mtc_task_file/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
+ std::ofstream o(ros::package::getPath("mtc") + "/mtc_task_file/dummy.yaml");
tf2::Transform target_start = task_grasps_[0].front();
tf2::Transform target_end = task_grasps_[0].back();
+ ROS_INFO("%s %s %s ", std::to_string(target_end.getOrigin().getX()).c_str(), std::to_string(target_end.getOrigin().getY()).c_str(), std::to_string(target_end.getOrigin().getZ()).c_str());
+
YAML::Node node;
//YAML::Comment("yaml-language-server: $schema=/home/matteo/reachability/src/yaml_to_mtc/config/yaml_to_mtc_schema.json");
YAML::Node planner_node;
@@ -153,7 +155,7 @@ void Map_loader::write_task(){
planner_node["id"] = "sampling";
planner_node["type"] = "PipelinePlanner";
planner_node["properties"]["step_size"] = 0.005f;
- planner_node["properties"]["goal_joint_tolerance"] = 0.00001;
+ planner_node["properties"]["goal_joint_tolerance"] = (double) 0.00001;
node["planners"].push_back(planner_node);
planner_node.reset();
@@ -239,7 +241,7 @@ void Map_loader::write_task(){
stage_in_stage["stage"]["type"] = "GenerateGraspPose";
stage_in_stage["stage"]["propertiesConfigureInitFrom"]["source"] = "PARENT";
- stage_in_stage["stage"]["properties"]["object"] = "bottle/body";
+ stage_in_stage["stage"]["properties"]["object"] = "bottle";
stage_in_stage["stage"]["properties"]["angle_delta"] = 1.571f;
stage_in_stage["stage"]["properties"]["pregrasp"] = "open";
stage_in_stage["stage"]["set"]["monitored_stage"] = "ready";
@@ -323,7 +325,7 @@ void Map_loader::write_task(){
stage_in_stage["stage"]["type"] = "GeneratePose";
stage_in_stage["stage"]["set"]["monitored_stage"] = "pick_up";
- stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: 0.4f, y: 0.5f, z: 0.1 }"); // Hier objekt
+ stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: 0.1, y: 0.7, z: 0.9305 }"); // Hier objekt
stage_in_stage["stage"]["set"]["pose"]["orientation"] = YAML::Load("{ x: 0.0, y: 0.0, z: 0.0, w: 1.0}");
stage["stages"].push_back(stage_in_stage);
stage_in_stage.reset();
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index dbb98ea..6d85a3d 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -37,7 +37,41 @@ bool Moveit_mediator::check_collision(const int& robot){
}
void Moveit_mediator::mediate(){
- /*Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
+ // create christophs 'bottle'
+ moveit_msgs::CollisionObject bottle;
+ bottle.id = "bottle";
+ bottle.header.frame_id = "world";
+ bottle.header.stamp = ros::Time::now();
+ bottle.primitives.resize(1);
+ bottle.primitives[0].type = bottle.primitives[0].BOX;
+ bottle.primitives[0].dimensions.resize(3);
+ bottle.primitives[0].dimensions[0] = box_size.getX();
+ bottle.primitives[0].dimensions[1] = box_size.getY();
+ bottle.primitives[0].dimensions[2] = box_size.getZ();
+
+
+ bottle.primitive_poses.resize(1);
+ bottle.primitive_poses[0].position.x = objects_[0].front().getOrigin().getX();
+ bottle.primitive_poses[0].position.y = -0.4f;
+ bottle.primitive_poses[0].position.z = 0.9305f;
+ bottle.primitive_poses[0].orientation.x = 0;
+ bottle.primitive_poses[0].orientation.y = 0;
+ bottle.primitive_poses[0].orientation.z = 0;
+ bottle.primitive_poses[0].orientation.w = 1;
+
+ bottle.operation = bottle.ADD;
+
+ moveit_msgs::PlanningScene planning_scene;
+ planning_scene.is_diff = true;
+ planning_scene.world.collision_objects.push_back(bottle);
+ planning_scene_diff_publisher_.publish(planning_scene);
+
+
+
+
+
+
+ Yaml_Mtc_Parser parser = Yaml_Mtc_Parser();
moveit::task_constructor::Task task = parser.init_task(this->nh_);
int max_planning_solutions = 1; // default one solution
this->nh_.getParam("max_planning_solutions", max_planning_solutions);
@@ -50,7 +84,7 @@ void Moveit_mediator::mediate(){
}
ros::waitForShutdown();
- */
+
};
void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
--
GitLab