def test(args):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
fileName = os.path.join("mjcf", args.mjcf)
print("fileName")
print(fileName)
p.loadMJCF(fileName)
while (1):
p.stepSimulation()
p.getCameraImage(320,240)
time.sleep(0.01)
def _render(self, mode, close): if (mode=="human"): self.isRender = True if mode != "rgb_array": return np.array([]) base_pos=[0,0,0] if (hasattr(self,'robot')): if (hasattr(self.robot,'body_xyz')): base_pos = self.robot.body_xyz view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._cam_dist, yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width)/self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage( width=self._render_width, height=self._render_height, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL ) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array
def getExtendedObservation(self):
#camEyePos = [0.03,0.236,0.54]
#distance = 1.06
#pitch=-56
#yaw = 258
#roll=0
#upAxisIndex = 2
#camInfo = p.getDebugVisualizerCamera()
#print("width,height")
#print(camInfo[0])
#print(camInfo[1])
#print("viewMatrix")
#print(camInfo[2])
#print("projectionMatrix")
#print(camInfo[3])
#viewMat = camInfo[2]
#viewMat = p.computeViewMatrixFromYawPitchRoll(camEyePos,distance,yaw, pitch,roll,upAxisIndex)
viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0]
#projMatrix = camInfo[3]#[0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
img_arr = p.getCameraImage(width=self._width,height=self._height,viewMatrix=viewMat,projectionMatrix=projMatrix)
rgb=img_arr[2]
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
self._observation = np_img_arr
return self._observation
def _get_observation(self):
"""Return the observation as an image.
"""
img_arr = p.getCameraImage(width=self._width,
height=self._height,
viewMatrix=self._view_matrix,
projectionMatrix=self._proj_matrix)
rgb = img_arr[2]
np_img_arr = np.reshape(rgb, (self._height, self._width, 4))
return np_img_arr[:, :, :3]
def step(self,action):
p.stepSimulation()
start = time.time()
yaw = next(self.iter)
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=1,lightDirection=[1,1,1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2]
return np.array(self._observation), 0, 0, {}
def test(num_runs=300, shadow=1, log=True, plot=False):
if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
if plot:
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca()
times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0,360,10))
for i, yaw in zip(range(num_runs),yaw_gen):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=shadow,lightDirection=[1,1,1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1./duration
#print("fps=",fps)
else:
fps=0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot:
rgb = img_arr[2]
image.set_data(rgb)#np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
def generate_random_object(view_matrix, projection_matrix):
p.resetSimulation()
hw = 128
# Load object
p.loadURDF("plane.urdf")
# x_bound: [0.5, 0.65]
# y_bound: [-0.17, 0.21]
obj_x = random.uniform(0.55, 0.6)
obj_y = random.uniform(-0.1, 0.15)
obj_pos = [obj_x, obj_y, 0.02]
obj_rot = random.uniform(-math.pi / 3, math.pi / 3)
obj_euler_orient = [math.pi / 2, 0, obj_rot]
obj_quater_orient = p.getQuaternionFromEuler(obj_euler_orient)
obj = p.loadURDF("./0002/0002.urdf", obj_pos, obj_quater_orient)
# Get image of the objects without loading the robot
_, _, _, _, seg_img = p.getCameraImage(width=hw,
height=hw,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix)
seg_img = np.reshape(seg_img, (hw, hw, 1))
return seg_img, obj, obj_pos, obj_rot
def render(self, mode='human'):
view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=[0, 0, 0],
distance=5,
yaw=90,
pitch=-20,
roll=0,
upAxisIndex=2)
proj_matrix = p.computeProjectionMatrixFOV(fov=60,
aspect=float(960) / 720,
nearVal=0.1,
farVal=100.0)
(_, _, px, _,
_) = p.getCameraImage(width=960,
height=720,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_array = np.array(px, dtype=np.uint8)
rgb_array = np.reshape(rgb_array, (720, 960, 4))
rgb_array = rgb_array[:, :, :3]
return rgb_array
def getCameraImage(self):
"""Return camera image from CAMERA_JOINT_INDEX.
"""
# compute eye and target position for viewMatrix
pos, orn, _, _, _, _ = p.getLinkState(self.robotId,
self.CAMERA_JOINT_INDEX)
cameraPos = np.array(pos)
cameraMat = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3).T
eyePos = cameraPos + self.CAMERA_EYE_SCALE * cameraMat[
self.CAMERA_EYE_INDEX]
targetPos = cameraPos + self.CAMERA_TARGET_SCALE * cameraMat[
self.CAMERA_EYE_INDEX]
up = self.CAMERA_UP_SCALE * cameraMat[self.CAMERA_UP_INDEX]
# p.addUserDebugLine(eyePos, targetPos, lineColorRGB=[1, 0, 0], lifeTime=0.1) # red line for camera vector
# p.addUserDebugLine(eyePos, eyePos + up * 0.5, lineColorRGB=[0, 0, 1], lifeTime=0.1) # blue line for up vector
viewMatrix = p.computeViewMatrix(eyePos, targetPos, up)
image = p.getCameraImage(self.CAMERA_PIXEL_WIDTH,
self.CAMERA_PIXEL_HEIGHT,
viewMatrix,
self.projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
return image
def render(self, mode="human"):
view_mat = p.computeViewMatrix(
cameraTargetPosition=[0, 0, 0.2],
cameraEyePosition=[2, 0, 0.2],
cameraUpVector=[0, 0, 1],
)
nearVal = 0.05
farVal = 100
proj_mat = p.computeProjectionMatrixFOV(fov=45,
aspect=1,
nearVal=nearVal,
farVal=farVal)
(_, _, rgb_px, _, _) = p.getCameraImage(
width=512,
height=512,
viewMatrix=view_mat,
projectionMatrix=proj_mat,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
)
rgb_array = np.array(rgb_px, "uint8")
rgb_array = rgb_array.reshape(512, 512, 4)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _get_seg_mask_for_target(target_position, cam_position, world=None):
"""
Calculates the view and projection Matrix and returns the Segmentation mask
The segmentation mask indicates for every pixel the visible Object.
:param cam_position: The position of the Camera as a list of x,y,z and orientation as quaternion
:param target_position: The position to which the camera should point as a list of x,y,z and orientation as quaternion
:return: The Segmentation mask from the camera position
"""
world, world_id = _world_and_id(world)
# TODO: Might depend on robot cameras, if so please add these camera parameters to RobotDescription object
# TODO: of your robot with a CameraDescription object.
fov = 300
aspect = 256 / 256
near = 0.2
far = 10
view_matrix = p.computeViewMatrix(cam_position[0], target_position[0],
[0, 0, -1])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
return p.getCameraImage(256,
256,
view_matrix,
projection_matrix,
physicsClientId=world_id)[4]
def render_physics(self):
robot_pos, _ = p.getBasePositionAndOrientation(self.robot_tracking_id)
view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=robot_pos,
distance=self.tracking_camera["distance"],
yaw=self.tracking_camera["yaw"],
pitch=self.tracking_camera["pitch"],
roll=0,
upAxisIndex=2)
proj_matrix = p.computeProjectionMatrixFOV(
fov=60,
aspect=float(self._render_width) / self._render_height,
nearVal=0.1,
farVal=100.0)
with Profiler("render physics: Get camera image"):
(_, _, px, _, _) = p.getCameraImage(width=self._render_width,
height=self._render_height,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
renderer=p.ER_TINY_RENDERER)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def get_camera_image(cam_target_pos, cam_dist, cam_yaw, cam_pitch, cam_roll,
fov, render_width, render_height, nearval, farval):
view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=cam_target_pos,
distance=cam_dist,
yaw=cam_yaw,
pitch=cam_pitch,
roll=cam_roll,
upAxisIndex=2)
proj_matrix = p.computeProjectionMatrixFOV(fov=fov,
aspect=float(render_width) /
float(render_height),
nearVal=nearval,
farVal=farval)
(_, _, px, _, _) = p.getCameraImage(width=render_width,
height=render_height,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix)
rgb_array = np.array(px, dtype=np.uint8)
rgb_array = np.reshape(np.array(px), (render_height, render_width, -1))
rgb_array = rgb_array[:, :, :3]
return rgb_array
def plot_humanoid():
camTargetPos = [0, 0, 0]
cameraUp = [0, 0, 1]
cameraPos = [1, 1, 1]
p.setGravity(0, 0, -10)
pitch = -10.0
roll = 0
upAxisIndex = 2
camDistance = 3
pixelWidth = 1080
pixelHeight = 790
nearPlane = 0.01
farPlane = 100
fov = 90
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
yaw = 0
for r in range(2):
for c in range(2):
yaw += 60
pylab.figure(figsize=(10, 5))
viewMatrix = p.computeViewMatrixFromYawPitchRoll(
camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = p.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix)
w = img_arr[0]
h = img_arr[1]
rgb = img_arr[2]
dep = img_arr[3]
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr * (1. / 255.)
pylab.imshow(np_img_arr, interpolation='none', animated=True)
def get_camera_image(self):
image = np.zeros((100, 100, 4))
proj_matrix = p.computeProjectionMatrixFOV(fov=80,
aspect=1,
nearVal=0.01,
farVal=100)
pos, ori = [
list(l)
for l in p.getBasePositionAndOrientation(self.id, self.client)
]
pos[2] = 0.2
# Rotate camera direction
rot_mat = np.array(p.getMatrixFromQuaternion(ori)).reshape(3, 3)
camera_vec = np.matmul(rot_mat, [1, 0, 0])
up_vec = np.matmul(rot_mat, np.array([0, 0, 1]))
view_matrix = p.computeViewMatrix(pos, pos + camera_vec, up_vec)
w, h, rgb, depth, self.segmask = p.getCameraImage(
100, 100, view_matrix, proj_matrix)
rgb = rgb[:, :, :-1] / 255.
rgbd = np.concatenate([rgb, np.expand_dims(depth, 2)], axis=2)
return rgbd
pixelWidth = 320
pixelHeight = 200
nearPlane = 0.01
farPlane = 100
fov = 60
main_start = time.time()
while(1):
for yaw in range (0,360,10) :
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time()
print ("renderImage %f" % (stop - start))
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
#print(rgb)
print ('width = %d height = %d' % (w,h))
#note that sending the data using imshow to matplotlib is really slow, so we use set_data
#plt.imshow(rgb,interpolation='none')
def __init__(self,
display=False,
steps_to_roll=10,
episode_len=600,
pause_frac=0.66,
n_cells=1000):
self.display = display
if self.display:
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
# set up the world, endEffector is the tip of the left finger
self.baxterId, self.endEffectorId, self.objectId = setUpWorld()
# change friction of object
p.changeDynamics(self.objectId, -1, lateralFriction=1)
# save the state of the environment for future reset (save time on URDF loading)
self.savestate = p.saveState()
# get joint ranges
self.lowerLimits, self.upperLimits, self.jointRanges, self.restPoses = getJointRanges(
self.baxterId, includeFixed=False)
# deal with display
if self.display:
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetDebugVisualizerCamera(2., 180, 0., [0.52, 0.2, np.pi / 4.])
p.getCameraImage(320, 200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
sleep(1.)
# number of pybullet steps to roll at each gym step
self.steps_to_roll = steps_to_roll
# number of gym steps to roll before the end of the movement
self.move_len = int(episode_len * pause_frac)
# number of pybullet steps to roll after the movement keeping the same action
self.pause_steps = (episode_len - self.episode_len) * steps_to_roll
# counts the number of gym steps
self.step_counter = 0
# variable to store grasping orientation
self.diff_or_at_grab = None
# create the discretisation for the goal spaces (quaternion spaces)
bounds = [[-1, 1], [-1, 1], [-1, 1], [-1, 1]]
self.cvt = utils.CVT(n_cells, bounds)
self.grasped = False
# observation and action space
self.observation_space = spaces.Dict({
'observation':
spaces.Box(-1, 1, shape=(34, )),
'desired_goal':
spaces.Discrete(n_cells),
'achieved_goal':
spaces.Discrete(n_cells)
})
self.action_space = spaces.Box(-1, 1, shape=(8, ))
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
images_depth5 = []
images_rgb6 = []
images_depth6 = []
images_rgb7 = []
images_depth7 = []
images_rgb8 = []
images_depth8 = []
for i in range(0, 200):
print(i)
# p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
if i % 4 == 0: # 30 frames a second:
# camera 1
images = p.getCameraImage(width,
height,
view_matrix_camera_1,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2],
(height, width, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (
far - (far - near) * depth_buffer_opengl)
images_rgb1.append(rgb_opengl)
images_depth1.append(depth_opengl)
# camera 2
images = p.getCameraImage(width,
height,
view_matrix_camera_2,
projection_matrix,
positionGain=0.03,
velocityGain=1)
if __name__ == "__main__":
eef_pos, eef_orn = p.getLinkState(kukaId, kukaEndEffectorIndex)[4:6]
eef_orn = p.getEulerFromQuaternion(eef_orn)
eef_pos = np.array(eef_pos)
eef_orn = np.array(eef_orn)
while True:
images = p.getCameraImage(
width,
height,
view_matrix,
projection_matrix,
# shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
# クリックされたobject_uidを取得
object_uid = -1
mouse_events = p.getMouseEvents()
for e in mouse_events:
if ((e[0] == 2) and (e[3] == 0)
and (e[4] & p.KEY_WAS_TRIGGERED)): # 左クリック判定
mouse_x = e[1]
mouse_y = e[2]
object_uid = get_object_uid(
mouse_x, mouse_y) # -1 for no object was clicked
def _step(self,action):
done = False
#reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward.
#done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.)
#observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game.
#info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning.
def convertSensor(finger_index):
if finger_index == self.indexId:
return random.uniform(-1,1)
#return 0
else:
#return random.uniform(-1,1)
return 0
def convertAction(action):
#converted = (action-30)/10
#converted = (action-16)/10
if action == 6:
converted = -1
elif action == 25:
converted = 1
#print("action ",action)
#print("converted ",converted)
return converted
aspect = 1
camTargetPos = [0,0,0]
yaw = 40
pitch = 10.0
roll=0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional
fov = 50 #10 or 50
hand_po = pb.getBasePositionAndOrientation(self.agent)
ho = pb.getQuaternionFromEuler([0.0, 0.0, 0.0])
# So when trying to modify the physics of the engine, we run into some problems. If we leave
# angular damping at default (0.04) then the hand rotates when moving up and dow, due to torque.
# If we set angularDamping to 100.0 then the hand will bounce off into the background due to
# all the stored energy, when it makes contact with the object. The below set of parameters seem
# to have a reasonably consistent performance in keeping the hand level and not inducing unwanted
# behavior during contact.
pb.changeDynamics(self.agent, linkIndex=-1, angularDamping=0.9999)
if action == 65298 or action == 0: #down
pb.changeConstraint(self.hand_cid,(hand_po[0][0]+self.move,hand_po[0][1],hand_po[0][2]),ho, maxForce=self.tinyForce)
elif action == 65297 or action == 1: #up
pb.changeConstraint(self.hand_cid,(hand_po[0][0]-self.move,hand_po[0][1],hand_po[0][2]),ho, maxForce=self.tinyForce)
elif action == 65295 or action == 2: #left
pb.changeConstraint(self.hand_cid,(hand_po[0][0],hand_po[0][1]+self.move,hand_po[0][2]),ho, maxForce=self.tinyForce)
elif action== 65296 or action == 3: #right
pb.changeConstraint(self.hand_cid,(hand_po[0][0],hand_po[0][1]-self.move,hand_po[0][2]),ho, maxForce=self.tinyForce)
elif action == 44 or action == 4: #<
pb.changeConstraint(self.hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]+self.move),ho, maxForce=self.tinyForce)
elif action == 46 or action == 5: #>
pb.changeConstraint(self.hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]-self.move),ho, maxForce=self.tinyForce)
elif action >= 6 and action <= 7:
# keeps the hand from moving towards origin
pb.changeConstraint(self.hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]),ho, maxForce=200)
if action == 7:
action = 25 #bad kludge redo all this code
index = convertAction(action) # action = 6 or 25 due to kludge -> return -1 or 1
# Getting positions of the index joints to use for moving to a relative position
joint17Pos = pb.getJointState(self.agent, 17)[0]
joint19Pos = pb.getJointState(self.agent, 19)[0]
joint21Pos = pb.getJointState(self.agent, 21)[0]
# need to keep the multiplier relatively small otherwise the joint will continue to move
# when you take other actions
finger_jump = 0.1
newJoint17Pos = joint17Pos + index*finger_jump
newJoint19Pos = joint19Pos + index*finger_jump
newJoint21Pos = joint21Pos + index*finger_jump
# following values found by experimentation
if newJoint17Pos <= -0.7:
newJoint17Pos = -0.7
elif newJoint17Pos >= 0.57:
newJoint17Pos = 0.57
if newJoint19Pos <= 0.13:
newJoint19Pos = 0.13
elif newJoint19Pos >= 0.42:
newJoint19Pos = 0.42
if newJoint21Pos <= -0.8:
newJoint21Pos = -0.8
elif newJoint21Pos >= 0.58:
newJoint21Pos = 0.58
pb.setJointMotorControl2(self.agent,17,pb.POSITION_CONTROL,newJoint17Pos)
pb.setJointMotorControl2(self.agent,19,pb.POSITION_CONTROL,newJoint19Pos)
pb.setJointMotorControl2(self.agent,21,pb.POSITION_CONTROL,newJoint21Pos)
if self.downCameraOn: viewMatrix = down_view()
else: viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(fov,aspect,nearPlane,farPlane)
w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_TINY_RENDERER)
#w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL)
new_obs = np.absolute(depths-1.0)
new_obs[new_obs > 0] =1
self.current_observation = new_obs.flatten()
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps +=1
#reward if moving towards the object or touching the object
reward = 0
max_steps = 1000
if self.is_touching():
touch_reward = 10
if 'debug' in self.options and self.options['debug'] == True:
print("TOUCHING!!!!")
else:
touch_reward = 0
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(sum([(xi-yi)**2 for xi,yi in zip(obj_po[0],hand_po[0])])) #TODO faster euclidean
#distance = np.linalg.norm(obj_po[0],hand_po[0])
#print("distance:",distance)
if distance < self.prev_distance:
reward += 1 * (max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
if self.steps >= max_steps or self.is_touching():
done = True
return self.current_observation,reward,done,info
upAxisIndex = 2
camDistance = 4
pixelWidth = 1920
pixelHeight = 1080
nearPlane = 0.01
farPlane = 1000
fov = 60
main_start = time.time()
for pitch in range (0,360,10) :
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, [0,1,0])
stop = time.time()
print ("renderImage %f" % (stop - start))
w=img_arr[0] #width of the image, in pixels
h=img_arr[1] #height of the image, in pixels
rgb=img_arr[2] #color data RGB
dep=img_arr[3] #depth data
print 'width = %d height = %d' % (w,h)
#note that sending the data to matplotlib is really slow
#show
plt.imshow(rgb,interpolation='none')
#plt.show()
plt.pause(0.01)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True)
p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000])
while (1):
p.stepSimulation()
time.sleep(0.01)
#p.saveWorld("test.py")
viewMat = p.getDebugVisualizerCamera()[2]
#projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = p.getDebugVisualizerCamera()[3]
width=640
height=480
img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)
oneOverWidth = float(1)/float(width)
oneOverHeight = float(1)/float(height)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
lendHor = math.sqrt(dHor[0]*dHor[0]+dHor[1]*dHor[1]+dHor[2]*dHor[2])
lendVer = math.sqrt(dVer[0]*dVer[0]+dVer[1]*dVer[1]+dVer[2]*dVer[2])
cornersX = [0,width,width,0]
cornersY = [0,0,height,height]
corners3D=[]
imgW = int(width/10)
imgH = int(height/10)
img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer=img[2]
depthBuffer = img[3]
print("rgbBuffer.shape=",rgbBuffer.shape)
print("depthBuffer.shape=",depthBuffer.shape)
#disable rendering temporary makes adding objects faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1,1,1,1], radius=0.03 )
collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
for i in range (4):
w = cornersX[i]
h = cornersY[i]
import pybullet as p
useEGL = False
useEGLGUI = False
if useEGL:
if useEGLGUI:
p.connect(p.GUI, "window_backend=2")
else:
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK )
p.stepSimulation()
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i * 2) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 4, 1],
useMaximalCoordinates=False)
p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
p.getCameraImage(64, 64, renderer=p.ER_BULLET_HARDWARE_OPENGL)
while (1):
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid >= 0):
import pybullet as p
p.connect(p.GUI)
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
for l in range(p.getNumJoints(r2d2)):
print(p.getJointInfo(r2d2, l))
p.loadURDF("r2d2.urdf", [2, 0, 1])
p.loadURDF("r2d2.urdf", [4, 0, 1])
p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
segLinkIndex = 1
verbose = 0
while (1):
keys = p.getKeyboardEvents()
#for k in keys:
# print("key=",k,"state=", keys[k])
if ord('1') in keys:
state = keys[ord('1')]
if (state & p.KEY_WAS_RELEASED):
verbose = 1 - verbose
if ord('s') in keys:
state = keys[ord('s')]
if (state & p.KEY_WAS_RELEASED):
segLinkIndex = 1 - segLinkIndex
#print("segLinkIndex=",segLinkIndex)
flags = 0
if (segLinkIndex):
flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
img = p.getCameraImage(320, 200, flags=flags)
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1])
width = 128
height = 128
fov = 60
aspect = width / height
near = 0.02
far = 1
view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255.
depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height])*1./255.
time.sleep(1)
# Get depth values using Tiny renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny= np.reshape(images[2], (height, width, 4))*1./255.
seg_tiny = np.reshape(images[4],[width,height])*1./255.
def get_camera_view(self,
camera_pose,
camera,
target_position=None,
occlusion_threshold=0.01,
rendering_ratio=1.0):
visible_tracks = []
rot = quaternion_matrix(camera_pose.quaternion())
trans = translation_matrix(camera_pose.position().to_array().flatten())
if target_position is None:
target = translation_matrix([0.0, 0.0, 1000.0])
target = translation_from_matrix(np.dot(np.dot(trans, rot),
target))
else:
target = target_position.position().to_array()
view_matrix = p.computeViewMatrix(camera_pose.position().to_array(),
target, [0, 0, 1])
width = camera.width
height = camera.height
projection_matrix = p.computeProjectionMatrixFOV(
camera.fov,
float(width) / height, camera.clipnear, camera.clipfar)
rendered_width = int(width / rendering_ratio)
rendered_height = int(height / rendering_ratio)
width_ratio = width / rendered_width
height_ratio = height / rendered_height
if self.use_gui is True:
camera_image = p.getCameraImage(
rendered_width,
rendered_height,
viewMatrix=view_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
projectionMatrix=projection_matrix)
else:
camera_image = p.getCameraImage(rendered_width,
rendered_height,
viewMatrix=view_matrix,
renderer=p.ER_TINY_RENDERER,
projectionMatrix=projection_matrix)
rgb_image = cv2.resize(np.array(camera_image[2]),
(width, height))[:, :, :3]
depth_image = np.array(camera_image[3], np.float32).reshape(
(rendered_height, rendered_width))
far = camera.clipfar
near = camera.clipnear
real_depth_image = far * near / (far - (far - near) * depth_image)
mask_image = camera_image[4]
unique, counts = np.unique(np.array(mask_image).flatten(),
return_counts=True)
for sim_id, count in zip(unique, counts):
if sim_id > 0:
cv_mask = np.array(mask_image.copy())
cv_mask[cv_mask != sim_id] = 0
cv_mask[cv_mask == sim_id] = 255
xmin, ymin, w, h = cv2.boundingRect(cv_mask.astype(np.uint8))
mask = cv_mask[ymin:ymin + h, xmin:xmin + w]
visible_area = w * h + 1
screen_area = rendered_width * rendered_height + 1
if screen_area - visible_area == 0:
confidence = 1.0
else:
confidence = visible_area / float(screen_area -
visible_area)
#TODO compute occlusion score as a ratio between visible 2d bbox and projected 2d bbox areas
if confidence > occlusion_threshold:
depth = real_depth_image[int(ymin + h / 2.0)][int(xmin +
w / 2.0)]
xmin = int(xmin * width_ratio)
ymin = int(ymin * height_ratio)
w = int(w * width_ratio)
h = int(h * height_ratio)
id = self.reverse_entity_id_map[sim_id]
scene_node = self.get_entity(id)
det = Detection(int(xmin),
int(ymin),
int(xmin + w),
int(ymin + h),
id,
confidence,
depth=depth,
mask=mask)
track = SceneNode(detection=det)
track.static = scene_node.static
track.id = id
track.mask = det.mask
track.shapes = scene_node.shapes
track.pose = scene_node.pose
track.label = scene_node.label
track.description = scene_node.description
visible_tracks.append(track)
return rgb_image, real_depth_image, visible_tracks
table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase)
xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase)
jointIds = []
paramIds = []
for j in range(p.getNumJoints(xarm)):
p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(xarm, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
skip_cam_frames = 10
while (1):
p.stepSimulation()
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
skip_cam_frames -= 1
if (skip_cam_frames<0):
p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL )
skip_cam_frames = 10
time.sleep(1./240.)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])
shift=0
gui = 1
frame=0
states=[]
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
reverting = False
p.getCameraImage(320,200)#,renderer=p.ER_BULLET_HARDWARE_OPENGL )
while (1):
updateCam = 0
keys = p.getKeyboardEvents()
for k,v in keys.items():
if (reverting or (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED))):
reverting=True
stateIndex = len(states)-1
#print("prestateIndex=",stateIndex)
time.sleep(timestep)
updateCam=1
if (stateIndex>0):
stateIndex-=1
states=states[:stateIndex+1]
def run_simulation(dt,
t_stance,
duration,
f,
A_lat,
th0_lat,
th0_abd,
thf_abd,
z_rotation,
girdle_friction,
body_friction,
last_link_friction,
leg_friction,
Kp_lat,
Kp_r_abd,
Kp_l_abd,
Kp_flex,
Kv_lat,
Kv_r_abd,
Kv_l_abd,
Kv_flex,
keep_in_check=True,
graphic_mode=False,
plot_graph_joint=False,
plot_graph_COM=False,
video_logging=False,
video_name="./video/dump/dump"):
# dt, t_stance, duration = time parameter, dt MUST be the same used to create the model
# A,f1,n,t2_off,delta_off,bias = parameters for the generation of the torques
# theta_rg0, theta_lg0, A_lat_0, theta_lat_0= parameters for setting the starting position
# girdle_friction, body_friction, leg_friction = lateral friction values
##########################################
####### PYBULLET SET-UP ##################
if graphic_mode:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
p.setTimeStep(dt)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
### plane set-up
planeID = p.loadURDF("plane100.urdf", [0, 0, 0])
p.changeDynamics(
planeID,
linkIndex=-1,
lateralFriction=1,
spinningFriction=1,
rollingFriction=1,
#restitution = 0.5,
#contactStiffness = 0,
#contactDamping = 0
)
##########################################
####### MODEL SET-UP #####################
model = crw.Crawler(
urdf_path="/home/fra/Uni/Tesi/crawler",
dt_simulation=dt,
base_position=[0, 0, 0.005],
base_orientation=[0, 0, sin(z_rotation / 2),
cos(z_rotation / 2)],
mass_distribution=True)
# for i,elem in enumerate(model.links_state_array):
# print("Link %d "%i, elem["world_com_trn"])
# print(model.links_state_array)
# Set motion damping ("air friction")
for index in range(-1, model.num_joints):
p.changeDynamics(model.Id,
index,
linearDamping=0.001,
angularDamping=0.001)
# Girdle link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=-1,
lateralFriction=girdle_friction,
spinningFriction=girdle_friction / 10000,
rollingFriction=girdle_friction / 10000,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Body dynamic properties
for i in range(1, model.num_joints - 4, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=body_friction,
spinningFriction=body_friction / 50,
rollingFriction=body_friction / 100,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# # Last link dynamic properties
p.changeDynamics(
model.Id,
linkIndex=(model.control_indices[0][-1] + 1),
lateralFriction=last_link_friction,
spinningFriction=last_link_friction / 10000,
rollingFriction=last_link_friction / 10000,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Legs dynamic properties
for i in range(model.num_joints - 3, model.num_joints, 2):
p.changeDynamics(
model.Id,
linkIndex=i,
lateralFriction=leg_friction,
spinningFriction=leg_friction / 10000,
rollingFriction=leg_friction / 10000,
restitution=0.1,
#contactStiffness = 0,
#contactDamping = 0
)
# Dorsal joints "compliance"
model.turn_off_crawler()
for i in range(1, model.control_indices[0][-1] + 1, 2):
p.setJointMotorControl2(model.Id,
i,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=0.05)
# Starting position
model.set_bent_position(theta_rg=th0_abd,
theta_lg=-thf_abd,
A_lat=A_lat,
theta_lat_0=th0_lat)
##########################################
####### CONTROLLER PARAMETERS ############
Kp = model.generate_Kp(Kp_lat, Kp_r_abd, Kp_l_abd, Kp_flex)
Kv = model.generate_Kv(Kv_lat, Kv_r_abd, Kv_l_abd, Kv_flex)
model.set_low_pass_lateral_qa(fc=20)
model.set_low_pass_tau(fc=90)
##########################################
####### MISCELLANEOUS ####################
if graphic_mode:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(cameraDistance=0.11,
cameraYaw=50,
cameraPitch=-60,
cameraTargetPosition=[0, 0, 0])
if (video_logging and graphic_mode):
video_writer = imageio.get_writer(video_name, fps=int(1 / dt))
##########################################
####### PERFORMANCE EVALUATION ###########
loss = 0.0
max_loss = 10000000.0 # value to set when something goes wrong, if constraints are "hard"
max_COM_height = 4 * model.body_sphere_radius
max_COM_speed = 10 #TODO: check this value
n_mean = 6
##########################################
####### TIME SERIES CREATION #############
steps = int(duration / dt)
steps_stance = int(t_stance / dt)
num_lat = len(model.control_indices[0])
f_walk = 1 / (2 * t_stance)
# Torques evolution
q_des_time_array, qd_des_time_array, qdd_des_time_array = model.generate_trajectory_time_array(
duration=duration,
steps=steps,
f=f,
A_lat=A_lat,
th0_lat=th0_lat,
th0_abd=th0_abd,
thf_abd=thf_abd,
include_base=True)
print("QDD_DES SHAPE: ", qdd_des_time_array.shape)
print(qdd_des_time_array[model.mask_act + model.mask_both_legs])
# Data array initialization
p_COM_time_array = np.zeros((steps, 3))
v_COM_time_array = np.zeros((steps, 3))
tau_time_array = np.zeros((steps, model.state.nqd))
joint_power_time_array = np.zeros((steps, model.state.nqd))
q_time_array = np.zeros((steps, model.state.nq))
qd_time_array = np.zeros((steps, model.state.nqd))
qdd_time_array = np.zeros((steps, model.state.nqd))
eq_time_array = np.zeros((steps, model.state.nq))
# Integrator for the work done by each joint
joint_power_integrator = crw.Integrator_Forward_Euler(
dt, np.zeros(model.state.nqd))
joint_energy_time_array = np.zeros((steps, model.state.nqd))
##########################################
####### RUN SIMULATION ###################
t = 0.0
# let the legs fall and leave the time to check the starting position
for i in range(100):
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
# Initial Condition
model.state.update()
model.integrator_lateral_qa.reset(model.state.q[model.mask_act_shifted])
model.free_right_foot()
model.fix_left_foot()
model.fix_tail()
# walk and record data
for i in range(steps):
# UPDATE CONSTRAINTS
if not (i % steps_stance):
model.invert_feet()
print("step")
for tmp in range(10):
p.stepSimulation()
# UPDATE STATE
model.state.update()
p_COM_curr = np.array(model.COM_position_world())
v_COM_curr = np.array(model.COM_velocity_world())
# ADD FRAME TO VIDEO
if (graphic_mode and video_logging):
img = p.getCameraImage(800,
640,
renderer=p.ER_BULLET_HARDWARE_OPENGL)[2]
video_writer.append_data(img)
# ACTUATE MODEL AND STEP SIMULATION
tau_des, eq = model.solve_computed_torque_control(
q_des=q_des_time_array[i],
qd_des=qd_des_time_array[i],
qdd_des=qdd_des_time_array[i],
Kp=Kp,
Kv=Kv,
#rho=rho,
verbose=False)
tau_applied = model.apply_torques(tau_des=tau_des, filtered=True)
# UPDATE TIME-ARRAYS
if plot_graph_joint:
q_time_array[i] = model.state.q.copy()
qd_time_array[i] = model.state.qd.copy()
qdd_time_array[i] = model.state.qdd.copy()
p_COM_time_array[i] = p_COM_curr
v_COM_time_array[i] = v_COM_curr
eq_time_array[i] = q_des_time_array[i] - q_time_array[i]
tau_time_array[i] = tau_applied
for j, (v_j, tau_j) in enumerate(zip(model.state.qd, tau_applied)):
joint_power_time_array[i, j] = abs(v_j * tau_j)
joint_energy = joint_power_integrator.integrate(
joint_power_time_array[i])
joint_energy_time_array[i] = joint_energy
# print("eq: ", np.round(eq[model.mask_act],2), np.round(eq[model.mask_both_legs],2))
# print("tau: ", np.round(tau_applied[model.mask_act],2), np.round(tau_applied[model.mask_both_legs],2))
# print("qdd_des: ", np.round(qdd_des_time_array[i][model.mask_act],2), np.round(qdd_des_time_array[i][model.mask_both_legs],2))
#UPDATE LOSS - VARIATIONAL VERSION -d(x-displacement) + d(total energy expenditure)
# loss += (
# -100*(p_COM_time_array[i][0]-p_COM_time_array[i-1][0]) +
# 0.1*(joint_power_time_array[i]).dot(joint_power_time_array[i])
# )
# STEP SIMULATION
p.stepSimulation()
if graphic_mode:
time.sleep(dt)
t += dt
# CHECK SIMULATION to avoid that the model get schwifty
if keep_in_check:
# check COM height, model shouldn't fly or go through the ground
if (p_COM_curr[-1] > (max_COM_height)) or (p_COM_curr[-1] < 0.0):
print("\nLook at me I'm jumping\n")
loss += max_loss
break
if (np.linalg.norm(np.mean(v_COM_time_array[i - n_mean:i + 1], 0))
> max_COM_speed):
print("\nFaster than light. WOOOOOOOSH\n")
loss += max_loss
break
p.disconnect()
####### END SIMULATION ###################
if (video_logging and graphic_mode):
video_writer.close()
##########################################
####### PLOT DATA ########################
if plot_graph_joint:
fig_lat, axs_lat = plt.subplots(2, 3)
for i in range(0, num_lat):
#axs_lat[i//3,i%3].plot(qd_time_array[:,model.mask_act][:,i], color="xkcd:dark yellow", label="qd")
#axs_lat[i//3,i%3].plot(qdd_time_array[:,model.mask_act][:,i], color="xkcd:pale yellow", label="qdd")
#axs_lat[i//3,i%3].plot(joint_power_time_array[:,model.mask_act][:,i], color="xkcd:light salmon", label="Power")
#axs_lat[i//3,i%3].plot(joint_energy_time_array[:,model.mask_act][:,i], color="xkcd:dark salmon", label="Energy")
axs_lat[i // 3, i % 3].plot(tau_time_array[:, model.mask_act][:,
i],
color="xkcd:salmon",
label="Torque")
axs_lat[i // 3,
i % 3].plot(q_time_array[:, model.mask_act_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_lat[i // 3,
i % 3].plot(q_des_time_array[:, model.mask_act_shifted][:,
i],
color="xkcd:dark teal",
label="q_des")
#axs_lat[i//3,i%3].plot(eq_time_array[:,model.mask_act_shifted][:,i], color="xkcd:red", label="error")
#axs_lat[i//3,i%3].set_title("Lateral joint %d" %i)
handles_lat, labels_lat = axs_lat[0, 0].get_legend_handles_labels()
fig_lat.legend(handles_lat, labels_lat, loc='center right')
#
fig_girdle, axs_girdle = plt.subplots(2, 2)
for i in range(0, len(model.mask_both_legs)):
#axs_girdle[i//2,i%2].plot(qd_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark yellow", label="qd")
#axs_girdle[i//2,i%2].plot(qdd_time_array[:,model.mask_both_legs][:,i], color="xkcd:pale yellow", label="qdd")
#axs_girdle[i//2,i%2].plot(joint_power_time_array[:,model.mask_both_legs][:,i], color="xkcd:light salmon", label="Power")
#axs_girdle[i//2,i%2].plot(joint_energy_time_array[:,model.mask_both_legs][:,i], color="xkcd:dark salmon", label="Energy")
axs_girdle[i // 2,
i % 2].plot(tau_time_array[:, model.mask_both_legs][:,
i],
color="xkcd:salmon",
label="Torque")
axs_girdle[i // 2, i % 2].plot(
q_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:yellow",
label="q")
axs_girdle[i // 2, i % 2].plot(
q_des_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:dark teal",
label="q_des")
axs_girdle[i // 2, i % 2].plot(
eq_time_array[:, model.mask_both_legs_shifted][:, i],
color="xkcd:red",
label="error")
axs_girdle[0, 0].set_title("Right abduction")
axs_girdle[0, 1].set_title("Right flexion")
axs_girdle[1, 0].set_title("Left abduction")
axs_girdle[1, 1].set_title("Left flexion")
handles_girdle, labels_girdle = axs_girdle[
0, 0].get_legend_handles_labels()
fig_girdle.legend(handles_girdle, labels_girdle, loc='center right')
#
plt.show()
#
if plot_graph_COM:
fig_COM = plt.figure()
axs_COM = fig_COM.add_subplot(111)
axs_COM.plot(p_COM_time_array[:, 0],
p_COM_time_array[:, 1],
color="xkcd:teal")
axs_COM.set_title("COM x-y trajectory")
axs_COM.set_aspect('equal')
fig_COM_3D = plt.figure()
axs_COM_3D = fig_COM_3D.add_subplot(111, projection='3d')
axs_COM_3D.plot(p_COM_time_array[:, 0],
p_COM_time_array[:, 1],
p_COM_time_array[:, 2],
color="xkcd:teal")
axs_COM_3D.set_title("COM 3D trajectory")
axs_COM_3D.set_aspect('equal')
#
plt.show()
return loss
import pybullet as p
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
p.loadSDF("newsdf.sdf")
while (1):
p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation()
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
for j in range (p.getNumJoints(quadruped)):
p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480,320)
p.setRealTimeSimulation(0)
joints=[]
with open(pd.getDataPath()+"/laikago/data1.txt","r") as filestream:
for line in filestream:
print("line=",line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints=currentline[2:14]
import pybullet as p
p.connect(p.GUI)
plugin = p.loadPlugin(
"d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
"_testPlugin")
print("plugin=", plugin)
p.loadURDF("r2d2.urdf")
while (1):
p.getCameraImage(320, 200)
import pybullet as p
import time
import pybullet_data
from hamstir_gym.modder import Modder
from tqdm import tqdm
mod = Modder()
physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# planeId = p.loadURDF("plane.urdf")
room = p.loadURDF("./hamstir_gym/data/room12x12.urdf")
mod.load(room)
# mod.randomize()
#
pbar = tqdm()
frames = 0
try:
while True:
p.stepSimulation()
p.getCameraImage(128, 128, renderer=p.ER_BULLET_HARDWARE_OPENGL)
mod.randomize()
# time.sleep(1/20.)
frames += 1
pbar.update(frames)
except:
print('bye')
main_start = time.time()
while (1):
for yaw in range(0, 360, 10):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(
camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(
pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time()
print("renderImage %f" % (stop - start))
w = img_arr[0] #width of the image, in pixels
h = img_arr[1] #height of the image, in pixels
rgb = img_arr[2] #color data RGB
dep = img_arr[3] #depth data
print('width = %d height = %d' % (w, h))
#note that sending the data to matplotlib is really slow
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)
def get_observation(self):
# get observation
img_info = p.getCameraImage(
width=self.w,
height=self.h,
viewMatrix=self.view_matrix,
projectionMatrix=self.proj_matrix,
shadow=-1,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_TINY_RENDERER)
img = img_info[2][:, :, :3]
if self.opt.add_mask:
mask = (img_info[4] > 10000000)
mask_id_label = [
234881025, 301989889, 285212673, 268435457, 318767105,
335544321, 201326593, 218103809, 167772161
]
for item in mask_id_label:
mask = mask * (img_info[4] != item)
box_mask_label = [
117440516, 234881028, 184549380, 33554436, 167772164, 50331652,
134217732, 16777220, 67108868, 150994948, 251658244, 234881028,
100663300, 218103812, 83886084, 268435460
]
for item in box_mask_label:
mask = mask * (img_info[4] != item)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img[mask] = [127, 151, 182]
if self.opt.write_img:
cv2.imwrite(
os.path.join(self.log_path,
'{:06d}.jpg'.format(self.seq_num - 1)), img)
else:
self.img_buffer.append(img)
np.save(
os.path.join(self.log_root,
'state_{}.npy'.format(self.opt.load_embedding)),
img)
if self.epoch_num > self.opt.start_write:
cv2.imwrite(
os.path.join(self.log_path,
'{:06d}.jpg'.format(self.seq_num - 1)), img)
self.observation = img
if self.opt.observation == 'joint_pose':
self.observation = np.array([
p.getJointState(self.robotId, i)[0]
for i in range(self.robotEndEffectorIndex)
])
elif self.opt.observation == 'end_pos':
self.observation = np.array(p.getLinkState(self.robotId, 7)[0])
elif self.opt.observation == 'before_cnn':
self.observation = np.array(self.observation)
self.start_pos = p.getLinkState(self.robotId, 7)[0]
return self.get_reward()
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
stop = time.time()
print ("stepSimulation %f" % (stop - start))
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
projectionMatrix = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0]
start = time.time()
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, shadow=1,lightDirection=[1,1,1])
stop = time.time()
print ("renderImage %f" % (stop - start))
#time.sleep(.1)
#print("img_arr=",img_arr)
p.unloadPlugin(plugin)
def main(args, no_connect=False):
print(args.visual)
save_path = join(SAVE_ROOT_DIR, args.demo)
demo_path = join(DEMOS_DIR, args.play)
demo_info = os.path.basename(demo_path).split('_')
demo_num = int(demo_info[2].split('.')[0])
print('DEMO NUM', demo_num)
args.env = demo_info[1]
# args.env = 'bowlenv1_occ'
module = importlib.import_module('bullet.simenv.' + args.env)
envClass = getattr(module, 'UserEnv')
env = envClass()
cid = -1
if demo_path is None:
cid = p.connect(p.SHARED_MEMORY)
if cid<0 and not no_connect:
cid = p.connect(p.GUI)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
# Env is all loaded up here
h, o = env.load()
print('Total Number:', p.getNumBodies())
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)
##show this for 10 seconds
#now = time.time()
#while (time.time() < now+10):
# p.stepSimulation()
p.setRealTimeSimulation(1)
# Replay and generate object centeric log.
recordID = 0
log = readLogFile(demo_path)
recordNum = len(log)
itemNum = len(log[0])
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
init_setup = True
startID = log[0][2]
imgCount = 0
timeCount = 0
if args.visual:
rgb_folder, depth_folder, seg_folder, flow_folder, info_folder, base_folder = create_data_folders(args.visual, save_path)
else:
rgb_folder, depth_folder, seg_folder, info_folder, base_folder = create_data_folders(args.visual, save_path)
image_buffer = []
rgb_writer = imageio.get_writer('{}/{}.mp4'.format(
rgb_folder, demo_num), fps=SAMPLE_RATE)
end_effector_trajectory = []
# Save high level plan
# objects of interest
objects = env.objects()
obj_start_pose = None
current_label = -1
excluded_ids = ['kuka', 'pr2_gripper','pr2_cid', 'pr2_cid2']
relevant_ids = {val: key for key, val in h.items() if key not in excluded_ids}
# relevant_ids = {h.cubeid: 'cube1', h.bowlid: 'bowl'}
relevant_ids_names = {val: str(key) for key, val in relevant_ids.items()}
object_trajectories = {key: [] for key in relevant_ids.keys()}
init_object_poses = {}
# Set arm invisible
# set_obj_invisible(objects[0])
set_obj_invisible(1)
# Optical flow related
prev = None
hsv = np.zeros((IMG_SIZE, IMG_SIZE, 3))
hsv[...,1] = 255
# For record
info_record = {}
for objseq, obj in enumerate(objects):
info_record[objseq] = []
cam_pose = getPose(4)[:3]
# cam_pose[0] -= 0.4
for i, record in enumerate(log):
Id = record[2]
if i != 0 and Id == startID and init_setup:
init_setup = False
obj_start_pose = [getPose(x) for x in objects[1:]]
if init_setup:
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
if Id == 6:
pos = [0.9032135611658497, -0.3798930514455489, 0.6500000245783583]
if Id == 7:
pos = [0.9032135611658497, -0.6830514455489, 0.6500000245783583]
p.resetBasePositionAndOrientation(Id,pos,orn)
if Id in relevant_ids.keys():
# if Id == 5:
# set_trace()
init_object_poses[Id] = pos + orn
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
elif Id == h.kuka: # can also be objects[0]
numJoints = p.getNumJoints(Id)
for i in range(numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if i not in (env.o.kukaobject.lf_id, env.o.kukaobject.rf_id) and qIndex > -1:
p.setJointMotorControl2(Id, jointIndex=i, controlMode=p.POSITION_CONTROL,
targetPosition=record[qIndex-7+17], targetVelocity=0,
force=env.o.kukaobject.maxForce,
positionGain=0.12,
velocityGain=1)
rf = record[p.getJointInfo(Id, env.o.kukaobject.rf_id)[3]-7+17]
lf = record[p.getJointInfo(Id, env.o.kukaobject.lf_id)[3]-7+17]
position = max(abs(rf), abs(lf))
if position > 1e-4:
env.o.kukaobject.instant_close_gripper()
else:
env.o.kukaobject.instant_open_gripper()
env.o.kukaobject.gripper_centered()
p.setGravity(0.000000,0.000000,-10.000000)
p.stepSimulation()
time.sleep(0.003)
if Id == startID:
# Don't take pic from the first frame 0
if timeCount % SAMPLE_RATE == 1 and timeCount // SAMPLE_RATE > 0:
info = o.kukaobject.get_gripper_pose()
info.append(o.kukaobject.is_gripper_open)
# 0, the glory number reserved for kuka arm
info_record[0].append(np.array(info))
for objseq, obj in enumerate(objects[1:]):
objseq += 1
pose = np.array(getPose(obj))
info_record[objseq].append(pose)
viewMat = p.computeViewMatrixFromYawPitchRoll( cam_pose, #TARGET_POSITION, #getPose(objects[-1])[:3],
distance=CAMERA_DISTANCE,
yaw=YAW,
pitch=PITCH,
roll=0,
upAxisIndex=2)
projMat = p.computeProjectionMatrixFOV(FOV, aspect=1, nearVal=NEARVAL, farVal=FARVAL)
results = p.getCameraImage(width=IMG_SIZE,
height=IMG_SIZE,
viewMatrix=viewMat,
projectionMatrix=projMat)
### DEBUG for 3D RECONSTRUCTION#####
# viewMat2 = p.computeViewMatrixFromYawPitchRoll(getPose(4)[:3],
# distance=CAMERA_DISTANCE,
# yaw=-10,
# pitch=-10,
# roll=0,
# upAxisIndex=2)
# projMat2 = p.computeProjectionMatrixFOV(FOV, aspect=1, nearVal=NEARVAL, farVal=FARVAL)
# results2 = p.getCameraImage(width=IMG_SIZE,
# height=IMG_SIZE,
# viewMatrix=viewMat2,
# projectionMatrix=projMat2)
# vin = np.linalg.pinv(np.reshape(projMat, [4,4]).dot(np.reshape(viewMat, [4,4])))
# vin2 = np.linalg.pinv(np.reshape(projMat2, [4,4]).dot(np.reshape(viewMat2, [4,4])))
# pin = np.linalg.pinv(np.reshape(projMat, [4,4]))
# pin2 = np.linalg.pinv(np.reshape(projMat2, [4,4]))
# d1 = results[3]
# d2 = results2[3]
# d1 = np.swapaxes(copy(d1), 0, 1)
# d2 = np.swapaxes(copy(d2), 0, 1)
# d1_r = tinyDepth_no_rescale(copy(d1), NEARVAL, FARVAL)
# d2_r = tinyDepth_no_rescale(copy(d2), NEARVAL, FARVAL)
# # d2_r2 = linDepth(d2, NEARVAL, FARVAL)
# # d2_r3 = nonLinearDepth(d2, NEARVAL, FARVAL)
# print("tiny: ",d2_r[120,120])
# w1, h1 = 193, 133
# w2, h2 = 123, 143
# vun1 = vin.dot(np.array([w1, h1, d1[w1, h1], 1]))
# vun2 = vin2.dot(np.array([w2, h2, d2[w2, h2], 1]))
# pun1 = pin.dot(np.array([w1, h1, d1[w1, h1], 1]))
# pun2 = pin2.dot(np.array([w2, h2, d2[w2, h2], 1]))
# pun1 /= pun1[-1]
# pun2 /= pun2[-1]
# vun1 /= vun1[-1]
# vun2 /= vun2[-1]
# w1_bowl, h1_bowl = 89, 136
# w2_bowl, h2_bowl = 34, 117
# vun1_bowl = vin.dot(np.array([w1_bowl, h1_bowl, d1[w1_bowl, h1_bowl], 1]))
# vun2_bowl = vin2.dot(np.array([w2_bowl, h2_bowl, d2[w2_bowl, h2_bowl], 1]))
# pun1_bowl = pin.dot(np.array([w1_bowl, h1_bowl, d1[w1_bowl, h1_bowl], 1]))
# pun2_bowl = pin2.dot(np.array([w2_bowl, h2_bowl, d2[w2_bowl, h2_bowl], 1]))
# # pun1_bowl /= pun1_bowl[-1]
# # pun2_bowl /= pun2_bowl[-1]
# # vun1_bowl /= vun1_bowl[-1]
# # vun2_bowl /= vun2_bowl[-1]
# vun1_bowl[2] = d1_r[w1_bowl, h1_bowl]
# vun2_bowl[2] = d2_r[w2_bowl, h2_bowl]
# vun1[2] = d1_r[w1, h1]
# vun2[2] = d2_r[w2, h2]
# dist1_z = d1_r[w1_bowl, h1_bowl] - d1_r[w1, h1]
# dist2_z = d2_r[w2_bowl, h2_bowl] - d2_r[w2, h2]
# dist1 = pun1_bowl[:2] - pun1[:2]
# dist2 = pun2_bowl[:2] - pun2[:2]
# w1 = 71
# h1 = 122
# x_cube_1 = (w1 - 120) * d2_r[w1,h1] / 3.732/100
# y_cube_1 = (h1 - 120) * d2_r[w1,h1] / 3.732/100
# z_cube_1 = d1_r[w1, h1]
# p3d_cube_1 = np.array([x_cube_1, y_cube_1, z_cube_1])
# x_red_1 = (120 - 120) * d2_r[120,120] / 3.732/100
# y_red_1 = (120 - 120) * d2_r[120,120] / 3.732/100
# z_red_1 = d1_r[120, 120]
# p3d_red_1 = np.array([x_red_1, y_red_1, z_red_1])
# w2 = 136
# h2 = 128
# x_cube_2 = (120 - 120) * d2_r[120,120] / 3.732/100
# y_cube_2 = (120 - 120) * d2_r[120,120] / 3.732/100
# z_cube_2 = d2_r[120, 120]
# p3d_cube_2 = np.array([x_cube_2, y_cube_2, z_cube_2])
# x_red_2 = (w2 - 120) * d2_r[w2,h2] / 3.732/100
# y_red_2 = (h2 - 120) * d2_r[w2,h2] / 3.732/100
# z_red_2 = d2_r[w2, h2]
# p3d_red_2 = np.array([x_red_2, y_red_2, z_red_2])
# # print(np.linalg.norm(p3d_red_1 - p3d_cube_1))
# # print(np.linalg.norm(p3d_red_2 - p3d_cube_2))
# persp = np.reshape(projMat, [4,4])
# persp[0:2, 2] = 120
# alpha, s, x0, beta, y0 = persp[0, 0], persp[0,1], persp[0,2], persp[1,1], persp[1,2]
# K = np.array([[alpha, s, x0], [0, beta, y0], [0,0,-1]])
# I_m = np.eye(3)
# I_m[1,1] = -1.0
# I_h = np.eye(3)
# I_h[1,2] = 120.0
# Ks = I_h.dot(I_m).dot(K)
# cx = Ks[0,2]
# cy = Ks[1,2]
# fx = Ks[0,0]*100
# fy = Ks[1,1]*100
# w_eeright = 131
# h_eeright = 88
# w_eeleft= 103
# h_eeleft = 88
# # w_eeleft= 120
# # h_eeleft = 120
# x_eeleft_1 = (w_eeleft - cx) * d2_r[w_eeleft,h_eeleft] / fx
# y_eeleft_1 = (h_eeleft - cy + 120) * d2_r[w_eeleft,h_eeleft] / fy
# z_eeleft_1 = d1_r[w_eeleft, h1]
# p3d_eeleft_1 = np.array([x_eeleft_1, y_eeleft_1, z_eeleft_1])
# x_eeright_1 = (w_eeright - cx) * d2_r[w_eeright,h_eeright] / fx
# y_eeright_1 = (h_eeright - cy + 120) * d2_r[w_eeright,h_eeright] / fy
# z_eeright_1 = d1_r[w_eeright, h_eeright]
# p3d_eeright_1 = np.array([x_eeright_1, y_eeright_1, z_eeright_1])
# w_eeright = 140
# h_eeright = 92
# w_eeleft= 133
# h_eeleft = 89
# x_eeleft_2 = (w_eeleft - cx) * d2_r[w_eeleft,h_eeleft] / fx
# y_eeleft_2 = (h_eeleft - cy + 120) * d2_r[w_eeleft,h_eeleft] / fy
# z_eeleft_2 = d2_r[w_eeleft, h_eeleft]
# p3d_eeleft_2 = np.array([x_eeleft_2, y_eeleft_2, z_eeleft_2])
# x_eeright_2 = (w_eeright - cx) * d2_r[w_eeright,h_eeright] / fx
# y_eeright_2 = (h_eeright - cy + 120) * d2_r[w_eeright,h_eeright] / fy
# z_eeright_2 = d2_r[w_eeright, h_eeright]
# p3d_eeright_2 = np.array([x_eeright_2, y_eeright_2, z_eeright_2])
# print(np.linalg.norm(p3d_eeright_1 - p3d_eeleft_1))
# print(np.linalg.norm(p3d_eeright_2 - p3d_eeleft_2))
# plt.figure()
# plt.imshow(results[2])
# plt.figure()
# plt.imshow(results2[2])
# # plt.figure()
# # plt.imshow(results[3])
# # plt.figure()
# # plt.imshow(results2[3])
# # plt.show()
# set_trace()
############# END DEBUG #####3
set_trace()
rgb_writer.append_data(results[2])
for key in relevant_ids.keys():
object_trajectories[key].append(getPose(key))
link_state = p.getLinkState(o.kukaobject.kukaId, o.kukaobject.kukaEndEffectorIndex)
end_effector_trajectory.append(np.array(link_state[0] + link_state[1]))
# Note: we can compute flow online
imageio.imwrite('{0}/{1}_{2:05d}.png'.format(
rgb_folder, demo_num, imgCount), results[2])
imageio.imwrite('{0}/{1}_{2:05d}.png'.format(
depth_folder, demo_num, imgCount), results[3])
imageio.imwrite('{0}/{1}_{2:05d}.png'.format(
seg_folder, demo_num, imgCount), results[4])
np.save('{0}/{1}_{2:05d}_mask.npy'.format(
seg_folder, demo_num, imgCount), results[4])
# With visual flag, save visualizable imgs
if args.visual:
cur = cv2.cvtColor(results[2], cv2.COLOR_BGR2GRAY)
# First frame
if timeCount // SAMPLE_RATE == 1:
prev = cur
flow = compute_optical_flow(prev, cur)
# Visualize optical flow
mag, ang = cv2.cartToPolar(flow[...,0], flow[...,1])
hsv[...,0] = ang*180/np.pi/2
hsv[...,2] = cv2.normalize(mag,None,0,255,cv2.NORM_MINMAX)
hsv = hsv.astype(np.uint8)
bgr = cv2.cvtColor(hsv,cv2.COLOR_HSV2BGR)
scipy.misc.imsave('{}/flow_{}_{}.png'.format(
flow_folder, demo_num, imgCount), bgr)
prev = cur
imgCount += 1
timeCount += 1
print("Saving files to ", base_folder)
np.save('{}/{}_ee.npy'.format(
base_folder, demo_num), np.array(end_effector_trajectory))
with open('{}/{}_objects.json'.format(base_folder, demo_num), 'w') as f:
json.dump(object_trajectories, f)
with open('{}/{}_init_object_poses.json'.format(base_folder, demo_num), 'w') as f:
json.dump(init_object_poses, f)
with open('{}/{}_relevant_ids_names.json'.format(base_folder, demo_num), 'w') as f:
json.dump(relevant_ids_names, f)
# Check if consistent with above paramsd
camera_params = {"cameraTargetPosition": TARGET_POSITION, "distance": CAMERA_DISTANCE, "yaw": YAW, "pitch": PITCH,
"roll": 0.0, "upAxisIndex": 2, "nearPlane": NEARVAL, "farPlane": FARVAL, "fov": FOV, "aspect": 1}
with open('{}/{}_camera_params.json'.format(base_folder, demo_num), 'w') as f:
json.dump(camera_params, f)
if args.visual:
for i in range(len(objects)):
with open('{}/test_{}_{}.txt'.format(
info_folder, demo_num, i), 'w') as f:
print_list = [' '.join(map(str, x)) + '\n' for x in info_record[i]]
f.writelines(print_list)
else:
torch.save(info_record, '{}/info-{}-.tch'.format(info_folder, demo_num))
rgb_writer.close()
return 0
RGB=False,
title='seg_' + str(img_counter).zfill(4),
time_stamp=False)
sensor.camera.save_image(depth_equilibrium,
path,
RGB=False,
title='depth_' +
str(img_counter).zfill(4),
time_stamp=False)
img_counter += 1
if args.debug:
camera_info = p.getDebugVisualizerCamera()
_, _, debug_rgb, _, _ = p.getCameraImage(
640,
480,
camera_info[2],
camera_info[3],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
sensor.camera.save_image(debug_rgb,
path,
RGB=True,
title='debug_' +
str(img_counter).zfill(4),
time_stamp=False)
# sensor.camera.show_image(debug_rgb, title="Debug", save=False)
if args.show_image:
sensor.camera.show_image(rgb_img, title='Raw RGB', save=False)
sensor.camera.show_image(tactile_img,
title='Tactile RGB',
save=False)
import pybullet as p
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS)
print(visualData)
curTexUid = visualData[0][8]
print(curTexUid)
texUid = p.loadTexture("tex256.png")
print("texUid=", texUid)
p.changeVisualShape(plane, -1, textureUniqueId=texUid)
for i in range(100):
p.getCameraImage(320, 200)
p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
for i in range(100):
p.getCameraImage(320, 200)
def computeVisibilities(self, world_name, camera_id):
"""
"""
visibilities = {}
mean_distances_from_center = {}
nb_pixel = {}
relative_sizes = {}
if camera_id in self.ctx.worlds()[world_name].scene().nodes():
camera_node = self.ctx.worlds()[world_name].scene().nodes(
)[camera_id]
position = [
camera_node.position.pose.position.x,
camera_node.position.pose.position.y,
camera_node.position.pose.position.z
]
orientation = [
camera_node.position.pose.orientation.x,
camera_node.position.pose.orientation.y,
camera_node.position.pose.orientation.z,
camera_node.position.pose.orientation.w
]
euler = tf.transformations.euler_from_quaternion(orientation)
view_matrix = p.computeViewMatrixFromYawPitchRoll(
position, -0.5, math.degrees(euler[2]), math.degrees(euler[1]),
math.degrees(euler[1]), 2)
fov = float(self.ctx.worlds()
[world_name].scene().nodes().get_node_property(
camera_id, "hfov"))
clipnear = float(self.ctx.worlds()
[world_name].scene().nodes().get_node_property(
camera_id, "clipnear"))
clipfar = float(self.ctx.worlds()
[world_name].scene().nodes().get_node_property(
camera_id, "clipfar"))
aspect = float(self.ctx.worlds()
[world_name].scene().nodes().get_node_property(
camera_id, "aspect"))
proj_matrix = p.computeProjectionMatrixFOV(40.0, aspect, clipnear,
clipfar)
width, height, rgb, depth, seg = p.getCameraImage(
self.width,
self.height,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
max_nb_pixel = 0
background_nb_pixel = 0
max_visibility = 0
r = min(width, height)
for line in range(0, height):
for col in range(0, width):
pixel = seg[line][col]
if (pixel >= 0):
bullet_id = pixel & ((1 << 24) - 1)
if bullet_id in self.reverse_node_id_map:
uwds_id = self.reverse_node_id_map[bullet_id]
if uwds_id != self.ctx.worlds()[world_name].scene(
).root_id():
if uwds_id not in mean_distances_from_center:
mean_distances_from_center[uwds_id] = 0.0
line_dist = (line - (line / 2.0))
col_dist = (col - (col / 2.0))
#dist_from_center = math.sqrt(line_dist*line_dist+col_dist*col_dist)
dist_from_center = math.sqrt(col_dist *
col_dist)
mean_distances_from_center[
uwds_id] += dist_from_center
if uwds_id not in nb_pixel:
nb_pixel[uwds_id] = 0
nb_pixel[uwds_id] += 1
if uwds_id not in visibilities:
visibilities[uwds_id] = 0
visibilities[uwds_id] += 1 - min(
1, dist_from_center / r)
if max_visibility < visibilities[uwds_id]:
max_visibility = visibilities[uwds_id]
else:
background_nb_pixel += 1.0
if len(mean_distances_from_center) > 0:
print "camera <%s> :" % self.worlds[world_name].scene.nodes[
camera_id].name
for node_id, mean_dist in mean_distances_from_center.items():
mean_distances_from_center[
node_id] = mean_dist / nb_pixel[node_id]
camera_node = self.worlds[world_name].scene.nodes[
camera_id]
object_node = self.worlds[world_name].scene.nodes[node_id]
if mean_distances_from_center[node_id] < r:
visibilities[node_id] = 1 - mean_distances_from_center[
node_id] / r
else:
visibilities[node_id] = 0
if visibilities[node_id] > self.min_treshold:
print " - see object <%s> with %5f confidence" % (
object_node.name, visibilities[node_id])
else:
del visibilities[node_id]
return visibilities
def __init__(self, quadruped):
self.q = quadruped
# self.base_link = p.getLinkState(self.q, 0)
self.base_link = "chassis"
self.bl_link = "RL_lower_leg"
self.br_link = "RR_lower_leg"
self.fl_link = "FL_lower_leg"
self.fr_link = "FR_lower_leg"
self.prepare_kin_dyn()
self._BuildJointNameToIdDict()
self._BuildMotorIdList()
self.kpp = 1000
self.kdp = 100
self.kpw = 200
self.kdw = 20
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self._motor_direction = jointDirections
self.jointDirections = jointDirections
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC
or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
self.jointIds = jointIds
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
p.setGravity(0,0,0)
else:
p.stepSimulation()
while 1:
viewMatrix = p.computeViewMatrix(cameraEyePosition=[0, 0, 2],
cameraTargetPosition=[0, 0, 0],
cameraUpVector=[0, 1, 0])
projectionMatrix = p.computeProjectionMatrixFOV(fov=60.0,
aspect=1.0,
nearVal=0.02,
farVal=3.1)
width, height, rgbImg, depthImg, segImg = p.getCameraImage(
width=480,
height=480,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
model = core.Model.load('/home/developer/garage/grocery_detector.pth', \
['ambrosia','apple','banana','bottle','cereal','coke',\
'lipton','lysol','milk','nutella','orange','oreo','pepsi'])
rgbImg = Image.fromarray(rgbImg).convert('RGB')
predictions = model.predict(rgbImg)
camera_view = cv2.cvtColor(np.array(rgbImg), cv2.COLOR_RGB2BGR)
# camera_view = np.array(rgbImg)
labels, boxes, scores = predictions
boxes = boxes.numpy()
scores = scores.numpy()
env.render = True
reward_sums = []
for epd in range(4):
obs = env.reset()
done = False
sum_rewards = 0.0
step = 0
while not done:
action = population.elite_population[epd % 8]\
.get_action(obs)
obs, reward, done, info = env.step(action)
time.sleep(0.01)
sum_rewards += reward
img = p.getCameraImage(512,512)
skimage.io.imsave("./imgs/epd{}step{}.png".format(epd, step), img[2])
step += 1
reward_sums.append(sum_rewards)
print("average sum of rewards: {:.3e} min: {:.3e} max: {:.3e}"\
.format(np.mean(reward_sums), np.min(reward_sums), np.max(reward_sums)))
import pdb; pdb.set_trace();
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.01
farPlane = 1000
lightDirection = [0,1,0]
lightColor = [1,1,1]#optional argument
fov = 60
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
#renderImage(w, h, view[16], projection[16])
#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
for pitch in range (0,360,10) :
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor)
w=img_arr[0]
h=img_arr[1]
rgb=img_arr[2]
dep=img_arr[3]
#print 'width = %d height = %d' % (w,h)
# reshape creates np array
np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr*(1./255.)
#show
plt.imshow(np_img_arr,interpolation='none')
plt.pause(0.01)
pybullet.resetSimulation()
#render Camera at 10Hertz
if (nowTime-lastTime>.1):
ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
camPos = ls[0]
camOrn = ls[1]
camMat = p.getMatrixFromQuaternion(camOrn)
upVector = [0,0,1]
forwardVec = [camMat[0],camMat[3],camMat[6]]
#sideVec = [camMat[1],camMat[4],camMat[7]]
camUpVec = [camMat[2],camMat[5],camMat[8]]
camTarget = [camPos[0]+forwardVec[0]*10,camPos[1]+forwardVec[1]*10,camPos[2]+forwardVec[2]*10]
camUpTarget = [camPos[0]+camUpVec[0],camPos[1]+camUpVec[1],camPos[2]+camUpVec[2]]
viewMat = p.computeViewMatrix(camPos, camTarget, camUpVec)
projMat = camInfo[3]
#p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
lastTime=nowTime
nowControlTime = time.time()
nowLidarTime = time.time()
#lidar at 20Hz
if (nowLidarTime-lastLidarTime>.3):
#print("Lidar!")
numThreads=0
results = p.rayTestBatch(rayFrom,rayTo,numThreads, parentObjectUniqueId=car, parentLinkIndex=hokuyo_joint)
for i in range (numRays):
hitObjectUid=results[i][0]
hitFraction = results[i][2]
hitPosition = results[i][3]
if (hitFraction==1.):
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf")
sphereUid = p.loadURDF("sphere_transparent.urdf", [0, 0, 2])
redSlider = p.addUserDebugParameter("red", 0, 1, 1)
greenSlider = p.addUserDebugParameter("green", 0, 1, 0)
blueSlider = p.addUserDebugParameter("blue", 0, 1, 0)
alphaSlider = p.addUserDebugParameter("alpha", 0, 1, 0.5)
while (1):
red = p.readUserDebugParameter(redSlider)
green = p.readUserDebugParameter(greenSlider)
blue = p.readUserDebugParameter(blueSlider)
alpha = p.readUserDebugParameter(alphaSlider)
p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha])
p.getCameraImage(320,
200,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(0.01)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf", [0, 0, 1])
viewMatrix = p.computeViewMatrix(
cameraEyePosition=[2, 1, 3],
cameraTargetPosition=[0, 0, 0],
cameraUpVector=[0, 1, 0])
projectionMatrix = p.computeProjectionMatrixFOV(
fov=45.0,
aspect=1.0,
nearVal=0.1,
farVal=3.1)
width, height, rgbImg, depthImg, segImg = p.getCameraImage(
width=224,
height=224,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix)
imgW = width #int(width / 10)
imgH = height #int(height / 10)
#Dimg = Image.fromarray(depthImg, 'F')
depth_img_buffer = np.reshape(depthImg, [imgW, imgH])
pointCloud = np.empty([np.int(imgH/1), np.int(imgW/1), 4])
projectionMatrix = np.asarray(projectionMatrix).reshape([4,4],order='F')
viewMatrix = np.asarray(viewMatrix).reshape([4,4],order='F')
tran_pix_world = np.linalg.inv(np.matmul(projectionMatrix, viewMatrix))
for h in range(0, imgH):
for w in range(0, imgW):
x = (2*w - imgW)/imgW
y = -(2*h - imgH)/imgH # be careful！ deepth and its corresponding position
def reset(self, target=None):
self.img_buffer = []
try:
self.destroy()
except:
print("initial start!")
self.physical_id = self.p
self.p.setAdditionalSearchPath(pybullet_data.getDataPath())
self.p.setGravity(0, 0, -9.8)
self.p.setTimeStep(1 / 250.)
# epoch_max = -1
# for file in os.listdir (self.log_root):
# if ('epoch' in file) and ('txt' not in file):
# epoch_max = max (epoch_max, int (file.split ('-')[1]))
# self.epoch_num = epoch_max + 1
# print ('epoch_max:{}'.format (epoch_max))
self.epoch_num += 1
print('epoch_max:{}'.format(self.epoch_num))
lock = 1
while (lock):
try:
self.log_path = safe_path(
os.path.join(self.log_root,
'epoch-{}'.format(self.epoch_num)))
self.log_info = open(
os.path.join(self.log_root,
'epoch-{}.txt'.format(self.epoch_num)), 'w')
lock = 0
except:
self.epoch_num += 1
# self.evaluator.update (img_path=self.log_path, start_id=0)
self.seq_num = 0
self.init_dmp()
self.init_motion()
self.init_rl()
self.reset_obj()
for j in range(6):
index_ = self.robot.activeGripperJointIndexList[j]
p.resetJointState(self.robotId, index_, 0, 0)
if self.opt.use_embedding:
action_p = random.random()
action_p = int(action_p * len(self.opt.embedding_list))
if self.opt.fine_tune:
fine_tune_list = self.opt.fine_tune_list
action_p = random.random()
action_p = int(action_p * len(fine_tune_list))
action_p = np.where(
np.array(self.opt.embedding_list) ==
fine_tune_list[action_p])[0][0]
if target is not None:
action_p = np.where(
np.array(self.opt.embedding_list) == target)[0][0]
if self.opt.nlp_embedding:
self.opt.load_embedding = self.opt.embedding_list[action_p]
self.action_embedding = self.embedding_data[
self.opt.load_embedding]
else:
self.action_embedding = np.array([0] * self.opt.embedding_dim)
self.action_embedding[action_p] = 1
self.opt.load_embedding = self.opt.embedding_list[action_p]
self.init_grasp()
img_info = p.getCameraImage(
width=self.w,
height=self.h,
viewMatrix=self.view_matrix,
projectionMatrix=self.proj_matrix,
shadow=-1,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_TINY_RENDERER)
observation = img_info[2][:, :, :3]
if self.opt.add_mask:
mask = (img_info[4] > 10000000)
mask_id_label = [
234881025, 301989889, 285212673, 268435457, 318767105,
335544321, 201326593, 218103809, 167772161
]
for item in mask_id_label:
mask = mask * (img_info[4] != item)
box_mask_label = [
117440516, 234881028, 184549380, 33554436, 167772164, 50331652,
134217732, 16777220, 67108868, 150994948, 251658244, 234881028,
100663300, 218103812, 83886084, 268435460
]
for item in box_mask_label:
mask = mask * (img_info[4] != item)
observation = cv2.cvtColor(observation, cv2.COLOR_RGB2BGR)
observation[mask] = [127, 151, 182]
if self.opt.observation == 'joint_pose':
observation = np.array([
p.getJointState(self.robotId, i)[0]
for i in range(self.robotEndEffectorIndex)
])
elif self.opt.observation == 'end_pos':
observation = np.array(p.getLinkState(self.robotId, 7)[0])
elif self.opt.observation == 'before_cnn':
observation = np.array(observation)
if self.opt.use_embedding:
self.observation = [self.action_embedding, observation]
else:
self.observation = observation
self.info = ''
return self.observation
