def test(args):
count = 0
env = gym.make(args.env)
env.env.configure(args)
print("args.render=", args.render)
if (args.render == 1):
env.render(mode="human")
env.reset()
w, h, vmat,projmat,camup,camfwd,hor,ver,yaw,pitch,dist,target= p.getDebugVisualizerCamera()
dist = 0.4
yaw = 0
p.resetDebugVisualizerCamera(dist,yaw, pitch,target)
for obindex in range (p.getNumBodies()):
obuid = p.getBodyUniqueId(obindex)
p.changeDynamics(obuid, -1, linearDamping=0, angularDamping=0)
for l in range (p.getNumJoints(obuid)):
p.changeDynamics(obuid, l, linearDamping=0, angularDamping=0, jointDamping=0)
#if (l==0):
# p.setJointMotorControl2(obuid,l,p.POSITION_CONTROL,targetPosition=0)
if (l==2):
jp,jv,_,_ = p.getJointState(obuid,l)
p.resetJointState(obuid,l, jp,0.01 )
if (args.resetbenchmark):
while (1):
env.reset()
print("p.getNumBodies()=", p.getNumBodies())
print("count=", count)
count += 1
print("action space:")
sample = env.action_space.sample()
action = sample * 0.0
action = [0,0]#sample * 0.0
print("action=")
print(action)
for i in range(args.steps):
obs, rewards, done, _ = env.step(action)
if (args.rgb):
print(env.render(mode="rgb_array"))
print("obs=")
print(obs)
print("rewards")
print(rewards)
print("done")
print(done)
def _reset(self):
assert(self._robot_introduced)
assert(self._scene_introduced)
debugmode = 1
if debugmode:
print("Episode: steps:{} score:{}".format(self.nframe, self.reward))
body_xyz = self.robot.body_xyz
print("[{}, {}, {}],".format(body_xyz[0], body_xyz[1], body_xyz[2]))
print("Episode count: {}".format(self.eps_count))
self.eps_count += 1
self.nframe = 0
self.eps_reward = 0
BaseEnv._reset(self)
if not self.ground_ids:
self.parts, self.jdict, self.ordered_joints, self.robot_body = self.robot.addToScene(
[])
self.ground_ids = set(self.scene.scene_obj_list)
## Todo: (hzyjerry) this part is not working, robot_tracking_id = -1
for i in range (p.getNumBodies()):
if (p.getBodyInfo(i)[0].decode() == self.robot_body.get_name()):
self.robot_tracking_id=i
#print(p.getBodyInfo(i)[0].decode())
i = 0
eye_pos, eye_quat = self.get_eye_pos_orientation()
pose = [eye_pos, eye_quat]
observations = self.render_observations(pose)
pos = self.robot._get_scaled_position()
orn = self.robot.get_orientation()
pos = (pos[0], pos[1], pos[2] + self.tracking_camera['z_offset'])
p.resetDebugVisualizerCamera(self.tracking_camera['distance'],self.tracking_camera['yaw'], self.tracking_camera['pitch'],pos)
return observations
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def test_multiagent():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
turtlebot3 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
s.import_robot(turtlebot3)
turtlebot1.set_position([1, 0, 0.5])
turtlebot2.set_position([0, 0, 0.5])
turtlebot3.set_position([-1, 0, 0.5])
nbody = p.getNumBodies()
for i in range(100):
#turtlebot1.apply_action(1)
#turtlebot2.apply_action(1)
#turtlebot3.apply_action(1)
s.step()
s.disconnect()
assert nbody == 7
def run(self) -> Dict[core.PhysicalObject, Dict[str, list]]:
steps_per_frame = self.scene.step_rate // self.scene.frame_rate
max_step = (self.scene.frame_end + 1) * steps_per_frame
obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
animation = {
obj_id: {
"position": [],
"quaternion": [],
"velocity": [],
"angular_velocity": []
}
for obj_id in obj_idxs
}
for current_step in range(max_step):
if current_step % steps_per_frame == 0:
for obj_idx in obj_idxs:
position, quaternion = self.get_position_and_rotation(
obj_idx)
velocity, angular_velocity = self.get_velocities(obj_idx)
animation[obj_idx]["position"].append(position)
animation[obj_idx]["quaternion"].append(quaternion)
animation[obj_idx]["velocity"].append(velocity)
animation[obj_idx]["angular_velocity"].append(
angular_velocity)
pb.stepSimulation()
return {
self.objects_to_pybullet.inverse[obj_idx]: anim
for obj_idx, anim in animation.items()
}
def whiten_materials(self, body_id=None, link_id=None):
"""
Helper method for setting all material colors to white.
Args:
body_id (int): unique body id when you load the body. If body_id
is not provided, all the bodies will be whitened.
link_id (int): the index of the link on the body. If link_id is not
provided and body_id is provided, all the links of the body
will be whitened.
"""
if body_id is None:
body_num = p.getNumBodies(physicsClientId=self._pb_id)
for body_idx in range(body_num):
if not self._check_body_exist(body_idx):
continue
num_jnts = p.getNumJoints(body_idx,
physicsClientId=self._pb_id)
# start from -1 for urdf that has no joint but one link
start = -1 if num_jnts == 0 else 0
for i in range(start, num_jnts):
self.set_rgba(body_idx, i, rgba=[1, 1, 1, 1])
else:
if link_id is None:
num_jnts = p.getNumJoints(body_id, physicsClientId=self._pb_id)
# start from -1 for urdf that has no joint but one link
start = -1 if num_jnts == 0 else 0
for i in range(start, num_jnts):
self.set_rgba(body_id, i, rgba=[1, 1, 1, 1])
else:
self.set_rgba(body_id, link_id, rgba=[1, 1, 1, 1])
def get_num_bodies(self):
"""
Get the number of bodies in the physics server
:return: int
The number of bodies present
"""
return p.getNumBodies(physicsClientId=self.client_id)
def test(args):
count = 0
env = gym.make(args.env)
env.env.configure(args)
print("args.render=", args.render)
if (args.render == 1):
env.render(mode="human")
env.reset()
if (args.resetbenchmark):
while (1):
env.reset()
print("p.getNumBodies()=", p.getNumBodies())
print("count=", count)
count += 1
print("action space:")
sample = env.action_space.sample()
action = sample * 0.0
print("action=")
print(action)
for i in range(args.steps):
obs, rewards, done, _ = env.step(action)
if (args.rgb):
print(env.render(mode="rgb_array"))
print("obs=")
print(obs)
print("rewards")
print(rewards)
print("done")
print(done)
def test(args):
count = 0
env = gym.make(args.env)
env.env.configure(args)
print("args.render=",args.render)
if (args.render==1):
env.render(mode="human")
env.reset()
if (args.resetbenchmark):
while (1):
env.reset()
print("p.getNumBodies()=",p.getNumBodies())
print("count=",count)
count+=1
print("action space:")
sample = env.action_space.sample()
action = sample*0.0
print("action=")
print(action)
for i in range(args.steps):
obs,rewards,done,_ =env.step(action)
if (args.rgb):
print(env.render(mode="rgb_array"))
print("obs=")
print(obs)
print("rewards")
print (rewards)
print ("done")
print(done)
def test_turtlebot():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_humanoid():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_jr2():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
jr2 = JR2(config)
s.import_robot(jr2)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_quadrotor():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
quadrotor = Quadrotor(config)
s.import_robot(quadrotor)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def reset(self):
obs = super().reset()
p.setPhysicsEngineParameter(**self.physics_params)
for body in range(p.getNumBodies()):
p.changeDynamics(body, -1, **self.dynamics_params)
for joint in range(p.getNumJoints(body)):
p.changeDynamics(body, joint)
return obs
def robot_specific_reset(self):
"""
Humanoid robot specific reset
Add spherical radiance/glass shield to protect the robot's camera
"""
humanoidId = -1
numBodies = p.getNumBodies()
for i in range(numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
# Spherical radiance/glass shield to protect the robot's camera
super(Humanoid, self).robot_specific_reset()
if self.glass_id is None:
glass_path = os.path.join(self.physics_model_dir,
"humanoid/glass.xml")
glass_id = p.loadMJCF(glass_path)[0]
self.glass_id = glass_id
p.changeVisualShape(self.glass_id, -1, rgbaColor=[0, 0, 0, 0])
p.createMultiBody(baseVisualShapeIndex=glass_id,
baseCollisionShapeIndex=-1)
cid = p.createConstraint(
humanoidId,
-1,
self.glass_id,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, self.glass_offset],
childFramePosition=[0, 0, 0])
robot_pos = list(self.get_position())
robot_pos[2] += self.glass_offset
robot_orn = self.get_orientation()
p.resetBasePositionAndOrientation(self.glass_id, robot_pos, robot_orn)
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += [
"right_hip_x", "right_hip_z", "right_hip_y", "right_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"left_hip_x", "left_hip_z", "left_hip_y", "left_knee"
]
self.motor_power += [100, 100, 300, 200]
self.motor_names += [
"right_shoulder1", "right_shoulder2", "right_elbow"
]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def save_bullet_state(physicsClientId=0):
state = {}
for i in range(p.getNumBodies()):
pos, orn = p.getBasePositionAndOrientation(
i, physicsClientId=physicsClientId)
linVel, angVel = p.getBaseVelocity(i, physicsClientId=physicsClientId)
state[i] = (pos, orn, linVel, angVel)
return state
def dumpStateToFile(file):
for i in range(p.getNumBodies()):
pos, orn = p.getBasePositionAndOrientation(i)
linVel, angVel = p.getBaseVelocity(i)
txtPos = "pos=" + str(pos) + "\n"
txtOrn = "orn=" + str(orn) + "\n"
txtLinVel = "linVel" + str(linVel) + "\n"
txtAngVel = "angVel" + str(angVel) + "\n"
file.write(txtPos)
file.write(txtOrn)
file.write(txtLinVel)
file.write(txtAngVel)
def dumpStateToFile(file): for i in range (p.getNumBodies()): pos,orn = p.getBasePositionAndOrientation(i) linVel,angVel = p.getBaseVelocity(i) txtPos = "pos="+str(pos)+"\n" txtOrn = "orn="+str(orn)+"\n" txtLinVel = "linVel"+str(linVel)+"\n" txtAngVel = "angVel"+str(angVel)+"\n" file.write(txtPos) file.write(txtOrn) file.write(txtLinVel) file.write(txtAngVel)
def setup_bullet(self):
module = importlib.import_module('python.bullet.simenv.' +
self.env_name)
envClass = getattr(module, 'UserEnv')
self.env = envClass()
cid = -1
demo_path = None
if demo_path is None:
cid = p.connect(p.SHARED_MEMORY)
if cid < 0:
if self.gui_on:
cid = p.connect(p.GUI)
else:
cid = p.connect(p.DIRECT)
p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
# Env is all loaded up here
self.h, self.o = self.env.load()
print('Total Number:', p.getNumBodies())
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
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)
self._numJoints = p.getNumJoints(self.o.kukaobject.kukaId)
self.emb_dim = self._hyperparams['sensor_dims'][image_feature]
self.n_pose = self._hyperparams['sensor_dims'][OBJECT_POSE]
self._joint_idx = list(range(self._numJoints))
self._vel_idx = [i + self._numJoints for i in self._joint_idx]
self._pos_idx = [i + 2 * self._numJoints for i in range(7)]
self._emb_idx = [
i + 7 + 2 * self._numJoints for i in range(self.emb_dim)
]
self._obj_cent_idx = [
i + self._hyperparams['sensor_dims'][image_feature] + 3 +
2 * self._numJoints for i in range(self.n_pose)
]
self._anchor_obj_cent_idx = [
i + 7 + self._hyperparams['sensor_dims'][image_feature] + 3 +
2 * self._numJoints for i in range(self.n_pose)
]
def reset_goal(self):
# Set the goal to a random target
x = (self.np_random.uniform(2, 10)
if self.np_random.randint(2) else self.np_random.uniform(-2, -10))
y = (self.np_random.uniform(2, 10)
if self.np_random.randint(2) else self.np_random.uniform(-2, -10))
self.goal = (x, y)
# Visual element of the goal
if (p.getNumBodies() == 3):
p.removeBody(
2
) # I hope this removes the goal before adding a new one... need a better way of doing this though
Goal(self.client, self.goal)
def test_humanoid_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
humanoid.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = humanoid.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def main():
print("create env")
# env = gym.make("HumanoidFlagrunHarderPyBulletEnv-v0")
env = gym.make("ParkourBiped-v0")
env.render(mode="human")
print(env.observation_space)
print(type(env.action_space))
pi = ReactivePolicy(env.observation_space, env.action_space)
while 1:
frame = 0
score = 0
restart_delay = 0
obs = env.reset()
torsoId = -1
for i in range(p.getNumBodies()):
print(p.getBodyInfo(i))
if p.getBodyInfo(i)[0].decode() == "base":
torsoId = i
print("found humanoid torso")
while 1:
time.sleep(0.02)
a = pi.act(obs)
obs, r, done, _ = env.step(a)
score += r
frame += 1
humanPos, humanOrn = p.getBasePositionAndOrientation(torsoId)
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance = camInfo[10]
yaw = camInfo[8]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * humanPos[0],
0.95 * curTargetPos[1] + 0.05 * humanPos[1], curTargetPos[2]
]
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
still_open = env.render("human")
if still_open is None:
return
if not done:
continue
if restart_delay == 0:
print("score=%0.2f in %i frames" % (score, frame))
restart_delay = 60 * 2 # 2 sec at 60 fps
else:
restart_delay -= 1
if restart_delay == 0:
break
def check_collision(self, other_id=None, collision_distance=0.0):
others_id = [other_id]
if other_id is None:
others_id = [
p.getBodyUniqueId(i) for i in range(p.getNumBodies())
if p.getBodyUniqueId(i) != self.id
]
for other_id in others_id:
if len(
p.getClosestPoints(bodyA=self.id,
bodyB=other_id,
distance=collision_distance)) != 0:
return True
return False
def test_ant():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
s.disconnect()
assert nbody == 6
def test_turtlebot_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = turtlebot.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def check_overlap(self, obj: core.PhysicalObject) -> bool:
obj_idx = self.objects_to_pybullet[obj]
body_ids = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
for body_id in body_ids:
if body_id == obj_idx:
continue
overlap_points = pb.getClosestPoints(obj_idx, body_id, distance=0)
if overlap_points:
# TODO: we can easily get a suggested correction here
# i = np.argmin([o[8] for o in overlap_points], axis=0) # find the most overlapping point
# push = np.array(overlap_points[i][7]) * (overlap_points[i][8] + margin)
return True
return False
def test_ant():
s = Simulator(mode='headless', timestep=1 / 40.0)
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
#ant.apply_action(np.random.randint(17))
#ant2.apply_action(np.random.randint(17))
s.disconnect()
assert nbody == 6
def run(self, duration: float = 1.0) -> Dict[Object3D, Dict[str, list]]:
max_step = math.floor(self.step_rate * duration)
steps_per_frame = self.step_rate // self.frame_rate
obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())]
animation = {obj_id: {"position": [], "orient_quat": []}
for obj_id in obj_idxs}
for current_step in range(max_step):
if current_step % steps_per_frame == 0:
for obj_idx in obj_idxs:
pos, quat = pb.getBasePositionAndOrientation(obj_idx)
animation[obj_idx]["position"].append(pos)
animation[obj_idx]["orient_quat"].append(quat) # roll, pitch, yaw
pb.stepSimulation()
return {self.objects_by_idx[obj_idx]: anim for obj_idx, anim in animation.items()}
def high_contrast_bodies(alpha: float = 0.5):
"""Makes all bodies transparent and gives each body an individual color.
Args:
alpha (float): Regulates the strength of transparency.
"""
num_bodies = p.getNumBodies()
hsv = [(x * 1.0 / num_bodies, 0.5, 0.5) for x in range(num_bodies)]
rgb = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv))
for i in range(num_bodies):
body_unique_id = p.getBodyUniqueId(i)
for link_index in range(-1, p.getNumJoints(body_unique_id)):
p.changeVisualShape(
body_unique_id,
link_index,
rgbaColor=[rgb[i][0], rgb[i][1], rgb[i][2], alpha])
def get_sensor_image(self):
"""
Returns the raw and clipped sensor image. The clipped image
is generated based on the thickness of the tactile layer.
Returns:
(np.array, np.array, np.array, np.array) : RGB image, clipped RGB image, clipped depth image, seg image.
"""
# update sensor position and parameters
self._update_pose()
self._update_sensor()
rgb_img, depth_img, seg_img = self._camera.get_pybullet_image()
# update the contact information
self._contacts = Contact(self._sensor_id)
# clip the images to the depth of the tactile sensor
mask = np.where(depth_img >= self.max_buffer_depth)
depth_img[mask] = self.max_buffer_depth
# basically all the RGB image should be the color of the tactile surface
clipped_rgb_img = np.copy(rgb_img)
clipped_rgb_img[:, :, :] = self.background_color
# clip the segmentation image
clipped_seg_img = np.copy(seg_img)
clipped_seg_img[mask] = -1
if self._use_force:
# store the raw clipped images in the image buffer with the Z position of the object
obj_id = p.getBodyUniqueId(p.getNumBodies() - 1)
position, _ = p.getBasePositionAndOrientation(obj_id)
self._image_buf.store(clipped_rgb_img, depth_img, clipped_seg_img,
position[-1], self._time)
# compute the penetration based on the equilibrium of contact information and the spring models
img_equilibrium = self.compute_equilibrium()
return rgb_img, img_equilibrium['rgb_img'], img_equilibrium[
'depth_img'], seg_img, img_equilibrium['seg_img']
else:
return rgb_img, clipped_rgb_img, depth_img, seg_img, clipped_seg_img
def randomize(self, mode='all', exclude=None):
"""
Randomize all the links in the scene.
Args:
mode (str): one of `all`, `rgb`, `noise`, `gradient`.
exclude (dict): exclude bodies or links from randomization.
`exclude` is a dict with body_id as the key,
and a list of link ids as the value. If the value (link ids)
is an empty list, then all links on the body will be excluded.
"""
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
0,
physicsClientId=self._pb_id)
mode_to_func = {
'all': self.rand_all,
'rgb': self.rand_rgb,
'noise': self.rand_noise,
'gradient': self.rand_gradient,
'texture': self.rand_texture,
}
body_num = p.getNumBodies(physicsClientId=self._pb_id)
if exclude is None:
sep_bodies = set()
else:
sep_bodies = set(exclude.keys())
for body_idx in range(body_num):
if not self._check_body_exist(body_idx):
continue
if body_idx in sep_bodies and not exclude[body_idx]:
continue
num_jnts = p.getNumJoints(body_idx, physicsClientId=self._pb_id)
# start from -1 for urdf that has no joint but one link
start = -1 if num_jnts == 0 else 0
for link_idx in range(start, num_jnts):
if body_idx in sep_bodies and link_idx in exclude[body_idx]:
continue
mode_to_func[mode](body_idx, link_idx)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self._pb_id)
def getSceneAABB():
aabbMins = []
aabbMaxs = []
for i in range(p.getNumBodies()):
uid = p.getBodyUniqueId(i)
aabb = p.getAABB(uid)
aabbMins.append(np.array(aabb[0]))
aabbMaxs.append(np.array(aabb[1]))
if len(aabbMins):
sceneAABBMin = aabbMins[0]
sceneAABBMax = aabbMaxs[0]
for aabb in aabbMins:
sceneAABBMin = np.minimum(sceneAABBMin, aabb)
for aabb in aabbMaxs:
sceneAABBMax = np.maximum(sceneAABBMax, aabb)
print("sceneAABBMin=", sceneAABBMin)
print("sceneAABBMax=", sceneAABBMax)
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.
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")
for b in range (p.getNumBodies()):
p.changeVisualShape(b,linkIndex=-1,textureUniqueId=textureId)
for j in range(p.getNumJoints(b)):
p.changeVisualShape(b,linkIndex=j,textureUniqueId=textureId)
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]
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_opengl= np.reshape(images[2], (height, width, 4))*1./255.
time.sleep(1)
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_TINY_RENDERER, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
proj_tiny= np.reshape(images[2], (height, width, 4))*1./255.
# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
plt.subplot(4, 2, 1)
record.append(values[i])
log.append(record)
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf",[0,0,0])
p.loadURDF("block.urdf",[0,0,2])
log = readLogFile("data/block_grasp_log.bin")
recordNum = len(log)
itemNum = len(log[0])
objectNum = p.getNumBodies()
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
for objectId in range(objectNum):
record = log[stepIndex*objectNum+objectId]
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
for x in range(32):
for y in range(32):
for z in range(10):
batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=", p.getNumBodies())
p.stopStateLogging(logId)
p.setGravity(0, 0, -10)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)
