def setJointPositionNonBlock(self, clientID, joint, angle): handle = self.jointHandleMapping[joint] error = False if (handle == 0): error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_streaming)
def setJointPosition(clientID, joint, angle): if (jointHandleMapping[joint] > 0): jhandle=jointHandleMapping[joint] else: error, jhandle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) jointHandleMapping[joint]=jhandle vrep.simxSetJointPosition(clientID,jhandle,angle,vrep.simx_opmode_oneshot_wait)
def setJointPosition(self, clientID, joint, angle): error = False handle = self.jointHandleMapping[joint] if (handle == 0): errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) error = errorGetObjetHandle or error self.jointHandleMapping[joint]=handle error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait) return error
def setJointPosition(self, clientID, joint, angle): error = False handle = self.jointHandleMapping[joint] if handle == 0: errorGetObjetHandle, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot_wait) error = errorGetObjetHandle or error self.jointHandleMapping[joint]=handle #error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot_wait) error = error or vrep.simxSetJointPosition(clientID,handle,angle,vrep.simx_opmode_oneshot) #using oneshot instead of oneshot_wait make it a nonblocking call but is the only way to avoid 5 seconds wait in the execution of the individual if self.synchronous: vrep.simxSynchronousTrigger(clientID) return error
# cube_y_pos = np.random.choice([-1.0, 1.0]) * np.random.uniform(0.1, 0.4) cube_y_pos = np.random.uniform(0.1, 0.5) cube_z_pos = init_cube_pos[2] vrep.simxSetObjectPosition(clientID, cube, -1, [cube_x_pos, cube_y_pos, cube_z_pos], vrep.simx_opmode_oneshot) # Reset joint pos # jh[0]/joint1, range: [-1/2*pi, 1/2*pi] # jh[1]/joint2, range: [-3/4*pi, 0] # jh[2]/joint3, range: [-pi, 0] joint1_pos = np.random.uniform(0.25 * -np.pi, 0.25 * np.pi) joint2_pos = np.random.uniform(0.5 * -np.pi, 0) joint3_pos = np.random.uniform(0.5 * -np.pi, 0) current_config = [joint1_pos, joint2_pos, joint3_pos] vrep.simxSetJointPosition(clientID, joint_handles[0], joint1_pos, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, joint_handles[1], joint2_pos, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, joint_handles[2], joint3_pos, vrep.simx_opmode_oneshot) if path_type == 'ik': err, ikpath = generate_ikpath() if err != 0: err_count += 1 print("Non successful pathfinding, redoing epoch...") else: success_count += 1 follow_path(ikpath, joint_handles, vs_diag, distance_handle, textfile) elif path_type == 'interpolate': end_config = get_end_config()
joint3_grid = np.linspace(-0.75 * np.pi, 0, grid_range) epoch_counter = 1 epochs = grid_range**5 for cx in cube_x_grid: for cy in cube_y_grid: for j1 in joint1_grid: for j2 in joint2_grid: for j3 in joint3_grid: print("Epoch {}/{}".format(epoch_counter, epochs)) epoch_counter += 1 vrep.simxSetObjectPosition( clientID, cube, -1, [cx, cy, cz], vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, joint_handles[0], j1, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, joint_handles[1], j2, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, joint_handles[2], j3, vrep.simx_opmode_oneshot) err, ikpath = generate_ikpath() if err != 0: err_count += 1 errorfile.write( str(cx) + " " + str(cy) + " " + str(cz) + " " + str(j1) + " " + str(j2) + " " + str(j3) + "\n") else:
print counter # move the robot somehow vrep.simxSetJointTargetVelocity(clientID,frontleftMotorHandle,0.5,vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,rearleftMotorHandle,0.5,vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,rearrightMotorHandle,-0.5,vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,frontrightMotorHandle,-0.5,vrep.simx_opmode_oneshot) # generate random target positions for the arm rand_base_rotate = numpy.random.uniform(-40,40)*math.pi/180 rand_base_lower = numpy.random.uniform(15,35) *math.pi/180 rand_elbow_bend = numpy.random.uniform(0,50) *math.pi/180 rand_hand_bend = numpy.random.uniform(10,90) *math.pi/180 # instruct the arm to move (all joints at once) vrep.simxPauseCommunication(clientID,True) vrep.simxSetJointPosition(clientID,armJoint0Handle, rand_base_rotate,vrep.simx_opmode_streaming) # base - rotates the entire arm - [-40..40] vrep.simxSetJointPosition(clientID,armJoint1Handle, rand_base_lower ,vrep.simx_opmode_streaming) # base - lowers the arm - [5..35] vrep.simxSetJointPosition(clientID,armJoint2Handle, rand_elbow_bend ,vrep.simx_opmode_streaming) # elbow - bends the arm - [0..50] vrep.simxSetJointPosition(clientID,armJoint3Handle, rand_hand_bend ,vrep.simx_opmode_streaming) # hand - bends the lower part of the arm - [0..90] vrep.simxSetJointPosition(clientID,armJoint4Handle, 0.0,vrep.simx_opmode_streaming) # wrist - turns the gripper vrep.simxPauseCommunication(clientID,False) # give the arm some time to move time.sleep(1) # verify that the arm has approximately reached its target position err0 = vrep.simxGetJointPosition(clientID,armJoint0Handle,vrep.simx_opmode_streaming)[1] - rand_base_rotate err1 = vrep.simxGetJointPosition(clientID,armJoint1Handle,vrep.simx_opmode_streaming)[1] - rand_base_lower err2 = vrep.simxGetJointPosition(clientID,armJoint2Handle,vrep.simx_opmode_streaming)[1] - rand_elbow_bend err3 = vrep.simxGetJointPosition(clientID,armJoint3Handle,vrep.simx_opmode_streaming)[1] - rand_hand_bend err4 = vrep.simxGetJointPosition(clientID,armJoint4Handle,vrep.simx_opmode_streaming)[1] - 0.0
def set_angles(self, angles): for q, handle in zip(angles, self._joint_handles): vrep.simxSetJointPosition(self._client_id, handle, q, vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self._client_id)
def render(self,angles): time.sleep(0.0001) for i in range(len(self.jointHandles)): vrep.simxSetJointPosition(self.clientID, self.jointHandles[i], angles[i], vrep.simx_opmode_oneshot)
def train(load_model_path, save_model_path, number_training_steps, data_path, image_shape, ratio=1, batch_size=2, save_online_batch=False, save_online_batch_path=None): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # Load data data_file = h5py.File(data_path,"r") x = h5py_to_array(data_file['images'][:1000], image_shape) y = data_file['joint_vel'][:1000] datapoints = x.shape[0] # Initialize ratios online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1))) data_images_batchsize = int(batch_size - online_batchsize) current_online_batch = 0 x_batch = np.empty((batch_size,) + image_shape) y_batch = np.empty((batch_size, 6)) jointpos_array = np.empty((batch_size, 6)) nextpos_array = np.empty((batch_size, 6)) print "Batch size: ", batch_size print "Online batch size: ", online_batchsize print "Dataset batch size: ", data_images_batchsize # Load keras model model = load_model(load_model_path) # Use client id from connection clientID = connection.clientID # Get joint handles jhList = [-1, -1, -1, -1, -1, -1] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking) jhList[i] = jh # Initialize joint position jointpos = np.zeros(6) for i in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming) jointpos[i] = jp # Initialize vision sensor res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming) vrep.simxGetPingTime(clientID) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) # Initialize distance handle err, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming) # Initialize online batch hdf5 file if save_online_batch: f = h5py.File(save_online_batch_path, "w") dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32) dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32) dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32) dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32) dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32) # Step while IK movement has not begun returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) # Iterate over number of steps to train model online path_empty_counter = 0 step_counter = 0 while step_counter < number_training_steps: # 1. Obtain image from vision sensor and next path position from Lua script err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network img = img.astype(np.uint8) img = img.astype(np.float32) img /= 255 inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'getNextPathPos', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) # 2. Pass into neural network to get joint velocities jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector stepsize = 1 jointvel *= stepsize # 3. Apply joint velocities to arm in V-REP for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot) err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer) # Check if next pos is valid before fitting if len(next_pos) != 6: path_empty_counter += 1 continue ik_jointvel = joint_difference(jointpos, next_pos) ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target ik_jointvel = np.resize(ik_jointvel, (1, 6)) x_batch[current_online_batch] = img[0] y_batch[current_online_batch] = ik_jointvel[0] jointpos_array[current_online_batch] = jointpos nextpos_array[current_online_batch] = next_pos dset4[current_online_batch] = distance_to_target current_online_batch += 1 if current_online_batch == online_batchsize: # Step counter print "Training step: ", step_counter step_counter += 1 # Random sample from dataset if ratio > 0: indices = random.sample(range(int(datapoints)), int(data_images_batchsize)) indices = sorted(indices) x_batch[online_batchsize:] = x[indices] y_batch[online_batchsize:] = y[indices] dset4[online_batchsize:] = data_file["distance"][indices] dset2[online_batchsize:] = data_file["joint_pos"][indices] # 4. Fit model model.fit(x_batch, y_batch, batch_size=batch_size, epochs=1) # Reset counter current_online_batch = 0 # Save to online batch dataset dset1[:] = x_batch dset5[:] = y_batch dset2[:online_batchsize] = jointpos_array[:online_batchsize] dset3[:] = nextpos_array # Print statements # print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel)) # print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel)) # print "Distance to cube: ", distanceToCube # trigger next step and wait for communication time vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) # save updated model and delete model model.save(save_model_path) del model # Close file if save_online_batch: f.close() # Error check print "No of times path is empty:", path_empty_counter
def control(load_model_path, total_episodes=10, im_function=False): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # Load keras model model = load_model(load_model_path, custom_objects={'empty': empty, 'mmd2_rbf_quad': empty}) # Initialize adam optimizer adam = keras.optimizers.adam(lr=0.001) model.compile(loss='mean_squared_error', optimizer=adam, metrics=['mae']) # Use client id from connection clientID = connection.clientID # Get joint handles jhList = [-1, -1, -1, -1, -1, -1] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking) jhList[i] = jh # Initialize joint position jointpos = np.zeros(6) for i in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming) jointpos[i] = jp # Initialize vision sensor res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming) vrep.simxGetPingTime(clientID) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) # Initialize distance handle err, distanceToCubeHandle = vrep.simxGetDistanceHandle(clientID, "tipToCube", vrep.simx_opmode_blocking) err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_streaming) err, distanceToTargetHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_streaming) # numpy print options np.set_printoptions(precision=5) # Step while IK movement has not begun returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) # Iterate over total steps desired image_shape = (128, 128, 3) current_episode = 0 step_counter = 0 while current_episode < total_episodes + 1: # obtain current episode inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() episode_table = [] while len(episode_table) == 0: err, episode_table, _, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'episodeCount', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) if episode_table[0] > current_episode: step_counter = 0 print "Episode: ", episode_table[0] current_episode = episode_table[0] step_counter += 1 # 1. Obtain image from vision sensor err, resolution, img = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) img = np.array(img, dtype=np.uint8) img = np.resize(img, (1,) + image_shape) # resize into proper shape for input to neural network img = img.astype('float32') img = img / 255 # normalize input image original_img = img if im_function: #img = adjust_gamma(img, gamma=2) img = tint_images(img, [1,1,0]) if (current_episode==1) and (step_counter==1): print "Displaying before and after image transformation" display_2images(original_img[0], img[0]) plt.show() # 2. Pass into neural network to get joint velocities jointvel = model.predict(img, batch_size=1)[0][0] # output is a 2D array of 1X6, access the first variable to get vector stepsize = 1 jointvel *= stepsize # 3. Apply joint velocities to arm in V-REP for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot) # Obtain distance err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_buffer) err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_buffer) # Print statements # print "Step: ", step_counter print "Joint velocities: ", jointvel, " Abs sum: %.3f" % np.sum(np.absolute(jointvel)) # print "Distance to cube: %.3f" % distanceToCube, ", Distance to target: %.3f" % distanceToTarget # trigger next step and wait for communication time vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # obtain performance metrics inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() err, minDistStep, minDist, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'performanceMetrics', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print "Min distance: ", minDist minThreshold = 0.2 print "Success rate: ", 1.0*np.sum(np.array(minDist) <= minThreshold)/len(minDist) print "Total episodes: ", len(minDist) print "Average min distance: %.3f" % np.mean(minDist) print "Standard deviation: %.3f" % np.std(minDist) # other performance metrics such as success % can be defined (i.e. % reaching certain min threshold) # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) # delete model del model return
-1,# Setting the absolute position position=des_x, operationMode=vrep.simx_opmode_blocking ) """ # Calculate desired endeffector pose des_pose = np.concatenate((des_x, np.zeros((3, )))) # set the joint angles position J = np.reshape(retFloats, [6, 7]) J_pinv = np.linalg.pinv(J) for joint_handle, des_joint_angle in zip(joint_handles, des_pose): des_joint_angles = np.matmul(J_pinv, des_pose) vrep.simxSetJointPosition(clientID, joint_handle, des_joint_angle, vrep.simx_opmode_blocking) # move simulation ahead one time step vrep.simxSynchronousTrigger(clientID) else: raise Exception('Failed connecting to remote API server') finally: # stop the simulation vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) # Before closing the connection to V-REP, # make sure that the last command sent out had time to arrive. vrep.simxGetPingTime(clientID)
def setJointPosition(self, jointID, pos): #Set a certain joint to given position vrep.simxSetJointPosition(clientID, jointID, pos, vrep.simx_opmode_streaming)
def start_simulation(self): # start our simulation in lockstep with our code vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) vrep.simxSetJointPosition(self.clientID, self.joint_handles[1], 1.57, vrep.simx_opmode_oneshot)
ec1, motor1_handle = vrep.simxGetObjectHandle(clientID, 'PhantomXPincher_joint1', vrep.simx_opmode_oneshot_wait) ec2, motor2_handle = vrep.simxGetObjectHandle(clientID, 'PhantomXPincher_joint2', vrep.simx_opmode_oneshot_wait) ec3, motor3_handle = vrep.simxGetObjectHandle(clientID, 'PhantomXPincher_joint3', vrep.simx_opmode_oneshot_wait) ec4, motor4_handle = vrep.simxGetObjectHandle(clientID, 'PhantomXPincher_joint4', vrep.simx_opmode_oneshot_wait) ec5, auxMotor2_handle = vrep.simxGetObjectHandle(clientID, 'uarm_auxMotor2', vrep.simx_opmode_oneshot_wait) ec6, gripperMotor2_handle = vrep.simxGetObjectHandle( clientID, 'uarmGripper_motor2Method2', vrep.simx_opmode_oneshot_wait) #vrep.simxSetJointTargetPosition(clientID, motor1_handle, pi, vrep.simx_opmode_oneshot) #rotates motor1, takes radians, default is at pi/2 #vrep.simxSetJointTargetPosition(clientID, motor2_handle, 1, vrep.simx_opmode_blocking)# motor2 and 3 work simultaneously #vrep.simxSetJointPosition(clientID, motor2_handle, 0.5, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, motor3_handle, 1, vrep.simx_opmode_oneshot) """ #If not sure about commands being received, add a 'pioneer p3dx' to the scene and run this. _, left_motor_handle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.2, vrep.simx_opmode_streaming) """ time.sleep(1) vrep.simxFinish(clientID)
def evaluate_individual(weightMatrix): print('Evaluating individual...') if clientID == -1: # This means that the instance was not initialised! raise ValueError("The V-REP instance was not initialised!") else: deltaTime = 0.01 # Shoulders should point downwards by default! jointIndices = [15, 13, 11, 12, 14, 16, 1, 2] jointOffsets = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -math.radians(90.0), math.radians(90.0) ] # Get handles _, pivotHandle = vrep.simxGetObjectHandle( clientID, 'Pivot', vrep.simx_opmode_oneshot_wait) _, shoulderRightHandle = vrep.simxGetObjectHandle( clientID, 'Joint3', vrep.simx_opmode_oneshot_wait) _, shoulderLeftHandle = vrep.simxGetObjectHandle( clientID, 'Joint4', vrep.simx_opmode_oneshot_wait) _, hipRightHandle = vrep.simxGetObjectHandle( clientID, 'Joint7', vrep.simx_opmode_oneshot_wait) _, hipLeftHandle = vrep.simxGetObjectHandle( clientID, 'Joint8', vrep.simx_opmode_oneshot_wait) jointHandles = [] for i in jointIndices: _, h = vrep.simxGetObjectHandle(clientID, 'Joint' + str(i), vrep.simx_opmode_oneshot_wait) jointHandles.append(h) print('Object handles received.') # Get bioloid handle _, torsoHandle = vrep.simxGetObjectHandle( clientID, 'Bioloid', vrep.simx_opmode_oneshot_wait) # Start simulation vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) vrep.simxSynchronous(clientID, True) print('Sim started.') # Speed up simulation vrep.simxSetIntegerParameter(clientID, vrep.sim_intparam_speedmodifier, 10, vrep.simx_opmode_oneshot_wait) # Set initial joint angles for i in range(0, len(jointHandles)): vrep.simxSetJointPosition(clientID, jointHandles[i], jointOffsets[i], vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(clientID, jointHandles[i], jointOffsets[i], vrep.simx_opmode_oneshot_wait) # Set joints that are not considered by the optimizaton shoulderAngle = math.radians(80.0) vrep.simxSetJointPosition(clientID, shoulderRightHandle, -shoulderAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(clientID, shoulderRightHandle, -shoulderAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointPosition(clientID, shoulderLeftHandle, shoulderAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(clientID, shoulderLeftHandle, shoulderAngle, vrep.simx_opmode_oneshot_wait) hipAngle = math.radians(45.0) vrep.simxSetJointPosition(clientID, hipRightHandle, -hipAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(clientID, hipRightHandle, -hipAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointPosition(clientID, hipLeftHandle, hipAngle, vrep.simx_opmode_oneshot_wait) vrep.simxSetJointTargetPosition(clientID, hipLeftHandle, hipAngle, vrep.simx_opmode_oneshot_wait) # Evaluate for a number of integration steps _, lastPos = vrep.simxGetObjectPosition(clientID, torsoHandle, -1, vrep.simx_opmode_oneshot_wait) position = 0 maxPosition = 0 lowestPos = 5 #starting way above cumZPos = 0 furthestDistanceIteration = 0 bn = cpg.bioloid_network.BioloidNetwork(weightMatrix, deltaTime) nrIterations = 2000 for iteration in range(0, nrIterations): # Print progress if iteration % 100 == 0: print("Evaluation iteration: " + str(iteration) + " / " + str(nrIterations)) # CPG network step jointAngles = bn.get_output() # Set joint angles for i in range(0, len(jointHandles)): vrep.simxSetJointTargetPosition( clientID, jointHandles[i], jointOffsets[i] + jointAngles[i], vrep.simx_opmode_oneshot) # VREP simulation step vrep.simxSynchronousTrigger(clientID) # Measure how far forward the robot moves in this timestep if iteration % 10 == 0: # Measure delta _, currPos = vrep.simxGetObjectPosition( clientID, torsoHandle, -1, vrep.simx_opmode_oneshot_wait) delta2D = [currPos[0] - lastPos[0], currPos[1] - lastPos[1]] lastPos = currPos # Measure forward vector _, currLocalPos = vrep.simxGetObjectPosition( clientID, torsoHandle, pivotHandle, vrep.simx_opmode_oneshot_wait) forward2D = [currLocalPos[1], -currLocalPos[0]] forward2DLength = math.sqrt(forward2D[0] * forward2D[0] + forward2D[1] * forward2D[1]) if forward2DLength == 0.0: forward2D = [0.0, 0.0] else: forward2D = [ forward2D[0] / forward2DLength, forward2D[1] / forward2DLength ] # What is the distance along the forward vector? forwardDistance = delta2D[0] * forward2D[0] + delta2D[ 1] * forward2D[1] position += forwardDistance if position > maxPosition: # save new best furthestDistanceIteration furthestDistanceIteration = iteration maxPosition = max(position, maxPosition) lowestPos = min(lowestPos, currPos[2]) cumZPos += currPos[2] if math.isnan(position): print('ROBOT POSITION IS NAN! Returning zero fitness.') break if currPos[2] < 0.1: print('ROBOT HAS FALLEN! Stopping evaluation.') break if iteration - furthestDistanceIteration > 300: # if we haven't improved the max distance for in 300 iterations, break print('ROBOT IS NOT MOVING FORWARD! Stopping evaluation.') break # Measure movement print('Last known position: ' + str(position)) print('Max position: ' + str(maxPosition)) print('Lowest position: ' + str(lowestPos)) # Stop simulation vrep.simxSynchronous(clientID, False) vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) print('Sim stopped.') print('Done.') fitness = maxPosition * lowestPos #fitness = cumZPos #fitness = maxPosition # Only return valid fitness values if math.isnan(fitness) or fitness >= 1000.0 or math.isnan(position): fitness = 0.0 return fitness
def set_joints_position(self, client_id, positions): for handle, pos in zip(self.handlesList, positions): vrep.simxSetJointPosition(client_id, handle, pos, self.opmode)
if clientID != -1: print("Connection Established!") else: sys.exit("Error: No connection could be established") # gathers object handles _, J1_handle = vrep.simxGetObjectHandle(clientID, 'J1', vrep.simx_opmode_oneshot_wait) _, J2_handle = vrep.simxGetObjectHandle(clientID, 'J2', vrep.simx_opmode_oneshot_wait) _, J3_handle = vrep.simxGetObjectHandle(clientID, 'J3', vrep.simx_opmode_oneshot_wait) _, J4_handle = vrep.simxGetObjectHandle(clientID, 'J4', vrep.simx_opmode_oneshot_wait) _, cam_handle = vrep.simxGetObjectHandle(clientID, 'Vision1', vrep.simx_opmode_oneshot_wait) while(1): # randomly chooses joint movement vrep.simxSetJointPosition(clientID, J1_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, J2_handle, math.radians(random.randint(-65, 65)), vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, J3_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, J4_handle, math.radians(random.randint(0, 360)), vrep.simx_opmode_oneshot) _, resolution, image = vrep.simxGetVisionSensorImage(clientID, cam_handle, 0, vrep.simx_opmode_streaming) time.sleep(1) # saves camera frame, rotates image and converts to BGR _, resolution, image =vrep.simxGetVisionSensorImage(clientID,cam_handle,0, vrep.simx_opmode_buffer) img = np.array(image, dtype=np.uint8) img.resize([resolution[0], resolution[1], 3]) img = np.rot90(img,2) img = np.fliplr(img) img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) # converts img to hsv and detect colors hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) green_low = np.array([49, 50, 50], dtype=np.uint8)
def set_jj(jj): for i in range(6): err = vrep.simxSetJointPosition(clientID, jjh[i], jj[i], vrep.simx_opmode_blocking)
import vrep import numpy as np print("start vrep successfully ") vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 500, 5) if clientID != -1: res, objs = vrep.simxGetObjects(clientID, vrep.sim_handle_all, vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: print('Number of objects in the scene: ', len(objs)) ret1, LBR4p_joint1 = vrep.simxGetObjectHandle(clientID, 'LBR4p_joint1#', vrep.simx_opmode_buffer) ret2 = vrep.simxSetJointPosition(clientID, LBR4p_joint1, .25, vrep.simx_opmode_buffer) else: print('Remote API function call returned with error code: ', res) vrep.simxFinish(clientID) else: print('Failed connecting to remote API server') print('Program ended')
# Mush the quaternion and pos into a pose matrix! # Our function takes care of the V-REP quaternion mismatch pose = quaternion.matrix_from_quaternion(dummy_quaternion) pose[0, 3] = dummy_pos[0] pose[1, 3] = dummy_pos[1] pose[2, 3] = dummy_pos[2] print("Pose is: \n{}".format(pose)) # Get the thetas required to move the robot to the desired position theta_list = inverse_kinematics.inverse_kinematics(pose) print("Thetas are: {}".format(theta_list)) if theta_list is not None: print("Moving robot") for i in range(7): # Set the position of each joint vrep.simxSetJointPosition(clientID, joint_handles[i], theta_list[i], vrep.simx_opmode_oneshot_wait) # print("Setting joint", i+1, "to", theta_list[i]) sleep(0.25) # Grab the actual pose errorCode, pos = vrep.simxGetObjectPosition( clientID, joint_handles[6], joint_handles[0], vrep.simx_opmode_oneshot_wait) errorCode, angles = vrep.simxGetObjectQuaternion( clientID, joint_handles[6], joint_handles[0], vrep.simx_opmode_oneshot_wait) print("Actual pos:", pos) print("Actual qua:", angles) # Print each joint's angle
ret,joint1_handler = vrep.simxGetObjectHandle(\ clientID,"redundantRob_joint1",vrep.simx_opmode_oneshot_wait) ret,joint1 = vrep.simxGetJointPosition(\ clientID,joint1_handler,vrep.simx_opmode_streaming) ret,joint2_handler = vrep.simxGetObjectHandle(\ clientID,"redundantRob_joint2",vrep.simx_opmode_oneshot_wait) ret,joint2 = vrep.simxGetJointPosition(\ clientID,joint2_handler,vrep.simx_opmode_streaming) startSim(clientID) ret,joint1 = vrep.simxGetJointPosition(\ clientID,joint1_handler,vrep.simx_opmode_buffer) print joint1 #Get position joint 1 ret,joint2 = vrep.simxGetJointPosition(\ clientID,joint2_handler,vrep.simx_opmode_buffer) print joint2 #Get position joint 2 ret = vrep.simxSetJointPosition(\ clientID,joint1_handler,pi/2,vrep.simx_opmode_oneshot) #Set pi/2 to joint 1 time.sleep(2) stopSim(clientID) time.sleep(1) disconnectVREP(clientID)
def set_complete_pose(self,joint_positions): if(len(joint_positions) != self.NUM_JOINTS): raise RuntimeError("error: vrep_robot joint command does not match with number of joints: NUM_JOINTS="+str(self.NUM_JOINTS)+", given length="+str(len(joint_positions))) for i in range(self.NUM_JOINTS): return_code = vrep.simxSetJointPosition(self.client_id,self.joint_handles[i],joint_positions[i],vrep.simx_opmode_blocking) self.check_for_errors(return_code)
def test(self,jointNum,angle): vrep.simxSetJointPosition(self.clientID,self.jointHandles[jointNum-1],angle,vrep.simx_opmode_oneshot)
def turn_joint(clientID, joint_handle, angle, joint_num, sleep_time): time.sleep(sleep_time) theta = get_joint_val(clientID, joint_handle) vrep.simxSetJointPosition(clientID, joint_handle, theta + angle, vrep.simx_opmode_oneshot) time.sleep(sleep_time)
def move_lidar_motor(handle, part, locatie, clientID): vrep.simxSetJointPosition(clientID, handle[part], locatie, vrep.simx_opmode_oneshot)
def step(self, action): # action = (node1 angular v, node2 angular v) action = np.clip(action, *self.action_bound) for i in range(7): self.a[i] += action[i] * self.dt self.a[i] %= np.pi p = -np.pi / 2 + self.a[i] vrep.simxSetJointPosition(self.ID, self.h[i], p, vrep.simx_opmode_oneshot) ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam, 0, vrep.simx_opmode_streaming) img1 = np.array(im, dtype=np.uint8) img1.resize([128, 128, 3]) ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam1, 0, vrep.simx_opmode_streaming) img2 = np.array(im, dtype=np.uint8) img2.resize([128, 128, 3]) ec, res, im = vrep.simxGetVisionSensorImage(self.ID, self.cam2, 0, vrep.simx_opmode_streaming) img3 = np.array(im, dtype=np.uint8) img3.resize([128, 128, 3]) hsv = cv2.cvtColor(img1, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.gl, self.gu) # mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] hsv = cv2.cvtColor(img2, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.gl, self.gu) # mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts1 = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] hsv = cv2.cvtColor(img3, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, self.gl, self.gu) # mask = cv2.erode(mask, None, iterations=2) mask = cv2.dilate(mask, None, iterations=2) cnts2 = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] if len(cnts) == 2: M = cv2.moments(cnts[0]) self.x1 = int(M["m10"] / M["m00"]) self.y1 = int(M["m01"] / M["m00"]) M = cv2.moments(cnts[1]) self.x2 = int(M["m10"] / M["m00"]) self.y2 = int(M["m01"] / M["m00"]) self.d1 = np.sqrt( np.square(self.x1 - self.x2) + np.square(self.y1 - self.y2)) else: self.d1 = self.od1 if len(cnts1) == 2: M = cv2.moments(cnts1[0]) self.x3 = int(M["m10"] / M["m00"]) self.y3 = int(M["m01"] / M["m00"]) M = cv2.moments(cnts1[1]) self.x4 = int(M["m10"] / M["m00"]) self.y4 = int(M["m01"] / M["m00"]) self.d2 = np.sqrt( np.square(self.x4 - self.x3) + np.square(self.y3 - self.y4)) else: self.d2 = self.od2 if len(cnts2) == 2: M = cv2.moments(cnts2[0]) self.x5 = int(M["m10"] / M["m00"]) self.y5 = int(M["m01"] / M["m00"]) M = cv2.moments(cnts2[1]) self.x6 = int(M["m10"] / M["m00"]) self.y6 = int(M["m01"] / M["m00"]) self.d3 = np.sqrt( np.square(self.x6 - self.x5) + np.square(self.y6 - self.y5)) else: self.d3 = self.od3 s = np.hstack([ self.a[0], self.a[1], self.a[2], self.a[3], self.a[4], self.a[5], self.a[6], self.a[7], self.x1, self.y1, self.x2, self.y2, self.x3, self.y3, self.x4, self.y4, self.x5, self.y5, self.x6, self.y6 ]) r, self.done = self._r_func(self.d1, self.d2, self.d3) return s, r, self.done, {}
def setPose(self, pose): vrep.simxPauseCommunication(self.id, True) for j, p in zip(self.joints, pose): vrep.simxSetJointPosition(self.id, j, p, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(self.id, False)
def move_lidar_motor(part_name, location): vrep.simxSetJointPosition(clientID, handle[part_name], location, vrep.simx_opmode_oneshot)
assert client_id != -1, 'Failed connecting to remote API server' e = vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait) # print ping time sec, msec = vrep.simxGetPingTime(client_id) print "Ping time: %f" % (sec + msec / 1000.0) # Handle of neck joint e, neck = vrep.simxGetObjectHandle(client_id, 'Bill_neck', vrep.simx_opmode_oneshot_wait) e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_streaming) print "Current position = %0.3f degrees" % neck_pos e = vrep.simxSetJointPosition(client_id, neck, -60, vrep.simx_opmode_streaming) e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_streaming) print "Current position = %0.3f degrees" % neck_pos # Sleep for a small time for i in range(0,1800): #time.sleep(0.1) print "setting current position as %d" % math.fmod(i,360) e = vrep.simxSetJointPosition(client_id, neck, i/180. * math.pi, vrep.simx_opmode_oneshot_wait) e, neck_pos = vrep.simxGetJointPosition(client_id, neck, vrep.simx_opmode_oneshot_wait) print "Current position = %0.3f degrees" % (neck_pos/math.pi * 180)
def set_joint_angle(clientID, joint_handle, angle, joint_num, sleep_time): time.sleep(sleep_time) vrep.simxSetJointPosition(clientID, joint_handle, angle, vrep.simx_opmode_oneshot) time.sleep(sleep_time)
def setJointPosition(incAngle, steps): for i in range(steps): errorCode = vrep.simxSetJointPosition(clientID, Revolute_joint_handle, i * incAngle * deg, vrep.simx_opmode_oneshot_wait)
def set_motor_position(self, motor_handler, position): """Sets the motor position""" #debut = time.time() vrep.simxSetJointPosition(self.client_id, motor_handler, position, vrep.simx_opmode_oneshot_wait)
def show_entry_fields(self): deg = math.pi/180 self.getNumber1 = self.entry1.get() self.getNumber2 = self.entry2.get() self.getNumber3 = self.entry3.get() x = self.getNumber1 y = self.getNumber2 z = self.getNumber3 if x =='' or y==''or z =='': x = 0 y = 0 z = 0 print("Error") else: x = float(self.getNumber1) y = float(self.getNumber2) z = float(self.getNumber3) """ if float(x) >= 125: x = 125 print("X_axis is out of range") if float(y) >= 125: y = 125 print("Y_axis is out of range") if float(z) >= 400: z = 400 print("Z_axis is out of range") if float(x) <= -125: x = -125 print("X_axis is out of range") if float(y) <= -125: y = -125 print("Y_axis is out of range") if float(z) < 0: z = 0 print("Z_axis is out of range") if (float(x) <= -125*math.sin(30*deg)) & (float(y) >= 125*math.cos(30*deg)): x = -125*math.sin(30*deg) y = 125*math.cos(30*deg) if (float(x) <= -125*math.sin(30*deg)) & (float(y) <= -125*math.cos(30*deg)): x = -125*math.sin(30*deg) y = -125*math.cos(30*deg) if (float(x) >= 125*math.sin(30*deg)) & (float(y) >= 125*math.cos(30*deg)): x = 125*math.sin(30*deg) y = 125*math.cos(30*deg) if (float(x) >= -125*math.sin(30*deg)) & (float(y) <= -125*math.cos(30*deg)): x = 125*math.sin(30*deg) y = -125*math.cos(30*deg) if float(z) >= 330: z = 330 """ e=x # v-rep world unit is meter , r=y/1000 t=z/1000 t = t+0.1165 # t = 0 , heater touch the heat bed ( heat bed is 0.1165 high ) if t <=0: t = 0 vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID!= -1: print("Connected to remote server") else: print('Connection not successful') # sys.exit('Could not connect') errorCode,joint1=vrep.simxGetObjectHandle(clientID,'joint1',vrep.simx_opmode_oneshot_wait) if errorCode == -1: print('Can not find plate') # sys.exit() errorCode=vrep.simxSetJointPosition(clientID,joint1,x, vrep.simx_opmode_oneshot) errorCode,joint2=vrep.simxGetObjectHandle(clientID,'joint2',vrep.simx_opmode_oneshot_wait) if errorCode == -1: print('Can not find plate') # sys.exit() errorCode=vrep.simxSetJointPosition(clientID,joint2,y, vrep.simx_opmode_oneshot) errorCode,joint3=vrep.simxGetObjectHandle(clientID,'joint3',vrep.simx_opmode_oneshot_wait) if errorCode == -1: print('Can not find plate') # sys.exit() errorCode=vrep.simxSetJointPosition(clientID,joint3,z, vrep.simx_opmode_oneshot) print(x,y,z)
errorCode,Revolute_joint_handle=vrep.simxGetObjectHandle(clientID,'Revolute_joint',vrep.simx_opmode_oneshot_wait) errorCode0,Revolute_joint_handle0=vrep.simxGetObjectHandle(clientID,'Revolute_joint0',vrep.simx_opmode_oneshot_wait) if errorCode == -1: print('Can not find left or right motor') sys.exit() deg = math.pi/180 #vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) for i in range(36): vrep.simxSynchronous(clientID,True) vrep.simxPauseCommunication(clientID,True) vrep.simxSetJointPosition(clientID, Revolute_joint_handle, 10*deg, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, Revolute_joint_handle0, 10*deg, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) vrep.simxSynchronous(clientID, False) vrep.simxSynchronous(clientID,True) vrep.simxPauseCommunication(clientID,True) vrep.simxSetJointPosition(clientID, Revolute_joint_handle, -10*deg, vrep.simx_opmode_oneshot) vrep.simxSetJointPosition(clientID, Revolute_joint_handle0, -10*deg, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) vrep.simxSynchronous(clientID, False)