From 0baf738a1274742a88d7825d49e7ae533fd4dd10 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Wed, 1 Dec 2021 23:04:53 +0100
Subject: [PATCH] ...
---
.../profiles/default/devel_collisions.txt | 2 +-
.gitignore | 1 +
.../launch/moveit.rviz | 522 +++++-------------
.../robots/dual_panda_example.urdf.xacro | 20 +-
.../robots/panda_arm_hand.urdf.xacro | 2 +-
.../Positionsanalyse/Base_eval1.yaml | 17 +
.../Positionsanalyse/Base_eval3.yaml | 22 +
.../Positionsanalyse/Single_Base_task10.yaml | 15 +
.../Positionsanalyse/schnitt.yaml | 17 +
src/reachability/include/map_analyse.h | 81 ++-
src/reachability/include/task_viewer.h | 25 +-
...ck_and_place.launch => double_pick.launch} | 0
src/reachability/launch/task_viewer.launch | 4 +-
src/reachability/launch/visualization.launch | 5 -
src/reachability/src/eval_analyse.cpp | 6 +-
src/reachability/src/map_analyse.cpp | 18 +-
src/reachability/src/pick_and_place.cpp | 54 +-
src/reachability/src/single_pick.cpp | 12 +-
src/reachability/src/task_viewer.cpp | 21 -
src/reachability/src/visualization.cpp | 86 ++-
src/reachability/task/eval1.yaml | 8 +
src/reachability/task/eval2.yaml | 10 +
src/reachability/task/eval3.yaml | 11 +
src/reachability/task/schnitt.yaml | 8 +
24 files changed, 442 insertions(+), 525 deletions(-)
create mode 100644 src/reachability/Positionsanalyse/Base_eval1.yaml
create mode 100644 src/reachability/Positionsanalyse/Base_eval3.yaml
create mode 100644 src/reachability/Positionsanalyse/Single_Base_task10.yaml
create mode 100644 src/reachability/Positionsanalyse/schnitt.yaml
rename src/reachability/launch/{pick_and_place.launch => double_pick.launch} (100%)
delete mode 100644 src/reachability/launch/visualization.launch
create mode 100644 src/reachability/task/eval1.yaml
create mode 100644 src/reachability/task/eval2.yaml
create mode 100644 src/reachability/task/eval3.yaml
create mode 100644 src/reachability/task/schnitt.yaml
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 1e89ca8..11f581c 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,2 +1,2 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 5987
+/home/matteo/reachability/devel/./cmake.lock 6278
diff --git a/.gitignore b/.gitignore
index 05f1c70..e2192d1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -15,3 +15,4 @@ msg/*Result.msg
msg/_*.py
build_isolated/
devel_isolated/
+#
\ No newline at end of file
diff --git a/src/dual_panda_moveit_config/launch/moveit.rviz b/src/dual_panda_moveit_config/launch/moveit.rviz
index 3e2b1a2..e781d07 100644
--- a/src/dual_panda_moveit_config/launch/moveit.rviz
+++ b/src/dual_panda_moveit_config/launch/moveit.rviz
@@ -4,9 +4,16 @@ Panels:
Name: Displays
Property Tree Widget:
Expanded:
+ - /Global Options1
+ - /Status1
- /MotionPlanning1
- Splitter Ratio: 0.74256
- Tree Height: 664
+ - /MotionPlanning1/Scene Geometry1
+ - /MotionPlanning1/Scene Robot1
+ - /MotionPlanning1/Planning Request1
+ - /MotionPlanning1/Planning Metrics1
+ - /MotionPlanning1/Planned Path1
+ Splitter Ratio: 0.7425600290298462
+ Tree Height: 398
- Class: rviz/Help
Name: Help
- Class: rviz/Views
@@ -14,6 +21,10 @@ Panels:
- /Current View1
Name: Views
Splitter Ratio: 0.5
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -23,7 +34,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
- Line Width: 0.03
+ Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -35,302 +46,169 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- - Class: moveit_rviz_plugin/MotionPlanning
+ - Acceleration_Scaling_Factor: 0.1
+ Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
- MoveIt_Goal_Tolerance: 0
+ Move Group Namespace: ""
+ MoveIt_Allow_Approximate_IK: false
+ MoveIt_Allow_External_Program: false
+ MoveIt_Allow_Replanning: false
+ MoveIt_Allow_Sensor_Positioning: false
+ MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5
+ MoveIt_Use_Cartesian_Path: false
MoveIt_Use_Constraint_Aware_IK: true
MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829
+ MoveIt_Workspace:
+ Center:
+ X: 0
+ Y: 0
+ Z: 0
+ Size:
+ X: 2
+ Y: 2
+ Z: 2
Name: MotionPlanning
Planned Path:
+ Color Enabled: false
+ Interrupt Display: false
Links:
- base_bellow_link:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_1_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- base_footprint:
+ panda_1_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- base_link:
+ panda_1_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_l_wheel_link:
+ panda_1_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_r_wheel_link:
+ panda_1_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_rotation_link:
+ panda_1_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- br_caster_l_wheel_link:
+ panda_1_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- br_caster_r_wheel_link:
+ panda_1_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- br_caster_rotation_link:
+ panda_1_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- double_stereo_link:
+ panda_1_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- fl_caster_l_wheel_link:
+ panda_1_link8:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- laser_tilt_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_link:
+ panda_1_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_l_finger_tip_link:
+ panda_2_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_motor_accelerometer_link:
+ panda_2_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_palm_link:
+ panda_2_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_r_finger_link:
+ panda_2_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_r_finger_tip_link:
+ panda_2_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_shoulder_lift_link:
+ panda_2_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_shoulder_pan_link:
+ panda_2_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_upper_arm_link:
+ panda_2_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_upper_arm_roll_link:
+ panda_2_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_wrist_flex_link:
+ panda_2_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_wrist_roll_link:
+ panda_2_link8:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- sensor_mount_link:
+ panda_2_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- torso_lift_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- Loop Animation: true
+ Loop Animation: false
Robot Alpha: 0.5
+ Robot Color: 150; 50; 150
Show Robot Collision: false
- Show Robot Visual: true
+ Show Robot Visual: false
Show Trail: false
State Display Time: 0.05 s
+ Trail Step Size: 1
Trajectory Topic: move_group/display_planned_path
Planning Metrics:
Payload: 1
@@ -338,13 +216,14 @@ Visualization Manager:
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
+ TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
- Planning Group: left_arm
+ Planning Group: hand1
Query Goal State: true
Query Start State: false
Show Workspace: false
@@ -355,300 +234,151 @@ Visualization Manager:
Scene Geometry:
Scene Alpha: 1
Scene Color: 50; 230; 50
- Scene Display Time: 0.2
+ Scene Display Time: 0.20000000298023224
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
- base_bellow_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_footprint:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- double_stereo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_gripper_r_finger_tip_link:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ panda_1_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_shoulder_lift_link:
+ panda_1_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_shoulder_pan_link:
+ panda_1_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_upper_arm_link:
+ panda_1_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_upper_arm_roll_link:
+ panda_1_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_wrist_flex_link:
+ panda_1_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_wrist_roll_link:
+ panda_1_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- laser_tilt_mount_link:
+ panda_1_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_elbow_flex_link:
+ panda_1_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_forearm_link:
+ panda_1_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_forearm_roll_link:
+ panda_1_link8:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- r_gripper_l_finger_link:
+ panda_1_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_l_finger_tip_link:
+ panda_2_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_motor_accelerometer_link:
+ panda_2_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_palm_link:
+ panda_2_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_r_finger_link:
+ panda_2_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_gripper_r_finger_tip_link:
+ panda_2_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_shoulder_lift_link:
+ panda_2_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_shoulder_pan_link:
+ panda_2_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_upper_arm_link:
+ panda_2_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_upper_arm_roll_link:
+ panda_2_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_wrist_flex_link:
+ panda_2_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- r_wrist_roll_link:
+ panda_2_link8:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- sensor_mount_link:
+ panda_2_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- torso_lift_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
Robot Alpha: 0.5
- Show Scene Robot: true
+ Show Robot Collision: false
+ Show Robot Visual: false
Value: true
+ Velocity_Scaling_Factor: 0.1
Enabled: true
Global Options:
Background Color: 48; 48; 48
+ Default Light: true
Fixed Frame: world
+ Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
@@ -659,31 +389,41 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
- Distance: 2.9965
+ Distance: 3.546934127807617
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
Focal Point:
- X: 0.113567
- Y: 0.10592
- Z: 2.23518e-07
+ X: -0.259317547082901
+ Y: 0.2674747109413147
+ Z: 1.043086967911222e-07
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
Name: Current View
- Near Clip Distance: 0.01
- Pitch: 0.509797
- Target Frame: /world
- Value: XYOrbit (rviz)
- Yaw: 5.65995
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5447972416877747
+ Target Frame: world
+ Yaw: 0.2699483335018158
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1337
+ Height: 1031
Help:
collapsed: false
Hide Left Dock: false
Hide Right Dock: false
- Motion Planning:
+ MotionPlanning:
+ collapsed: false
+ MotionPlanning - Trajectory Slider:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000100000000000002a2000003adfc0200000007fb000000100044006900730070006c006100790073010000003d0000021f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000262000001880000017d00ffffff00000490000003ad00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
- Width: 1828
- X: 459
- Y: -243
+ Width: 1848
+ X: 72
+ Y: 27
diff --git a/src/franka_description/robots/dual_panda_example.urdf.xacro b/src/franka_description/robots/dual_panda_example.urdf.xacro
index d1e6449..49145e1 100644
--- a/src/franka_description/robots/dual_panda_example.urdf.xacro
+++ b/src/franka_description/robots/dual_panda_example.urdf.xacro
@@ -17,25 +17,7 @@
<link name="world"/>
- <!-- box shaped table as base for the 2 Pandas
- <link name="base">
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0"/>
- <geometry>
- <box size="0 0 0" />
- </geometry>
- <material name="White">
- <color rgba="1.0 1.0 1.0 1.0"/>
- </material>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0"/>
- <geometry>
- <box size="0 0 0" />
- </geometry>
- </collision>
- </link>
- -->
+
<!-- right arm with gripper -->
diff --git a/src/franka_description/robots/panda_arm_hand.urdf.xacro b/src/franka_description/robots/panda_arm_hand.urdf.xacro
index c121e6f..12af2d0 100644
--- a/src/franka_description/robots/panda_arm_hand.urdf.xacro
+++ b/src/franka_description/robots/panda_arm_hand.urdf.xacro
@@ -17,7 +17,7 @@
<xacro:property name="pos_1" value="${props['Basispositionen']['panda_arm1']}" />
- <xacro:panda_arm arm_id="panda" connected_to="world" xyz="${pos_1}"/>
+ <xacro:panda_arm arm_id="panda" connected_to="world" xyz="0 0 0"/>
<xacro:hand ns="panda" rpy="0 0 ${-pi/4}" connected_to="panda_link8"/>
diff --git a/src/reachability/Positionsanalyse/Base_eval1.yaml b/src/reachability/Positionsanalyse/Base_eval1.yaml
new file mode 100644
index 0000000..c0e9442
--- /dev/null
+++ b/src/reachability/Positionsanalyse/Base_eval1.yaml
@@ -0,0 +1,17 @@
+Referenzsystem: Franka Emika 'Panda'
+Name_der_Aufgabe: eval1
+Roboter_Anzahl: 2
+Map_Aufloesung: 0.125
+Zeit: 489.611901452
+Move_Groups:
+ objekt0:
+ panda_arm1:
+ - -0.257868 -0.300013 0.209432
+ - -0.244427 0.322470 0.175040
+ objekt1:
+ panda_arm2:
+ - 0.212868 0.330279 0.122680
+ - 0.259288 -0.316690 0.215937
+Basispositionen:
+ panda_arm1: -0.187500 0.062500 0.000000
+ panda_arm2: 0.437500 0.062500 0.000000
\ No newline at end of file
diff --git a/src/reachability/Positionsanalyse/Base_eval3.yaml b/src/reachability/Positionsanalyse/Base_eval3.yaml
new file mode 100644
index 0000000..5636084
--- /dev/null
+++ b/src/reachability/Positionsanalyse/Base_eval3.yaml
@@ -0,0 +1,22 @@
+Referenzsystem: Franka Emika 'Panda'
+Name_der_Aufgabe: eval3
+Roboter_Anzahl: 3
+Map_Aufloesung: 0.125
+Zeit: 732.125775855
+Move_Groups:
+ objekt0:
+ panda_arm1:
+ - 0.106047 -0.812031 0.173828
+ - -0.624037 -0.633452 0.189452
+ objekt1:
+ panda_arm2:
+ - 0.697399 0.000000 0.177046
+ - 0.655525 0.702519 0.284044
+ objekt2:
+ panda_arm3:
+ - -0.688845 0.632081 0.121845
+ - -0.224445 0.270074 0.168632
+Basispositionen:
+ panda_arm1: -0.812500 -0.437500 0.000000
+ panda_arm2: 0.312500 -0.062500 0.000000
+ panda_arm3: 0.125000 0.312500 0.000000
\ No newline at end of file
diff --git a/src/reachability/Positionsanalyse/Single_Base_task10.yaml b/src/reachability/Positionsanalyse/Single_Base_task10.yaml
new file mode 100644
index 0000000..588cd3f
--- /dev/null
+++ b/src/reachability/Positionsanalyse/Single_Base_task10.yaml
@@ -0,0 +1,15 @@
+Referenzsystem: Franka Emika 'Panda'
+Name_der_Aufgabe: task7
+Roboter_Anzahl: 1
+Map_Aufloesung: 0.125
+Zeit: 483.778259198
+Move_Groups:
+ objekt0:
+ panda_arm1:
+ - 0.840637 0.000000 0.380226
+ - 0.429307 -0.706689 0.413022
+ - -0.235124 0.010056 0.351257
+ - -0.715926 -0.695213 0.358164
+ - -1.195868 0.083197 0.445089
+Basispositionen:
+ panda_arm1: 1 1 1
diff --git a/src/reachability/Positionsanalyse/schnitt.yaml b/src/reachability/Positionsanalyse/schnitt.yaml
new file mode 100644
index 0000000..ca0629f
--- /dev/null
+++ b/src/reachability/Positionsanalyse/schnitt.yaml
@@ -0,0 +1,17 @@
+Referenzsystem: Franka Emika 'Panda'
+Name_der_Aufgabe: task10
+Roboter_Anzahl: 2
+Map_Aufloesung: 0.125
+Zeit: 499.61566182
+Move_Groups:
+ objekt0:
+ panda_arm1:
+ - 0.000000 -0.690402 0.150186
+ - -0.549918 -0.093641 0.165091
+ objekt1:
+ panda_arm2:
+ - 0.073455 0.478572 0.176422
+ - -0.624557 0.957446 0.172993
+Basispositionen:
+ panda_arm1: -0.562500 -0.812500 0.000000
+ panda_arm2: -0.062500 0.750000 0.000000
\ No newline at end of file
diff --git a/src/reachability/include/map_analyse.h b/src/reachability/include/map_analyse.h
index 8b36c72..a3d64ef 100644
--- a/src/reachability/include/map_analyse.h
+++ b/src/reachability/include/map_analyse.h
@@ -25,28 +25,105 @@
void chaining(std::vector<std::vector<tf2::Vector3>>& ok, std::vector<std::vector<std::vector<tf2::Vector3>>>& ok_total);
// Orientierungen für jedes Kettenglied generieren
-void task_ring(std::vector<tf2::Quaternion>& quat );
+
+/**
+ * Generiert alle Orienteirungen für eine Aufgabe
+ * @param quat Quaternion Vektor für alle Orientierungen
+ */
void task_sphere(std::vector<tf2::Quaternion>& quat );
-std::vector<pcl::PointXYZ> generateBoxTree(const tf2::Vector3& origin, float diameter, float resolution);
+
+/**
+ * Generiert Box Struktur zum Abtasten des Octree
+ *
+ * @param origin Mittelpunkt der Struktur
+ * @param diameter Intervall
+ * @param resolution Auflösung
+ * @return std::vector<pcl::PointXYZ> Input für Voxel_Search
+ */
+std::vector<pcl::PointXYZ> BoxTree(const tf2::Vector3& origin, float diameter, float resolution);
+
+/**
+ * XY-Ebene
+ * @param origin Mittelpunkt
+ * @param diameter Intervall
+ * @param resolution Auflösung
+ * @return std::vector<pcl::PointXYZ> Input für Voxel_Search
+ */
std::vector<pcl::PointXYZ> generateGround(const tf2::Vector3& origin, float diameter, float resolution);
+
+/**
+ * Umwandlung in eine PointCloud
+ *
+ * @param vector Vektors mit pcl::Punkten
+ * @return pcl::PointCloud< pcl::PointXYZ >::Ptr PointCloud
+ */
pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
// Filtert den Arbeitsraum nach Schnittmengen in den Subketten
+
+/**
+ * Filtert die BoxTree Struktur nach Kettengliedern bezüglich der Teilkette
+ *
+ * @param intersections Resultat
+ * @param ok_total Operationskette mit Subketten pro kooperativem Teil
+ * @param voxel_dictionary Vektor aus Boxtree Struktur mit Kettenglied abhängigen Einträgen
+ */
void intersection_filter(std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, float>>>>& intersections, std::vector<std::vector<std::vector<tf2::Vector3>>>& ok_total,
std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, std::vector<std::pair<int, float>>>>>>& voxel_dictionary);
// Marker pro Kettenlied, voxelisiert und auf den XY Ebene projeziert
+
+/**
+ * Voxelisierte Marker pro Kettenglied
+ *
+ * @param marker_arr
+ * @param ok_total
+ * @param voxel_dictionary
+ */
void marker_voxel(visualization_msgs::MarkerArray& marker_arr, std::vector<std::vector<std::vector<tf2::Vector3>>>& ok_total,
std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, std::vector<std::pair<int, float>>>>>>& voxel_dictionary);
+
+/**
+ * Marker pro Kettenglied
+ *
+ * @param marker_arr
+ * @param P_total
+ * @param index_total
+ */
void marker_per_glied(visualization_msgs::MarkerArray& marker_arr, std::vector<std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>>& P_total,
std::vector<std::vector<std::vector<float>>>& index_total);
+
+/**
+ * Marker auf der XY-Ebene
+ *
+ * @param marker_arr
+ * @param ok_total
+ * @param voxel_dictionary
+ * @param resolution
+ * @param ground
+ */
void marker_voxel_ground(visualization_msgs::MarkerArray& marker_arr, std::vector<std::vector<std::vector<tf2::Vector3>>>& ok_total,
std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, std::vector<std::pair<int, float>>>>>>& voxel_dictionary, float& resolution,
std::vector<pcl::PointXYZ>& ground);
+
// Marker pro Schnittmenge, voxelisiert und auf den XY Ebene projeziert
+
+/**
+ * Marker Pro Schnittmenge
+ *
+ * @param marker_arr
+ * @param intersections
+ */
void marker_per_intersection(visualization_msgs::MarkerArray& marker_arr, std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, float>>>>& intersections);
+
+/**
+ * Marker pro Schnittmenge auf XY-Ebene
+ *
+ * @param marker_arr
+ * @param ground
+ */
void marker_intersection_to_ground(visualization_msgs::MarkerArray& marker_arr, std::vector<std::vector<std::vector<std::pair<pcl::PointXYZ, float>>>>& ground);
diff --git a/src/reachability/include/task_viewer.h b/src/reachability/include/task_viewer.h
index 1a3fad0..844a02d 100644
--- a/src/reachability/include/task_viewer.h
+++ b/src/reachability/include/task_viewer.h
@@ -18,7 +18,7 @@ boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;
std::vector<interactive_markers::MenuHandler::EntryHandle> all_entrys;
interactive_markers::MenuHandler menu_handler;
-// Alle Funktionen hinsichtlich der Serverkommunkation und des erstellens von Interaktiven Markern
+// Alle Funktionen hinsichtlich der Serverkommunkation und des erstellens von Interaktiven Marker
InteractiveMarkerControl& makeBoxControl( InteractiveMarker &msg, std_msgs::ColorRGBA& col);
void frameCallback(const ros::TimerEvent&);
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
@@ -28,10 +28,31 @@ void make6DofMarker(visualization_msgs::InteractiveMarker& int_marker);
void initMenu(std::vector<interactive_markers::MenuHandler::EntryHandle> &all_entrys);
// Folgende Optionen sind, abhängig der Operaktionsketten-Beschaffenheit, über Rechtsklick auf das 1. Glied (glied_0), möglich.
+/**
+ * Fügt ein Kettenglied hinzu
+ *
+ * @param feedback
+ */
void add_node( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
-void reset( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
+/**
+ * Füge schneidet die Ketten in disjunkte Teilketten
+ *
+ * @param feedback
+ */
void split_here( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
+
+/**
+ * Dokemtation der Aufgabenbeschreibung
+ *
+ * @param feedback
+ */
void write_yaml( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
+
+/**
+ * Transformation eines kooperativen Glieds
+ *
+ * @param feedback
+ */
void add_coop_node( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
diff --git a/src/reachability/launch/pick_and_place.launch b/src/reachability/launch/double_pick.launch
similarity index 100%
rename from src/reachability/launch/pick_and_place.launch
rename to src/reachability/launch/double_pick.launch
diff --git a/src/reachability/launch/task_viewer.launch b/src/reachability/launch/task_viewer.launch
index 12a1d0f..44def2e 100644
--- a/src/reachability/launch/task_viewer.launch
+++ b/src/reachability/launch/task_viewer.launch
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<launch>
- <arg name="name" default="task" />
- <param name="task_name" value="$(arg name)" />
+ <arg name="NAME" default="task" />
+ <param name="task_name" value="$(arg NAME)" />
<node pkg="reachability" type="task_viewer" name="task_viewer" output="screen" required="true"/>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find reachability)/rviz/task_viewer.rviz" required="true" />
</launch>
\ No newline at end of file
diff --git a/src/reachability/launch/visualization.launch b/src/reachability/launch/visualization.launch
deleted file mode 100644
index 4da1c79..0000000
--- a/src/reachability/launch/visualization.launch
+++ /dev/null
@@ -1,5 +0,0 @@
-<?xml version="1.0"?>
-<launch>
- <include file="$(find panda_moveit_config)/launch/demo.launch"></include>
- <node pkg="reachability" type="visualization" name="visualization" output="screen" required="true"/>
-</launch>
\ No newline at end of file
diff --git a/src/reachability/src/eval_analyse.cpp b/src/reachability/src/eval_analyse.cpp
index 1b6694d..3d71898 100644
--- a/src/reachability/src/eval_analyse.cpp
+++ b/src/reachability/src/eval_analyse.cpp
@@ -562,8 +562,8 @@ int main(int argc, char **argv){
ros::Publisher marker_pub = n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1);
while (ros::ok()){
- marker_pub.publish(marker_arr);
- ros::spinOnce();
- loop_rate.sleep();
+ marker_pub.publish(marker_arr);
+ ros::spinOnce();
+ loop_rate.sleep();
}
}
\ No newline at end of file
diff --git a/src/reachability/src/map_analyse.cpp b/src/reachability/src/map_analyse.cpp
index 0488fc5..7351870 100644
--- a/src/reachability/src/map_analyse.cpp
+++ b/src/reachability/src/map_analyse.cpp
@@ -1,16 +1,6 @@
#include "../include/map_analyse.h"
-void task_ring(std::vector<tf2::Quaternion>& quat ){
- const double DELTA = M_PI / 4.0f;
- for (double phi = 0; phi < 2*M_PI; phi += DELTA){
- tf2::Quaternion rot;
- rot.setRPY(0, (M_PI / 2), phi);
- rot.normalize();
- quat.push_back(rot);
- }
-}
-
void task_sphere(std::vector<tf2::Quaternion>& quat ){
const double DELTA = M_PI / 4.0f;
for (double phi = 0; phi < 2*M_PI; phi += DELTA){
@@ -23,7 +13,7 @@ void task_sphere(std::vector<tf2::Quaternion>& quat ){
}
}
-std::vector<pcl::PointXYZ> generateBoxTree(const tf2::Vector3& origin, float diameter, float resolution){
+std::vector<pcl::PointXYZ> BoxTree(const tf2::Vector3& origin, float diameter, float resolution){
unsigned char depth = 16;
std::vector<pcl::PointXYZ> box;
octomap::OcTree* tree = new octomap::OcTree(resolution/2);
@@ -446,7 +436,7 @@ int main(int argc, char **argv){
ROS_INFO("Anzahl: %i", cloud_count);
// prepare grid structures for voxelisation
- std::vector<pcl::PointXYZ> spCenter = generateBoxTree(p, max_abstand, resolution);
+ std::vector<pcl::PointXYZ> spCenter = BoxTree(p, max_abstand, resolution);
std::vector<pcl::PointXYZ> grCenter = generateGround(p, max_abstand, resolution);
ROS_INFO("Arbeitsraum erstellt, starte Voxelisierung.");
@@ -539,7 +529,7 @@ int main(int argc, char **argv){
ROS_INFO("Ground filter");
std::vector<std::vector<std::vector<std::vector<bool>>>> ground_voxel_filter;
- float d = 0.325;
+ float d = 0.25;
for(int i = 0; i < ok_total.size(); i++){
std::vector<std::vector<std::vector<bool>>> filtere_voxel_col;
for(int j = 0; j < ok_total[i].size(); j++){
@@ -550,7 +540,7 @@ int main(int argc, char **argv){
for(int k = 0; k < ok_total[i][j].size(); k++){
tf2::Vector3 task = ok_total[i][j][k];
task.setZ(0);
- (tf2::tf2Distance(goal, task) >= d ) ? filtere_voxel_point.push_back(true) :filtere_voxel_point.push_back(false);
+ (tf2::tf2Distance(goal, task) >= d && tf2::tf2Distance(goal, task)) ? filtere_voxel_point.push_back(true) :filtere_voxel_point.push_back(false);
}
filtere_voxel.push_back(filtere_voxel_point);
}
diff --git a/src/reachability/src/pick_and_place.cpp b/src/reachability/src/pick_and_place.cpp
index c8eef8c..9cf286c 100644
--- a/src/reachability/src/pick_and_place.cpp
+++ b/src/reachability/src/pick_and_place.cpp
@@ -17,6 +17,20 @@
// The circle constant tau = 2*pi. One tau is one rotation in radians.
const double tau = 2 * M_PI;
+void task_ring(std::vector<tf2::Quaternion>& quat, std::vector<tf2::Vector3>& vecs, tf2::Vector3& task){
+ const double DELTA = M_PI / 4.0f;
+ for (double phi = 0; phi < 2*M_PI; phi += DELTA){
+ tf2::Quaternion rot;
+ float x, y;
+ x = 0.2 * cos(phi) * sin(0) + task.getX();
+ y = 0.2 * sin(phi) * sin(0) + task.getY();
+ vecs.push_back(tf2::Vector3(x,y,0));
+ rot.setRPY(0, 0, phi);
+ rot.normalize();
+ quat.push_back(rot);
+ }
+}
+
void openGripper(trajectory_msgs::JointTrajectory& posture, std::string& id){
for (int i = 1; i <= 2;i++){
posture.joint_names.push_back(id + std::to_string(i));
@@ -77,16 +91,19 @@ void pick(moveit::planning_interface::MoveGroupInterface& move_group, tf2::Vecto
move_group.pick("object"+ std::to_string(obj), grasps);
}
-void place(moveit::planning_interface::MoveGroupInterface& move_group, tf2::Vector3& pos, int obj){
+void place(moveit::planning_interface::MoveGroupInterface& move_group, tf2::Transform& trans, int obj){
+ tf2::Quaternion orientation = trans.getRotation();
+ tf2::Vector3 pos = trans.getOrigin();
+
std::vector<moveit_msgs::PlaceLocation> place_location;
std::string id;
place_location.resize(1);
place_location[0].place_pose.header.frame_id = "world";
- tf2::Quaternion orientation;
- orientation.setRPY(0, 0, 0); // A quarter turn about the z-axis
+
+ orientation.setRPY(0, 0, -M_PI); // A quarter turn about the z-axis
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
place_location[0].place_pose.pose.position.x = pos.getX();
@@ -183,6 +200,8 @@ int main(int argc, char** argv){
ros::AsyncSpinner spinner(1);
ros::WallDuration sleep_time(1.0);
spinner.start();
+ ros::Time startamdal = ros::Time::now();
+
std::regex reg("\\s+");
@@ -239,6 +258,7 @@ int main(int argc, char** argv){
std::vector<moveit::planning_interface::MoveGroupInterface> robots;
int table_pos = 0;
+ std::vector<std::string> robot_pos;
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = aufgabenverteilung.begin(); it != aufgabenverteilung.end(); ++it)
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it1 = it->second.begin(); it1 != it->second.end(); ++it1){
@@ -246,6 +266,12 @@ int main(int argc, char** argv){
robots.push_back(std::move(moveit::planning_interface::MoveGroupInterface(it1->first)));
}
+ std::string hallo;
+ nh.getParam("/Basispositionen/panda_arm1" , hallo);
+ robot_pos.push_back(hallo);
+ nh.getParam("/Basispositionen/panda_arm2" , hallo);
+ robot_pos.push_back(hallo);
+
collision_detection::AllowedCollisionMatrix acm = ps.getAllowedCollisionMatrix();
acm.setEntry("object0", "panda_1_leftfinger", true);
acm.setEntry("object0" , "panda_1_rightfinger", true);
@@ -266,22 +292,38 @@ int main(int argc, char** argv){
int table = 0;
*/
-
-
+ double amd = ros::Duration( ros::Time::now() - startamdal).toSec();
+ ROS_INFO("%f",amd);
+
for(int i = 0; i< ok_total.size(); i++){
+ ros::Time startit = ros::Time::now();
for(int j = 0; j < ok_total[i].size();j++){
for(int k = 0; k < (ok_total[i][j].size()-1);k++){
+ tf2::Transform trans;
+ tf2::Quaternion orientation;
+ trans.setOrigin(ok_total[i][j][k+1]);
+
+ std::sregex_token_iterator to_it(robot_pos[i+j].begin(), robot_pos[i+j].end(),reg,-1);
+ std::vector<std::string> temp{to_it, {}};
+ tf2::Vector3 robot_vec(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2]));
+ (robot_vec.getX()<ok_total[i][j][k].getX()) ? orientation.setRPY(0,0,0) :orientation.setRPY(0,0,M_PI);
+ trans.setRotation(orientation);
+
robots[i+j].setSupportSurfaceName("table"+ std::to_string(table_pos));
pick(robots[i+j], ok_total[i][j][k], i);
table_pos++;
robots[i+j].setSupportSurfaceName("table"+ std::to_string(table_pos));
- place(robots[i+j], ok_total[i][j][k+1], i);
+ place(robots[i+j], trans, i);
}
if(ok_total[i].size()>1 && ok_total[i].size()!=j) table_pos--;
}
+ double dif = ros::Duration( ros::Time::now() - startit).toSec();
+ ROS_INFO("%f",dif);
table_pos++;
}
+
+
/*
// take json with positions
diff --git a/src/reachability/src/single_pick.cpp b/src/reachability/src/single_pick.cpp
index 7d418e3..864db99 100644
--- a/src/reachability/src/single_pick.cpp
+++ b/src/reachability/src/single_pick.cpp
@@ -178,6 +178,8 @@ int main(int argc, char** argv){
ros::WallDuration sleep_time(1.0);
spinner.start();
+ ros::Time startamdal = ros::Time::now();
+
std::regex reg("\\s+");
@@ -249,19 +251,23 @@ int main(int argc, char** argv){
int table = 0;
*/
-
+ double amd = ros::Duration( ros::Time::now() - startamdal).toSec();
+ ROS_INFO("%f",amd);
for(int i = 0; i< ok_total.size(); i++){
+ ros::Time startit = ros::Time::now();
for(int j = 0; j < ok_total[i].size();j++){
for(int k = 0; k < (ok_total[i][j].size()-1);k++){
robots[i+j].setSupportSurfaceName("table"+ std::to_string(table_pos));
- pick(robots[i+j], ok_total[i][j][k], i);
+ pick(robots[0], ok_total[i][j][k], i);
table_pos++;
robots[i+j].setSupportSurfaceName("table"+ std::to_string(table_pos));
- place(robots[i+j], ok_total[i][j][k+1], i);
+ place(robots[0], ok_total[i][j][k+1], i);
}
if(ok_total[i].size()>1 && ok_total[i].size()!=j) table_pos--;
}
+ double dif = ros::Duration( ros::Time::now() - startit).toSec();
+ ROS_INFO("%f",dif);
table_pos++;
}
diff --git a/src/reachability/src/task_viewer.cpp b/src/reachability/src/task_viewer.cpp
index 00116f1..8e0b180 100644
--- a/src/reachability/src/task_viewer.cpp
+++ b/src/reachability/src/task_viewer.cpp
@@ -101,27 +101,12 @@ void make6DofMarker(visualization_msgs::InteractiveMarker& int_marker){
server->setCallback(int_marker.name, &processFeedback);
}
-void reset( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
- menu_handler.setVisible(all_entrys[0], true);
- menu_handler.setVisible(all_entrys[1], false);
- menu_handler.setVisible(all_entrys[2], false);
- menu_handler.setVisible(all_entrys[3], false);
- menu_handler.setVisible(all_entrys[4], false);
- menu_handler.apply( *server, "glied_0" );
- for (int i = 1; i <= id; i++){
- server->erase("glied_" + std::to_string(i));
- }
- id = 0;
-
- server->applyChanges();
-}
void add_coop_node( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
// 0 add node
// 1 manipulate coop node
// 2 splits the task
// 3 store points into yaml file
- // 4 reset
InteractiveMarker int_marker;
tf::Point p(0,0,0);
std_msgs::ColorRGBA col;
@@ -138,7 +123,6 @@ void add_coop_node( const visualization_msgs::InteractiveMarkerFeedbackConstPtr
menu_handler.setVisible(all_entrys[1], false);
menu_handler.setVisible(all_entrys[2], false);
menu_handler.setVisible(all_entrys[3], false);
- menu_handler.setVisible(all_entrys[4], true);
menu_handler.apply( *server, "glied_0" );
@@ -150,7 +134,6 @@ void add_node( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feed
// 1 manipulate coop node
// 2 splits the task
// 3 store points into yaml file
- // 4 reset
InteractiveMarker int_marker;
tf::Point p(0,0,0);
std_msgs::ColorRGBA col;
@@ -185,7 +168,6 @@ void split_here( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &fe
// 1 manipulate coop node
// 2 splits the task
// 3 store points into yaml file
- // 4 reset
InteractiveMarker int_marker;
std_msgs::ColorRGBA col;
@@ -241,20 +223,17 @@ void initMenu(std::vector<interactive_markers::MenuHandler::EntryHandle> &all_en
ROS_INFO("(Option) Hier schneiden: Ab 2 Gliedern einer Operationskette beziehungsweise Teilkette ist ein Schnitt möglich, welcher die Kette in 2 disjunkte Teilketten teilt, zusätzlich wird ein weiteres grünes Glied erzeugt und initiiert den Sart der nächsten Kette.");
ROS_INFO("(Option) Hier Kooperationsglied: Ab 2 Gliedern pro Operationskette beziehungsweise Teilkette ist ein Kooperationspunkt möglich, dies ist an der Farbe des letzten Gliedes erkennbar. Es wird kein neues Glied initiiert, sondern das letzte Gleid ist gleichzeitig erstes Glied der neuen Teilkette.");
ROS_INFO("(Option) Generiere Aufgabenbeschreibung: Ab mehreren Teilketten mit >1 Gliedern ist die Formulierung der Aufgabenbeschreibung möglich.");
- ROS_INFO("(Option) Reset: Stellt den Ausgangszustand der Kette wieder her.");
all_entrys.push_back(menu_handler.insert( "Nächstes Glied", &add_node)); // 0 add node
all_entrys.push_back(menu_handler.insert( "Hier Kooperationsglied", &add_coop_node)); // 1 manipulate coop node
all_entrys.push_back(menu_handler.insert( "Hier schneiden", &split_here)); // 2 splits the task
all_entrys.push_back(menu_handler.insert( "Generiere Aufgabenbeschreibung", &write_yaml)); // 3 store points into yaml file
- all_entrys.push_back(menu_handler.insert( "Reset", &reset)); // 4 delete helper cube
menu_handler.setVisible(all_entrys[0], true);
menu_handler.setVisible(all_entrys[1], false);
menu_handler.setVisible(all_entrys[2], false);
menu_handler.setVisible(all_entrys[3], false);
- menu_handler.setVisible(all_entrys[4], false);
}
diff --git a/src/reachability/src/visualization.cpp b/src/reachability/src/visualization.cpp
index 91c9adc..e3934a4 100644
--- a/src/reachability/src/visualization.cpp
+++ b/src/reachability/src/visualization.cpp
@@ -54,8 +54,8 @@ void cube_dec(const float& abstand, const float& resolution, std::vector<geometr
}
-std::vector<visualization_msgs::Marker> sphere_point(tf2::Vector3& origin, double r){
- const double DELTA = M_PI / 4.0f;
+visualization_msgs::Marker sphere_point(tf2::Vector3& origin, double r){
+ const double DELTA = M_PI / 10.0f;
std::vector<visualization_msgs::Marker> p;
@@ -63,13 +63,25 @@ std::vector<visualization_msgs::Marker> sphere_point(tf2::Vector3& origin, doubl
std_msgs::ColorRGBA col;
col.a = 1.0f;
col.g = 1.0f;
-
- int i = 0;
+ visualization_msgs::Marker pose;
+ pose.header.stamp = ros::Time::now();
+ pose.header.frame_id = "world";
+ pose.ns = "sp";
+ pose.scale.x = 0.03;
+ pose.scale.y = 0.03;
+ pose.scale.z = 0.03;
+
+
+ pose.action = visualization_msgs::Marker::ADD;
+ pose.type = visualization_msgs::Marker::POINTS;
+ pose.lifetime = ros::Duration();
+ pose.pose.orientation.w = 1;
+
for (double phi = 0; phi < 2*M_PI; phi += DELTA){
- for (double theta = 0; theta <= M_PI*0.5; theta += DELTA){
- point.x = origin.getX();
- point.y = origin.getY();
- point.z = origin.getZ();
+ for (double theta = 0; theta <= M_PI; theta += DELTA){
+ point.x = 1 * sin(theta) * cos(phi);
+ point.y = 1 * sin(theta) * sin(phi);
+ point.z = 1 * cos(theta);
tf2::Quaternion rot;
rot.setRPY(0, ((M_PI / 2) + theta), phi);
@@ -95,21 +107,20 @@ std::vector<visualization_msgs::Marker> sphere_point(tf2::Vector3& origin, doubl
p.push_back(pose);
i++;
*/
+ std_msgs::ColorRGBA col;
+ col.a =col.g =1;
+ if (theta < M_PI *0.25f){
+ col.g = 0;
+ col.r = 1;
+ }
- visualization_msgs::Marker pose;
- pose.header.stamp = ros::Time::now();
- pose.header.frame_id="world";
- pose.pose.position = point;
- pose.pose.orientation.x= rot.getX();
- pose.pose.orientation.y= rot.getY();
- pose.pose.orientation.z= rot.getZ();
- pose.pose.orientation.w= rot.getW();
- p.push_back(pose);
+ pose.points.push_back(point);
+ pose.colors.push_back(col);
}
}
- return p;
+ return pose;
}
@@ -127,46 +138,11 @@ int main(int argc, char **argv){
ros::Rate loop_rate(1);
- ros::WallDuration(1.0).sleep();
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- moveit::planning_interface::MoveGroupInterface group("panda_arm");
- group.setPlanningTime(45.0);
-
-
-
// prepare marker for index
visualization_msgs::MarkerArray marker_arr;
visualization_msgs::Marker sphere_points, cube_points;
- std_msgs::ColorRGBA col;
-
- sphere_points.header.frame_id = "world";
- sphere_points.ns = "sp";
- sphere_points.header.stamp = ros::Time::now();
- sphere_points.action = visualization_msgs::Marker::ADD;
- sphere_points.type = visualization_msgs::Marker::MESH_RESOURCE;
- sphere_points.lifetime = ros::Duration();
- sphere_points.mesh_use_embedded_materials = true;
- sphere_points.mesh_resource = "package://franka_description/meshes/visual/hand.dae";
- sphere_points.scale.x = 1;
- sphere_points.scale.y = 1;
- sphere_points.scale.z = 1;
- std::vector<visualization_msgs::Marker> points = sphere_point(origin, 0.5f);
- sphere_points.pose.orientation.x = points[5].pose.orientation.x;
- sphere_points.pose.orientation.y = points[5].pose.orientation.y;
- sphere_points.pose.orientation.z = points[5].pose.orientation.z;
- sphere_points.pose.orientation.w = points[5].pose.orientation.w;
- /*
- cube_dec(0.8, 0.2, points);
-
-
- for (geometry_msgs::Point& point : points)
- sphere_points.points.push_back(point);
- */
-
-
- marker_arr.markers.push_back(points[5]);
- marker_arr.markers.push_back(sphere_points);
- ros::Publisher marker_pub = n.advertise< visualization_msgs::Marker>("visualization_marker_array", 1);
+ marker_arr.markers.push_back(sphere_point(origin, 0.5f));
+ ros::Publisher marker_pub = n.advertise< visualization_msgs::MarkerArray>("visualization_marker_array", 1);
while (ros::ok()){
marker_pub.publish(marker_arr);
ros::spinOnce();
diff --git a/src/reachability/task/eval1.yaml b/src/reachability/task/eval1.yaml
new file mode 100644
index 0000000..708f223
--- /dev/null
+++ b/src/reachability/task/eval1.yaml
@@ -0,0 +1,8 @@
+Name_der_Aufgabe: eval1
+Aufgabenbeschreibung:
+ object0:
+ - -0.257868 -0.300013 0.209432
+ - -0.244427 0.322470 0.175040
+ object1:
+ - 0.212868 0.330279 0.122680
+ - 0.259288 -0.316690 0.215937
\ No newline at end of file
diff --git a/src/reachability/task/eval2.yaml b/src/reachability/task/eval2.yaml
new file mode 100644
index 0000000..01f0694
--- /dev/null
+++ b/src/reachability/task/eval2.yaml
@@ -0,0 +1,10 @@
+Name_der_Aufgabe: eval2
+Aufgabenbeschreibung:
+ object0:
+ - -0.384174 -1.117620 0.183886
+ - -0.358958 -0.073666 0.213993
+ - 0.332617 -0.590213 0.155456
+ object1:
+ - -0.423227 1.000479 0.172109
+ - 0.264733 1.559023 0.148107
+ - 0.524469 0.690750 0.305442
\ No newline at end of file
diff --git a/src/reachability/task/eval3.yaml b/src/reachability/task/eval3.yaml
new file mode 100644
index 0000000..6a8a428
--- /dev/null
+++ b/src/reachability/task/eval3.yaml
@@ -0,0 +1,11 @@
+Name_der_Aufgabe: eval3
+Aufgabenbeschreibung:
+ object0:
+ - 0.106047 -0.812031 0.173828
+ - -0.624037 -0.633452 0.189452
+ object1:
+ - 0.697399 0.000000 0.177046
+ - 0.655525 0.702519 0.284044
+ object2:
+ - -0.688845 0.632081 0.121845
+ - -0.224445 0.270074 0.168632
\ No newline at end of file
diff --git a/src/reachability/task/schnitt.yaml b/src/reachability/task/schnitt.yaml
new file mode 100644
index 0000000..a37ac18
--- /dev/null
+++ b/src/reachability/task/schnitt.yaml
@@ -0,0 +1,8 @@
+Name_der_Aufgabe: task10
+Aufgabenbeschreibung:
+ object0:
+ - 0.000000 -0.690402 0.150186
+ - -0.549918 -0.093641 0.165091
+ object1:
+ - 0.073455 0.478572 0.176422
+ - -0.624557 0.957446 0.172993
--
GitLab