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")