diff --git a/README.md b/README.md
index d3b43613f0f8e479d8b7168d605745f0f9738f4a..eb0d304bdd312da742edd10582d81b3f7755440a 100644
--- a/README.md
+++ b/README.md
@@ -1,21 +1,17 @@
# Grocery Restocker
-The task we will be asking our robot to perform is restocking grocery shelves. The robot will check each of the cells on a shelf to determine which ones require a product. Furthermore, depending on which cell is empty, the robot will be able to determine which product belongs on that shelf, retrieve it from the crate, and place it in the appropriate spot. This is demonstrated by the diagram below.
+The task we will be asking our robot to perform is restocking grocery shelves. The robot will check each of the cells on a shelf to determine which ones require a product. Furthermore, depending on which cell is empty, the robot will be able to determine which product belongs on that shelf, retrieve it from the supply, and place it in the appropriate spot.
## To Run the Codes
- Clone a local repo on your VM. Follow the instructions on downloading the VM at https://uofi.app.box.com/s/3sqaarqs2zuniultzyc0ztz7yg4cn25h. There should also be an email sent out on this.
- When you clone the repo the local repo name will be "ECE470-project". Remember to change it to "project", otherwise when you ```catkin_make``` it won't be able to recognize the path.
- Instead of ```rosrun lab2pkg_py lab2_exec.py```, do ```rosrun grocery_py grocery_exec.py``` to run the program.
-- When you run ```roslaunch ur3_driver ur3_gazebo.launch``` it now spawns the world with the shelf model.
+- Open terminal and source the ```project``` folder by entering ```source devel/setup.bash```
+- Run ```roslaunch ur3_driver ur3_gazebo.launch``` to run the gazebo simulator and spawn the world with the shelf model.
+- In a separate terminal, source the ```project``` folder and enter ```rosrun grocery_py grocery_spawn1.py``` to spawn the supply products, represented by blocks. Enter ```n``` when prompted.
+- In the same terminal, enter ```rosrun grocery_py grocery_exec.py``` to run the program.
-## TODO (as of 11/3/2022):
-- Create the objects (I already created a shelf model). I can working on creating the objects (Mike). For now just use the blocks given.
-- Work on the codes so that the manipulator can perform a pick and place on an object (this part should already be done because I am using the move_block code from lab2). But test to see if works.
-- For each position of the shelf, we nedd a corresponding set of joint values, similar to task 2. However, I am not super sure how this part should be done considering that we can't manually move the manipulator in simulation as we did with the physical manipulator.
-## Temporary Update 11/18/2022 (Dylan):
-I modified grocery_exec.py to pick up a red block, and place it on top of the green block. The code is imperfect, capable of picking up the block but not setting it down gently.
-How to run:
-- Before running ```rosrun grocery_py grocery_exec.py``` you should run ```rosrun grocery_py grocery_spawn.py```
- - choose position 1 when prompted
-- When running ```rosrun grocery_py grocery_exec.py``` it will prompt for a loop count. Choose any nonzero option to run the code once. (I commented out the loop example movement code)
+## Update 12/06/2022:
+Because of the limitations of the physical robot and the inverse kinematics solutions, the robot will only stock a 3x3 grid of cells on the shelf. This grid is the +y+z most 3x3 grid. The robot will search each cell using the vacuum gripper input, and if it does not detect a product then it will pick up a product from its supply. The supply is represented by a similar 3x3 array of colored blocks. In practical application, the supply and the shelf can be assumed to be of a known position relative to the robot (once an additional robot-mobility program is added, which will allow the robot to roam the store).
+
diff --git a/src/grocery_py/scripts/grocery_exec.py b/src/grocery_py/scripts/grocery_exec.py
index c242b951a45b97c796d236df251ea2b31ff21043..059474f3bf280df71f1d0d083a337eb4cb8b1b37 100755
--- a/src/grocery_py/scripts/grocery_exec.py
+++ b/src/grocery_py/scripts/grocery_exec.py
@@ -519,27 +519,27 @@ def main():
############## Your Code Start Here ##############
# TODO: modify the code below so that program can get user input
- input_done = 0
- loop_count = 0
-
- while(not input_done):
- input_string = raw_input("Enter number of loops <Either 1 2 3 or 0 to quit> ")
- print("You entered " + input_string + "\n")
-
- if(int(input_string) == 1):
- input_done = 1
- loop_count = 1
- elif (int(input_string) == 2):
- input_done = 1
- loop_count = 2
- elif (int(input_string) == 3):
- input_done = 1
- loop_count = 3
- elif (int(input_string) == 0):
- print("Quitting... ")
- sys.exit()
- else:
- print("Please just enter the character 1 2 3 or 0 to quit \n\n")
+ # input_done = 0
+ # loop_count = 0
+
+ # while(not input_done):
+ # input_string = raw_input("Enter number of loops <Either 1 2 3 or 0 to quit> ")
+ # print("You entered " + input_string + "\n")
+
+ # if(int(input_string) == 1):
+ # input_done = 1
+ # loop_count = 1
+ # elif (int(input_string) == 2):
+ # input_done = 1
+ # loop_count = 2
+ # elif (int(input_string) == 3):
+ # input_done = 1
+ # loop_count = 3
+ # elif (int(input_string) == 0):
+ # print("Quitting... ")
+ # sys.exit()
+ # else:
+ # print("Please just enter the character 1 2 3 or 0 to quit \n\n")
@@ -579,29 +579,67 @@ def main():
# gripper(pub_command, loop_rate, suction_off)
+ #Initializing the shelf position matrix
+ Qi = [0.0, 0.0, 0.0]
+ shelfQ = [ [Qi, Qi, Qi], \
+ [Qi, Qi, Qi], \
+ [Qi, Qi, Qi] ]
+ for i in range(3):
+ for j in range(3):
+ shelfQ[i][j] = [-0.6182, 0.175 + (0.11 * i), 0.54 - (0.12 * j)]
+
+ #Initializing the supply matrix
+ supplyQ = [ [Qi, Qi, Qi], \
+ [Qi, Qi, Qi], \
+ [Qi, Qi, Qi] ]
+ for i in range(3):
+ for j in range(3):
+ supplyQ[i][j] = [0.4 - (0.1 * j), 0.25 - (0.1 * i), 0.026]
+
+ # while(loop_count > 0): #picks up the red block and places it at on top of the green block
+
+ # move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ # move_arm(pub_command, loop_rate, shelf_invk(shelfQ[0][0][0], shelfQ[0][0][1], shelfQ[0][0][2], 0.0), 4.0, 4.0)
+
+ # move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ # move_arm(pub_command, loop_rate, shelf_invk(shelfQ[0][1][0], shelfQ[0][1][1], shelfQ[0][1][2], 0.0), 4.0, 4.0)
+ # move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ # move_arm(pub_command, loop_rate, shelf_invk(shelfQ[0][2][0], shelfQ[0][2][1], shelfQ[0][2][2], 0.0), 4.0, 4.0)
+
+ # loop_count = 0
+
+ for i in range(3):
+ for j in range (3):
+ move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0], shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0] - (0.01 * k), shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
+ gripper(pub_command, loop_rate, suction_on)
+ time.sleep(0.5)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0] + (0.01 * k) - 0.1, shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
+ if gripper_ == 0:
+ #move to the supply, grab it
+ gripper(pub_command, loop_rate, suction_off)
+ move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ move_arm(pub_command, loop_rate, lab_invk(supplyQ[i][j][0], supplyQ[i][j][1], supplyQ[i][j][2]+ 0.1, 0.0), 4.0, 4.0)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, lab_invk(supplyQ[i][j][0], supplyQ[i][j][1], supplyQ[i][j][2] + 0.1 - (0.01 * k), 0.0), 4.0, 4.0)
+ gripper(pub_command, loop_rate, suction_on)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, lab_invk(supplyQ[i][j][0], supplyQ[i][j][1], supplyQ[i][j][2] + (0.01 * k), 0.0), 4.0, 4.0)
+ #move to the shelf, drop it
+ move_arm(pub_command, loop_rate, home, 4.0, 4.0)
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0], shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0] - (0.01 * k), shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
+ gripper(pub_command, loop_rate, suction_off)
+ for k in range (1,11):
+ move_arm(pub_command, loop_rate, shelf_invk(shelfQ[i][j][0] + (0.01 * k) - 0.1, shelfQ[i][j][1], shelfQ[i][j][2], 0.0), 4.0, 4.0)
- while(loop_count > 0): #picks up the red block and places it at on top of the green block
-
- move_arm(pub_command, loop_rate, home, 4.0, 4.0)
- move_arm(pub_command, loop_rate, lab_invk(0.2, 0.05, 0.0425, 0.0), 4.0, 4.0) #moves to the red block
- time.sleep(1.0)
-
- gripper(pub_command, loop_rate, suction_on)
- time.sleep(1.0)
- move_arm(pub_command, loop_rate, home, 4.0, 4.0)
- move_arm(pub_command, loop_rate, lab_invk(0.4, 0.05, 0.07, 0.0), 4.0, 4.0) #moves to an arbitrary position
- time.sleep(1.0)
- gripper(pub_command, loop_rate, suction_off)
- time.sleep(1.0)
-
- move_arm(pub_command, loop_rate, home, 4.0, 4.0)
-
- loop_count = 0
-
-
diff --git a/src/grocery_py/scripts/grocery_spawn1.py b/src/grocery_py/scripts/grocery_spawn1.py
new file mode 100755
index 0000000000000000000000000000000000000000..d8cdb7d719b2089a4db7b5940c4a0ca6c8fc5e50
--- /dev/null
+++ b/src/grocery_py/scripts/grocery_spawn1.py
@@ -0,0 +1,100 @@
+#!/usr/bin/env python
+
+import rospy
+import rospkg
+import os
+import sys
+import yaml
+import random
+from gazebo_msgs.srv import SpawnModel
+from gazebo_msgs.srv import DeleteModel
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Quaternion
+
+yamlpath = 'grocery_data.yaml'
+
+if __name__ == '__main__':
+
+ # Initialize rospack
+ rospack = rospkg.RosPack()
+ # Get path to yaml
+ grocery_path = rospack.get_path('grocery_py')
+ yamlpath = os.path.join(grocery_path, 'scripts', 'grocery_data.yaml')
+
+ with open(yamlpath, 'r') as f:
+ try:
+ # Load the data as a dict
+ data = yaml.load(f)
+ # Load block position
+ block_xy_pos = data['block_xy_pos']
+
+ except:
+ sys.exit()
+
+ # Initialize ROS node
+ rospy.init_node('ur3_gazebo_spawner', anonymous=True)
+ # Initialize ROS pack
+ rospack = rospkg.RosPack()
+ # Get path to block
+ ur_path = rospack.get_path('ur_description')
+ block_path = os.path.join(ur_path, 'urdf', 'block.urdf')
+ block1_path = os.path.join(ur_path, 'urdf', 'block_red.urdf')
+ block2_path = os.path.join(ur_path, 'urdf', 'block_yellow.urdf')
+ block3_path = os.path.join(ur_path, 'urdf', 'block_green.urdf')
+ block_paths = [block1_path, block2_path, block3_path]
+ # Wait for service to start
+ rospy.wait_for_service('gazebo/spawn_urdf_model')
+ spawn = rospy.ServiceProxy('gazebo/spawn_urdf_model', SpawnModel)
+ delete = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
+ """
+ # Starting location ?
+ starting_location = None
+ while not starting_location:
+ starting_location = raw_input("Enter starting location number <Either 1 2 or 3>: ")
+ starting_location = int(starting_location)
+ if (starting_location != 1) and (starting_location != 2) and (starting_location != 3):
+ starting_location = None
+ print("Wrong input \n\n")
+
+ # 0-indexed
+ starting_location -= 1
+ """
+
+ # Missing block ?
+ missing_block = None
+ while missing_block is None:
+ missing_block = raw_input("Missing Block?(y/n): ")
+ missing_block = str(missing_block)
+ if (missing_block != 'y') and (missing_block != 'n'):
+ missing_block = None
+ print("Wrong input \n\n")
+
+ missing_block = (missing_block == 'y')
+ """
+ # Delete previous blocks
+ for height in range(3):
+ block_name = 'block' + str(height + 1)
+ delete(block_name)
+ """
+
+ if not missing_block:
+ # Spawn three blocks
+ for height in range(3):
+ for starting_location in range(3):
+ block_name = 'block' + str(height + 1) + str(starting_location + 1)
+ pose = Pose(Point(block_xy_pos[starting_location][height][0],
+ block_xy_pos[starting_location][height][1], 0), Quaternion(0, 0, 0, 0))
+ spawn(block_name, open(block_paths[2-height], 'r').read(), 'block', pose, 'world')
+ """
+ else:
+ missing_block_height = random.randint(0, 2)
+ # Spawn two blocks
+ for height in range(3):
+ if height == missing_block_height:
+ continue
+ block_name = 'block' + str(height + 1)
+ pose = Pose(Point(block_xy_pos[starting_location][height][0],
+ block_xy_pos[starting_location][height][1], 0), Quaternion(0, 0, 0, 0))
+ spawn(block_name, open(block_paths[2-height], 'r').read(), 'block', pose, 'world')
+ """