def run(self):
self._initialize()
self._loadRobot(self.robot_file, self.start_pos, self.start_ori)
try:
if 'log_mp4' in self.options.keys():
self.logId = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,
self.options['log_mp4'],
[self.robotId])
except:
pass
self._initDebugParaGUI()
self._initDynamicDumpings()
if 'initMaxPulseZero' in self.init_config.keys(
) and self.init_config['initMaxPulseZero']:
self._initmaxMotorImpulse() ## force=0 for all rev joint
self._initMotorStatus()
self._initPhysicalParas()
self._Main_Loop()
self._dumpData()
try:
p.stopStateLogging(self.logId)
except:
pass
p.disconnect()
def main(_):
robot = ROBOT_CLASS_MAP[FLAGS.robot_type]
motor_control_mode = MOTOR_CONTROL_MODE_MAP[FLAGS.motor_control_mode]
env = env_builder.build_regular_env(robot,
motor_control_mode=motor_control_mode,
enable_rendering=True,
on_rack=FLAGS.on_rack,
wrap_trajectory_generator=False)
action_low, action_high = env.action_space.low, env.action_space.high
action_median = (action_low + action_high) / 2.
dim_action = action_low.shape[0]
action_selector_ids = []
for dim in range(dim_action):
action_selector_id = p.addUserDebugParameter(paramName='dim{}'.format(dim),
rangeMin=action_low[dim],
rangeMax=action_high[dim],
startValue=action_median[dim])
action_selector_ids.append(action_selector_id)
if FLAGS.video_dir:
log_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, FLAGS.video_dir)
for _ in tqdm(range(800)):
action = np.zeros(dim_action)
for dim in range(dim_action):
action[dim] = env.pybullet_client.readUserDebugParameter(
action_selector_ids[dim])
env.step(action)
if FLAGS.video_dir:
p.stopStateLogging(log_id)
def stop_video_recording(self):
"""Stops video recording if any.
"""
if hasattr(self, "file_name"):
pybullet.stopStateLogging(
pybullet.STATE_LOGGING_VIDEO_MP4, self.file_name
)
def test_env(self, file_name, max_step=100):
state = self.env.reset()
done = False
total_reward = 0
steps = 0
cdist = 3.0
cyaw = 90
cpitch = -89
basePos = self.robot.getBasePosition()
p.resetDebugVisualizerCamera(cameraDistance=cdist,
cameraYaw=cyaw,
cameraPitch=cpitch,
cameraTargetPosition=basePos)
loggingId = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, file_name)
while steps < max_step:
state = torch.FloatTensor(state).unsqueeze(0).to(self.device)
dist, _ = self.net(state)
next_state, reward, done, _ = self.env.step(
dist.sample().cpu().numpy()[0]) #Hack
print("Step No: {:3d}, Reward: {:2.3f} and done: {}".format(
steps, reward, done))
state = next_state
total_reward += reward
steps += 1
p.stopStateLogging(loggingId)
def test_import_igsdf(scene_name, scene_source):
hdr_texture = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background', 'probe_02.hdr')
hdr_texture2 = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background', 'probe_03.hdr')
if scene_source == "IG":
scene_dir = get_ig_scene_path(scene_name)
elif scene_source == "CUBICASA":
scene_dir = get_cubicasa_scene_path(scene_name)
else:
scene_dir = get_3dfront_scene_path(scene_name)
light_modulation_map_filename = os.path.join(
scene_dir, 'layout', 'floor_lighttype_0.png')
background_texture = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background',
'urban_street_01.jpg')
scene = InteractiveIndoorScene(
scene_name,
texture_randomization=False,
object_randomization=False,
scene_source=scene_source)
settings = MeshRendererSettings(env_texture_filename=hdr_texture,
env_texture_filename2=hdr_texture2,
env_texture_filename3=background_texture,
light_modulation_map_filename=light_modulation_map_filename,
enable_shadow=True, msaa=True,
light_dimming_factor=1.0)
s = Simulator(mode='iggui', image_width=960,
image_height=720, device_idx=0, rendering_settings=settings)
s.import_ig_scene(scene)
fpss = []
np.random.seed(0)
_,(px,py,pz) = scene.get_random_point()
s.viewer.px = px
s.viewer.py = py
s.viewer.pz = 1.7
s.viewer.update()
for i in range(3000):
if i == 2500:
logId = p.startStateLogging(loggingType=p.STATE_LOGGING_PROFILE_TIMINGS, fileName='trace_beechwood')
start = time.time()
s.step()
end = time.time()
print("Elapsed time: ", end - start)
print("Frequency: ", 1 / (end - start))
fpss.append(1 / (end - start))
p.stopStateLogging(logId)
s.disconnect()
print("end")
plt.plot(fpss)
plt.show()
def close(self):
if hasattr(self, "video_logger") and self.video_logger is not None:
pb.stopStateLogging(self.video_logger,
physicsClientId=self.bullet_cid)
try:
pb.disconnect(physicsClientId=self.bullet_cid)
except pb.error:
pass
def main():
env = PusherEnv(render=True)
# Ground truth push videos
logger.info("Recording ground truth videos")
ground_truth_data_path = "results/P1/true_pushes.csv"
for i, push in pd.read_csv(ground_truth_data_path, index_col=0).iterrows():
logger.info(f'Video {i}')
################
# state = push["state"]
# actions = push["action"]
state = np.array(push["state"])
action = np.array([push["d_x"], push["d_y"]])
# state = np.array([push["obj_x"], push["obj_y"]])
# actions = [np.array([push["start_push_x"], push["start_push_y"], push["end_push_x"], push["end_push_y"]])]
######################
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P1/vids/true_pushes{i}.mp4")
env.reset()
# for action in actions:
# state, _, _, _ = env.step(action=action)
state, _, _, _ = env.step(action=action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
# Predicted push videos
predicted_data_path = "results/P1/pred_pushes.csv"
logger.info("Recording prediction videos")
for i, push in pd.read_csv(predicted_data_path, index_col=0).iterrows():
logger.info(f'Video {i}')
#######################
# state = push["state"]
# actions = push["action"]
state = np.array(push["state"])
action = np.array([push["d_x"], push["d_y"]])
# state = np.array([push["obj_x"], push["obj_y"]])
# actions = [np.array([push["start_push_x"], push["start_push_y"], push["end_push_x"], push["end_push_y"]])]
########################
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P1/vids/pred_pushes{i}.mp4")
env.reset()
# for action in actions:
# state, _, _, _ = env.step(action=action)
state, _, _, _ = env.step(action=action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
def _compute_done(self):
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.botId)
if (cubePos[2] < 0.15 or self._envStepCounter >= 1500
or abs(cubeOrn[0]) > 0.5 or abs(cubeOrn[1]) > 0.5
or abs(cubeOrn[2]) > 0.5):
if (self._episode_count % 10 == 1):
p.stopStateLogging(self._state_log)
return True
else:
return False
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 repeat_trajectory(env, human_pos, actions, camera_position, file_name):
# Camera position
bullet.resetDebugVisualizerCamera(camera_position['distance'], camera_position['yaw'], camera_position['pitch'], human_pos)
id = bullet.startStateLogging(bullet.STATE_LOGGING_VIDEO_MP4, args.video_dir + file_name)
for step, action in enumerate(actions):
state, reward, done, _ = env.step(action)
bullet.stopStateLogging(id)
crop_video(file_name)
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 log_video(self, task_name):
"""
Logs video of each task being executed
"""
if not os.path.exists("video_logs/"):
os.makedirs("video_logs")
try:
p.stopStateLogging(self.curr_recording)
self.video_log_key += 1
except Exception:
print("No Video Currently Being Logged")
self.curr_recording = p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4, "video_logs/task_vid_" +
str(task_name) + "_" + str(self.video_log_key) + ".mp4")
def main():
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('expert_policy_file', type=str)
parser.add_argument('--render', action='store_true')
parser.add_argument('--num_rollouts', type=int, default=1,
help='Number of expert rollouts')
args = parser.parse_args()
print('loading and building expert policy')
lin_policy = np.load(args.expert_policy_file)
lin_policy = lin_policy.items()[0][1]
M = lin_policy[0]
# mean and std of state vectors estimated online by ARS.
mean = lin_policy[1]
std = lin_policy[2]
env = minitaur_gym_env.MinitaurBulletEnv(render=True)#gym.make(env_name)
# env = gym.make(args.envname)
returns = []
observations = []
actions = []
for i in range(args.num_rollouts):
print('iter', i)
obs = env.reset()
log_id = pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4, "/usr/local/google/home/jietan/Projects/ARS/data/minitaur{}.mp4".format(i))
done = False
totalr = 0.
steps = 0
while not done:
action = np.clip(np.dot(M, (obs - mean)/std), -1.0, 1.0)
observations.append(obs)
actions.append(action)
obs, r, done, _ = env.step(action)
totalr += r
steps += 1
if args.render:
env.render()
if steps % 100 == 0: print("%i/%i"%(steps, 1000))
if steps >= 1000:
break
pybullet.stopStateLogging(log_id)
returns.append(totalr)
print('returns', returns)
print('mean return', np.mean(returns))
print('std of return', np.std(returns))
def main():
env = PushingEnv(ifRender=True)
# Ground truth push videos
logger.info("Recording ground truth videos")
ground_truth_data_path = "results/P2/true_pushes.csv"
for i, push in pd.read_csv(ground_truth_data_path, index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x"], push["start_push_y"], push["end_push_x"],
push["end_push_y"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P2/vids/true_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
# Predicted push videos
logger.info("Recording prediction videos")
predicted_data_path = "results/P2/pred_pushes.csv"
for i, push in pd.read_csv(predicted_data_path, index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x"], push["start_push_y"], push["end_push_x"],
push["end_push_y"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P2/vids/pred_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
def ExploreWorker(rank, num_processes, childPipe, args):
print("hi:", rank, " out of ", num_processes)
import pybullet as op
import pybullet_data as pd
logName = ""
p1 = 0
n = 0
while True:
n += 1
try:
# Only block for short times to have keyboard exceptions be raised.
if not childPipe.poll(0.001):
continue
message, payload = childPipe.recv()
except (EOFError, KeyboardInterrupt):
break
if message == _RESET:
op.connect(op.DIRECT)
p1 = bullet_client.BulletClient()
p1.setAdditionalSearchPath(pd.getDataPath())
logName = str("batchsim") + str(rank)
p1.loadURDF("plane.urdf")
createInstance(p1, [0, 0, 0], useMaximalCoordinates=True, flags=0)
p1.setPhysicsEngineParameter(minimumSolverIslandSize=100)
p1.setGravity(0, 0, -10)
childPipe.send(["reset ok"])
logId = op.startStateLogging(op.STATE_LOGGING_PROFILE_TIMINGS,
logName)
continue
if message == _EXPLORE:
sum_rewards = rank
numSteps = int(5000 / num_processes)
for i in range(numSteps):
p1.stepSimulation()
print("logId=", logId)
print("numSteps=", numSteps)
childPipe.send([sum_rewards])
continue
if message == _CLOSE:
op.stopStateLogging(logId)
childPipe.send(["close ok"])
break
childPipe.close()
def showBestStanding(self, num=-1, log=0):
self.Disconnect()
self.RunSRV(1)
time.sleep(1)
'''print("\nInShow, {:.3f}".format(self.elites[-1][0][0]))'''
#angles = np.array([90-38+30, 180-83, 180-110, 63, 90-38+30, -(180-83), -(180-110), -63])*np.pi/180#self.population[sort[0]][0]
#botStartOrientation = pb.getQuaternionFromEuler([0,-30*np.pi/180,0])
#angles = np.array([90-38, 180-83, 180-110, 63, 90-38, -(180-83), -(180-110), -63])*np.pi/180
#botStartOrientation = pb.getQuaternionFromEuler([0,0,0])
#sort = np.argsort(-self.fitnesses)
#angles = self.population[sort[0]][0]
#botStartOrientation = pb.getQuaternionFromEuler([0, self.population[sort[0]][1], 0])
angles = self.elites[num][0]
botStartOrientation = pb.getQuaternionFromEuler(
[0, self.elites[num][1], 0])
cid, botId = self.Init(botStartOrientation, pb_type=pb.SHARED_MEMORY)
if log == 1:
logID = pb.startStateLogging(
loggingType=pb.STATE_LOGGING_VIDEO_MP4,
fileName="bestStanding.mp4")
print([cid, botId])
self.setLegs(angles, sleep=0, botID=botId, cid=cid)
self.Torques = []
for dur in range(0, 1000):
self.Step(1, sleep=1, cid=cid, botID=botId)
self.AdjustCamera(botID=botId, cid=cid)
pb.setJointMotorControlArray(botId,
range(8),
controlMode=pb.POSITION_CONTROL,
targetPositions=angles,
targetVelocities=[0] * 8,
forces=[99999999999] * 8)
torques = []
for j in range(8):
tmp, tmp, tmp, t = pb.getJointState(botId, j)
torques.append(t)
self.Torques.append(torques)
if log == 1:
pb.stopStateLogging(logID)
def stop_recording(self):
p.stopStateLogging(self.rec_id)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
controlMode=ctrl_mode,
force=maxforce)
p.setJointMotorControl2(linkage,
jointInfoList['linear_motor_rod_joint_mid_left'],
targetPosition=linear_motor_mid_left_posn,
controlMode=ctrl_mode,
force=maxforce)
p.setJointMotorControl2(linkage,
jointInfoList['linear_motor_rod_joint_mid_right'],
targetPosition=linear_motor_mid_right_posn,
controlMode=ctrl_mode,
force=maxforce)
p.setJointMotorControl2(linkage,
jointInfoList['linear_motor_rod_joint_far_right'],
targetPosition=linear_motor_far_right_posn,
controlMode=ctrl_mode,
force=maxforce)
p.setJointMotorControl2(linkage,
jointInfoList['skull_joint'],
targetPosition=skull_posn,
controlMode=ctrl_mode,
force=maxforce)
if useRealTime == False:
#events = p.getMouseEvents()
#for e in events :
#if (e[0] == p.MOUSE_BUTTON_EVENT) && ()
p.stepSimulation()
p.stopStateLogging(log_id)
p.disconnect()
def main():
env = PushingEnv(ifRender=True)
# Ground truth push videos (inv)
logger.info("Recording inverse ground truth videos")
inv_ground_truth_data_path = "results/P3/inv_true_pushes.csv"
for i, push in pd.read_csv(inv_ground_truth_data_path,
index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x_1"], push["start_push_y_1"],
push["end_push_x_1"], push["end_push_y_1"]
]),
np.array([
push["start_push_x_2"], push["start_push_y_2"],
push["end_push_x_2"], push["end_push_y_2"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P3/vids/inv_true_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
# Predicted push videos (inv)
inv_predicted_data_path = "results/P3/inv_pred_pushes.csv"
logger.info("Recording inverse prediction videos")
for i, push in pd.read_csv(inv_predicted_data_path,
index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x_1"], push["start_push_y_1"],
push["end_push_x_1"], push["end_push_y_1"]
]),
np.array([
push["start_push_x_2"], push["start_push_y_2"],
push["end_push_x_2"], push["end_push_y_2"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P3/vids/inv_pred_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
# Ground truth push videos (Forward)
logger.info("Recording fwd ground truth videos")
fwd_ground_truth_data_path = "results/P3/fwd_true_pushes.csv"
for i, push in pd.read_csv(fwd_ground_truth_data_path,
index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x_1"], push["start_push_y_1"],
push["end_push_x_1"], push["end_push_y_1"]
]),
np.array([
push["start_push_x_2"], push["start_push_y_2"],
push["end_push_x_2"], push["end_push_y_2"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P3/vids/fwd_true_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
# Predicted push videos (forward)
fwd_predicted_data_path = "results/P3/fwd_pred_pushes.csv"
logger.info("Recording fwd prediction videos")
for i, push in pd.read_csv(fwd_predicted_data_path,
index_col=0).iterrows():
logger.info(f'Video {i}')
state = np.array([push["obj_x"], push["obj_y"]])
actions = [
np.array([
push["start_push_x_1"], push["start_push_y_1"],
push["end_push_x_1"], push["end_push_y_1"]
]),
np.array([
push["start_push_x_2"], push["start_push_y_2"],
push["end_push_x_2"], push["end_push_y_2"]
])
]
# Record video
pybullet.startStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4,
f"results/P3/vids/fwd_pred_pushes{i}.mp4")
env.reset_box(pos=[state[0], state[1], env.box_z])
for action in actions:
_, state = env.execute_push(*action)
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
loggingID = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
'log.bin',
maxLogDof=100)
for i in range(1000):
p.stepSimulation()
time.sleep(1. / 240.)
p.stopStateLogging(loggingID)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos, cubeOrn)
p.disconnect()
def train():
rank = MPI.COMM_WORLD.Get_rank()
sess = utils.make_gpu_session(args.num_gpu)
sess.__enter__()
if rank == 0:
logger.configure()
else:
logger.configure(format_strs=[])
workerseed = args.seed + 10000 * MPI.COMM_WORLD.Get_rank()
set_global_seeds(workerseed)
if args.use_2D_env:
config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'configs', 'husky_space7_ppo2_2D.yaml')
else:
config_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'configs', 'husky_space7_ppo2.yaml')
if args.use_2D_env:
raw_env = Husky2DNavigateEnv(gpu_idx=args.gpu_idx,
config=config_file,
pos_interval=args.pos_interval,
use_other_room=args.use_other_room)
else:
raw_env = Husky1DNavigateEnv(gpu_idx=args.gpu_idx,
config=config_file,
ob_space_range=[0.0, 40.0])
# configure environment
raw_env.reset_state_space(use_goal_info=args.use_goal_info,
use_coords_and_orn=args.use_coords_and_orn,
raycast_num=args.raycast_num,
raycast_range=args.raycast_range,
start_varying_range=args.start_varying_range)
raw_env.reset_goal_range(goal_range=args.goal_range)
env = Monitor(
raw_env,
logger.get_dir() and os.path.join(logger.get_dir(), str(rank)))
env.seed(workerseed)
gym.logger.setLevel(logging.WARN)
policy_fn = FeedbackPolicy
base_dirname = os.path.join(currentdir, "simulation_and_analysis", "rslts")
if not os.path.exists(base_dirname):
os.mkdir(base_dirname)
dir_name = "husky_ppo2_"
if args.use_feedback:
dir_name += "hr"
if args.use_real_feedback:
dir_name += "_real_feedback"
elif args.use_rich_reward:
dir_name += "rl_rich"
else:
dir_name += "rl_sparse"
dir_name = addDateTime(dir_name)
dir_name = os.path.join(base_dirname, dir_name)
if not os.path.exists(dir_name):
os.mkdir(dir_name)
hyperparams = {
"seed": args.seed,
"nsteps": args.nsteps,
"total_timesteps": args.total_timesteps,
"use_2D_env": args.use_2D_env,
"use_other_room": args.use_other_room,
"use_rich_reward": args.use_rich_reward,
"use_multiple_starts": args.use_multiple_starts,
"use_goal_info": args.use_goal_info,
"use_coords_and_orn": args.use_coords_and_orn,
"raycast_num": args.raycast_num,
"raycast_range": args.raycast_range,
"goal_range": args.goal_range,
"start_varying_range": args.start_varying_range,
"use_feedback": args.use_feedback,
"use_real_feedback": args.use_real_feedback,
"only_use_hr_until": args.only_use_hr_until,
"trans_to_rl_in": args.trans_to_rl_in,
"good_feedback_acc": args.good_feedback_acc,
"bad_feedback_acc": args.bad_feedback_acc,
"ppo_lr": args.ppo_lr,
"ppo_batch_size": args.ppo_batch_size,
"ppo_minibatch_size": args.ppo_minibatch_size,
"init_rl_importance": args.init_rl_importance,
"ent_coef": args.ent_coef,
"gamma": args.gamma,
"lambda": args.lam,
"cliprange": args.cliprange,
"max_grad_norm": args.max_grad_norm,
"ppo_noptepochs": args.ppo_noptepochs,
"feedback_lr": args.feedback_lr,
"feedback_batch_size": args.feedback_batch_size,
"feedback_minibatch_size": args.feedback_minibatch_size,
"feedback_noptepochs": args.feedback_noptepochs,
"min_feedback_buffer_size": args.min_feedback_buffer_size,
"feedback_training_prop": args.feedback_training_prop,
"feedback_training_new_prop": args.feedback_training_new_prop,
"hf_loss_type": args.hf_loss_type,
"hf_loss_param": args.hf_loss_param,
"feedback_use_mixup": args.feedback_use_mixup,
"pos_interval": args.pos_interval,
"use_embedding": raw_env._use_embedding,
"use_raycast": raw_env._use_raycast,
"offline": raw_env.config['offline']
}
param_fname = os.path.join(dir_name, "param.json")
with open(param_fname, "w") as f:
json.dump(hyperparams, f, indent=4, sort_keys=True)
hf_loss_param = [
float(x) for x in args.hf_loss_param.split(",") if x != ""
]
video_name = os.path.join(dir_name, "video.mp4")
p_logging = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, video_name)
performance = learn(
policy=policy_fn,
env=env,
raw_env=raw_env,
use_2D_env=args.use_2D_env,
use_other_room=args.use_other_room,
use_multiple_starts=args.use_multiple_starts,
use_rich_reward=args.use_rich_reward,
use_feedback=args.use_feedback,
use_real_feedback=args.use_real_feedback,
only_use_hr_until=args.only_use_hr_until,
trans_to_rl_in=args.trans_to_rl_in,
nsteps=args.nsteps,
total_timesteps=args.total_timesteps,
ppo_lr=args.ppo_lr,
cliprange=args.cliprange,
max_grad_norm=args.max_grad_norm,
ent_coef=args.ent_coef,
gamma=args.gamma,
lam=args.lam,
ppo_noptepochs=args.ppo_noptepochs,
ppo_batch_size=args.ppo_batch_size,
ppo_minibatch_size=args.ppo_minibatch_size,
init_rl_importance=args.init_rl_importance,
feedback_lr=args.feedback_lr,
feedback_noptepochs=args.feedback_noptepochs,
feedback_batch_size=args.feedback_batch_size,
feedback_minibatch_size=args.feedback_minibatch_size,
min_feedback_buffer_size=args.min_feedback_buffer_size,
feedback_training_prop=args.feedback_training_prop,
feedback_training_new_prop=args.feedback_training_new_prop,
hf_loss_type=args.hf_loss_type,
hf_loss_param=hf_loss_param,
feedback_use_mixup=args.feedback_use_mixup,
good_feedback_acc=args.good_feedback_acc,
bad_feedback_acc=args.bad_feedback_acc,
log_interval=1,
save_interval=5,
reload_name=None,
base_path=dir_name)
p.stopStateLogging(p_logging)
performance_fname = os.path.join(dir_name, "performance.p")
with open(performance_fname, "wb") as f:
pickle.dump(performance, f)
import matplotlib.pyplot as plt
plt.figure()
plt.plot(performance["train_acc"], label="train_acc")
plt.plot(performance["train_true_acc"], label="train_true_acc")
plt.plot(performance["valid_acc"], label="valid_acc")
plt.title("feedback acc: {}".format(args.good_feedback_acc))
plt.legend()
plt.ylim([0, 1])
plt.savefig(os.path.join(dir_name, "acc.jpg"), dpi=300)
plt.close('all')
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)
import pybullet as p
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "LOG00048.TXT",
[quadruped])
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stepSimulation()
p.stopStateLogging(logId)
def stop_video_recording(self):
if self.record_video:
pybullet.stopStateLogging(pybullet.STATE_LOGGING_VIDEO_MP4)
test()
print("Testing DIRECT w/o shadow")
test(shadow=0)
with BulletSim(pybullet.GUI):
print("Testing GUI") # could have OpenGL?
test()
print("Testing GUI w/o shadow") # could have OpenGL?
test(shadow=0)
server_bin = "../../../build_cmake/examples/ExampleBrowser/App_ExampleBrowser"
server_f = lambda : subprocess.run([server_bin],shell=True)
server = Process(target=server_f)
#server.start()
'''
with BulletSim(pybullet.GUI):
logId = pybullet.startStateLogging(
pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
print("Testing SHARED")
test()
print("Testing SHARED w/o shadow")
test(shadow=0)
pybullet.stopStateLogging(logId)
# server.join()
def evaluate(parameters, realtime, guiEnabled, recordEnabled):
# Clean up old torque lists and angle records
global torqueList0
global toruqeList2
global hoppingHeightList
global pitchAngleList
global rewardList
global connected
global stepRewardList
if recordEnabled:
torqueList0 = []
torqueList2 = []
hoppingHeightList = []
pitchAngleList = []
rewardList = []
if guiEnabled:
p.disconnect()
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
if (len(sys.argv) > 1) and (str(sys.argv[1]) == '-r'):
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.ER_TINY_RENDERER, 0)
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1)
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
fileName=(str(os.getcwd()) + '/recordedVideos/' +
str(strftime("%Y-%m-%d%H:%M:%S", gmtime())) +
'.mp4'))
else:
if connected:
p.resetSimulation()
else:
p.connect(p.DIRECT)
connected = True
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
robotStartPos = [0, 0, 2.2]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
robotId = p.loadURDF("3d_hop_only.urdf",
robotStartPos,
robotStartOrientation,
useFixedBase=0)
robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)
p.setJointMotorControlArray(robotId, [0, 1, 2],
p.POSITION_CONTROL,
targetPositions=[0.0, 0, 0],
forces=[0, 0, 0])
n = 0
energyCost = sum(map(abs, parameters))
parameters = np.repeat(parameters, TimeSlotSteps)
torqueList = list(map(convertLogTorqueToLinearTorque, parameters))
smoothedTorqueList = scipy.ndimage.filters.gaussian_filter1d(torqueList, 2)
totalTorqueList = list(smoothedTorqueList)
t0 = totalTorqueList
totalCost = 0
for i in range(len(t0)):
stepRobotSimulation([t0[i]], robotId, planeId, recordEnabled)
# speedCost += sum(map(abs, list(p.getBaseVelocity(robotId)[1])))
positionCost = p.getBasePositionAndOrientation(robotId)[0][0]
# postureCost = p.getEulerFromQuaternion(p.getBasePositionAndOrientation(robotId)[1])[1]
postureCost = abs(p.getBasePositionAndOrientation(robotId)[1][1])
# postureCosty += abs(p.getBasePositionAndOrientation(robotId)[1][1])
heightCost = p.getBasePositionAndOrientation(robotId)[0][2]
# positionCost += abs(p.getBasePositionAndOrientation(robotId)[0][0])
torquePenulaty = abs(t0[i])
contactPoint = 0
contact = p.getContactPoints(bodyA=robotId, linkIndexA=-1)
if len(contact) > 0:
contactPoint = 1
tempList = 100 * heightCost - 0.5 * torquePenulaty
totalCost += tempList
if recordEnabled:
stepRewardList.append(tempList)
rewardList.append(totalCost)
if recordEnabled:
p.stopStateLogging(0)
return totalCost / len(t0),
def restore(self):
if self.log_id is not None:
p.stopStateLogging(self.log_id)
for i in range (numSteps): p.stepSimulation() for j in range (8): results = p.rayTestBatch(rayFrom,rayTo,j+1) #for i in range (10): # p.removeAllUserDebugItems() if (useGui): if (not replaceLines): p.removeAllUserDebugItems() for i in range (numRays): hitObjectUid=results[i][0] if (hitObjectUid<0): hitPosition =[0,0,0] p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor,replaceItemUniqueId=rayIds[i]) else: hitPosition = results[i][3] p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor,replaceItemUniqueId=rayIds[i]) #time.sleep(1./240.) if (not useGui): p.stopStateLogging(timingLog)
def showBestWalk(self, log=0, num=-1):
self.Disconnect()
self.RunSRV(1)
time.sleep(1)
now = datetime.now()
#sort = np.argsort(-self.fitnesses)
legt0 = self.elites[num][0]
legamp = self.elites[num][1]
legT = self.elites[num][2]
legamp2 = self.elites[num][3]
legT2 = self.elites[num][4]
#offSet = self.popBodyOffsets[popNum]
T = 0.5
simID, botId = self.setStanding(simtype=pb.SHARED_MEMORY, num=5)
botPos, botOrn = pb.getBasePositionAndOrientation(botId)
footLoc = pb.getLinkState(botId, linkIndex=3)[0][2]
heightDiff = botPos[2] - footLoc
sc = 0.25
T = abs(1.5 / (2 * self.fkine([
legt0[0] + legamp[0], legt0[1] + legamp[1], legt0[2] + legamp[2],
legt0[3] + legamp[3]
])[0] / 1000))
if T > 1.5: T = 1.5
if T > 1.5: T = 1.5
for i in range(10000):
t = float(i) * (self.T_fixed)
angles = np.array([
legt0[0] + sc * legamp[0] * np.sin(2 * np.pi *
(t + legT[0]) / T) +
sc * legamp2[0] * (np.sin(2 * np.pi *
(2 * t + legT2[0]) / T)), legt0[1] +
sc * legamp[1] * np.sin(2 * np.pi *
(t + legT[1]) / T) + sc * legamp2[1] *
(np.sin(2 * np.pi * (2 * t + legT2[1]) / T)), legt0[2] +
sc * legamp[2] * np.sin(2 * np.pi *
(t + legT[2]) / T) + sc * legamp2[2] *
(np.sin(2 * np.pi * (2 * t + legT2[2]) / T)), legt0[3] +
sc * legamp[3] * np.sin(2 * np.pi *
(t + legT[3]) / T) + sc * legamp2[3] *
(np.sin(2 * np.pi * (2 * t + legT2[3]) / T)), legt0[0] +
sc * legamp[0] * np.sin(np.pi + 2 * np.pi *
(t + legT[0]) / T) + sc * legamp2[0] *
(np.sin(np.pi + 2 * np.pi * (2 * t + legT2[0]) / T)),
-(legt0[1] + sc * legamp[1] * np.sin(np.pi + 2 * np.pi *
(t + legT[1]) / T) +
sc * legamp2[1] * (np.sin(np.pi + 2 * np.pi *
(2 * t + legT2[1]) / T))),
-(legt0[2] + sc * legamp[2] * np.sin(np.pi + 2 * np.pi *
(t + legT[2]) / T) +
sc * legamp2[2] * (np.sin(np.pi + 2 * np.pi *
(2 * t + legT2[2]) / T))),
-(legt0[3] + sc * legamp[3] * np.sin(np.pi + 2 * np.pi *
(t + legT[3]) / T) +
sc * legamp2[3] * (np.sin(np.pi + 2 * np.pi *
(2 * t + legT2[3]) / T)))
]) * np.pi / 180
if i == 0:
if log == 1:
logID = pb.startStateLogging(
loggingType=pb.STATE_LOGGING_VIDEO_MP4,
fileName="walk_{}.mp4".format(
now.strftime("%m%d%Y_%H.%M.%S")))
#pb.disconnect(simID)
#simID, botId = self.setStanding(simtype = pb.DIRECT, num = 3, anglesset = angles, height = heightDiff)
else:
if t > 1 and i > 10:
sc = 1
self.maxforce == 500
else:
self.maxforce = 500
pb.setJointMotorControlArray(botId,
range(8),
controlMode=pb.POSITION_CONTROL,
targetPositions=angles,
targetVelocities=[0] * 8,
forces=[self.maxforce] * 8,
physicsClientId=simID)
self.Step(steps=1, sleep=1, cid=simID, botID=botId)
self.AdjustCamera(botID=botId, cid=simID)
botPos, botOrn = pb.getBasePositionAndOrientation(botId)
if botPos[2] < 0.4 or botPos[2] > 2:
break
print(botPos)
if log == 1:
pb.stopStateLogging(loggingId=logID)
def record_trajectory(camera_positions):
env = gym.make(args.env_name)
# model
actor_critic = torch.load(os.path.join(args.model_dir, args.model_name+ ".pt"))
actor_critic.eval()
obs_shape = (env.observation_space.shape[0] * args.num_stack,)
current_state = torch.zeros(1, *obs_shape)
def update_current_state(state):
shape_dim0 = env.observation_space.shape[0]
state = torch.from_numpy(state).float()
if args.num_stack > 1:
current_state[:, :-shape_dim0] = current_state[:, shape_dim0:]
current_state[:, -shape_dim0:] = state
# building the environment
env.render('human')
state = env.reset()
update_current_state(state)
# identifying robot first link frame id
FRAME_ID = -1
for i in range(bullet.getNumBodies()):
if (bullet.getBodyInfo(i)[0].decode() == "lbr_iiwa_link_0"):
FRAME_ID = i
assert(FRAME_ID > -1)
human_pos, humanOrn = bullet.getBasePositionAndOrientation(FRAME_ID)
for trajectory_index in range(0,20):
done = False
distance = camera_positions[0]['distance']
yaw = camera_positions[0]['yaw']
pitch = camera_positions[0]['pitch']
file_name = "{}-trajectory_{}-position.mp4".format(trajectory_index, 0)
bullet.resetDebugVisualizerCamera(distance, yaw, pitch, human_pos)
record_id = bullet.startStateLogging(bullet.STATE_LOGGING_VIDEO_MP4, args.video_dir + file_name)
actions = []
total_reward = 0.
while not(done):
value, action = actor_critic.act(Variable(current_state, volatile=True),deterministic=True)
action = action.data.cpu().numpy()[0]
state, reward, done, _ = env.step(action)
total_reward += reward
# save action: repeat_trajectory
actions.append(action)
update_current_state(state)
bullet.stopStateLogging(record_id)
crop_video(file_name)
print("---NEW TRAJECTORY BUILT---")
print("Lastreward", reward)
print("Total reward", total_reward)
# camera positions
for position_index, position in enumerate(camera_positions[1:]):
state = env.reset()
file_name = "{}-trajectory_{}-position.mp4".format(trajectory_index, position_index + 1)
repeat_accuracy = repeat_trajectory(env, human_pos, actions, position, file_name)
print("Repeat Accuracy: ", repeat_accuracy)
state = env.reset()
import pybullet as p
import time
p.connect(p.GUI)
p.resetSimulation()
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,
"loadingBenchVR.json")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
print("load plane.urdf")
p.loadURDF("plane.urdf")
print("load r2d2.urdf")
p.loadURDF("r2d2.urdf", 0, 0, 1)
print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf")
for i in range(100):
p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(timinglog)
print("stopped state logging")
p.getCameraImage(320, 200)
while (1):
p.stepSimulation()
if recordVideo:
videoFile = Helper.findLog(prefix + '.mp4')
videoLogId = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, videoFile)
t = 0
n = 0
while True:
p.stepSimulation()
time.sleep(1 / 240)
controlSignal = RobotControl.realTimeControl(env.robotId, plan, n)
env.control(controlSignal)
n = n + 1
env.cameraControl()
RobotControl.addDebugItems()
t += 1 / 240
reachEnd = p.getContactPoints(bodyA=env.robotId, bodyB=env.targetId)
if reachEnd:
break
if recordVideo:
p.stopStateLogging(videoLogId)
with open(Helper.findLog(prefix + '.txt'), 'w') as f:
f.writelines(f'Total time: {t}')
print('Congratulatons!')
print(f'Total time: {t}')
def main(_):
robot = ROBOT_CLASS_MAP[FLAGS.robot_type]
motor_control_mode = MOTOR_CONTROL_MODE_MAP[FLAGS.motor_control_mode]
env = env_builder.build_regular_env(robot,
motor_control_mode=motor_control_mode,
enable_rendering=True,
on_rack=FLAGS.on_rack,
wrap_trajectory_generator=False)
action_low, action_high = env.action_space.low/3., env.action_space.high/3.
action_median = (action_low + action_high) / 2.
dim_action = action_low.shape[0]
action_selector_ids = []
for dim in range(dim_action):
action_selector_id = p.addUserDebugParameter(paramName='dim{}'.format(dim),
rangeMin=action_low[dim],
rangeMax=action_high[dim],
startValue=action_median[dim])
action_selector_ids.append(action_selector_id)
action_selector_id = p.addUserDebugParameter(paramName='height',
rangeMin=0,
rangeMax=2,
startValue=1)
action_selector_ids.append(action_selector_id)
action_selector_id = p.addUserDebugParameter(paramName='angle',
rangeMin=-3.14/2,
rangeMax=3.14/2,
startValue=0)
action_selector_ids.append(action_selector_id)
if FLAGS.video_dir:
log_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, FLAGS.video_dir)
#for _ in tqdm(range(800)):
last_state_vec = []
while True:
state_vec = []
rot = Rotation.from_euler('zxy', [0,0,env.pybullet_client.readUserDebugParameter(
action_selector_ids[-1])])
base = Rotation.from_quat([0.5,0.5,0.5,0.5])
final_quat = (rot*base).as_quat()
final_pos = [0,0,env.pybullet_client.readUserDebugParameter(
action_selector_ids[-2])]
env.moveRack(final_pos, final_quat)
state_vec.extend(final_pos)
state_vec.extend(final_quat)
action = np.zeros(dim_action)
for dim in range(dim_action):
action[dim] = env.pybullet_client.readUserDebugParameter(
action_selector_ids[dim])
state_vec.append(action[dim])
env.step(action)
if last_state_vec != state_vec:
print(state_vec)
last_state_vec = state_vec
if FLAGS.video_dir:
p.stopStateLogging(log_id)
