fix the direction of the palletizaion ++

Author: hganchev

ToDo.md

@@ -17,10 +17,10 @@
     - ~~set/get digital outputs~~
     - ~~servo control (power on, break release)~~
     - force control
-    - palletization
+    - ~~palletization~~
     - other?
 - [ ] Examples
     - ~~pick and place~~
-    - palletization
+    - ~~palletization~~
     - path following (generated from G-code) with force control
     - more examples?

examples/ReadMe.md

@@ -16,3 +16,18 @@ The `pick_and_place.py` script can be customized to fit specific picking and pla
 
 ## Palletization Example
 
+The code you provided appears to be a function named Palletization() that performs a series of actions related to palletization. Let's break down the logic step by step:
+
+1. Create a pallet: The code defines three corners of a pallet using coordinate values. Then, it calls a function ur_control.create_pallet() to create a pallet with 10 rows and 10 columns, using the specified corner coordinates.
+
+2. Set the tool center point (TCP): The code calls ur_control.set_tcp() function to set the tool center point to [0, 0, 0.2, 0, 0, 0]. The TCP is the reference point on the robot's end effector where the tool or gripper is attached.
+
+3. Set the payload: The code calls ur_control.set_payload() function to set the payload of the robot. In this case, the payload is set to 0 kg, with the center of gravity at [0, 0, 0] and the inertia matrix [0, 0, 0, 0, 0, 0].
+
+4. Go to pallet position: The code enters a loop that iterates 99 times. In each iteration, it performs the following actions:
+
+Calls `ur_control.go_to_pallet_position_with_offset()` function to move the robot to the pallet position with an offset of `[0, 0, 0.2, 0, 0, 0]`. This is the approach phase.
+Calls `ur_control.go_to_pallet_position()` function to move the robot to the pallet position without any offset. This is the main position.
+Calls `ur_control.go_to_pallet_position_with_offset()` function again to move the robot away from the pallet position with the same offset. This is the leave phase.
+Pauses the execution for 1 second using the sleep() function.
+This code seems to be controlling a robot to perform palletization tasks, such as picking up items from a pallet and placing them elsewhere. The specific implementation details of the functions `ur_control.create_pallet()`, `ur_control.set_tcp()`,` ur_control.set_payload()`, and `ur_control.go_to_pallet_position_with_offset()` are not provided in the code snippet, so it's difficult to provide further insights without more context.

examples/palletization/palletization.py

@@ -34,16 +34,16 @@ def Palletization():
     # set payload if there is any
     ur_control.set_payload(0, [0, 0, 0], [0,0,0,0,0,0])
 
-    # move to pallet position 1
-    ur_control.go_to_pallet_position(pallet1, 1)
-
-    # move to pallet position 2
-    ur_control.go_to_pallet_position(pallet1, 2)
-
-    # move to pallet position 2
-    ur_control.go_to_pallet_position(pallet1, 100)
+    # go to pallet position
+    for i in range(99):
+        ur_control.go_to_pallet_position_with_offset(pallet1, i + 1, [0, 0, 0.2, 0 ,0, 0]) # approach
+        ur_control.go_to_pallet_position(pallet1, i + 1)
+        ur_control.go_to_pallet_position_with_offset(pallet1, i + 1, [0, 0, 0.2, 0 ,0, 0]) # leave
+        sleep(1)
 
 
 if __name__ == '__main__':
     program()
 
+
+

src/pyURControl/ur_control.py

@@ -223,12 +223,12 @@ def create_pallet(rows: int, cols: int, corner1: list, corner2: list, corner3: l
     pallet = []
     for i in range(rows):
         for j in range(cols):
-            x = corner1[0] + i * (corner2[0] - corner1[0]) / (rows - 1) + j * (corner3[0] - corner1[0]) / (cols - 1)
-            y = corner1[1] + i * (corner2[1] - corner1[1]) / (rows - 1) + j * (corner3[1] - corner1[1]) / (cols - 1)
-            z = corner1[2] + i * (corner2[2] - corner1[2]) / (rows - 1) + j * (corner3[2] - corner1[2]) / (cols - 1)
-            rx = corner1[3] + i * (corner2[3] - corner1[3]) / (rows - 1) + j * (corner3[3] - corner1[3]) / (cols - 1)
-            ry = corner1[4] + i * (corner2[4] - corner1[4]) / (rows - 1) + j * (corner3[4] - corner1[4]) / (cols - 1)
-            rz = corner1[5] + i * (corner2[5] - corner1[5]) / (rows - 1) + j * (corner3[5] - corner1[5]) / (cols - 1)
+            x = corner1[0] + i * (corner3[0] - corner1[0]) / (rows - 1) + j * (corner2[0] - corner1[0]) / (cols - 1)
+            y = corner1[1] + i * (corner3[1] - corner1[1]) / (rows - 1) + j * (corner2[1] - corner1[1]) / (cols - 1)
+            z = corner1[2] + i * (corner3[2] - corner1[2]) / (rows - 1) + j * (corner2[2] - corner1[2]) / (cols - 1)
+            rx = corner1[3] + i * (corner3[3] - corner1[3]) / (rows - 1) + j * (corner2[3] - corner1[3]) / (cols - 1)
+            ry = corner1[4] + i * (corner3[4] - corner1[4]) / (rows - 1) + j * (corner2[4] - corner1[4]) / (cols - 1)
+            rz = corner1[5] + i * (corner3[5] - corner1[5]) / (rows - 1) + j * (corner2[5] - corner1[5]) / (cols - 1)
             pallet.append([x, y, z, rx, ry, rz])
     return pallet