import vrep import time import numpy as np # Close all open connections (just in case) vrep.simxFinish(-1) # Connect to V-REP (raise exception on failure) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: raise Exception('Failed connecting to remote API server') ################### Get handles for all joints ################################# result, joint_one_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint') result, joint_one_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint1_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint1_2') result, joint_two_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for second joint') result, joint_two_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint2_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint2_2') result, joint_three_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for third joint') result, joint_three_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint3_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint3_2') result, joint_four_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for fourth joint') result, joint_four_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint4_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for fourth joint4_2') result, joint_five_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for fifth joint') result, joint_five_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint5_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for first joint3_2') result, joint_six_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for sixth joint') result, joint_six_2_handle = vrep.simxGetObjectHandle(clientID, 'UR3_joint6_2', vrep.simx_opmode_blocking) if result != vrep.simx_return_ok: raise Exception('could not get object handle for sixth joint6_2') ################################################################################ # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) # Wait two seconds time.sleep(2) ############### Get the values for joint variables ############################# result1, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking) if result1 != vrep.simx_return_ok: raise Exception('could not get first joint variable') print('current value of first joint variable on UR3: theta = {:f}'.format(theta1)) result1_2, theta1_2 = vrep.simxGetJointPosition(clientID, joint_one_2_handle, vrep.simx_opmode_blocking) if result1_2 != vrep.simx_return_ok: raise Exception('could not get first_2 joint variable') print('current value of first joint variable on UR3_2: theta = {:f}'.format(theta1_2)) result2, theta2 = vrep.simxGetJointPosition(clientID, joint_two_handle, vrep.simx_opmode_blocking) if result2 != vrep.simx_return_ok: raise Exception('could not get second joint variable') print('current value of second joint variable on UR3: theta = {:f}'.format(theta2)) result2_2, theta2_2 = vrep.simxGetJointPosition(clientID, joint_two_2_handle, vrep.simx_opmode_blocking) if result2_2 != vrep.simx_return_ok: raise Exception('could not get second_2 joint variable') print('current value of second joint variable on UR3_2: theta = {:f}'.format(theta2_2)) result3, theta3 = vrep.simxGetJointPosition(clientID, joint_three_handle, vrep.simx_opmode_blocking) if result3 != vrep.simx_return_ok: raise Exception('could not get third joint variable') print('current value of third joint variable on UR3: theta = {:f}'.format(theta3)) result3_2, theta3_2 = vrep.simxGetJointPosition(clientID, joint_three_2_handle, vrep.simx_opmode_blocking) if result3_2 != vrep.simx_return_ok: raise Exception('could not get third_2 joint variable') print('current value of third joint variable on UR3_2: theta = {:f}'.format(theta3_2)) result4, theta4 = vrep.simxGetJointPosition(clientID, joint_four_handle, vrep.simx_opmode_blocking) if result4 != vrep.simx_return_ok: raise Exception('could not get fourth joint variable') print('current value of fourth joint variable on UR3: theta = {:f}'.format(theta4)) result4_2, theta4_2 = vrep.simxGetJointPosition(clientID, joint_four_2_handle, vrep.simx_opmode_blocking) if result4_2 != vrep.simx_return_ok: raise Exception('could not get fourth_2 joint variable') print('current value of fourth joint variable on UR3_2: theta = {:f}'.format(theta4_2)) result5, theta5 = vrep.simxGetJointPosition(clientID, joint_five_handle, vrep.simx_opmode_blocking) if result5 != vrep.simx_return_ok: raise Exception('could not get fifth joint variable') print('current value of fifth joint variable on UR3: theta = {:f}'.format(theta5)) result5_2, theta5_2 = vrep.simxGetJointPosition(clientID, joint_five_2_handle, vrep.simx_opmode_blocking) if result5_2 != vrep.simx_return_ok: raise Exception('could not get fifth_2 joint variable') print('current value of fifth joint variable on UR3_2: theta = {:f}'.format(theta5_2)) result6, theta6 = vrep.simxGetJointPosition(clientID, joint_six_handle, vrep.simx_opmode_blocking) if result6 != vrep.simx_return_ok: raise Exception('could not get sixth joint variable') print('current value of sixth joint variable on UR3: theta = {:f}'.format(theta6)) result6_2, theta6_2 = vrep.simxGetJointPosition(clientID, joint_six_2_handle, vrep.simx_opmode_blocking) if result6_2 != vrep.simx_return_ok: raise Exception('could not get sixth_2 joint variable') print('current value of sixth joint variable on UR3_2: theta = {:f}'.format(theta6_2)) ################################################################################ # Set the desired value of the joint variables vrep.simxSetJointTargetPosition(clientID, joint_one_handle, theta1 + (-np.pi / 6), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, joint_one_2_handle, theta1_2 + (np.pi / 6), vrep.simx_opmode_oneshot) time.sleep(1) vrep.simxSetJointTargetPosition(clientID, joint_two_handle, theta2 + (np.pi / 6), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, joint_two_2_handle, theta2_2 + (-np.pi / 6), vrep.simx_opmode_oneshot) time.sleep(1) vrep.simxSetJointTargetPosition(clientID, joint_three_handle, theta3 + (-np.pi / 4), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, joint_three_2_handle, theta3_2 + (np.pi / 4), vrep.simx_opmode_oneshot) time.sleep(1) vrep.simxSetJointTargetPosition(clientID, joint_four_handle, theta4 + (np.pi / 4), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, joint_four_2_handle, theta4_2 + (-np.pi / 4), vrep.simx_opmode_oneshot) time.sleep(1) vrep.simxSetJointTargetPosition(clientID, joint_five_handle, theta5 + (np.pi / 3), vrep.simx_opmode_oneshot) # vrep.simxSetJointTargetPosition(clientID, joint_five_2_handle, theta5_2 + (-np.pi / 3), vrep.simx_opmode_oneshot) time.sleep(1) vrep.simxSetJointTargetPosition(clientID, joint_six_handle, theta6 + (np.pi / 2), vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, joint_six_2_handle, theta6_2 + (-np.pi / 2.5), vrep.simx_opmode_oneshot) print("\n\n\n\n\n\n\n\n\n\n\nSay Hi!\n\n\n\n\n\n\n\n\n\n") vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'micoHand','true',vrep.simx_opmode_oneshot) # print('Hands should close now') time.sleep(0.6) vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'micoHand','false',vrep.simx_opmode_oneshot) # print('Hands should open now') time.sleep(0.6) vrep.simxSetStringSignal(clientID,'jacoHand','true',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'micoHand','true',vrep.simx_opmode_oneshot) # print('Hands should close now') time.sleep(0.6) vrep.simxSetStringSignal(clientID,'jacoHand','false',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'micoHand','false',vrep.simx_opmode_oneshot) # print('Hands should open now') time.sleep(3) ############### Get the current values for the joints to display ############### # Get the current value of the first joint variable result1, theta1 = vrep.simxGetJointPosition(clientID, joint_one_handle, vrep.simx_opmode_blocking) if result1 != vrep.simx_return_ok: raise Exception('could not get first joint variable') print('current value of first joint variable on UR3: theta = {:f}'.format(theta1)) result1_2, theta1_2 = vrep.simxGetJointPosition(clientID, joint_one_2_handle, vrep.simx_opmode_blocking) if result1_2 != vrep.simx_return_ok: raise Exception('could not get first joint_2 variable') print('current value of first joint_2 variable on UR3_2: theta = {:f}'.format(theta1_2)) result2, theta2 = vrep.simxGetJointPosition(clientID, joint_two_handle, vrep.simx_opmode_blocking) if result2 != vrep.simx_return_ok: raise Exception('could not get second joint variable') print('current value of second joint variable on UR3: theta = {:f}'.format(theta2)) result2_2, theta2_2 = vrep.simxGetJointPosition(clientID, joint_two_2_handle, vrep.simx_opmode_blocking) if result2_2 != vrep.simx_return_ok: raise Exception('could not get second joint_2 variable') print('current value of second joint_2 variable on UR3_2: theta = {:f}'.format(theta2_2)) result3, theta3 = vrep.simxGetJointPosition(clientID, joint_three_handle, vrep.simx_opmode_blocking) if result3 != vrep.simx_return_ok: raise Exception('could not get third joint variable') print('current value of third joint variable on UR3: theta = {:f}'.format(theta3)) result3_2, theta3_2 = vrep.simxGetJointPosition(clientID, joint_three_2_handle, vrep.simx_opmode_blocking) if result3_2 != vrep.simx_return_ok: raise Exception('could not get third joint_2 variable') print('current value of third joint_2 variable on UR3_2: theta = {:f}'.format(theta3_2)) result4, theta4 = vrep.simxGetJointPosition(clientID, joint_four_handle, vrep.simx_opmode_blocking) if result4 != vrep.simx_return_ok: raise Exception('could not get fourth joint variable') print('current value of fourth joint variable on UR3: theta = {:f}'.format(theta4)) result4_2, theta4_2 = vrep.simxGetJointPosition(clientID, joint_four_2_handle, vrep.simx_opmode_blocking) if result4_2 != vrep.simx_return_ok: raise Exception('could not get fourth joint_2 variable') print('current value of fourth joint_2 variable on UR3_2: theta = {:f}'.format(theta4_2)) result5, theta5 = vrep.simxGetJointPosition(clientID, joint_five_handle, vrep.simx_opmode_blocking) if result5 != vrep.simx_return_ok: raise Exception('could not get fifth joint variable') print('current value of fifth joint variable on UR3: theta = {:f}'.format(theta5)) result5_2, theta5_2 = vrep.simxGetJointPosition(clientID, joint_five_2_handle, vrep.simx_opmode_blocking) if result5_2 != vrep.simx_return_ok: raise Exception('could not get fifth joint_2 variable') print('current value of fifth joint_2 variable on UR3_2: theta = {:f}'.format(theta5_2)) result6, theta6 = vrep.simxGetJointPosition(clientID, joint_six_handle, vrep.simx_opmode_blocking) if result6 != vrep.simx_return_ok: raise Exception('could not get sixth joint variable') print('current value of sixth joint variable on UR3: theta = {:f}'.format(theta6)) result6_2, theta6_2 = vrep.simxGetJointPosition(clientID, joint_six_2_handle, vrep.simx_opmode_blocking) if result6_2 != vrep.simx_return_ok: raise Exception('could not get sixth joint_2 variable') print('current value of sixth joint_2 variable on UR3_2: theta = {:f}'.format(theta6_2)) ################################################################################ # Stop simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot) # Before closing the connection to V-REP, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): vrep.simxGetPingTime(clientID) # Close the connection to V-REP vrep.simxFinish(clientID)