def update(self):
state = State()
vrep.simxSynchronousTrigger(self._clientID)
# Simulate wheel encoders
rc, pos_right = vrep.simxGetJointPosition(
self._clientID, self._rightWheelJoint,
vrep.simx_opmode_oneshot_wait)
rc, pos_left = vrep.simxGetJointPosition(self._clientID,
self._leftWheelJoint,
vrep.simx_opmode_oneshot_wait)
if self._lastPosRight is not None:
dr = pos_right - self._lastPosRight
dl = pos_left - self._lastPosLeft
if dr >= 0:
dr = math.fmod(dr + math.pi, 2 * math.pi) - math.pi
else:
dr = math.fmod(dr - math.pi, 2 * math.pi) + math.pi
if dl >= 0:
dl = math.fmod(dl + math.pi, 2 * math.pi) - math.pi
else:
dl = math.fmod(dl - math.pi, 2 * math.pi) + math.pi
self._rightEncoderCount += dr * Specs.CountsPerRev / 2 / math.pi
self._leftEncoderCount += dl * Specs.CountsPerRev / 2 / math.pi
if self._rightEncoderCount > 32768:
self._rightEncoderCount -= 65547
if self._rightEncoderCount < -32768:
self._rightEncoderCount += 65547
if self._leftEncoderCount > 32768:
self._leftEncoderCount -= 65547
if self._leftEncoderCount < -32768:
self._leftEncoderCount += 65547
self._lastPosRight = pos_right
self._lastPosLeft = pos_left
if Sensor.LeftEncoderCounts in self._sensorIDs:
state.leftEncoderCounts = int(self._leftEncoderCount)
if Sensor.RightEncoderCounts in self._sensorIDs:
state.rightEncoderCounts = int(self._rightEncoderCount)
# mode
if Sensor.OIMode in self._sensorIDs:
state.mode = self._mode
# battery
if Sensor.BatteryCharge in self._sensorIDs:
state.batteryChargeInMAH = 980
if Sensor.BatteryCapacity in self._sensorIDs:
state.batteryCapacityInMAH = 1348
return state
def update(self):
state = State()
vrep.simxSynchronousTrigger(self._clientID)
# Simulate wheel encoders
rc, pos_right = vrep.simxGetJointPosition(self._clientID, self._rightWheelJoint, vrep.simx_opmode_oneshot_wait)
rc, pos_left = vrep.simxGetJointPosition(self._clientID, self._leftWheelJoint, vrep.simx_opmode_oneshot_wait)
if self._lastPosRight is not None:
dr = pos_right - self._lastPosRight
dl = pos_left - self._lastPosLeft
if dr >= 0:
dr = math.fmod(dr+math.pi, 2*math.pi) - math.pi
else:
dr = math.fmod(dr-math.pi, 2*math.pi) + math.pi
if dl >= 0:
dl = math.fmod(dl+math.pi, 2*math.pi) - math.pi
else:
dl = math.fmod(dl-math.pi, 2*math.pi) + math.pi
self._rightEncoderCount += dr * Specs.CountsPerRev / 2 / math.pi
self._leftEncoderCount += dl * Specs.CountsPerRev / 2 / math.pi
if self._rightEncoderCount > 32768:
self._rightEncoderCount -= 65547
if self._rightEncoderCount < -32768:
self._rightEncoderCount += 65547
if self._leftEncoderCount > 32768:
self._leftEncoderCount -= 65547
if self._leftEncoderCount < -32768:
self._leftEncoderCount += 65547
self._lastPosRight = pos_right
self._lastPosLeft = pos_left
if Sensor.LeftEncoderCounts in self._sensorIDs:
state.leftEncoderCounts = int(self._leftEncoderCount)
if Sensor.RightEncoderCounts in self._sensorIDs:
state.rightEncoderCounts = int(self._rightEncoderCount)
# mode
if Sensor.OIMode in self._sensorIDs:
state.mode = self._mode
# battery
if Sensor.BatteryCharge in self._sensorIDs:
state.batteryChargeInMAH = 980
if Sensor.BatteryCapacity in self._sensorIDs:
state.batteryCapacityInMAH = 1348
return state
def get_angles(self):
angles = []
for i in range(self.DoF):
code, angle = vrep.simxGetJointPosition(self.ID,
self.joint_handles[i],
const_v.simx_opmode_buffer)
angles.append(angle * 180 / math.pi)
return angles
def get_joint_position(self, handle, first_call=False):
opmode = vrep.simx_opmode_streaming if first_call else vrep.simx_opmode_buffer
status, pos = vrep.simxGetJointPosition(self.id, handle, opmode)
self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag)
return pos
def __init__(self):
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
self.out = cv2.VideoWriter('output.mp4', fourcc, 15.0, (1024, 1024))
self.DoF = 6
# self.action_bound = [[-15,15],[-10,110],[-30,30],[-120,120],[-180,180],[-180,180]]
self.action_bound = [[-180, 180], [-180, 180], [-180, 180],
[-180, 180], [-180, 180], [-180, 180]]
self.action_range = [-5, 5]
self.action_dim = self.DoF
self.action_space = gym.spaces.Box(shape=(self.DoF, ), low=-5, high=5)
self.observation_space = gym.spaces.Box(shape=(3 + 6 + self.DoF * 2, ),
low=-180,
high=180)
self.action_dim = self.action_space.shape[0]
self.state_dim = self.observation_space.shape[0]
p = subprocess.Popen(['ps', '-A'], stdout=subprocess.PIPE)
out, err = p.communicate()
for line in out.splitlines():
if b'vrep' in line:
pid = int(line.split(None, -1)[0])
os.kill(pid, signal.SIGKILL)
self.vrep_root = "/home/ali/Downloads/VREP"
self.scene_file = "/home/ali/RL_code/env/rozum_model.ttt"
#
os.chdir(self.vrep_root)
os.system("./vrep.sh -s " + self.scene_file + " &")
vrep.simxFinish(-1)
time.sleep(1)
# get the ID of the running simulation
self.ID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5)
# check the connection
if self.ID != -1:
print("Connected")
else:
sys.exit("Error")
# get handles
# for camera
self.cam_handle = self.get_handle('Vision_sensor')
(code, res,
im) = vrep.simxGetVisionSensorImage(self.ID, self.cam_handle, 0,
const_v.simx_opmode_streaming)
self.render_handle = self.get_handle('render')
(code, res,
im) = vrep.simxGetVisionSensorImage(self.ID, self.render_handle, 0,
const_v.simx_opmode_streaming)
# joints
self.joint_handles = []
for i in range(self.DoF):
tmp = self.get_handle("joint%d" % (i))
self.joint_handles.append(tmp)
code, angle = vrep.simxGetJointPosition(
self.ID, tmp, const_v.simx_opmode_streaming)
# gripper tip
self.tip_handle = self.get_handle("Tip")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.tip_handle, -1,
const_v.simx_opmode_streaming)
self.or_handle = self.get_handle("RG2_baseVisible")
(code,
pose) = vrep.simxGetObjectOrientation(self.ID, self.or_handle, -1,
const_v.simx_opmode_streaming)
# cube
self.cube_handle = self.get_handle("Cube")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.cube_handle, -1,
const_v.simx_opmode_streaming)
(code,
pose) = vrep.simxGetObjectOrientation(self.ID, self.cube_handle, -1,
const_v.simx_opmode_streaming)
# get the goal handle
self.goal_handle = self.get_handle("Goal")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.goal_handle, -1,
const_v.simx_opmode_streaming)
# angles' array
self.angles = self.get_angles()
# gripper handles (used in closing and opening gripper)
self.gripper_motor = self.get_handle('RG2_openCloseJoint')
# task part
self.task_part = 0
self.init_angles = self.get_angles()
self.init_orientation = self.get_orientation(self.or_handle)
self.init_pose_cube = self.get_position(self.cube_handle)
# print(self.init_pose_cube)
self.init_goal_pose = self.get_position(self.goal_handle)
# print(self.init_goal_pose)
self.open_gripper()
self.reset()
self.tip_position = self.get_position(self.tip_handle)
self.goal_l = (80, 0, 0)
self.goal_u = (120, 255, 255)
self.cube_l = (55, 50, 50)
self.cube_u = (80, 255, 255)
self.er_kernel = np.ones((2, 2), np.uint8)
self.di_kernel = np.ones((2, 22), np.uint8)
self.task_part = 0
self.part_1_center = np.array([300.0, 335.0])
self.part_2_center = np.array([320.0, 290.0])
self.part_1_area = 0.25
self.part_2_area = 0.75
if (collisionState):
print('\n*************************************')
print('COLLISION ON {}'.format(co))
print('*************************************\n')
err = True
break
if (not err):
joint_angles = [0.0] * 6
rcpos, pos = vrep.simxGetObjectPosition(clientID, dummy, -1,
vrep.simx_opmode_blocking)
rcori, ori = vrep.simxGetObjectOrientation(clientID, dummy, -1,
vrep.simx_opmode_blocking)
#vrep.simxSetObjectPosition(clientID, target, -1, pos, vrep.simx_opmode_oneshot)
#vrep.simxSetObjectOrientation(clientID, target, -1, ori, vrep.simx_opmode_oneshot)
for i in range(6):
re, angle = vrep.simxGetJointPosition(clientID, joint_handles[i],
vrep.simx_opmode_blocking)
joint_angles[i] = angle
print("Size = {}".format(len(position_data)))
print("Time = {}".format(
time.strftime("%H:%M:%S", time.gmtime(time.time() - t))))
print("Pos =")
print(pos)
print("Ori =")
print(ori)
print("JAs =")
print(joint_angles)
print("#===================================================\n")
pos.extend(ori)
if (pos not in position_data):
position_data.append(pos)
joint_data.append(joint_angles)
def get_position(self):
code, position = v.simxGetJointPosition(self._id, self._handle,
self._def_op_mode)
return position
def __init__(self):
self.DoF = 6
self.action_bound = [-5, 5]
self.action_dim = self.DoF
# os.chdir("/Vrep_server")
# os.system("git pull")
# os.chdir("/")
#
# self.vrep_root = "/V-REP_PRO_EDU_V3_5_0_Linux/"
# self.scene_file = "/Vrep_server/env/rozum_model.ttt"
#
# os.chdir(self.vrep_root)
# os.system("xvfb-run --auto-servernum --server-num=1 -s \"-screen 0 640x480x24\" ./vrep.sh -h -s " + self.scene_file + " &")
vrep.simxFinish(-1)
time.sleep(1)
# get the ID of the running simulation
self.ID = vrep.simxStart('127.0.0.1', 19999, True, False, 5000, 5)
# check the connection
if self.ID != -1:
print("Connected")
else:
sys.exit("Error")
# get handles
# for camera
self.cam_handle = self.get_handle('Vision_sensor')
(code, res,
im) = vrep.simxGetVisionSensorImage(self.ID, self.cam_handle, 0,
const_v.simx_opmode_streaming)
# joints
self.joint_handles = []
for i in range(self.DoF):
tmp = self.get_handle("joint%d" % (i))
self.joint_handles.append(tmp)
code, angle = vrep.simxGetJointPosition(
self.ID, tmp, const_v.simx_opmode_streaming)
# gripper tip
self.tip_handle = self.get_handle("Tip")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.tip_handle, -1,
const_v.simx_opmode_streaming)
# cube
self.cube_handle = self.get_handle("Cube")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.cube_handle, -1,
const_v.simx_opmode_streaming)
# get the goal handle
self.goal_handle = self.get_handle("Goal")
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.goal_handle, -1,
const_v.simx_opmode_streaming)
# angles' array
self.angles = self.get_angles()
# gripper handles (used in closing and opening gripper)
self.gripper_motor = self.get_handle('RG2_openCloseJoint')
# task part
self.task_part = 0
self.init_angles = self.get_angles()
self.init_pose_cube = self.get_position(self.cube_handle)
# print(self.init_pose_cube)
self.init_goal_pose = self.get_position(self.goal_handle)
# print(self.init_goal_pose)
self.open_gripper()
self.reset()
self.tip_position = self.get_position(self.tip_handle)
self.goal_l = (80, 0, 0)
self.goal_u = (120, 255, 255)
self.cube_l = (55, 50, 50)
self.cube_u = (80, 255, 255)
self.er_kernel = np.ones((2, 2), np.uint8)
self.di_kernel = np.ones((2, 22), np.uint8)
self.task_part = 0
self.part_1_center = np.array([300.0, 335.0])
self.part_2_center = np.array([320.0, 290.0])
self.part_1_area = 0.25
self.part_2_area = 0.75
