def linethemup(self):
print("line em up")
# you start with a predefined location of where each block is going to end
#for every block in board, if block is not in correct position:
#move block to correct position
#colors = ["black", "red", "orange", "yellow", "green", "blue", "violet", "pink"]
#temp_locs = [(-150, -150), (-150, -110), (-150, -70), (-150, -30), (-150, 10), (-150,50), (-150, 90), (-150, 130)]
colors = ["yellow", "red", "green", "blue"]
temp_locs = [(-150, -30), (-150, 130), (-150, 50)]
height = [0, 30, 0]
# height=[90, 0, 0, 0, 55, 0, 0, 0]
for i, color in enumerate(colors):
x = self.kinect.blocks[color]["centroid"][0]
y = self.kinect.blocks[color]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
print(color, " ", x, " ", y, " ", z)
theta = kinematics.IK([x * 1000, y * 1000, height[i] + 50])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
theta = kinematics.IK([x * 1000, y * 1000, height[i]])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.close_gripper()
#move up
theta = kinematics.IK([x * 1000, y * 1000, 100])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
#move to predetermined position
theta = kinematics.IK([temp_locs[i][0], temp_locs[i][1], 0])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.open_gripper()
#move up
theta = kinematics.IK([temp_locs[i][0], temp_locs[i][1], 100])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.set_next_state("idle")
def get_IK_solution(self, pos, omega_start, phi, ori, offset=True):
omega = omega_start
while True:
try:
theta = kinematics.IK(self.rexarm,
pos,
omega,
phi=phi,
ori=ori,
offset=offset)
return theta
except:
pass
if np.abs(omega_start - omega) > np.pi / 2.0:
raise ValueError('no IK solution')
elif omega_start < -np.pi / 4.0:
omega += 1 * np.pi / 180
else:
omega -= 1 * np.pi / 180
def click_and_grab_task3(self, block_coordinates, drop_coordinates):
print("Executing Task 1")
# Waiting for 3 seconds for user to click the block location
time.sleep(3)
# Check if the click has been made by the user
if(len(block_coordinates) == 2):
x=block_coordinates[0]
y=block_coordinates[1]
Z=self.find_z_at_xy(x,y)
world_value=self.pixel_to_world_coords(x,y)
# Generating the pose matrix (Multiplying X,Y,Z by 10 since inputs to IK are in mm)
# Pose 1 is position 3 cm above the block location
pose1=[world_value.item(0)*10,world_value.item(1)*10,(Z+3)*10]
# Pose 2 is position to grab the block
pose2=[world_value.item(0)*10,world_value.item(1)*10,Z*10]
# print ("X, Y, Z values of the location to pick block is ",pose2)
# Calling the Inverse Kinematics function to determine the required joint angles for Pose 1
execute_states = kine.IK(pose1)
# Trajectory Planning to Pick the block and drop it at place
if execute_states is None:
print ("Cannot go to pose above the block location for picking")
else:
print ("Goint to step 1 to pick item at pose",execute_states)
print("Z to pick up item 3 cm above is",Z+3)
self.rexarm.toggle_gripper() # open
self.execute_fast_movement(execute_states)
# Calling the Inverse Kinematics function to determine the required joint angles for Pose 2
down_states = kine.IK(pose2)
if down_states is None:
print("Cannot go to pose to drop the block")
else:
print ("Goint to step 2 to pick item at pose",down_states)
print("Z to pick up item above is",Z)
self.execute_slow_movement(down_states)
self.rexarm.toggle_gripper() #close
## Once the block has been picked the arm should open up to ensure block is properly gripped. This pose is defined by idlePos
self.execute_slow_movement(execute_states)
idlePos = [[0.0, 0, 0.0, 0.0, -np.pi/4,0]]
self.execute_fast_movement(idlePos)
self.rexarm.toggle_gripper() # Opening the gripper
self.rexarm.toggle_gripper() # Closing the gripper
# Dropping the block as per Block Drop Coordinates
x_drop=drop_coordinates[0][0] # These are in pixel reference frame
y_drop=drop_coordinates[1][0]
z_drop=self.find_z_at_xy(x_drop,y_drop)
if z_drop>118:
alpha = np.pi/2
else:
alpha = 0.0
world_value=self.pixel_to_world_coords(x_drop,y_drop)
# Constructing the pose for pose
pix_center=self.pixel_center_loc()
# X and Y locations in the RGB space in pixels with (0,0) at the robot base center
x_drop=x_drop-pix_center.item(0)
y_drop=pix_center.item(1)-y_drop
pose_drop_intermediate=[world_value.item(0)*10,world_value.item(1)*10,(z_drop+4)*10]
down_states_intermediate = kine.IK2(pose_drop_intermediate, alpha)
if down_states_intermediate is None:
print ("Cannot go to pose above the block location for dropping")
else:
print ("Goint to step 3 to drop item at pose",down_states_intermediate)
# print ("Z to drop up item 3 cm above is",z_drop+7)
self.execute_fast_movement(down_states_intermediate)
pose_drop=[world_value.item(0)*10,world_value.item(1)*10,(z_drop+1)*10]
down_states = kine.IK2(pose_drop, alpha)
print("Before Update down_states",down_states)
if down_states is None:
print("Cannot go to pose to drop the block (block picked up")
else:
print ("Goint to step 4 to drop item at pose",down_states)
# print ("Z to drop up item 1 cm above is",z_drop+3)
self.execute_slow_movement(down_states)
self.rexarm.toggle_gripper() # Opening the gripper
pose_interm_up=[world_value.item(0)*10,world_value.item(1)*10,(z_drop+7)*10]
intermediate_up_states = kine.IK2(pose_interm_up, alpha)
print("Before Update intermediate_up_states",intermediate_up_states)
if intermediate_up_states is None:
print("Cannot go to pose to drop the block (block picked up")
else:
print ("Goint to step 5 intermediate_up_states",intermediate_up_states)
self.execute_slow_movement(intermediate_up_states)
# self.rexarm.toggle_gripper() # Opening the gripper
idlePos = [[0.0, 0, 0.0, 0.0, -np.pi/4,0]]
self.execute_fast_movement(idlePos)
self.rexarm.toggle_gripper() # Opening the gripper
def click_and_grab(self):
print("Waiting to click the block")
# Waiting for 3 seconds for user to click the block location
time.sleep(3)
# Check if the click has been made by the user
if(self.kinect.new_click == True):
# X, Y desired coordinates in camera image frame
x =self.kinect.last_click[0].copy()
y=self.kinect.last_click[1].copy()
#############################################
# CAMERA FRAME TO DEPTH FRAME #
#############################################
# Taking in the pixel values in camera frame and transforming to the kinect depth frame
pixel_value=np.array([x,y])
# Converting 10 bit depth to real distance using provided analytical function
z = self.kinect.currentDepthFrame[int(pixel_value.item(1))][int(pixel_value.item(0))]
Z = 12.36 * np.tan(float(z)/2842.5 + 1.1863)
# 95 cm marks the z location of the base plane wrt to the camera. Subtracting 95 to measure +z from the base plane
Z = 95-Z
#############################################
# CAMERA FRAME TO WORLD FRAME #
#############################################
# Extracting the origin of the camera frame (Following 4 quadrant system)
pix_center=self.pixel_center_loc()
# X and Y locations in the RGB space in pixels with (0,0) at the robot base center
x=x-pix_center.item(0)
y=pix_center.item(1)-y
# Taking in the pixel values in camera frame and transforming to the world frame
pixel_value=np.array([x,y])
pixel_value=np.transpose(pixel_value)
# Extracting the affine matrix computed during camera calibration
affine=self.return_affine()
affine=affine[0:2,0:2]
# World x,y location corresponding to iamge frame x,y location
world_value=np.matmul(affine,pixel_value)
# Generating the pose matrix (Multiplying X,Y,Z by 10 since inputs to IK are in mm)
# Pose 1 is position 3 cm above the block location
pose1=[world_value.item(0)*10,world_value.item(1)*10,(Z+3)*10]
# Pose 2 is position to grab the block
pose2=[world_value.item(0)*10,world_value.item(1)*10,Z*10]
print ("X, Y, Z values of the location to pick block is ",pose2)
# Calling the Inverse Kinematics function to determine the required joint angles for Pose 1
execute_states = kine.IK(pose1)
# Trajectory Planning to Pick the block
if execute_states is not None:
self.rexarm.toggle_gripper() # open
for i, wp in enumerate(execute_states):
if i==0 and wp==np.zeros(self.rexarm.num_joints).tolist():
pass
else:
print(wp)
# print(type(wp))
initial_wp = self.tp.set_initial_wp()
final_wp = self.tp.set_final_wp(wp)
T = self.tp.calc_time_from_waypoints(initial_wp, final_wp, 1)
plan_pts, plan_velos = self.tp.generate_quintic_spline(initial_wp, final_wp,T)
self.tp.execute_plan(plan_pts, plan_velos)
self.rexarm.pause(1)
# Calling the Inverse Kinematics function to determine the required joint angles for Pose 2
down_states = kine.IK(pose2)
if down_states is not None:
for i, wp in enumerate(down_states):
if i==0 and wp==np.zeros(self.rexarm.num_joints).tolist():
pass
else:
print(wp)
print(type(wp))
initial_wp = self.tp.set_initial_wp()
final_wp = self.tp.set_final_wp(wp)
T = self.tp.calc_time_from_waypoints(initial_wp, final_wp, 0.2)
plan_pts, plan_velos = self.tp.generate_quintic_spline(initial_wp, final_wp,T)
self.tp.execute_plan(plan_pts, plan_velos)
self.rexarm.pause(1)
self.rexarm.toggle_gripper() #close
## Once the block has been picked the arm should open up to ensure block is properly gripped. This pose is defined by idlePos
idlePos = [[0.0, 0, 0.0, 0.0, -np.pi/4,0]]
for i, wp in enumerate(idlePos):
if i==0 and wp==np.zeros(self.rexarm.num_joints).tolist():
pass
else:
print(wp)
print(type(wp))
initial_wp = self.tp.set_initial_wp()
final_wp = self.tp.set_final_wp(wp)
T = self.tp.calc_time_from_waypoints(initial_wp, final_wp, 1)
plan_pts, plan_velos = self.tp.generate_quintic_spline(initial_wp, final_wp,T)
self.tp.execute_plan(plan_pts, plan_velos)
self.rexarm.pause(1)
self.rexarm.toggle_gripper()
self.rexarm.toggle_gripper()
# Now the system can accept new input from user
self.kinect.new_click = 0
else:
print("Not Reachable, Retry")
def execute_plan(self,
plan,
look_ahead=0,
viz=False,
controller='indep_joint'):
'''
Iterate through waypoints specified by:
plan=[np.array([j1,j2,j3,j4,j5]),...]
viz allows for plotting of the trajectories using matplotlib
'''
if controller == 'IK':
for waypoint in plan:
if np.shape(waypoint) == (1, ):
# Pause type waypoint
self.set_initial_wp()
self.pause(waypoint[0])
num_points = int(.25 * waypoint[0] / self.dt)
curr_traj_obj = [
np.vstack([self.initial_wp] * num_points),
np.vstack([np.zeros(6)] * num_points)
]
curr_world = np.vstack(
[kinematics.FK_dh(self.initial_wp, 6).t] * num_points)
self.history = np.vstack([self.history, curr_traj_obj[0]])
self.world_history = np.vstack(
[self.world_history, curr_world])
else:
# Movement type waypoint
joint_waypoint = kinematics.IK(
get_T_from_euler_pos(waypoint[3:-1], waypoint[:3]))
if waypoint[-1] == 1:
self.rexarm.close_gripper()
self.grip = 1
if waypoint[-1] == 0:
self.rexarm.open_gripper()
self.grip = 0
self.set_initial_wp()
self.set_final_wp(joint_waypoint)
num_points = 0 #int(.1/self.dt)
#curr_pause_obj = [np.vstack([self.final_wp] * num_points), np.vstack([np.zeros(6)] * num_points)]
curr_traj_obj = self.go(
max_speed=2.5, lookahead=look_ahead) #high speed = 4
#curr_traj_obj[0] = np.vstack([curr_traj_obj[0],curr_pause_obj[0]])
#curr_traj_obj[1] = np.vstack([curr_traj_obj[1],curr_pause_obj[1]])
self.traj_obj[0] = np.vstack(
[self.traj_obj[0], curr_traj_obj[0]])
self.traj_obj[1] = np.vstack(
[self.traj_obj[1], curr_traj_obj[1]])
self.stop()
if controller == 'indep_joint':
print('trajectory_planner: using independent joint control')
traj_list = 0
v_list = 0
for waypoint in plan:
if np.shape(waypoint) == (1, ):
# Pause type waypoint
self.set_initial_wp()
self.pause(waypoint[0])
curr_traj = self.generate_cubic_spline(
self.initial_wp, self.initial_wp, waypoint[0])[0]
else:
# Movement type waypoint
self.set_initial_wp()
self.set_final_wp(waypoint)
curr_traj_obj = self.go(max_speed=1, lookahead=look_ahead)
curr_traj = curr_traj_obj[0]
curr_v = curr_traj_obj[1]
# Add waypoint to list for plotting
if type(traj_list) == int:
traj_list = curr_traj
v_list = curr_v
else:
traj_list = np.vstack([traj_list, curr_traj])
v_list = np.vstack([v_list, curr_v])
self.stop()
if viz:
fig = plt.figure()
ax1 = plt.subplot(121)
for i in range(5):
ax1.plot(np.transpose(traj_list)[i],
label='Joint {} command'.format(i + 1),
color=COLORS[i],
linestyle='--')
ax1.plot(np.transpose(self.history)[i],
label='Joint {} trajectory'.format(i + 1),
color=COLORS[i],
linestyle='-')
ax1.legend(loc='best')
# Velocity plotting
ax2 = plt.subplot(122)
for i in range(5):
ax2.plot(np.transpose(v_list)[i],
label='Joint {} command'.format(i + 1),
color=COLORS[i],
linestyle='--')
#ax2.plot(np.transpose(self.history)[i], label='Joint {} trajectory'.format(i + 1), color=COLORS[i], linestyle='-')
ax2.legend(loc='best')
plt.tight_layout()
plt.show()
def clickgrabdrop(self):
self.status_message = "State: Click and Grab and Click and Drop"
self.current_state = "click grab drop"
while (not self.grab_position):
self.status_message = "Click on block to grab"
while (not self.drop_position):
self.status_message = "Click on drop position"
print("grab and drop are : ", self.grab_position, self.drop_position)
#Convert pixel to world coordinates
y = self.grab_position[1] - 45
x = self.grab_position[0] - 250
z = self.kinect.currentDepthFrame[y][x]
print(y, x, z)
camera_coordinates = np.array([[x], [y], [1]]).astype(np.float32)
xy_world = np.matmul(self.kinect.workcamera_affine, camera_coordinates)
z_w = 0.94 - .1236 * np.tan(z / 2842.5 + 1.1863)
#move to block 1
theta = kinematics.IK(
[xy_world[0][0] * 1000, -xy_world[1][0] * 1000, z_w * 1000 - 20])
print(theta)
#theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.close_gripper()
#move up
theta = kinematics.IK(
[xy_world[0][0] * 1000, -xy_world[1][0] * 1000, z_w * 1000 + 30])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
#drop block
#Convert pixel to world coordinates
y = self.drop_position[1] - 45
x = self.drop_position[0] - 250
z = self.kinect.currentDepthFrame[y][x]
#print(y,x,z)
camera_coordinates = np.array([[x], [y], [1]]).astype(np.float32)
xy_world = np.matmul(self.kinect.workcamera_affine, camera_coordinates)
z_w = 0.94 - .1236 * np.tan(z / 2842.5 + 1.1863)
theta = kinematics.IK(
[xy_world[0][0] * 1000, -xy_world[1][0] * 1000, z_w * 1000 + 30])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.open_gripper()
#print(xy_world, z_w)
self.grab_position = ()
self.drop_position = ()
if self.prev_state == "manual":
self.set_next_state("manual")
else:
self.set_next_state("idle")
def stackthemhigh(self):
colors = [
"black", "red", "orange", "yellow", "green", "blue", "violet",
"pink"
]
temp_locs = [(100, 100), (120, 120), (140, 140), (160, 160),
(180, 180), (200, 200), (220, 220)]
x_pred = -130.4
y_pred = 101.84
for i, color in enumerate(colors):
count = i
while type(self.kinect.blocks[color]["centroid"][0]) != "float":
count = count + 1
x = self.kinect.blocks[colors[count]]["centroid"][0]
y = self.kinect.blocks[colors[count]]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
theta = kinematics.IK([x * 1000, y * 1000, 50])
# theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v
] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
theta = kinematics.IK([x * 1000, y * 1000, 0])
# theta = kinematics.IK([166.57, -55.87])
[q, v
] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.close_gripper()
# move up
# print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x * 1000, y * 1000, 40 * i + 50])
# print(theta)
[q, v
] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
# move to predetermined position
theta = kinematics.IK([
temp_locs[count - 1][0], temp_locs[count - 1][1],
40 * i + 50
])
# theta = kinematics.IK([166.57, -55.87])
[q, v
] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.open_gripper()
x = self.kinect.blocks[color]["centroid"][0]
y = self.kinect.blocks[color]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
theta = kinematics.IK([x * 1000, y * 1000, 50])
# theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
theta = kinematics.IK([x * 1000, y * 1000, 0])
# theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.close_gripper()
# move up
# print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x * 1000, y * 1000, 40 * i + 50])
# print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
# move to predetermined position
theta = kinematics.IK([x_pred, y_pred, 40 * i + 50])
# theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.pause(2)
self.rexarm.open_gripper()
print("stack em high")
self.set_next_state("idle")
def pickandstack(self):
#predetermined stacking position
x_pred = -150.0
y_pred = -20.0
#moving above red block
x = self.kinect.blocks["red"]["centroid"][0]
y = self.kinect.blocks["red"]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
print("Red coordinates", x, y, z)
theta = kinematics.IK([x * 1000, y * 1000, 50])
#theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
theta = kinematics.IK([x * 1000, y * 1000, 0])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.close_gripper()
#move up
#print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x * 1000, y * 1000, 100])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
#move to predetermined position
theta = kinematics.IK([x_pred, y_pred, 0])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.open_gripper()
#move up
#print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x_pred, y_pred, 100])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
###########################################
#moving above bluee block
x = self.kinect.blocks["blue"]["centroid"][0]
y = self.kinect.blocks["blue"]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
print("Red coordinates", x, y, z)
theta = kinematics.IK([x * 1000, y * 1000, 50])
#theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
theta = kinematics.IK([x * 1000, y * 1000, 0])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.close_gripper()
#move up
#print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x * 1000, y * 1000, 80])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
x = self.kinect.blocks["red"]["centroid"][0]
y = self.kinect.blocks["red"]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
x = x_pred
y = y_pred
#move to predetermined position
theta = kinematics.IK([x - 10, y, 43])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.open_gripper()
#move up
#print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x, y, 80])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
###########################################
#moving above green block
x = self.kinect.blocks["yellow"]["centroid"][0]
y = self.kinect.blocks["yellow"]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
print("Red coordinates", x, y, z)
theta = kinematics.IK([x * 1000, y * 1000, 100])
#theta = kinematics.IK([166.57, -55.87])
self.rexarm.open_gripper()
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
theta = kinematics.IK([x * 1000, y * 1000, 0])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.close_gripper()
#move up
#print([x*1000, y*1000, z*1000 + 30])
theta = kinematics.IK([x * 1000, y * 1000, 100])
#print(theta)
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
x = self.kinect.blocks["blue"]["centroid"][0]
y = self.kinect.blocks["blue"]["centroid"][1]
x, y, z = self.worldCoordinates(x, y)
x = x_pred
y = y_pred
#move to predetermined position
theta = kinematics.IK([x + 30, y, 90])
#theta = kinematics.IK([166.57, -55.87])
[q, v] = self.tp.generate_cubic_spline(self.rexarm.get_positions(),
theta, 3)
self.tp.execute_plan([q, v])
self.rexarm.open_gripper()
self.set_next_state("idle")
