def _configure_physics_engine(self):
mode_dict = {Mode.GUI: p.GUI, Mode.DIRECT: p.DIRECT}
# if the mode we gave is in the dict then use it, otherwise use the given mode value as is
mode = mode_dict.get(self.mode) or self.mode
self.physics_client = p.connect(
mode) # p.GUI for GUI or p.DIRECT for non-graphical version
# disable useless menus on the left and right
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
if self.log_video:
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4, "{}_{}.mp4".format(
datetime.now().strftime('%Y_%m_%d_%H_%M_%S'),
self.randseed))
# use data provided by PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
if self.realtime:
p.setRealTimeSimulation(True)
else:
p.setRealTimeSimulation(False)
p.setTimeStep(self.sim_step_s)
def resetRobot(self):
if self.SIM_READDY == 1:
p.restoreState(self.initial_frame, physicsClientId=self.client_id)
if self.RANDSET == 1:
rnd_pos = np.random.uniform(
low=np.array(self.pos_limits_low),
high=np.array(self.pos_limits_high)).tolist()
rnd_vel = np.random.uniform(
low=np.array(self.pos_limits_low),
high=np.array(self.pos_limits_high)).tolist()
for i in range(p.getNumJoints(self.body_id)):
p.resetJointState(self.body_id,
i,
rnd_pos[i],
targetVelocity=rnd_vel[i],
physicsClientId=self.client_id) #####
if self.LOGDATA == 1:
self.state_seq = []
self.state_dot_seq = []
self.actions_seq = []
print("Starting video")
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
self.video_path +
"/Pendulum_training{}.mp4".format(self.sim_number),
physicsClientId=self.client_id)
self.observeState()
else:
raise Exception("Simulation not set up")
def __init__(self, dt=1e-3):
self.dt = dt
self.omega_xyz = None
self.omega = None
self.v = None
self.record_rt = False # record video in real time
self.record_stepped = False # record video in sim steps # TODO: Deprecated?
# print(p.getJointInfo(bot, 3))
# Record Video in real time
if self.record_rt is True:
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "file1.mp4")
if self.record_stepped is True:
output = "video.avi"
img = pyautogui.screenshot()
img = cv2.cvtColor(np.array(img), cv2.COLOR_RGB2BGR)
# get info from img
height, width, channels = img.shape
# Define the codec and create VideoWriter object
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
self.out = cv2.VideoWriter(output, fourcc, 20.0, (width, height))
p.setTimeStep(self.dt)
p.setRealTimeSimulation(useRealTime)
# Disable the default velocity/position motor:
for i in range(p.getNumJoints(bot)):
p.setJointMotorControl2(bot, i, p.VELOCITY_CONTROL, force=0.5)
# force=1 allows us to easily mimic joint friction rather than disabling
p.enableJointForceTorqueSensor(bot, i,
1) # enable joint torque sensing
def _resetClient(self):
if (self.physicsClientId >= 0):
if self.full_reset:
p.resetSimulation()
return
self.ownsPhysicsClient = True
self.physicsClientId = p.connect(self.connection_mode)
if self.isRender:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
else:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
if self.videoFile:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW,1)
# p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
# p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, self.videoFile)
if not self.full_reset:
self._loadSimulation()
def reset(self):
obs = self.env.reset()
self._episode_idx += 1
import pybullet
pybullet.startStateLogging(
pybullet.STATE_LOGGING_VIDEO_MP4,
os.path.join(self._dirname, '{}.mp4'.format(self._episode_idx)))
return obs
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 start_video_recording(self, file_name):
"""Starts video recording and save as a mp4 file.
Args:
file_name (str): The absolute path of the file to be saved.
"""
self.file_name = file_name
pybullet.startStateLogging(
pybullet.STATE_LOGGING_VIDEO_MP4, self.file_name
)
def start_engine(): connection_id = p.connect(p.SHARED_MEMORY) if connection_id < 0: p.connect(p.GUI) p.setAdditionalSearchPath(p_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=10) p.setTimeStep(1. / 120.) p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "log.json") p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1)
def setupSim(self):
if self.SIM_READDY == 1:
raise Exception("Simulation already set up")
if self.GUI_ENABLED == 1:
self.client_id = self.physicsClient = p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=0.8,
cameraYaw=90,
cameraPitch=-10,
cameraTargetPosition=[0, 0, 0],
physicsClientId=self.client_id)
if self.LOGDATA == 1:
self.state_seq = []
self.state_dot_seq = []
self.actions_seq = []
print("Starting video")
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
self.video_path +
"/Pendulum_training{}.mp4".format(self.sim_number),
physicsClientId=self.client_id)
else:
self.client_id = self.physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(self.urdf_path)
p.setGravity = (self.gravity[0], self.gravity[1], self.gravity[2],
self.client_id)
self.SIM_READDY = 1
p.setTimeStep(self.time_step, self.client_id)
self.loadRobot()
self.pos_limits_high = [
np.pi for i in range(p.getNumJoints(self.body_id))
] #rad
self.pos_limits_low = [
-np.pi for i in range(p.getNumJoints(self.body_id))
]
self.vel_limits_high = [
0.7 * np.pi for i in range(p.getNumJoints(self.body_id))
]
self.vel_limits_low = [
-0.7 * np.pi for i in range(p.getNumJoints(self.body_id))
]
p.stepSimulation(physicsClientId=self.client_id)
self.initial_frame = p.saveState(physicsClientId=self.client_id)
self.observeState()
print("Simulation is ready")
def reset(self):
self._env_step_counter = 0
p.resetSimulation()
p.setGravity(0, 0, -9.8) # m/s^2
p.setTimeStep(0.01) # sec
plane_id = p.loadURDF(
'plane.urdf') # This one comes with bullet already
# Set pose so the palm points upwards
start_position = [0, 0, 0.2]
start_orientation = p.getQuaternionFromEuler([-math.pi / 2.0, 0, 0])
path = os.path.abspath(os.path.dirname(__file__)) # This file's path
self.bot_id = p.loadURDF(os.path.join(path, 'shadowhand.urdf'),
start_position, start_orientation)
# STATE_LOGGING_VIDEO_MP4 requires file to be '.mp4' and ffmpeg installed
# STATE_LOGGING_GENERIC_ROBOT requires maxLogDof for robots with +12 DoF
log_file_name = datetime.datetime.now().strftime('%b%d_%H-%M-%S')
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
'./logs/' + log_file_name,
objectUniqueIds=[self.bot_id],
maxLogDof=p.getNumJoints(self.bot_id))
position, orientation = p.getBasePositionAndOrientation(self.bot_id)
euler_orientation = p.getEulerFromQuaternion(
orientation) # get pitch, roll, yaw
num_joints = p.getNumJoints(self.bot_id)
print('position:', position)
print('euler_orientation:', euler_orientation)
print('num_joints:', num_joints)
for i in range(num_joints):
info_joint = p.getJointInfo(self.bot_id, i)
print('\njoint_id:', info_joint[0])
print('name:', info_joint[1].decode('UTF-8'))
print('type:', info_joint[2])
print('lower:', info_joint[8])
print('upper:', info_joint[9])
print('child link name:', info_joint[12].decode('UTF-8'))
print('parent link id:', info_joint[16])
self._observation = [0.0, 0.0]
return np.array(self._observation)
def video(self):
# to record a video from this camera uncomment this line
if self.record:
if not os.path.exists('Videos'):
os.makedirs('Videos')
DateDir = time.strftime("%Y%m%d")
if not os.path.exists('Videos/' + DateDir):
os.makedirs('Videos/' + DateDir)
Count = len(os.listdir('Videos/' + DateDir))
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4, "./Videos/" + DateDir + "/" +
self.videoname + "_" + str(Count + 1) + ".MP4")
else:
pass
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 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 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 resetRobot(self):
if self.SIM_READDY == 1:
p.restoreState(self.initial_frame, physicsClientId=self.client_id)
if self.LOGDATA == 1:
self.state_seq = []
self.state_dot_seq = []
self.actions_seq = []
print("Starting video")
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
"1D_pendulum/Video/Pendulum_training{}.mp4".format(
self.sim_number),
physicsClientId=self.client_id)
self.observeState()
else:
raise Exception("Simulation not set up")
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 start_recording(self, save_dir):
if not os.path.exists(save_dir):
os.mkdir(save_dir)
now = datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
file = f'{save_dir}/{now}.mp4'
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self.rec_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, file)
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 __init__(self, options={}):
self.options = options
#print("options",options)
self.steps = 0
currentdir = os.path.dirname(
os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
if 'render' in self.options and self.options['render'] == True:
pb.connect(pb.GUI)
else:
pb.connect(pb.DIRECT)
if 'video' in self.options and self.options['video'] != True:
video_name = self.options[
'video'] #how to default to video.mp4 is empty
pb.startStateLogging(pb.STATE_LOGGING_VIDEO_MP4, video_name)
if 'debug' in self.options and self.options['debug'] == True:
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 1)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER, 1)
else:
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
pb.configureDebugVisualizer(pb.COV_ENABLE_TINY_RENDERER, 0)
pb.setGravity(0, 0, -10)
pb.setRealTimeSimulation(0)
self.move = 0.05 # 0.01
self.load_object()
self.load_agent()
self.pi = 3.1415926535
self.pinkId = 0
self.middleId = 1
self.indexId = 2
self.thumbId = 3
self.ring_id = 4
self.indexEndID = 21 # Need get position and orientation from index finger parts
self.offset = 0.02 # Offset from basic position
self.downCameraOn = False
self.prev_distance = 10000000
self.action_space = spaces.Discrete(8)
self.touch_width = 200
self.touch_height = 200
self.observation_space = spaces.Box(
0, 1, [self.touch_width, self.touch_height])
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 setup(self, beads=True, cup_offset=(0, 0, 0), new_bead_mass=None, table=False, cup_factor=4):
if self.real_init:
# setup world
self.is_real_time = 0
p.setRealTimeSimulation(self.is_real_time)
p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "/home/gridsan/alagrassa/scoop_demo.mp4")
g = 9.8
p.setGravity(0, 0, -g)
if table:
self.table = p.loadURDF(path + "table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
self.cupStartPos = (-0.04, -0.10, 0.708)
self.cupStartPos = (-0.17, 0, 0.6544)
else:
self.cupStartPos = (0, 0, 0)
self.cupStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
if self.visualize:
self.cupID = p.loadURDF(path + "urdf/cup/" + self.cup_name, self.cupStartPos, self.cupStartOrientation,
globalScaling=k * cup_factor)
else:
self.cupID = p.loadURDF(path + "urdf/cup/" + self.cup_name, self.cupStartPos, self.cupStartOrientation,
globalScaling=k * cup_factor)
blacken(self.cupID)
self.cup_constraint = p.createConstraint(self.cupID, -1, -1, -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0],
[0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 1])
p.changeConstraint(self.cup_constraint, self.cupStartPos, self.cupStartOrientation, maxForce=1000)
p.changeDynamics(self.cupID, -1, mass=5, lateralFriction=0.1, spinningFriction=0.1, rollingFriction=0.1,
restitution=0.7)
if beads:
if new_world:
self.bullet_id = p.saveState()
if new_bead_mass is not None:
[p.changeDynamics(droplet, -1, mass=float(new_bead_mass), lateralFriction=0.99,
spinningFriction=0.99, rollingFriction=0.99) for droplet in self.droplets]
# to be realistic
# p.setTimeStep(1/1200.)
p.setTimeStep(1 / 300.)
self.real_init = False
else:
self.real_init = True
p.resetSimulation()
self.setup(new_bead_mass=new_bead_mass, table=self.table)
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 __init__(self, path):
self.path = path
if path is None:
self.log_id = None
else:
name, ext = os.path.splitext(path)
assert ext == '.mp4'
# STATE_LOGGING_PROFILE_TIMINGS, STATE_LOGGING_ALL_COMMANDS
# p.submitProfileTiming("pythontest")
self.log_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4,
fileName=path,
physicsClientId=CLIENT)
def setupSim(self):
if self.SIM_READDY == 1:
raise Exception("Simulation already set up")
if self.GUI_ENABLED == 1:
self.client_id = self.physicsClient = p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=0.8,
cameraYaw=80,
cameraPitch=-10,
cameraTargetPosition=[0, 0, 0],
physicsClientId=self.client_id)
if self.LOGDATA == 1:
self.state_seq = []
self.state_dot_seq = []
self.actions_seq = []
print("Starting video")
p.startStateLogging(
p.STATE_LOGGING_VIDEO_MP4,
"1D_pendulum/Video/Pendulum_training{}.mp4".format(
self.sim_number),
physicsClientId=self.client_id)
else:
self.client_id = self.physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath("/urdf")
p.setGravity = (self.gravity[0], self.gravity[1], self.gravity[2],
self.client_id)
self.SIM_READDY = 1
self.loadRobot()
p.stepSimulation(physicsClientId=self.client_id)
self.initial_frame = p.saveState(physicsClientId=self.client_id)
self.observeState()
print("Simulation is ready")
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 init_sim(numObjects=0, numRobots=0): # PYBULLET INIT
physClient = pbc.BulletClient(connection_mode=p.GUI)
physClient.setAdditionalSearchPath(pybullet_data.getDataPath())
physClient.setGravity(0, 0, -9.807)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
physClient.resetDebugVisualizerCamera(4, 0, -89, [0, 0, 0]) # overhead camera perspective
# physClient.resetDebugVisualizerCamera(6, 0, -89, [2.5, 2.5, 0]) # overhead camera perspective (pos grid)
# (width, height, viewMat, projMat, _, _, _, _, _, _, _, _) = p.getDebugVisualizerCamera(physClient)
p.startStateLogging(loggingType=p.STATE_LOGGING_VIDEO_MP4, fileName="output.mp4".format(SIM_SEQUENCE))
# LOAD PLANE
lib.load_plane(physClient)
# LOAD OBJECTS
if numObjects:
for obj in lib.load_objects(physClient, generate_obj_coordinates(DEFAULT_BOUNDS, numObjects)):
objects.append(obj)
# LOAD ROBOTS
if numRobots:
for robot in lib.load_robots(physClient, generate_robot_coordinates(DEFAULT_BOUNDS, numRobots)):
robots.append(robot)
return physClient
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 _init(self):
self.bullet_cid = pb.connect(pb.GUI if self._render else pb.DIRECT)
pb.setTimeStep(self.info["time_step"], physicsClientId=self.bullet_cid)
pb.setGravity(self.info["gravity"][0],
self.info["gravity"][1],
self.info["gravity"][2],
physicsClientId=self.bullet_cid)
if self.recorder:
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI,
0,
physicsClientId=self.bullet_cid)
self.video_logger = pb.startStateLogging(
pb.STATE_LOGGING_VIDEO_MP4,
self.recorder,
physicsClientId=self.bullet_cid)
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)
import pybullet as p
import time
p.connect(p.GUI)
t = time.time()+0.1
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
while (time.time()<t):
p.submitProfileTiming("pythontest")
p.stopStateLogging(logId)
]
rayTo = [
rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
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()
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
hasPrevPose = 0
useNullSpace = 0
count = 0
useOrientation = 1
useSimulation = 1
useRealTimeSimulation = 1
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 15
logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyUniqueIdA=2)
for i in xrange(5):
print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+0.1
if (useSimulation and useRealTimeSimulation==0):
p.stepSimulation()
t=0. prevPose=[0,0,0] prevPose1=[0,0,0] hasPrevPose = 0 useNullSpace = 0 count = 0 useOrientation = 1 useSimulation = 1 useRealTimeSimulation = 1 p.setRealTimeSimulation(useRealTimeSimulation) #trailDuration is duration (in seconds) after debug lines will be removed automatically #use 0 for no-removal trailDuration = 15 logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) while 1: if (useRealTimeSimulation): dt = datetime.now() t = (dt.second/60.)*2.*math.pi else: t=t+0.1 if (useSimulation and useRealTimeSimulation==0): p.stepSimulation() for i in range (1): pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)] #end effector points down, not up (in case useOrientation==1) orn = p.getQuaternionFromEuler([0,-math.pi,0])
import pybullet as p
import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
#while (True):
# time.sleep(0.01)
#p.stepSimulation()
print("quadruped Id = ")
print(quadruped)
p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.bin",[quadruped])
#jump
t = 0.0
t_end = t + 100
i=0
ref_time = time.time()
while t < t_end:
if (useRealTime):
t = time.time()-ref_time
import time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(numSubSteps=1)
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
ob = objects[1]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
print("Starting benchmark")
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
for i in range(1000):
p.stepSimulation()
p.stopStateLogging(logId)
print("ended benchmark")
import pybullet as p
import math, time
import difflib,sys
numSteps = 500
numSteps2 = 30
p.connect(p.GUI, options="--width=1024 --height=768")
numObjects = 50
verbose = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
def setupWorld():
p.resetSimulation()
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
for i in range (numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
#p.changeDynamics(cube,-1,mass=100)
p.stepSimulation()
p.setGravity(0,0,-10)
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"
p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
for i in range (int(num*50)):
num=(radius*2*math.pi)/0.08
radius += 0.05/float(num)
orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
numDominoes+=1
rayHitColor = [1,0,0] rayMissColor = [0,1,0] replaceLines = True for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) if (replaceLines): rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) else: rayIds.append(-1) if (not useGui): timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") numSteps = 10 if (useGui): numSteps = 327680 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()
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
prevPosition = [None] * p.VR_MAX_CONTROLLERS
colors = [0., 0.5, 0.5] * p.VR_MAX_CONTROLLERS
widths = [3] * p.VR_MAX_CONTROLLERS
a = [0, 0, 0]
#use a few default colors
colors[0] = [0, 0, 0]
colors[1] = [0.5, 0, 0]
colors[2] = [0, 0.5, 0]
colors[3] = [0, 0, 0.5]
colors[4] = [0.5, 0.5, 0.]
colors[5] = [.5, .5, .5]
p.startStateLogging(p.STATE_LOGGING_VR_CONTROLLERS, "vr_hmd.bin", deviceTypeFilter=p.VR_DEVICE_HMD)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "generic_data.bin")
p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "contact_points.bin")
while True:
events = p.getVREvents(p.VR_DEVICE_HMD + p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
pos = e[POSITION]
mat = p.getMatrixFromQuaternion(e[ORIENTATION])
dir0 = [mat[0], mat[3], mat[6]]
dir1 = [mat[1], mat[4], mat[7]]
dir2 = [mat[2], mat[5], mat[8]]
lineLen = 0.1
dir = [-mat[2], -mat[5], -mat[8]]
to = [pos[0] + lineLen * dir[0], pos[1] + lineLen * dir[1], pos[2] + lineLen * dir[2]]
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)
p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
width = 256
height = 256
pixels = [255] * width * height * 3
colorR = 0
colorG = 0
colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
blue = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
for i in range(100000):
p.stepSimulation()
for i in range(width):
for j in range(height):
pixels[(i + j * width) * 3 + 0] = i
pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
pixels[(i + j * width) * 3 + 2] = blue
blue = blue + 1
p.changeTexture(texUid, pixels, width, height)
start = time.time()
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL)
end = time.time()
print("rendering duraction")
print(end - start)
p.stopStateLogging(logId)
import pybullet as p
import time
p.connect(p.GUI)
logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS,"commandLog.bin")
p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf",[0,0,1])
p.stopStateLogging(logId)
p.resetSimulation();
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS,"commandLog.bin")
while(p.isConnected()):
time.sleep(1./240.)
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
[0.710965, 0.218117, 0.519402, -0.420923])
jointPositions = [
-0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000,
-0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000,
0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000,
-0.007374, 0.000000
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
print("Starting benchmark")
fileName = "pybullet_humanoid_timings.json"
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, fileName)
for i in range(1000):
p.stepSimulation()
p.stopStateLogging(logId)
print("ended benchmark")
print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
