def __init__(self, robot, host="127.0.0.1", port=19997): self.robot = robot self.sim_joints = [0, 0, 0, 0, 0, 0] sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart(host, port, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID != -1: print('Connected to remote API server') else: print("coppeliasim connection failed") return res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) self.jh = [0, 0, 0, 0, 0, 0] for i in range(6): res, self.jh[i] = sim.simxGetObjectHandle( self.clientID, 'UR5_joint{}'.format(i + 1), sim.simx_opmode_blocking) sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
def start_Simulation(self): print('Program started') # Just in case, close all opened connections sim.simxFinish(-1) # Connect to CoppeliaSim, as continuous remote API Server self.clientID = sim.simxStart('127.0.0.1', 19997, False, True, 5000, 5) if self.clientID != -1: print('Connected to remote API server') #Start simulation sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(self.clientID, 'Connection Succeed!', sim.simx_opmode_oneshot) # Before closing the connection to CoppeliaSim, make sure that the last command sent # out had time to arrive. You can guarantee this with (for example): sim.simxGetPingTime(self.clientID) # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) else: print('Failed connecting to remote API server') return self.clientID
def vrepInterface(port): global angles_handler angles_handler = np.zeros(12) global angles_error angles_error = np.zeros(12) global clientID clientID = 0 global inital_name print('Program started') sim.simxFinish(-1) # just in case, close all opened connections ID = sim.simxStart('127.0.0.1', port, True, True, 5000, 5) # Connect to V-REP print(ID) if ID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(ID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) else: print('DIDNOT CONNECT!!!') intial_name = [ 'ab3', 'bc3', 'cd3', 'ab4', 'bc4', 'cd4', 'ab1', 'bc1', 'cd1', 'ab2', 'bc2', 'cd2' ] for i in range(angles_handler.shape[0]): angles_error[i], angles_handler[i] = sim.simxGetObjectHandle( clientID, intial_name[i], sim.simx_opmode_blocking) return ID
def create_dataset(path): try: shutil.rmtree(path + "/mav0") except: print("Removed before") os.makedirs(path + "/mav0/cam0/data") os.makedirs(path + "/mav0/cam1/data") print('Argument List:', str(sys.argv[1])) sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim print('Connected to remote API server') #Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) er, t_rightWheel = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_blocking) er, t_leftWheel = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_blocking) er, cam_handle_left = sim.simxGetObjectHandle( clientID, 'anaglyphStereoSensor_leftSensor', sim.simx_opmode_blocking) er, cam_handle_right = sim.simxGetObjectHandle( clientID, 'anaglyphStereoSensor_rightSensor', sim.simx_opmode_blocking) i = 0 fl = open(path + "/mav0/timestamps.txt", "w") while (True): err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage( clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait) err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage( clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait) img_left_1 = np.array(colorCam_left, dtype=np.uint8) img_right_1 = np.array(colorCam_right, dtype=np.uint8) img_left_1.resize([resolution_left[1], resolution_left[0], 3]) img_right_1.resize([resolution_right[1], resolution_right[0], 3]) img_left_2 = np.flipud(img_left_1) img_right_2 = np.flipud(img_right_1) img_left_3 = img_left_2[..., ::-1].copy() img_right_3 = img_right_2[..., ::-1].copy() milli_time = int(round(time.time() * 1000)) * 1000000000 st = str(milli_time) + '\n' st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time) st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time) cv2.imwrite(st_left, img_left_3) cv2.imwrite(st_right, img_right_3) cv2.waitKey(1) i = i + 1 fl.write(st) sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1, sim.simx_opmode_streaming) fl.close()
def main(): print("Program started") sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart("127.0.0.1", 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print("Connected to remote API server") # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print("Number of objects in the scene: ", len(objs)) print("Client ID: ", clientID) else: print("Remote API function call returned with error code: ", res) sys.exit("Could not connect") pioneerRob = Pioneer_p3dx(clientID) pioneerRob.actuation_sample() """ err_code, l_motor_handle = sim.simxGetObjectHandle(clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_blocking) err_code, r_motor_handle = sim.simxGetObjectHandle(clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_blocking) err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 1.0, sim.simx_opmode_streaming) err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming) time.sleep(5) err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, -1.0, sim.simx_opmode_streaming) err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming) time.sleep(2) err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 0.0, sim.simx_opmode_streaming) err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 0.0, sim.simx_opmode_streaming) time.sleep(2) err_code = sim.simxSetJointTargetVelocity(clientID, l_motor_handle, 1.0, sim.simx_opmode_streaming) err_code = sim.simxSetJointTargetVelocity(clientID, r_motor_handle, 1.0, sim.simx_opmode_streaming) time.sleep(2) """ # Now close the connection to CoppeliaSim: sim.simxFinish(clientID) else: print("Failed connecting to remote API server") print("Program ended")
def run_coppelia(self): print('Program started') sim.simxFinish(-1) # just in case, close all opened connections self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim if self.clientID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(self.clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Found %d objects in the scene.' % len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) robot_name = 'UR5' errorCodes, self.joints = [], [] for i in range(1, 7): errorCode, joint = sim.simxGetObjectHandle( self.clientID, robot_name + '_joint' + str(i), sim.simx_opmode_oneshot_wait) errorCodes += [errorCode] self.joints += [joint] if -1 in errorCodes: print("Handles creation error") return (self.clientID, self.joints) else: print('Failed connecting to remote API server')
print('Make sure both are in the same folder as this file,') print('or appropriately adjust the file "sim.py"') print('--------------------------------------------------------------') print('') import time print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) # Now retrieve streaming data (i.e. in a non-blocking fashion): startTime = time.time() sim.simxGetIntegerParameter( clientID, sim.sim_intparam_mouse_x, sim.simx_opmode_streaming) # Initialize streaming while time.time() - startTime < 5: returnCode, data = sim.simxGetIntegerParameter( clientID, sim.sim_intparam_mouse_x,
def main(): global hit_wall global ball_coord global ball_linear global ball_hit global detect_handle global prox_handle global stop_prog print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking) if res == sim.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) #Giving an initial velocity to the ball before starting the thread detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking) prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking) racket_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor0', sim.simx_opmode_blocking) dummy_handle = sim.simxGetObjectHandle(clientID, 'Cylinder', sim.simx_opmode_blocking) sim.simxSetObjectPosition(clientID, detect_handle[1], -1, [0.65, 0.47, 0.0463], sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, True) sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3001, 1, sim.simx_opmode_oneshot) #sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3000, -0.01, sim.simx_opmode_oneshot) sim.simxSetObjectFloatParameter(clientID, detect_handle[1], 3002, 1, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) sim.simxReadProximitySensor(clientID, prox_handle[1], sim.simx_opmode_streaming) sim.simxGetObjectVelocity(clientID, detect_handle[1], sim.simx_opmode_streaming) sim.simxReadProximitySensor(clientID, racket_handle[1], sim.simx_opmode_streaming) sim.simxGetObjectPosition(clientID, dummy_handle[1], -1, sim.simx_opmode_streaming) ball_thread = threading.Thread(target=get_ball_info, args=({ clientID: clientID })) try: #getting joint handles and initializing the joints in the simulation print("1. getting joint angles") jointHandles = [] for i in range(6): handle = sim.simxGetObjectHandle( clientID, 'UR3_joint' + str(i + 1) + '#0', sim.simx_opmode_blocking) jointHandles.append(handle) time.sleep(0.01) for i in range(6): print(jointHandles[i]) #hit_thread = threading.Thread(target=hit_ball, args=({clientID:clientID})) ball_thread.daemon = True #hit_thread.daemon = True #hit_thread.start() ball_thread.start() # #Get handles for detecting object # detect_handle = sim.simxGetObjectHandle(clientID, 'Sphere', sim.simx_opmode_blocking) # #Get handles for detecting the proximity sensor # prox_handle = sim.simxGetObjectHandle(clientID, 'Proximity_sensor', sim.simx_opmode_blocking) #Create an instance to compute the ball trajectory print("1. Initializing bouncing trajectory function") b_ball = bouncing_ball.BouncingBall() #Set-up some of the RML vectors: vel = 60 accel = 10 jerk = 20 currentVel = [0, 0, 0, 0, 0, 0, 0] currentAccel = [0, 0, 0, 0, 0, 0, 0] maxVel = [ vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180, vel * math.pi / 180 ] maxAccel = [ accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180, accel * math.pi / 180 ] maxJerk = [ jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180, jerk * math.pi / 180 ] targetVel = [0, 0, 0, 0, 0, 0] sim.simxPauseCommunication(clientID, True) for i in range(6): sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) #start the feedback loop while True: if not hit_wall: #print("We haven't hit the wall yet") continue if hit_wall: print("We hit wall ....") #Predict the position of the ball from the wall and transform it to world coordinates print("Predicting Trajectory ...") pred_pos = b_ball.trajectory(ball_coord[0], ball_coord[1], ball_coord[2], ball_linear) T_prox = np.array([[-1, 0, 0, 0.025], [0, -1, 0, 2.85], [0, 0, 1, -.05], [0, 0, 0, 1]]) prox_pos = np.array([[pred_pos[0]], [pred_pos[1]], [pred_pos[2]], [1]]) ball_pos = T_prox @ prox_pos print(ball_pos) #set the left flag to true if on left side left = 0 if ball_pos[0, :] < 0: left = 1 #convert to right-side coordinates for IK ball_pos[0, :] = np.abs(ball_pos[0, :]) if ball_pos[0, :] > 0.9: print("Ball too far away from robot. will not hit it ...") raise ValueError elif ball_pos[0, :] < 0.2: print("Ball too close. cannot hit it...") raise ValueError twist_angle = (-92.85) * (ball_pos[0, :]) + 90 print(ball_pos, "left?: ", left) targetPos0_ik = ik.findJointAngles(ball_pos[0, :], ball_pos[1, :], ball_pos[2, :] + 0.15) #Invert joint angles if on left side of robot for i in range(len(targetPos0_ik)): targetPos0_ik[i] = ((-2 * left) + 1) * targetPos0_ik[i] print("Applying the hitting motion") #Apply the hitting motion using the new joint angles end_pose = [] targetPos0_ik[0] = targetPos0_ik[0] + ( (-2 * left) + 1) * (0 * np.pi / 180) #To simulate a hit targetPos0_ik[4] = targetPos0_ik[4] + ( (-2 * left) + 1) * (0 * np.pi / 180) sim.simxPauseCommunication(clientID, True) for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) #Now read from the proximity sensor to see if it detects any ball ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor( clientID, racket_handle[1], sim.simx_opmode_buffer) while (dS == 0): _, end_pose = sim.simxGetObjectPosition( clientID, dummy_handle[1], -1, sim.simx_opmode_buffer) ret, dS, dP, dOH, dSNV = sim.simxReadProximitySensor( clientID, racket_handle[1], sim.simx_opmode_buffer) print("Status of Ball is: ", dS) #Now, attach a proximity sensor to the racquet and see if it detects a ball. If it does, let's start the #hitting motion #Actually, for now, let's just not worry about this. Let's make sure that the trajectory generation and the ball hitting #works print("Twist angle is: ", twist_angle) targetPos0_ik[0] = targetPos0_ik[0] + ((-2 * left) + 1) * ( (1 * twist_angle) * np.pi / 180) #To simulate a hit targetPos0_ik[4] = targetPos0_ik[4] + ( (-2 * left) + 1) * (-1 * twist_angle * np.pi / 180) # sim.simxPauseCommunication(clientID, True) # for i in range(6): # #print(jointHandles[i]) # #print(targetPos0[i]) # sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_buffer) # sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_buffer) # sim.simxPauseCommunication(clientID, False) # time.sleep(2) ball_hit = True hit_wall = False sim.simxPauseCommunication(clientID, True) for i in [0, 4]: sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], targetPos0_ik[i], sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) time.sleep(2) sim.simxPauseCommunication(clientID, True) #Reset after hitting ball for i in range(6): #print(jointHandles[i]) #print(targetPos0[i]) sim.simxSetJointTargetPosition(clientID, jointHandles[i][1], 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, jointHandles[i][1], targetVel[i], sim.simx_opmode_streaming) sim.simxPauseCommunication(clientID, False) ##Inverse Kinematics experiment and analysis: CAN BE COMMENTED OUT ball_pos[0, :] = (-2 * left + 1) * ball_pos[0, :] ball_pos[2, :] = ball_pos[2, :] + 0.15 end_pose = np.array(end_pose).reshape((3, 1)) print("Desired position: \n", ball_pos[:3]) print("End-effector position: \n", end_pose[:3]) # print("Error (MMSE): ", np.linalg.norm(ball_pos[:3] - end_pose[:3])) # x_err = np.abs((end_pose[0,:] - ball_pos[0,:])/(ball_pos[0,:])) * 100 # y_err = np.abs((end_pose[1,:] - ball_pos[1,:])/(ball_pos[1,:])) * 100 # z_err = np.abs((end_pose[2,:] - ball_pos[2,:])/(ball_pos[2,:])) * 100 # print("Error in X (%): ", x_err) # print("Error in Y (%): ", y_err) # print("Error in Z (%): ", z_err) # print("Total Error (%): ", (x_err + y_err + z_err)/3) except: print(sys.exc_info()) #hit_thread.join() stop_prog = True print("Exception encountered, stopping program") #ball_thread.join() print("Ball thread stopped") sim.simxGetPingTime(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended') sim.simxGetPingTime(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID)
import time import keyboard import numpy as np import math a = np.zeros(3, dtype=float) rot = np.zeros(3, dtype=float) print('Program started') vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19998, True, True, 5000, 5) # Connect to V-REP if clientID != -1: print('Connected to remote API server') # Now try to retrieve data in a blocking fashion (i.e. a service call): res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print('Number of objects in the scene: ', len(objs)) else: print('Remote API function call returned with error code: ', res) time.sleep(2) number, objecthandle = vrep.simxGetObjectHandle(clientID, 'Quadricopter_target', vrep.simx_opmode_blocking) while (a[0] == 0 and a[1] == 0 and a[2] == 0): es, a = vrep.simxGetObjectPosition(clientID, objecthandle, -1, vrep.simx_opmode_oneshot) es, rot = vrep.simxGetObjectOrientation(clientID, objecthandle, -1, vrep.simx_opmode_oneshot)