def get_orientation(self, handle, first_call=False, relative=False):
opmode = vrep.simx_opmode_streaming if first_call else vrep.simx_opmode_buffer
relative_mode = vrep.sim_handle_parent if relative else -1
status, pos = vrep.simxGetObjectOrientation(self.id, handle,
relative_mode, opmode)
self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag)
return pos
def get_orientation_vrep_buffering(self):
ret, ori = vrep.simxGetObjectOrientation(self.vrep_connection.client_id, self.object_handle,
self.frame_reference, vrep.simx_opmode_buffer)
if ret == vrep.simx_return_ok:
return ori
else:
raise Exception("Failed to get object orientation. Object name: " + self.object_name)
def get_orientation(self) -> EulerAngles:
"""
Retrieves the linear and angular velocity.
@rtype EulerAngles
"""
code, orient = v.simxGetObjectOrientation(self._id, self._handle, -1,
self._def_op_mode)
return EulerAngles(orient[0], orient[1], orient[2])
def activate_vrep_orientation_streaming(self):
ret, ori = vrep.simxGetObjectOrientation(self.vrep_connection.client_id, self.object_handle,
self.frame_reference, vrep.simx_opmode_streaming)
if ret == vrep.simx_return_ok:
print('Started streaming orientation')
return ori
elif ret == vrep.simx_return_remote_error_flag or ret == vrep.simx_return_local_error_flag:
raise Exception("Failed to stream object orientation. Object name: " + self.object_name)
else:
print('Possible started streaming orientation')
return [0, 0, 0]
def getOrentation(self):
_, angles = vrep.simxGetObjectOrientation(
self.clientID,
self.handle,
-1, # retrieve absolute, not relative, position
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception("VrepRobot could not get orientation")
return angles
def get_orientation(self, handle):
(code,
ornt) = vrep.simxGetObjectOrientation(self.ID, handle, -1,
const_v.simx_opmode_buffer)
# print(code)
return np.array([
np.sin(ornt[0]),
np.cos(ornt[0]),
np.sin(ornt[1]),
np.cos(ornt[1]),
np.sin(ornt[2]),
np.cos(ornt[2])
])
def __init__(self, ID, num):
self.ID = ID
self.num = num
self.angles = [np.pi, np.pi / 2.0, 0.0, 3 * np.pi / 2.0]
self.dir = 1
self.r = 0.0975
self.l = 0.55
# motors
self.left_handle = self.get_handle("motor%d_left" % (self.num))
self.right_handle = self.get_handle("motor%d_right" % (self.num))
self.ground = self.get_handle("Ground")
self.robot_handle = self.get_handle("R%d" % (self.num))
(code,
pose) = vrep.simxGetObjectPosition(self.ID, self.robot_handle, -1,
const_v.simx_opmode_streaming)
(code,
rot) = vrep.simxGetObjectOrientation(self.ID, self.robot_handle, -1,
const_v.simx_opmode_streaming)
self.points = []
time.sleep(0.1)
def get_phi(self):
(code, rot) = vrep.simxGetObjectOrientation(self.ID, self.robot_handle,
-1,
const_v.simx_opmode_buffer)
phi = rot[2]
return phi
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
print('Return Code = {}'.format(rc))
err = True
for co in collision_objects:
returnCode, collisionState = vrep.simxReadCollision(
clientID, co, vrep.simx_opmode_blocking)
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)
