def moveTo(self, dx, dy):
self.position = [dx, dy]
vrep.simxSetObjectPosition(
self.clientID,
self.handle,
-1, #world coordinates
[dx, dy, self.size[2] / 2],
vrep.simx_opmode_oneshot)
def reset(self):
self.task_part = 0
self.angles = self.init_angles
for i in range(self.DoF):
self.move_joint(i, self.angles[i])
vrep.simxSetObjectPosition(self.ID, self.cube_handle, -1,
self.init_pose_cube,
const_v.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(self.ID, self.goal_handle, -1,
self.init_goal_pose,
const_v.simx_opmode_oneshot_wait)
img = self.get_image(self.cam_handle)
return img
def reset(self):
self.task_part = 0
self.angles = self.init_angles.copy()
for i in range(self.DoF):
self.move_joint(i, self.angles[i])
self.open_gripper()
vrep.simxSetObjectPosition(self.ID, self.cube_handle, -1, self.init_pose_cube, const_v.simx_opmode_oneshot_wait)
vrep.simxSetObjectPosition(self.ID, self.goal_handle, -1, self.init_goal_pose, const_v.simx_opmode_oneshot_wait)
time.sleep(2)
angles = self.get_angles()
pose = self.get_position(self.tip_handle)
sin_cos = []
for a in angles:
sin_cos.append(np.sin(a))
sin_cos.append(np.cos(a))
s = np.concatenate([pose, sin_cos], axis=0)
return s
def set_position(self, handle, pos, relative=False):
relative_mode = vrep.sim_handle_parent if relative else -1
status = vrep.simxSetObjectPosition(self.id, handle, relative_mode,
pos, vrep.simx_opmode_oneshot)
self.check_return_code(status, tolerance=vrep.simx_return_novalue_flag)
def set_pose(self, position, yaw):
vrep.simxSetObjectPosition(self._clientID, self._obj, -1, position,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self._clientID, self._obj, -1, (0, 0, yaw),
vrep.simx_opmode_oneshot_wait)
def set_pose(self, position, yaw):
vrep.simxSetObjectPosition(self._clientID, self._obj, -1, position,
vrep.simx_opmode_oneshot_wait)
vrep.simxSetObjectOrientation(self._clientID, self._obj, -1, (0, 0, yaw),
vrep.simx_opmode_oneshot_wait)