def execute(self, joint_targets, gripper_targets): # Iterate through target joint positions for target in np.hstack((joint_targets, gripper_targets)): # Set new target position self.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(self.clientID) # print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions( self.clientID) # Calculate commands commands = self.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(self.clientID, commands) # Print current joint positions (comment out if you'd like) # print(sensed_joint_positions) vu.step_sim(self.clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = self.has_stably_converged_to_target()
def main(args): success = False try: # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) # Joint targets, radians for revolute joints and meters for prismatic joints gripper_targets = np.asarray([[-0.03, 0.03], [-0.03, 0.03]]) joint_targets = np.radians([[-80, 0, 0, 0, 0], [0, 60, -75, -75, 0]]) # Verify fk from utils import URDFRobot, print_pose robot = URDFRobot('urdf/locobot_description_v3.urdf') from forward_kinematics import ForwardKinematicsSolver fksolver = ForwardKinematicsSolver(robot) for target in joint_targets: print(target) pose = fksolver.getWristPose(target[:5], robot, vu.ARM_JOINT_NAMES[:5]) print_pose(pose) # Instantiate controller and run it controller = ArmController(clientID) controller.execute(joint_targets, gripper_targets) success = True except Exception as e: print(e) traceback.print_exc() finally: # Stop VREP simulation vu.stop_sim(clientID) # Plot time histories if success: controller.plot()
def main(args): global link_cuboid_spec global obstacle_cuboid_spec # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) deg_to_rad = np.pi / 180. link_cuboid_list = ("arm_base_link_joint_collision_cuboid", "shoulder_link_collision_cuboid", "elbow_link_collision_cuboid", "forearm_link_collision_cuboid", "wrist_link_collision_cuboid", "gripper_link_collision_cuboid", "finger_r_collision_cuboid", "finger_l_collision_cuboid") for link_cuboid in link_cuboid_list: d = vu.get_handle_by_name(clientID, link_cuboid) link_spec = {} link_spec["Origin"] = vu.get_object_position(clientID, d) link_spec["Orientation"] = vu.get_object_orientation(clientID, d) link_spec["Dimension"] = vu.get_object_bounding_box(clientID, d) link_cuboid_spec.append(link_spec) obstacle_cuboid_list = ("cuboid_0", "cuboid_1", "cuboid_2", "cuboid_3", "cuboid_4", "cuboid_5") for obstacle_cuboid in obstacle_cuboid_list: d = vu.get_handle_by_name(clientID, obstacle_cuboid) obstacle_spec = {} obstacle_spec["Origin"] = vu.get_object_position(clientID, d) obstacle_spec["Orientation"] = vu.get_object_orientation(clientID, d) obstacle_spec["Dimension"] = vu.get_object_bounding_box(clientID, d) obstacle_cuboid_spec.append(obstacle_spec) print("done")
def control(self, clientID, target): self.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(clientID) print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = self.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) print(sensed_joint_positions) vu.step_sim(clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = self.has_stably_converged_to_target()
def main(args): # Connect to V-REP print ('Connecting to V-REP...') clientID = vu.connect_to_vrep() print ('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50.*np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) deg_to_rad = np.pi/180. # Joint targets. Specify in radians for revolute joints and meters for prismatic joints. # The order of the targets are as follows: # joint_1 / revolute / arm_base_link <- shoulder_link # joint_2 / revolute / shoulder_link <- elbow_link # joint_3 / revolute / elbow_link <- forearm_link # joint_4 / revolute / forearm_link <- wrist_link # joint_5 / revolute / wrist_link <- gripper_link # joint_6 / prismatic / gripper_link <- finger_r # joint_7 / prismatic / gripper_link <- finger_l joint_targets = [[ 0., 0., 0., 0., 0., - 0.07, 0.07], \ [-45.*deg_to_rad, -15.*deg_to_rad, 20.*deg_to_rad, 15.*deg_to_rad, -75.*deg_to_rad, - 0.03, 0.03], \ [ 30.*deg_to_rad, 60.*deg_to_rad, -65.*deg_to_rad, 45.*deg_to_rad, 0.*deg_to_rad, - 0.05, 0.05]] # Instantiate controller controller = ArmController() i=0 # Iterate through target joint positions for target in joint_targets: # Set new target position controller.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: i+=1 timestamp = vu.get_sim_time_seconds(clientID) print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = controller.calculate_commands_from_feedback(timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) #print(sensed_joint_positions) vu.step_sim(clientID, 1) if(i==1): joint_pos = np.array(sensed_joint_positions) target_pos = np.array(target) time_history = np.array(timestamp) else: joint_pos = np.vstack((joint_pos,sensed_joint_positions)) target_pos = np.vstack((target_pos,target)) time_history = np.hstack((time_history,timestamp)) # Determine if we've met the condition to move on to the next point steady_state_reached = controller.has_stably_converged_to_target() vu.stop_sim(clientID) plt.figure(1) plt.subplot(331) plt.xlabel('Timestamp (s)') plt.ylabel('Angle(rad)') plt.title('Joint 1 position over time') plt.plot(time_history,joint_pos[:,0],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,0],'-r',label='Target position') plt.subplot(332) plt.xlabel('Timestamp (s)') plt.ylabel('Angle(rad)') plt.title('Joint 2 position over time') plt.plot(time_history,joint_pos[:,1],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,1],'-r',label='Target position') plt.subplot(333) plt.xlabel('Timestamp (s)') plt.ylabel('Angle(rad)') plt.title('Joint 3 position over time') plt.plot(time_history,joint_pos[:,2],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,2],'-r',label='Target position') plt.subplot(334) plt.xlabel('Timestamp (s)') plt.ylabel('Angle(rad)') plt.title('Joint 4 position over time') plt.plot(time_history,joint_pos[:,3],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,3],'-r',label='Target position') plt.subplot(335) plt.xlabel('Timestamp (s)') plt.ylabel('Angle(rad)') plt.title('Joint 5 position over time') plt.plot(time_history,joint_pos[:,4],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,4],'-r',label='Target position') plt.subplot(336) plt.xlabel('Timestamp (s)') plt.ylabel('Position(m)') plt.title('Joint 6 position over time') plt.plot(time_history,joint_pos[:,5],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,5],'-r',label='Target position') plt.subplot(337) plt.xlabel('Timestamp (s)') plt.ylabel('Position(m)') plt.title('Joint 7 position over time') plt.plot(time_history,joint_pos[:,6],'-b',label='Sensed position') plt.plot(time_history,target_pos[:,6],'-r',label='Target position') plt.show()
def main(args): # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) # Joint targets. Specify in radians for revolute joints and meters for prismatic joints. # The order of the targets are as follows: # joint_1 / revolute / arm_base_link <- shoulder_link # joint_2 / revolute / shoulder_link <- elbow_link # joint_3 / revolute / elbow_link <- forearm_link # joint_4 / revolute / forearm_link <- wrist_link # joint_5 / revolute / wrist_link <- gripper_link # joint_6 / prismatic / gripper_link <- finger_r # joint_7 / prismatic / gripper_link <- finger_l joint_targets = [[0., 0., 0., 0., 0., -0.00, 0.00]] # Instantiate controller controller = ArmController() # Iterate through target joint positions for target in joint_targets: # Set new target position controller.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(clientID) print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = controller.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) print(sensed_joint_positions) vu.step_sim(clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = controller.has_stably_converged_to_target() vu.stop_sim(clientID) titles = [ 'Joint_1', 'Joint_2', 'Joint_3', 'Joint_4', 'Joint_5', 'Joint_6', 'Joint_7' ] for i in range(7): plt.figure(i) #plt.subplot(4,2,i+1) plt.plot(np.asarray(controller.history['timestamp']), np.asarray(controller.history['joint_feedback'])[:, i]) plt.plot(np.asarray(controller.history['timestamp']), np.asarray(controller.history['joint_target'])[:, i], 'r--') if (i < 5): plt.ylabel('Joint Angle') else: plt.ylabel('Joint Position') plt.xlabel('Time') plt.title(titles[i]) plt.show()
def main(args): # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) deg_to_rad = np.pi / 180. # Joint targets. Specify in radians for revolute joints and meters for prismatic joints. # The order of the targets are as follows: # joint_1 / revolute / arm_base_link <- shoulder_link # joint_2 / revolute / shoulder_link <- elbow_link # joint_3 / revolute / elbow_link <- forearm_link # joint_4 / revolute / forearm_link <- wrist_link # joint_5 / revolute / wrist_link <- gripper_link # joint_6 / prismatic / gripper_link <- finger_r # joint_7 / prismatic / gripper_link <- finger_l joint_targets = [[ 0., 0., 0., 0., 0., - 0.07, 0.07], \ [-45.*deg_to_rad, -15.*deg_to_rad, 20.*deg_to_rad, 15.*deg_to_rad, -75.*deg_to_rad, - 0.03, 0.03], \ [ 30.*deg_to_rad, 60.*deg_to_rad, -65.*deg_to_rad, 45.*deg_to_rad, 0.*deg_to_rad, - 0.05, 0.05]] # Instantiate controller controller = ArmController() # Iterate through target joint positions for target in joint_targets: # Set new target position controller.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(clientID) print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = controller.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) print(sensed_joint_positions) vu.step_sim(clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = controller.has_stably_converged_to_target() vu.stop_sim(clientID)
for i in range(orientation.shape[0]): R = createRotationMatrix(orientation[i, :]) volume.append( np.matmul(R.transpose(), dimension[i, :].transpose()).transpose()) volume = np.stack(volume) return (volume) if __name__ == "__main__": # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') vu.reset_sim(clientID) vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) deg_to_rad = np.pi / 180. controller = locobot_joint_ctrl.ArmController() obstacles = np.load('obstacles.npy') prm = PRM.PRM_Map(obstacles) start = np.array([[-80, 0, 0, 0, 0]]) target = np.array([[0, 60, -75, -75, 0]]) path = prm.Planner(start, target) vu.step_sim(clientID) for joints in path: joints = (joints * deg_to_rad).tolist()
def main(args): # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) deg_to_rad = np.pi / 180. link_cuboid_list = ("arm_base_link_joint_collision_cuboid", "shoulder_link_collision_cuboid", "elbow_link_collision_cuboid", "forearm_link_collision_cuboid", "wrist_link_collision_cuboid", "gripper_link_collision_cuboid", "finger_r_collision_cuboid", "finger_l_collision_cuboid") for link_cuboid in link_cuboid_list: d = vu.get_handle_by_name(clientID, link_cuboid) link_spec = {} link_spec["Origin"] = vu.get_object_position(clientID, d) link_spec["Orientation"] = vu.get_object_orientation(clientID, d) link_spec["Dimension"] = vu.get_object_bounding_box(clientID, d) link_cuboid_spec.append(link_spec) obstacle_cuboid_list = ("cuboid_0", "cuboid_1", "cuboid_2", "cuboid_3", "cuboid_4", "cuboid_5") for obstacle_cuboid in obstacle_cuboid_list: d = vu.get_handle_by_name(clientID, obstacle_cuboid) obstacle_spec = {} obstacle_spec["Origin"] = vu.get_object_position(clientID, d) obstacle_spec["Orientation"] = vu.get_object_orientation(clientID, d) obstacle_spec["Dimension"] = vu.get_object_bounding_box(clientID, d) obstacle_cuboid_spec.append(obstacle_spec) print("done") joint_targets = get_cuboid_config() # Instantiate controller controller = ArmController() # Iterate through target joint positions for target in joint_targets: # Set new target position controller.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(clientID) # print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = controller.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) vu.step_sim(clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = controller.has_stably_converged_to_target() vu.stop_sim(clientID) # Post simulation cleanup -- save results to a pickle, plot time histories, etc ##### # Fill this out here (optional) or in your own script # If you use a separate script, don't forget to include it in the deliverables # ... ##################################################################################### plt.figure() for i in range(7): plt.subplot(2, 4, i + 1) plt.title('Joint %d' % (i + 1)) plt.xlabel('Time') plt.ylabel('Joint angle') a1 = np.array(controller.history['joint_feedback'])[:, i] a2 = np.array(controller.history['joint_target'])[:, i] plt.plot(0.05 * np.arange(len(a1)), a1) plt.plot(0.05 * np.arange(len(a2)), a2) plt.show()
def execute(): # Connect to V-REP print('Connecting to V-REP...') clientID = vu.connect_to_vrep() print('Connected.') # Reset simulation in case something was running vu.reset_sim(clientID) # Initial control inputs are zero vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) # Despite the name, this sets the maximum allowable joint force vu.set_arm_joint_forces(clientID, 50. * np.ones(vu.N_ARM_JOINTS)) # One step to process the above settings vu.step_sim(clientID) deg_to_rad = np.pi / 180. # Joint targets. Specify in radians for revolute joints and meters for prismatic joints. # The order of the targets are as follows: # joint_1 / revolute / arm_base_link <- shoulder_link # joint_2 / revolute / shoulder_link <- elbow_link # joint_3 / revolute / elbow_link <- forearm_link # joint_4 / revolute / forearm_link <- wrist_link # joint_5 / revolute / wrist_link <- gripper_link # joint_6 / prismatic / gripper_link <- finger_r # joint_7 / prismatic / gripper_link <- finger_l planned = np.load("execute_path.npy") res = [] for plan in planned: res.append(plan.tolist()) joint_targets = res # Instantiate controller controller = ArmController() # Iterate through target joint positions for i, target in enumerate(joint_targets): # Set new target position controller.set_target_joint_positions(target) steady_state_reached = False while not steady_state_reached: timestamp = vu.get_sim_time_seconds(clientID) #print('Simulation time: {} sec'.format(timestamp)) # Get current joint positions sensed_joint_positions = vu.get_arm_joint_positions(clientID) # Calculate commands commands = controller.calculate_commands_from_feedback( timestamp, sensed_joint_positions) # Send commands to V-REP vu.set_arm_joint_target_velocities(clientID, commands) # Print current joint positions (comment out if you'd like) #print(sensed_joint_positions) vu.step_sim(clientID, 1) # Determine if we've met the condition to move on to the next point steady_state_reached = controller.has_stably_converged_to_target() #visualizer(clientID) #saveingCuboid(clientID,i) #savingJoints(clientID,i) vu.stop_sim(clientID) print "terminated" file = open('pickle_history', 'wb') pickle.dump(controller.history, file) file.close()