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)
