diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 1e89ca889ac2472418898e3b7862d151feeaf844..11f581c86b4fd6b6f1fc110e7a6e41d3a9116def 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 05f1c70aff0eee93eb2ee0d5b6d0ab1842adc985..e2192d13eef72508ec3fdf740eb49033bd7f1001 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 3e2b1a2debb5a7faa6dc913ccfdddd4758d68636..e781d070c5e3f8d4c03b927b4dfcdc713bf297e4 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 d1e6449b8700b39cad5675160b4aa6f2f41dc316..49145e13ead28d1fa383e95d75ddb66ea85a25eb 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 c121e6fca8690e7a6006c58a90cd5988f13ccf39..12af2d0de123548446f7ba476da3305789d7b07f 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 0000000000000000000000000000000000000000..c0e94427bc169c126d766a1e8a2004427310e3ce
--- /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 0000000000000000000000000000000000000000..56360845700a58870273d31ba9ad4b0a361c222f
--- /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 0000000000000000000000000000000000000000..588cd3feae1b6139f888fa7a8f69177bf6af5124
--- /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 0000000000000000000000000000000000000000..ca0629fb7eab0465ea2554f6b4b2053e25752e53
--- /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 8b36c727bac6d7a145ae5c3b53bb4f120983e93a..a3d64ef6a54df5c4c1a578ffbbb04395051085dd 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 1a3fad0183793689a389a503fe978211b573f1de..844a02d95a016a71a644db0dc3c42b1c7cfa796c 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 12a1d0f724ace214c88ffa5b7b7865711b14f6a8..44def2ea54bcc942b0b02d76e794865760eb9d0f 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 4da1c79b9517f01cb6d7b7659ba50d4552f6a81e..0000000000000000000000000000000000000000
--- 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 1b6694da962a4550fc31afa40426781317a7d9d2..3d718989d27a34cb305562ef1fe07d0601ea5ce2 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 0488fc5eddfdb48afaf550254ebee4288b906f5e..7351870c9a453535e3dc463387c261a02cd6ae9a 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 c8eef8c5cb961a5031aed0e91202cb55a728a892..9cf286caf155b8c0089dce1476b4e31b13ca1cfa 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 7d418e324528824ff2e5a6ee8c231adff1df205a..864db998c786cc37925192ab157cb6a5fd5d8f36 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 00116f1c41067912aaa4b616a4c8b025db7692a3..8e0b1808ed3fc11bb0cd5e4edfdee9f270559ca4 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 91c9adcc4612383dc580571ebdaaa3a409d93269..e3934a4898ee0f65b5d6f5c5afb2ccb02dd6bb05 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 0000000000000000000000000000000000000000..708f2237481f1631d5a9c472b1d252958d1d1c5a
--- /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 0000000000000000000000000000000000000000..01f0694b25a462304bb3a22c8fb35534d3af454b
--- /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 0000000000000000000000000000000000000000..6a8a428a25c3badf5227f1200aedf558cc1e8c58
--- /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 0000000000000000000000000000000000000000..a37ac18576c3c78bf0e8c4a34809a58bd06f2f95
--- /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