def __init__(self): # n_actions:3 (target pos), n_states:6 (3pos+3force) self.metadata = {'render.modes': ['human']} super().__init__() sim.simxFinish(-1) for _ in range(5): self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: # print('[INFO] Connected to CoppeliaSim.') break if self.clientID == -1: raise IOError('[ERROR] Could not connect to CoppeliaSim.') sim.simxSynchronous(self.clientID, True) # sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) self.stepCount = 0 self.reward = 0 self.n_substeps = 10 # self.sim_timestep = 0.5 # set in coppeliaSim (not implemented) self.n_actions = 3 self.n_states = 6 self.action_space = spaces.Box(-1., 1, shape=(self.n_actions, ), dtype='float32') self.observation_space = spaces.Box(-np.inf, np.inf, shape=(self.n_states, ), dtype='float32') self._getHandles() sim.simxGetPingTime(self.clientID)
def close(self): sim.simxAddStatusbarMessage(self.clientID, 'Goodbye CoppeliaSim!', sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) time.sleep(2) sim.simxFinish(self.clientID)
def exit_remote_api_server(): """ Purpose: --- This function should wait for the last command sent to arrive at the Coppeliasim server before closing the connection and then end the communication thread with server i.e. CoppeliaSim using simxFinish Remote API. Input Arguments: --- None Returns: --- None Example call: --- exit_remote_api_server() NOTE: This function will be automatically called by test_task_2a executable after ending the simulation. """ global client_id ############## ADD YOUR CODE HERE ############## sim.simxGetPingTime(client_id) sim.simxFinish(client_id)
def close_connection(self): sim.simxGetPingTime( self.client_id ) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. sim.simxFinish( self.client_id) # Now close the connection to CoppeliaSim: print('Connection closed')
def close_connection(self): self.actuate([0]*len(self.motors)) # Before closing the connection of CoppeliaSim, # make sure that the last command sent out had time to arrive. sim.simxGetPingTime(self.client_id) sim.simxFinish(self.client_id) # Now close the connection of CoppeliaSim: print('Connection closed')
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 reset(self): sim.simxSetFloatSignal(self.clientID, 'renderEnv', 0, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) self.resetSim() self.prepareSim() obs = self._get_obs() return obs
def stop_simulation(self): # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(self.clientID, 'Quit', 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 close the connection to CoppeliaSim: sim.simxFinish(self.clientID)
def disconnect_from_server(self): # We wait for all the conveyor timers to expire while self.current_thread is not None: # We skip the execution of this loop pass # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive sim.simxGetPingTime(self.client_id) # Now close the connection to CoppeliaSim sim.simxFinish(self.client_id) # Helps store the client id allocated by the coppeliasim server self.client_id = -1
def getHandle(self, name, ignoreError=False, attempts=5): for _ in range(attempts): res, out_handle = sim.simxGetObjectHandle(self.clientID, name, sim.simx_opmode_blocking) if res == sim.simx_return_ok or ignoreError: # print('[INFO] {} handle obtained.'.format(name)) break sim.simxGetPingTime(self.clientID) # if res!=sim.simx_return_ok and not ignoreError: # print('[WARNING] Failed to find {} with error {}.'.format(name, res)) return out_handle
def _set_action(self, action): actionX = action[0] actionY = action[1] actionZ = action[2] # print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ)) sim.simxSetFloatSignal(self.clientID, 'actionX', actionX, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionY', actionY, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID)
def stop_Simulation(self): #Stop Run Simulation sim.simxStopSimulation(self.clientID, 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 close the connection to CoppeliaSim: sim.simxFinish(self.clientID) print('Program ended') return None
def resetSim(self): # print('[INFO] reseting sim...') ret = sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking) if ret == sim.simx_return_ok: # print('[INFO] sim reset successfully.') time.sleep(1) else: raise IOError('[ERROR] problem in reseting sim...') sim.simxGetPingTime(self.clientID) # print('[INFO] starting sim in synchronous mode...') sim.simxSynchronous(self.clientID, True) sim.simxGetPingTime(self.clientID) ret = sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # if ret == sim.simx_return_ok: # print('[INFO] sim started successfully.') sim.simxGetPingTime(self.clientID) sim.simxClearFloatSignal(self.clientID, 'actionX', sim.simx_opmode_blocking) sim.simxClearFloatSignal(self.clientID, 'actionY', sim.simx_opmode_blocking) sim.simxClearFloatSignal(self.clientID, 'actionZ', sim.simx_opmode_blocking) sim.simxGetPingTime(self.clientID) self.stepCount = 0 self.reward = 0
def getForce(self, initialize=True): if initialize: F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0], sim.simx_opmode_streaming)[2] F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1], sim.simx_opmode_streaming)[2] F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2], sim.simx_opmode_streaming)[2] else: F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0], sim.simx_opmode_buffer)[2] F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1], sim.simx_opmode_buffer)[2] F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2], sim.simx_opmode_buffer)[2] sim.simxGetPingTime(self.clientID) return [F1, F2, F3]
def getKinectXYZ(self, initialize=True): if initialize: X = sim.simxGetFloatSignal(self.clientID, 'actX', sim.simx_opmode_streaming)[1] Y = sim.simxGetFloatSignal(self.clientID, 'actY', sim.simx_opmode_streaming)[1] Z = sim.simxGetFloatSignal(self.clientID, 'actZ', sim.simx_opmode_streaming)[1] else: X = sim.simxGetFloatSignal(self.clientID, 'actX', sim.simx_opmode_buffer)[1] Y = sim.simxGetFloatSignal(self.clientID, 'actY', sim.simx_opmode_buffer)[1] Z = sim.simxGetFloatSignal(self.clientID, 'actZ', sim.simx_opmode_buffer)[1] sim.simxGetPingTime(self.clientID) return X, Y, Z
def getForceMagnitude(self, initialize=True): if initialize: F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0], sim.simx_opmode_streaming)[2] F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1], sim.simx_opmode_streaming)[2] F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2], sim.simx_opmode_streaming)[2] else: F1 = sim.simxReadForceSensor(self.clientID, self.fsHandles[0], sim.simx_opmode_buffer)[2] F2 = sim.simxReadForceSensor(self.clientID, self.fsHandles[1], sim.simx_opmode_buffer)[2] F3 = sim.simxReadForceSensor(self.clientID, self.fsHandles[2], sim.simx_opmode_buffer)[2] sim.simxGetPingTime(self.clientID) return [np.linalg.norm(F1), np.linalg.norm(F2), np.linalg.norm(F3)]
def doTracking(self): self.resetSim() self.prepareSim() pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone', sim.simx_opmode_streaming)[1] posX = [] posY = [] posZ = [] tactile1 = [] reward = [] while pathIsDone == 0: X, Y, Z = self.getKinectXYZ(False) # forces = self.getForce(False) # diffForce = (forces[0][0] + ((forces[1][0] + forces[2][0]) / 2))/2 # print('[DATA] diff force: {:.4f}'.format(diffForce)) actionX = X # + 0.005*diffForce actionY = Y actionZ = Z # print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ)) sim.simxSetFloatSignal(self.clientID, 'actionX', actionX, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionY', actionY, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxSynchronousTrigger(self.clientID) forces = self.getForceMagnitude(False) posX.append(X) posY.append(Y) posZ.append(Z) tactile1.append(forces[0]) reward.append(self._getReward()) sim.simxGetPingTime(self.clientID) pathIsDone = sim.simxGetFloatSignal(self.clientID, 'movePathDone', sim.simx_opmode_buffer)[1] # time.sleep(0.5) # print('[INFO] tracking is done.') # print('[DATA] accumulated reward: {}'.format(np.sum(np.array(reward)))) sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)
def full_sim_reset(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_blocking) is_running = True while is_running: error_code, ping_time = vrep.simxGetPingTime(self.clientID) error_code, server_state = vrep.simxGetInMessageInfo( self.clientID, vrep.simx_headeroffset_server_state) if server_state == 0: is_running = False vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) time.sleep(1)
def prepareSim(self): self.initializeFunctions() isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking', sim.simx_opmode_streaming)[1] while isTracking != 1: sim.simxSynchronousTrigger(self.clientID) isTracking = sim.simxGetFloatSignal(self.clientID, 'isTracking', sim.simx_opmode_buffer)[1] sim.simxGetPingTime(self.clientID) X, Y, Z = self.getKinectXYZ(True) actionX = X actionY = Y actionZ = Z # print(actionX,actionY,actionZ) # print('[DATA] X:{:.4f}, Y:{:.4f}, Z:{:.4f}'.format(actionX,actionY,actionZ)) sim.simxSetFloatSignal(self.clientID, 'actionX', actionX, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionY', actionY, sim.simx_opmode_oneshot) sim.simxSetFloatSignal(self.clientID, 'actionZ', actionZ, sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID)
def transfer_genomes(self, timeout, project): """ transfer the genome files to the evo-client """ ret, ping_time = sim.simxGetPingTime(self.clientID) checksum = 0 sim.simxClearIntegerSignal(self.clientID, "ClientID_Signal", mode1) sim.simxSetIntegerSignal(self.clientID, "ClientID_Signal", self.clientID, mode1) for i, filename in enumerate(self.genome_filenames): file_to_transfer = project.path_to_gene_pool + filename ret = sim.simxTransferFile(self.clientID, file_to_transfer, filename, timeout, mode1) # default: mode2 if ret == 0: checksum += 1 return checksum
def main(): np.set_printoptions(precision=3, suppress=True) # Restrict decimals in console output dof = 6 home_position, screw = ur5_params() # Get UR5 parameters theta_0 = np.array([0, 0, 0, 0, 0, 0]) # Initial position theta_d = np.array([0,-m.pi/2, 0, 0, m.pi/2, 0]) # Desired position, converted to x_d in the solver T_0 = compute_T(screw, theta_0, dof, home_position) T_d = compute_T(screw, theta_d, dof, home_position) print("Home position: \n", T_0, "\n") print("Desired position: \n", T_d, "\n") # Find solution to the system theta, delta, n = root_finding(theta_0, theta_d, 20, dof, home_position, screw) T_theta = compute_T(screw, theta, dof, home_position) print('Solution : \n', theta, '\n', 'Transformation : \n', T_theta, '\n') R = T_theta[0:3][0:3] # Get RPY for the solution r_roll = m.atan2(R[2][1],R[2][2]) r_pitch = m.atan2(-R[2][0],m.sqrt(R[2][2]*R[2][2] + R[2][1]*R[2][1])) r_yaw = m.atan2(R[1][0],R[0][0]) # Begin connection with coppeliaSim. # Robot simulation must be running in coppeliaSim to connect sim.simxFinish(-1) # just in case, close all opened connections clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim # clientID stores the ID assingned by coppeliaSim, if the connection failed it will be assigned -1 if clientID!=-1: print ('Connected to remote API server') # Get ID handles for each joint of the robot res,UR5_joint1 =sim.simxGetObjectHandle( clientID, 'UR5_joint1', sim.simx_opmode_blocking) res,UR5_joint2 =sim.simxGetObjectHandle( clientID, 'UR5_joint2', sim.simx_opmode_blocking) res,UR5_joint3 =sim.simxGetObjectHandle( clientID, 'UR5_joint3', sim.simx_opmode_blocking) res,UR5_joint4 =sim.simxGetObjectHandle( clientID, 'UR5_joint4', sim.simx_opmode_blocking) res,UR5_joint5 =sim.simxGetObjectHandle( clientID, 'UR5_joint5', sim.simx_opmode_blocking) res,UR5_joint6 =sim.simxGetObjectHandle( clientID, 'UR5_joint6', sim.simx_opmode_blocking) res,UR5_endEffector =sim.simxGetObjectHandle( clientID, 'UR5_connection', sim.simx_opmode_blocking) # Store the handles as a list UR5 = [UR5_joint1, UR5_joint2, UR5_joint3, UR5_joint4, UR5_joint5, UR5_joint6] # Just grab the first 3 for now to test # Get current coordinates of each joint UR5_q = [] for joint in UR5: res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) UR5_q.append(value) # Store current values # Set new coordinates for the robot in coppeliaSim # Add time delays so the animation can be displayed correctly steps = 100 position_desired = theta for t in range(steps): i = 0 k = 0 for joint in UR5: sim.simxSetJointTargetPosition(clientID, joint, t*(position_desired[i])/steps, sim.simx_opmode_streaming) res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) if t == 0 or t == 99: k += 1 res, position = sim.simxGetObjectPosition(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) res, orientation = sim.simxGetObjectOrientation(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) i += 1 time.sleep(2/steps) # Convert robot angles to the 4x4 matrix representation for comparison c1, s1 = np.cos(orientation[0]), np.sin(orientation[0]) c2, s2 = np.cos(orientation[1]), np.sin(orientation[1]) c3, s3 = np.cos(orientation[2]), np.sin(orientation[2]) R_ur5 = np.block([ [c2 * c3, -c2 * s3, s2], [c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], [s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] ]) p_ur5 = np.array(position).reshape((3,1)) T_ur5 = np.block([ [R_ur5, p_ur5], [0, 0, 0, 1] ]) print('\n Robot coordinates: \n ', T_ur5 ) # print('\n Absolute Error : \n', abs(T_theta - T_ur5)) time.sleep(1) # print('\n Returning to home position...') # for joint in UR5: # sim.simxSetJointTargetPosition(clientID, joint, 0, sim.simx_opmode_streaming) # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(clientID,'Excuting forward kinematics in Python',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(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID)
def isConnected(self): c, result = sim.simxGetPingTime(self.client_id) # Return true if the robot is connected return result > 0
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)
#control_force = 400 if np.sign(control_force) >= 0: set_force = control_force set_velocity = 9999 else: set_force = -control_force set_velocity = -9999 # 控制命令需要同时方式,故暂停通信,用于存储所有控制命令一起发送 vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetVelocity(clientID, jointHandle, set_velocity, vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID, jointHandle, set_force, vrep.simx_opmode_oneshot) #vrep.simxSetJointForce(clientID,jointHandle,set_force,vrep.simx_opmode_oneshot); # vrep.simxSetJointTargetPosition(clientID, jointHandle, 100, vrep.simx_opmode_oneshot) # vrep.simxSetJointTargetPosition(clientID, jointHandle[1], 5/RAD2DEG, vrep.simx_opmode_oneshot) # vrep.simxSetJointTargetVelocity(clientID, jointHandle[2], 500/RAD2DEG, vrep.simx_opmode_oneshot) # vrep.simxSetJointTargetVelocity(clientID, jointHandle[3], 500/RAD2DEG, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) # *** lastCmdTime = currCmdTime # 记录当前时间 vrep.simxSynchronousTrigger(clientID) # 进行下一步 vrep.simxGetPingTime(clientID) # 使得该仿真步走完
def _step_simulation(self): """Шаг симуляции для режима синхронизации """ sim.simxSynchronousTrigger(self.client) sim.simxGetPingTime(self.client)
vrep.simxSetObjectPosition(clientID, objecthandle, -1, a, vrep.simx_opmode_oneshot) if keyboard.is_pressed("end"): a[2] -= 0.0006 vrep.simxSetObjectPosition(clientID, objecthandle, -1, a, vrep.simx_opmode_oneshot) if keyboard.is_pressed("page up"): rot[2] += 0.0004 vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot, vrep.simx_opmode_oneshot) if keyboard.is_pressed("page down"): rot[2] -= 0.0004 vrep.simxSetObjectOrientation(clientID, objecthandle, -1, rot, vrep.simx_opmode_oneshot) if keyboard.is_pressed('q'): break #break # if user pressed a key other than the given key the loop will break vrep.simxAddStatusbarMessage(clientID, 'Hello V-REP!', vrep.simx_opmode_oneshot) vrep.simxGetPingTime(clientID) vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
packedMovementData=msgpack.packb(movementData) sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) targetConfig=[0,0,0,0,0,0] targetVel=[0,0,0,0,0,0] movementData={"id":"movSeq3","type":"mov","targetConfig":targetConfig,"targetVel":targetVel,"maxVel":maxVel,"maxAccel":maxAccel} packedMovementData=msgpack.packb(movementData) sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiMovementDataFunction',[],[],[],packedMovementData,sim.simx_opmode_oneshot) # Execute second and third movement sequence: sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq2',sim.simx_opmode_oneshot) sim.simxCallScriptFunction(client.id,targetArm1,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) sim.simxCallScriptFunction(client.id,targetArm2,sim.sim_scripttype_childscript,'legacyRapiExecuteMovement',[],[],[],'movSeq3',sim.simx_opmode_oneshot) # Wait until above 2 movement sequences finished executing: waitForMovementExecuted1('movSeq3') waitForMovementExecuted1('movSeq3') sim.simxStopSimulation(client.id,sim.simx_opmode_blocking) sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_discontinue) sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_discontinue) sim.simxGetPingTime(client.id) # Now close the connection to CoppeliaSim: sim.simxFinish(client.id) else: print ('Failed connecting to remote API server')
Sensor = sim.simxReadProximitySensor(clientID, sensor, sim.simx_opmode_buffer) print(Sensor[1]) return Sensor def LineSensor(Sensor): Vsensor = sim.simxReadVisionSensor(clientID, Sensor, sim.simx_opmode_buffer) return Vsensor[-1][0][1:4] #Conectandose a Coppelia clientID = Connect(19997) Velocity = 100 * math.pi / 180 print(sim.simxGetPingTime(clientID)) #Handle de los joints Sensor_Handle = Handle('Sensor') Carro = Handle('Pioneer_p3dx') MotorL = Handle('Left') MotorR = Handle('Right') Vsensor_L = Handle('Vsensor_L') Vsensor_R = Handle('Vsensor_R') #Inicializar sensores sim.simxReadProximitySensor(clientID, Sensor_Handle, sim.simx_opmode_streaming) sim.simxReadVisionSensor(clientID, Vsensor_L, sim.simx_opmode_streaming) sim.simxReadVisionSensor(clientID, Vsensor_R, sim.simx_opmode_streaming) sim.simxGetObjectPosition(clientID, Carro, -1, sim.simx_opmode_streaming)
# Set Robot Position sim.simxSetObjectPosition(clientID, LocM1, -1, Loc1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(clientID, LocM2, -1, Loc2, sim.simx_opmode_oneshot) # Start the Simulation print("Robot1 Position:", Loc1) print("Robot2 Position:", Loc2) print("Robot1 Orientation:", OriRobo1) print("Robot2 Orientation:", OriRobo2) print("Simulation Running ...") #sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) time.sleep(1) sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait) iter += 1 # 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(clientID) # End Connection to V-Rep sim.simxFinish(clientID) else: print("Failed connecting to remote API server") print("Program Ended")
def main(argv): # https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm --> how to remote API # Refer to https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxStart sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID == -1: print('Failed connecting to remote API server') else: print('Connected to remote API client') numberOfRuns = 10 numberOfMeasurements = 1 variationSize = 0.001 for i in range(numberOfRuns): count = 0 dt = 0.0167 sim.simxSynchronous(clientID, True) ################################################### init variables ################################################### ros_handle = simROS.SIMROS(argv) targetAngle = 0 #targetAngles = list(np.random.randint(-25, 26, numberOfMeasurements)) targetAngles = list(np.zeros(numberOfMeasurements)) print(targetAngles) randomVar = [] for i in range(18): randomVar.append( list( np.random.uniform(-variationSize, variationSize, numberOfMeasurements))) if newFile: with open(dataFilePath, 'w+') as myfile: wr = csv.writer(myfile, quoting=csv.QUOTE_ALL) wr.writerow([ "Inclination", "LFx", "LFy", "LFz", "LMx", "LMy", "LMz", "LHx", "LHy", "LHz", "RFx", "RFy", "RFz", "RMx", "RMy", "RMz", "RHx", "RHy", "RHz" ]) counter = 0 Done = False #Start the simulation sim.simxStartSimulation(clientID, sim.simx_opmode_blocking) ################################################### Control Loop ################################################### while not Done: #code here #print(sim.simxReadForceSensor(clientID, "3D_force0", sim.simx_opmode_blocking)) if counter == 150: saveData(force, targetAngle) print("Runs remaining: ", len(targetAngles)) if len(targetAngles) <= 0: Done = True else: targetAngle = targetAngles.pop(0) motor_positions = [ 0, randomVar[1].pop() + 2.059, randomVar[2].pop(), 0, randomVar[4].pop() + 2.059, randomVar[5].pop(), 0, randomVar[7].pop() + 2.059, randomVar[8].pop(), 0, randomVar[10].pop() + 2.059, randomVar[11].pop(), 0, randomVar[13].pop() + 2.059, randomVar[14].pop(), 0, randomVar[16].pop() + 2.059, randomVar[17].pop() ] counter = 0 ros_handle.setSlopeAngle(targetAngle) ros_handle.setLegMotorPosition(motor_positions) counter = counter + 1 #Step the simulation sim.simxSynchronousTrigger(clientID) sim.simxStopSimulation(clientID, sim.simx_opmode_blocking) sim.simxGetPingTime(clientID) sim.simxFinish(clientID)