Skip to content
This repository has been archived by the owner on Dec 13, 2024. It is now read-only.

Commit

Permalink
Simplify CNC collision to fix motion planning failures when moving be…
Browse files Browse the repository at this point in the history 
…tween `Grasp Left` and `Grasp Right` (#77)

* Simplify CNC collision

* Enable parallel planning

* Use simplifies collision DAE files
  • Loading branch information
@Abishalini
Abishalini authored Aug 1, 2023
1 parent baef76e commit 12aba06
Show file tree
Hide file tree
Showing 4 changed files with 286 additions and 7 deletions.
2 changes: 1 addition & 1 deletion src/picknik_ur_base_config/objectives/move_to_known_pose.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
<Action ID="RetrieveWaypoint" waypoint_joint_state="{target_joint_state}" waypoint_name="{waypoint_name}"/>
<Action ID="InitializeMTCTask" task_id="move_to_known_pose" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}"/>
<Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="true"/>
<Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
<Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
</Control>
Expand Down
13 changes: 7 additions & 6 deletions src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,10 +102,11 @@
<mesh filename="file://$(find picknik_ur_mock_hw_config)/meshes/cnc.dae" scale="1 1 1"/>
</geometry>
</visual>
<!-- CNC base collision -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="0 0.1 0" rpy="0 0 0"/>
<geometry>
<mesh filename="file://$(find picknik_ur_mock_hw_config)/meshes/cnc.dae" scale="1 1 1"/>
<mesh filename="file://$(find picknik_ur_mock_hw_config)/meshes/cnc-collision.dae" scale="1 1 1"/>
</geometry>
</collision>
</link>
Expand Down Expand Up @@ -147,9 +148,9 @@
</geometry>
</visual>
<collision>
<origin xyz="0 0 0.29" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1.0 1.0 0.58"/>
<mesh filename="file://$(find picknik_ur_mock_hw_config)/meshes/table-collision.dae" scale="1 1 1"/>
</geometry>
</collision>
</link>
Expand All @@ -168,9 +169,9 @@
</geometry>
</visual>
<collision>
<origin xyz="0 0 0.29" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="1.0 1.0 0.58"/>
<mesh filename="file://$(find picknik_ur_mock_hw_config)/meshes/table-collision.dae" scale="1 1 1"/>
</geometry>
</collision>
</link>
Expand Down
208 changes: 208 additions & 0 deletions src/picknik_ur_mock_hw_config/meshes/cnc-collision.dae
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,208 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 3.6.1 commit date:2023-07-17, commit time:12:50, hash:8bda729ef4dc</authoring_tool>
</contributor>
<created>2023-08-01T21:32:52</created>
<modified>2023-08-01T21:32:52</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_effects/>
<library_images/>
<library_geometries>
<geometry id="Cube_005-mesh" name="Cube.005">
<mesh>
<source id="Cube_005-mesh-positions">
<float_array id="Cube_005-mesh-positions-array" count="24">-1 -1 -1 -1 -1 1 -1 1 -1 -1 1 1 1 -1 -1 1 -1 1 1 1 -1 1 1 1</float_array>
<technique_common>
<accessor source="#Cube_005-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_005-mesh-normals">
<float_array id="Cube_005-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube_005-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_005-mesh-map-0">
<float_array id="Cube_005-mesh-map-0-array" count="72">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
<technique_common>
<accessor source="#Cube_005-mesh-map-0-array" count="36" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_005-mesh-vertices">
<input semantic="POSITION" source="#Cube_005-mesh-positions"/>
</vertices>
<triangles count="12">
<input semantic="VERTEX" source="#Cube_005-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_005-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Cube_005-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
</triangles>
</mesh>
</geometry>
<geometry id="Cube_006-mesh" name="Cube.006">
<mesh>
<source id="Cube_006-mesh-positions">
<float_array id="Cube_006-mesh-positions-array" count="24">-1 -1 0.03761059 -1 -1 0.9118407 -1 1 0.03761059 -1 1 0.9118407 1 -1 0.03761059 1 -1 0.9118407 1 1 0.03761059 1 1 0.9118407</float_array>
<technique_common>
<accessor source="#Cube_006-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_006-mesh-normals">
<float_array id="Cube_006-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube_006-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_006-mesh-map-0">
<float_array id="Cube_006-mesh-map-0-array" count="72">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
<technique_common>
<accessor source="#Cube_006-mesh-map-0-array" count="36" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_006-mesh-vertices">
<input semantic="POSITION" source="#Cube_006-mesh-positions"/>
</vertices>
<triangles count="12">
<input semantic="VERTEX" source="#Cube_006-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_006-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Cube_006-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
</triangles>
</mesh>
</geometry>
<geometry id="Cube_007-mesh" name="Cube.007">
<mesh>
<source id="Cube_007-mesh-positions">
<float_array id="Cube_007-mesh-positions-array" count="24">-1 -1 0.03761059 -1 -1 0.9118407 -1 1 0.03761059 -1 1 0.9118407 1 -1 0.03761059 1 -1 0.9118407 1 1 0.03761059 1 1 0.9118407</float_array>
<technique_common>
<accessor source="#Cube_007-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_007-mesh-normals">
<float_array id="Cube_007-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube_007-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_007-mesh-map-0">
<float_array id="Cube_007-mesh-map-0-array" count="72">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
<technique_common>
<accessor source="#Cube_007-mesh-map-0-array" count="36" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_007-mesh-vertices">
<input semantic="POSITION" source="#Cube_007-mesh-positions"/>
</vertices>
<triangles count="12">
<input semantic="VERTEX" source="#Cube_007-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_007-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Cube_007-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
</triangles>
</mesh>
</geometry>
<geometry id="Cube_008-mesh" name="Cube.008">
<mesh>
<source id="Cube_008-mesh-positions">
<float_array id="Cube_008-mesh-positions-array" count="24">-1 -1 -1.43962 -1 -1 1 -1 1 -1.43962 -1 1 1 1 -1 -1.43962 1 -1 1 1 1 -1.43962 1 1 1</float_array>
<technique_common>
<accessor source="#Cube_008-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_008-mesh-normals">
<float_array id="Cube_008-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube_008-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_008-mesh-map-0">
<float_array id="Cube_008-mesh-map-0-array" count="72">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
<technique_common>
<accessor source="#Cube_008-mesh-map-0-array" count="36" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_008-mesh-vertices">
<input semantic="POSITION" source="#Cube_008-mesh-positions"/>
</vertices>
<triangles count="12">
<input semantic="VERTEX" source="#Cube_008-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_008-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Cube_008-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="Top_collision" name="Top collision" type="NODE">
<matrix sid="transform">0.8820289 0 0 -0.1269467 0 0.6688613 0 0.04338684 0 0 0.04052223 1.869295 0 0 0 1</matrix>
<instance_geometry url="#Cube_005-mesh" name="Top collision"/>
</node>
<node id="Right_collision" name="Right collision" type="NODE">
<matrix sid="transform">0.2883302 0 0 -0.7209587 0 0.6688613 0 0.04338684 0 0 0.9536631 0.9553806 0 0 0 1</matrix>
<instance_geometry url="#Cube_006-mesh" name="Right collision"/>
</node>
<node id="Left_collision" name="Left collision" type="NODE">
<matrix sid="transform">0.2372117 0 0 0.5176404 0 0.6688613 0 0.04338684 0 0 0.9536631 0.9553806 0 0 0 1</matrix>
<instance_geometry url="#Cube_007-mesh" name="Left collision"/>
</node>
<node id="Bottom_collision" name="Bottom collision" type="NODE">
<matrix sid="transform">0.8820289 0 0 -0.1269467 0 0.6688613 0 0.04338684 0 0 0.4037348 0.5826219 0 0 0 1</matrix>
<instance_geometry url="#Cube_008-mesh" name="Bottom collision"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>
70 changes: 70 additions & 0 deletions src/picknik_ur_mock_hw_config/meshes/table-collision.dae
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
<asset>
<contributor>
<author>Blender User</author>
<authoring_tool>Blender 3.6.1 commit date:2023-07-17, commit time:12:50, hash:8bda729ef4dc</authoring_tool>
</contributor>
<created>2023-08-01T21:33:12</created>
<modified>2023-08-01T21:33:12</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_effects/>
<library_images/>
<library_geometries>
<geometry id="Cube-mesh" name="Cube">
<mesh>
<source id="Cube-mesh-positions">
<float_array id="Cube-mesh-positions-array" count="24">-1 -1 -1 -1 -1 1 -1 1 -1 -1 1 1 1 -1 -1 1 -1 1 1 1 -1 1 1 1</float_array>
<technique_common>
<accessor source="#Cube-mesh-positions-array" count="8" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube-mesh-normals">
<float_array id="Cube-mesh-normals-array" count="18">-1 0 0 0 1 0 1 0 0 0 -1 0 0 0 -1 0 0 1</float_array>
<technique_common>
<accessor source="#Cube-mesh-normals-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube-mesh-map-0">
<float_array id="Cube-mesh-map-0-array" count="72">0.625 0 0.375 0.25 0.375 0 0.625 0.25 0.375 0.5 0.375 0.25 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.75 0.375 1 0.375 0.75 0.375 0.5 0.125 0.75 0.125 0.5 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0 0.625 0.25 0.375 0.25 0.625 0.25 0.625 0.5 0.375 0.5 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.75 0.625 1 0.375 1 0.375 0.5 0.375 0.75 0.125 0.75 0.875 0.5 0.875 0.75 0.625 0.75</float_array>
<technique_common>
<accessor source="#Cube-mesh-map-0-array" count="36" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube-mesh-vertices">
<input semantic="POSITION" source="#Cube-mesh-positions"/>
</vertices>
<triangles count="12">
<input semantic="VERTEX" source="#Cube-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube-mesh-normals" offset="1"/>
<input semantic="TEXCOORD" source="#Cube-mesh-map-0" offset="2" set="0"/>
<p>1 0 0 2 0 1 0 0 2 3 1 3 6 1 4 2 1 5 7 2 6 4 2 7 6 2 8 5 3 9 0 3 10 4 3 11 6 4 12 0 4 13 2 4 14 3 5 15 5 5 16 7 5 17 1 0 18 3 0 19 2 0 20 3 1 21 7 1 22 6 1 23 7 2 24 5 2 25 4 2 26 5 3 27 1 3 28 0 3 29 6 4 30 4 4 31 0 4 32 3 5 33 1 5 34 5 5 35</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="Table_collision" name="Table collision" type="NODE">
<matrix sid="transform">0.3757816 0 0 -5.04553e-5 0 0.3903046 0 0.006199756 0 0 0.2840506 0.2873469 0 0 0 1</matrix>
<instance_geometry url="#Cube-mesh" name="Table collision"/>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>

0 comments on commit 12aba06

Please sign in to comment.