def _get_obs(self):
# positions
grip_pos = self.sim.data.get_site_xpos('dobot:grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('dobot:grip') * dt
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
if self.has_object:
object_pos = self.sim.data.get_site_xpos('site:object0')
# rotations
object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('site:object0'))
# velocities
object_velp = self.sim.data.get_site_xvelp('site:object0') * dt
object_velr = self.sim.data.get_site_xvelr('site:object0') * dt
# gripper state
object_rel_pos = object_pos - grip_pos
object_velp -= grip_velp
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = np.squeeze(object_pos.copy())
obs = np.concatenate([
grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),
object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
# positions
grip_pos = self.sim.data.get_site_xpos('dobot:grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('dobot:grip') * dt
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
objPosList = []
if self.has_object:
for i in range(self.clutter_num + 1):
if i == 0:
object_pos = self.sim.data.get_site_xpos('site:object' + str(i))
objPosList.append(object_pos)
# rotations
object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('site:object' + str(i)))
# velocities
object_velp = self.sim.data.get_site_xvelp('site:object' + str(i)) * dt
object_velr = self.sim.data.get_site_xvelr('site:object' + str(i)) * dt
# gripper state
object_rel_pos = object_pos - grip_pos
object_velp -= grip_velp
else:
object_pos = self.sim.data.get_site_xpos('site:object' + str(i))
objPosList.append(object_pos)
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0)
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = np.squeeze((objPosList[0]).copy())
obs = np.concatenate([
grip_pos, objPosList[0].ravel(), object_rel_pos.ravel(), gripper_state,
])
num = 6
for i in range(1,self.clutter_num+1):
if np.linalg.norm(objPosList[0][:2]-objPosList[i][:2]) < 0.050 and num:
obs = np.concatenate([obs,objPosList[i]])
num -= 1
for i in range(num):
obs = np.concatenate([obs,np.zeros(3)])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}
def _get_obs(self):
grip_pos = self.sim.data.get_site_xpos('dobot:grip')
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
grip_velp = self.sim.data.get_site_xvelp('dobot:grip') * dt
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
if self.has_object:
object_pos = self.sim.data.get_site_xpos('site:object0')
# rotations
object_rot = rotations.mat2euler(
self.sim.data.get_site_xmat('site:object0'))
# velocities
object_velp = self.sim.data.get_site_xvelp('site:object0') * dt
object_velr = self.sim.data.get_site_xvelr('site:object0') * dt
# gripper state
object_rel_pos = object_pos - grip_pos
object_velp -= grip_velp
else:
object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(
0)
gripper_state = robot_qpos[-2:]
gripper_vel = robot_qvel[
-2:] * dt # change to a scalar if the gripper is made symmetric
if not self.has_object:
achieved_goal = grip_pos.copy()
else:
achieved_goal = np.squeeze(object_pos.copy())
# self.render()
import cv2
blackBox = grip_pos
redCircle = self.goal
blueBox = object_pos
height = 273
width = 467
img = np.zeros((height, width, 3), np.uint8)
img[:, :] = (16, 163, 127)
# Updated
# x_range = np.random.uniform(0.6, 1.)
# y_range = np.random.uniform(0.57, 0.8)
half_block_len = int(38 / 2)
gripper_len = 12
sphere_rad = 18
# Mark the block position
mapBlue = self.map_cor(blueBox)
xx = int(mapBlue[0])
yy = int(mapBlue[1])
img[xx - half_block_len:xx + half_block_len,
yy - half_block_len:yy + half_block_len] = (84, 54, 218)
# Mark the sphere position
mapRed = self.map_cor(redCircle)
xx = int(mapRed[0])
yy = int(mapRed[1])
cv2.circle(img, (yy, xx), 16, (255, 0, 0), -1)
# Mark the gripper position
mapBlack = self.map_cor(blackBox)
xx = int(mapBlack[0])
yy = int(mapBlack[1])
img[xx - gripper_len:xx + gripper_len,
yy - gripper_len:yy + gripper_len] = (0, 0, 0)
image = cv2.resize(img, (50, 50))
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)[:, :]
# cv2.imwrite('./images/test'+str(self.count)+'.png',cv2.cvtColor(img, cv2.COLOR_RGB2BGR)[:,:])
# image = self.capture()
# obs = np.concatenate([
# grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(),
# object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel,
# ])
obs = np.concatenate([
grip_pos,
object_pos.ravel(),
object_rel_pos.ravel(),
image.ravel(), #object_rot.ravel(),
# object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel,
])
return {
'observation': obs.copy(),
'achieved_goal': achieved_goal.copy(),
'desired_goal': self.goal.copy(),
}