def move_robot(self, v_left, v_right): code = vrep.simxSetJointTargetVelocity(self.ID, self.left_handle, v_left, const_v.simx_opmode_streaming) code = vrep.simxSetJointTargetVelocity(self.ID, self.right_handle, v_right, const_v.simx_opmode_streaming)
def setWheelsVelocity(self, leftWheel, rightWheel): #max theoretical : 360 #recommanded max = 30 vrep.simxSetJointTargetVelocity(self.clientID, self.leftJointHandle, leftWheel, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(self.clientID, self.rightJointHandle, rightWheel, vrep.simx_opmode_oneshot)
def drive_direct(self, right_wheel_velocity_in_mm_per_sec, left_wheel_velocity_in_mm_per_sec): # vrep: rad/s vrep.simxSetJointTargetVelocity( self._clientID, self._rightWheelJoint, right_wheel_velocity_in_mm_per_sec / (Specs.WheelDiameterInMM * math.pi) * 2 * math.pi, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity( self._clientID, self._leftWheelJoint, left_wheel_velocity_in_mm_per_sec / (Specs.WheelDiameterInMM * math.pi) * 2 * math.pi, vrep.simx_opmode_oneshot)
def set_value_engines(self, x, y): error_code_left = vrep.simxSetJointTargetVelocity( self.clientId, self.left_motor_handle, x * 0.2, vrep.simx_opmode_oneshot_wait) error_code_right = vrep.simxSetJointTargetVelocity( self.clientId, self.right_motor_handle, y * 0.2, vrep.simx_opmode_oneshot_wait) if error_code_left == -1: print('Can' 't find left or right motor') if error_code_right == -1: print('Can' 't find left or right motor')
def open_gripper(self): code = vrep.simxSetJointForce(self.ID, self.gripper_motor, 20, const_v.simx_opmode_blocking) # print(code) code = vrep.simxSetJointTargetVelocity(self.ID, self.gripper_motor, 0.05, const_v.simx_opmode_blocking)
def close_gripper(self, render=False): code = vrep.simxSetJointForce(self.ID, self.gripper_motor, 20, const_v.simx_opmode_blocking) # print(code) code = vrep.simxSetJointTargetVelocity(self.ID, self.gripper_motor, -0.05, const_v.simx_opmode_blocking) if render: self.render()
def __init__(self, name, clientId, handle): self.name = name self.clientID = clientId self.handle = handle #get handles for the wheel motors and stop them _, self.leftJointHandle = vrep.simxGetObjectHandle( self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_blocking) _, self.rightJointHandle = vrep.simxGetObjectHandle( self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.clientID, self.leftJointHandle, 0, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(self.clientID, self.rightJointHandle, 0, vrep.simx_opmode_oneshot) # get handles for the proximity sensors self.proximitySensorsHandles = [None] * 8 #empty table of size 8 for i in range(8): _, self.proximitySensorsHandles[i] = vrep.simxGetObjectHandle( self.clientID, 'ePuck_proxSensor' + str(i + 1), vrep.simx_opmode_blocking)
def set_joint_target_velocity(self, handle, vel): status = vrep.simxSetJointTargetVelocity(self.id, handle, vel, vrep.simx_opmode_oneshot) self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag)
def set_target_velocity(self, target): v.simxSetJointTargetVelocity(self._id, self._handle, target, self._def_op_mode)
def step(self, action): # Activate the motors vrep.simxSetJointTargetVelocity(self.client_id, self.left_motor_handle, action[0], vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.client_id, self.right_motor_handle, action[1], vrep.simx_opmode_blocking) # Get observations observations = {} observations['proxy_sensor'] = [] observations['light_sensor'] = [] # Fetch the vals of proxy sensors for sensor in self.proxy_sensors: _, _, detectedPoint, _, _ = vrep.simxReadProximitySensor( self.client_id, sensor, vrep.simx_opmode_blocking) # Append to list of values observations['proxy_sensor'].append(np.linalg.norm(detectedPoint)) # Fetch the vals of light sensors for sensor in self.light_sensors: # Fetch the initial value in the suggested mode _, _, image = vrep.simxGetVisionSensorImage( self.client_id, sensor, 1, vrep.simx_opmode_blocking) # extract image from list image = image[0] if len(image) else -1 # Append to the list of values observations['light_sensor'].append(image) # vrep gives a positive value for the black strip and negative for the # floor so convert it into 0 and 1 observations['light_sensor'] = np.asarray(observations['light_sensor']) observations['light_sensor'] = np.sign(observations['light_sensor']) # Assign reward reward = {} # For light sensors # If any of the center 2 sensors is 1 give high reward if (observations['light_sensor'][[3, 4]] > 0).any(): reward['light_sensor'] = 5 # If any of second, third, sixth or seventh is 1 elif (observations['light_sensor'][[1, 2, 5, 6]] > 0).any(): reward['light_sensor'] = 2 # If first or last are high elif (observations['light_sensor'][[0, 7]] > 0).any(): reward['light_sensor'] = 0 # Bot is completly out of line else: reward['light_sensor'] = -5 # For proximity sensors reward['proxy_sensor'] = 0 # Should be rewarded for movement r = np.sum(np.sign(action)) * 2 reward['light_sensor'] += r reward['proxy_sensor'] += r # reward['combined'] += r return observations, reward
def left_spd(self, value): vrep.simxSetJointTargetVelocity(self._clientID, self._left_joint, value, vrep.simx_opmode_oneshot) self._left_spd = copy(value) self._fwd_spd, self._rot_spd = self._lr_2_fwd_rot(self._left_spd, self._right_spd)
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait) errorCode, sensor1 = vrep.simxGetObjectHandle( clientID, 'Pioneer_p3dx_ultrasonicSensor1', vrep.simx_opmode_oneshot_wait) errorCode, cam1 = vrep.simxGetObjectHandle(clientID, 'cam2', vrep.simx_opmode_oneshot_wait) # #vrep.simxReadProximitySensor returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( clientID, sensor1, vrep.simx_opmode_streaming) time.sleep(1) returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor( clientID, sensor1, vrep.simx_opmode_buffer) #returnCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor1,vrep.simx_opmode_streaming) print returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector # vrep.simxSetJointTargetVelocity(clientID, left_Motor, 0, vrep.simx_opmode_streaming) ##time.sleep(5) vrep.simxSetJointTargetVelocity(clientID, right_Motor, 0, vrep.simx_opmode_streaming) ##vrep.simxGetCollectionHandle() (clientID,'Pioneer_p3dx',vrep.simxServiceCall()) a = vrep.simxGetObjects(clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait) #print a #vrep.simxGetVisionSensorImage(clientID,cam1,,simx_opmode_streaming) returnCode, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam1, 0, vrep.simx_opmode_streaming) time.sleep(1) returnCode, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam1, 0, vrep.simx_opmode_buffer)