Add wood texture and blocks to mock sim (#24)

* Replace blue block with wood block * Fix link and gazebo xacro * Add cubes and waypoints
PickNikRobotics · May 31, 2023 · 05aef75 · 05aef75
1 parent beccab0
commit 05aef75
Show file tree

Hide file tree

Showing 4 changed files with 108 additions and 11 deletions.
diff --git a/src/picknik_ur_base_config/description/picknik_ur5e.xacro b/src/picknik_ur_base_config/description/picknik_ur5e.xacro
@@ -37,6 +37,7 @@
 
   <!-- Import environment macros -->
   <xacro:include filename="$(find picknik_accessories)/descriptions/geometry/collision_and_visual/cube_collision_and_visual.urdf.xacro" />
+  <xacro:include filename="$(find picknik_accessories)/descriptions/geometry/visual/cube_visual.urdf.xacro" />
 
   <link name="world" />
   <!-- arm -->
@@ -100,16 +101,89 @@
 
   <!-- Environment -->
   <link name="environment">
-    <!-- Table -->
-    <xacro:cube_collision_and_visual length="2.0" width="2.0" height="0.25" alpha="0.3">
-      <origin xyz="0 0 ${-0.25/2}" rpy="0 0 0" />
-    </xacro:cube_collision_and_visual>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.5 0.5 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.5 0.5 0.1"/>
+      </geometry>
+    </collision>
   </link>
 
-  <joint name="base_to_environment" type="fixed">
+  <joint name="table_joint" type="fixed">
     <parent link="base" />
     <child link="environment" />
-    <origin rpy="0 0 0" xyz="0 0 0" />
+    <origin rpy="0 0 0" xyz="0 0 -0.05" />
+  </joint>
+
+  <link name="box_1">
+    <xacro:cube_visual length="0.05" width="0.05" height="0.05">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <color rgba="0.1 0.1 0.1 0.9" />
+    </xacro:cube_visual>
+  </link>
+
+  <joint name="box1_on_table" type="fixed">
+    <parent link="environment" />
+    <child link="box_1" />
+    <origin rpy="0 0 0" xyz="0.65 -0.35 0.1" />
+  </joint>
+
+  <link name="box_2">
+    <xacro:cube_visual length="0.05" width="0.05" height="0.05">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <color rgba="0.5 0.5 0.5 0.9" />
+    </xacro:cube_visual>
+  </link>
+
+  <joint name="box2_on_table" type="fixed">
+    <parent link="environment" />
+    <child link="box_2" />
+    <origin rpy="0 0 0" xyz="0.65 0.35 0.1" />
+  </joint>
+
+  <link name="box_3">
+    <xacro:cube_visual length="0.05" width="0.05" height="0.05">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <color rgba="0.8 0.3 0.1 0.9" />
+    </xacro:cube_visual>
+  </link>
+
+  <joint name="box3_on_table" type="fixed">
+    <parent link="environment" />
+    <child link="box_3" />
+    <origin rpy="0 0 0" xyz="0.65 0 0.1" />
+  </joint>
+
+  <link name="box_4">
+    <xacro:cube_visual length="0.05" width="0.05" height="0.05">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <color rgba="0.8 0.3 0.1 0.9" />
+    </xacro:cube_visual>
+  </link>
+
+  <joint name="box4_on_table" type="fixed">
+    <parent link="environment" />
+    <child link="box_4" />
+    <origin rpy="0 0 0" xyz="-0.65 0.25 0.1" />
+  </joint>
+
+  <link name="box_5">
+    <xacro:cube_visual length="0.05" width="0.05" height="0.05">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <color rgba="0.1 0.3 0.8 0.9" />
+    </xacro:cube_visual>
+  </link>
+
+  <joint name="box5_on_table" type="fixed">
+    <parent link="environment" />
+    <child link="box_5" />
+    <origin rpy="0 0 0" xyz="-0.65 -0.25 0.1" />
   </joint>
 
   <!-- External Camera -->

diff --git a/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml b/src/picknik_ur_base_config/waypoints/ur5e_waypoints.yaml
@@ -170,3 +170,25 @@
     position: [-0.51890546480287725, -1.6439539394774378, -0.50593507289886475, -2.1812287769713343, 1.8930472135543823, -0.48202354112734014]
     velocity: []
     effort: []
+- name: Place Block
+  joint_state:
+    header:
+      frame_id: world
+      stamp:
+        sec: 0
+        nanosec: 0
+    name: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
+    position: [0.139626, -2.05949, -1.48353, -1.20428, 1.69297, 0.20944]
+    velocity: []
+    effort: []
+- name: Pick Block
+  joint_state:
+    header:
+      frame_id: world
+      stamp:
+        sec: 0
+        nanosec: 0
+    name: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
+    position: [2.89725, -2.0944, -1.50098, -1.0472, 1.69297, 0.20944]
+    velocity: []
+    effort: []
diff --git a/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro b/src/picknik_ur_gazebo_config/description/picknik_ur_gazebo.xacro
@@ -81,8 +81,9 @@
   <!-- Environment -->
   <link name="environment">
     <!-- Table -->
-    <xacro:cube_collision_and_visual length="2.0" width="2.0" height="0.25" alpha="0.0">
+    <xacro:cube_collision_and_visual length="2.0" width="2.0" height="0.25">
       <origin rpy="0 0 0" xyz="0 0 ${-0.25/2}" />
+      <color rgba="0 0 1 0" />
     </xacro:cube_collision_and_visual>
   </link>
 

diff --git a/src/picknik_ur_site_config/objectives/3_waypoint_pick_and_place.xml b/src/picknik_ur_site_config/objectives/3_waypoint_pick_and_place.xml
@@ -12,19 +12,19 @@
                 <Control ID="Sequence">
                     <!-- Pick object from "Grab", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Grab" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
                         <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
-                        <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
                         <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToJointState" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
                         <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
-                        <Action ID="MoveToJointState" waypoint_name="Grab" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
+                        <Action ID="MoveToJointState" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
                         <Action ID="MoveToJointState" waypoint_name="Behind" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller"/>
                     </Control>