def __init__(self, disp=False, hz=240):
"""Creates OpenAI Gym-style environment with PyBullet.
Args:
disp: show environment with PyBullet's built-in display viewer
hz: PyBullet physics simulation step speed. Set to 480 for deformables.
"""
self.pix_size = 0.003125
self.obj_ids = {'fixed': [], 'rigid': [], 'deformable': []}
self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
self.agent_cams = cameras.RealSenseD415.CONFIG
# Start PyBullet.
p.connect(p.GUI if disp else p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(enableFileCaching=0)
assets_path = os.path.dirname(os.path.abspath(__file__))
p.setAdditionalSearchPath(assets_path)
p.setTimeStep(1. / hz)
# If using --disp, move default camera closer to the scene.
if disp:
target = p.getDebugVisualizerCamera()[11]
p.resetDebugVisualizerCamera(cameraDistance=1.1,
cameraYaw=90,
cameraPitch=-25,
cameraTargetPosition=target)
def kinectviewMat(self):
# kinectv2のカメラ座標取得
kinect = p.getLinkState(self._robonyan.robonyanUid, 56)
kinectPos, kinectOrn = kinect[0], kinect[1]
kinectPos = list(kinectPos)
kinectPos[0] += 0.2
kinectPos[1] += 0.002
kinectPos[2] += 0.05
#kinectPos = [-0.112 + 0.03153696, 0, 1.304 + 0.03153696]
kinectPos = tuple(kinectPos)
camInfo = p.getDebugVisualizerCamera()
kinectMat = p.getMatrixFromQuaternion(kinectOrn)
upVector = [0, 0, 1]
forwardVec = [kinectMat[0], kinectMat[3], kinectMat[6]]
#sideVec = [camMat[1],camMat[4],camMat[7]]
kinectUpVec = [kinectMat[2], kinectMat[5], kinectMat[8]]
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
kinectTarget = blockPos
kinectUpTarget = [
kinectPos[0] + kinectUpVec[0], kinectPos[1] + kinectUpVec[1],
kinectPos[2] + kinectUpVec[2]
]
kinectviewMat = p.computeViewMatrix(kinectPos, kinectTarget,
kinectUpVec)
return kinectviewMat
def changeCam(self):
cubePos, cubeOrn = p.getBasePositionAndOrientation(self.robot)
a = p.getDebugVisualizerCamera()
cyaw = a[8]
cpitch = a[9]
cdist = a[10]
cubePos = a[11]
keys = p.getKeyboardEvents()
#Keys to change camera
if keys.get(100): #D
cyaw += 0.1
if keys.get(97): #A
cyaw -= 0.1
if keys.get(99): #C
cpitch += 0.1
if keys.get(102): #F
cpitch -= 0.1
if keys.get(122): #Z
cdist += .01
if keys.get(120): #X
cdist -= .01
p.resetDebugVisualizerCamera(cameraDistance=cdist,
cameraYaw=cyaw,
cameraPitch=cpitch,
cameraTargetPosition=cubePos)
def __init__(self,car,use_plot=False):
self.car=car
self.camInfo = p.getDebugVisualizerCamera()
self.viewtime=-100
self.im=None
self.fig=None
self.use_plot=use_plot
def set_camera(self, pos=None, target=torch.zeros(3), dist=None):
if pos is None:
camera_params = p.getDebugVisualizerCamera(
physicsClientId=self.sim.id)
camera_forward = torch.tensor(camera_params[5])
camera_target = torch.tensor(camera_params[-1])
if dist is None:
target_dist = camera_params[-2]
else:
target_dist = dist
pos = camera_target - target_dist * camera_forward
if isinstance(target, Object):
target = target.get_pos()
pos = totensor(pos)
target = totensor(target)
disp = target - pos
dist = disp.norm()
yaw = np.arctan2(-disp[0], disp[1]) * 180 / np.pi
pitch = np.arctan2(disp[2],
np.sqrt(disp[0]**2 + disp[1]**2)) * 180 / np.pi
p.resetDebugVisualizerCamera(cameraDistance=dist,
cameraYaw=yaw,
cameraPitch=pitch,
cameraTargetPosition=target.tolist(),
physicsClientId=self.sim.id)
def __init__(self, disp=False, hz=240):
"""Creates OpenAI gym-style env with support for PyBullet threading.
Args:
disp: Whether or not to use PyBullet's built-in display viewer.
Use this either for local inspection of PyBullet, or when
using any soft body (cloth or bags), because PyBullet's
TinyRenderer graphics (used if disp=False) will make soft
bodies invisible.
hz: Parameter used in PyBullet to control the number of physics
simulation steps. Higher values lead to more accurate physics
at the cost of slower computaiton time. By default, PyBullet
uses 240, but for soft bodies we need this to be at least 480
to avoid cloth intersecting with the plane.
"""
self.ee = None
self.task = None
self.objects = []
self.running = False
self.fixed_objects = []
self.pix_size = 0.003125
self.homej = np.array([-1, -0.5, 0.5, -0.5, -0.5, 0]) * np.pi
self.primitives = {'push': self.push,
'sweep': self.sweep,
'pick_place': self.pick_place}
# Set default movej timeout limit. For most tasks, 15 is reasonable.
self.t_lim = 15
# From Xuchen: need this for using any new deformable simulation.
self.use_new_deformable = True
self.hz = hz
# Start PyBullet.
p.connect(p.GUI if disp else p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setPhysicsEngineParameter(enableFileCaching=0)
assets_path = os.path.dirname(os.path.abspath(__file__))
p.setAdditionalSearchPath(assets_path)
# Check PyBullet version (see also the cloth/bag task scripts!).
p_version = pkg_resources.get_distribution('pybullet').version
tested = ['2.8.4', '3.0.4']
assert p_version in tested, f'PyBullet version {p_version} not in {tested}'
# Move the camera a little closer to the scene. Most args are not used.
# PyBullet defaults: yaw=50 and pitch=-35.
if disp:
_, _, _, _, _, _, _, _, _, _, _, target = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(
cameraDistance=1.0,
cameraYaw=90,
cameraPitch=-25,
cameraTargetPosition=target,)
# Control PyBullet simulation steps.
self.step_thread = threading.Thread(target=self.step_simulation)
self.step_thread.daemon = True
self.step_thread.start()
def camera_look_at(self, target):
"""Control the look of the visualizer camera
Arguments:
target {tuple} -- target as (x,y,z) tuple
"""
if self.gui:
params = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(params[10], params[8], params[9],
target)
def getRayFromTo(self, mouseX, mouseY):
'''Given a point on the GUI, get a ray from the current position to infinity and beyond'''
width, height, _, _, _, camForward, horizon, \
vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera()
camPos = [
camTarget[0] - dist * camForward[0],
camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]),
(camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0] * rayForward[0] +
rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
invLen = farPlane * 1. / lenFwd
rayForward = [
invLen * rayForward[0], invLen * rayForward[1],
invLen * rayForward[2]
]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [
horizon[0] * oneOverWidth, horizon[1] * oneOverWidth,
horizon[2] * oneOverWidth
]
dVer = [
vertical[0] * oneOverHeight, vertical[1] * oneOverHeight,
vertical[2] * oneOverHeight
]
_ = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1],
rayFrom[2] + rayForward[2]
]
ortho = [
-0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], -0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
-0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] -
float(mouseY) * dVer[2]
]
rayTo = [
rayFrom[0] + rayForward[0] + ortho[0],
rayFrom[1] + rayForward[1] + ortho[1],
rayFrom[2] + rayForward[2] + ortho[2]
]
lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] +
ortho[2] * ortho[2])
alpha = math.atan(lenOrtho / farPlane)
return rayFrom, rayTo, alpha
def main():
# environment = KukaCamGymEnv(renders=True,isDiscrete=False)
environment = KukaCamGymEnv_Reconfigured(renders=True,isDiscrete=False)
environment._reset()
# num_of_objects = 50
# num_of_objects_var = 10
num_of_objects = 20
num_of_objects_var = 4
randomObjs = add_random_objs_to_scene(num_of_objects)
done = False
try:
with open("sim_images/serial_num_log.txt",'r') as f:
img_serial_num = int(f.read())
except:
print("read serial number failed!")
img_serial_num = 0
step = 0
snapshot_interval = 50#42#20 before, 42 would make about 10 snapshot per try, like in the real world dataset
viewMat = get_randomized_ViewMat()#sigma = 0.001
camInfo = p.getDebugVisualizerCamera()
# viewMat = camInfo[2]
projMatrix = camInfo[3]
action_keys = ["grasp/0/commanded_pose/transforms/base_T_endeffector/vec_quat_7", "grasp/1/commanded_pose/transforms/base_T_endeffector/vec_quat_7"]
data_folder = "/Users/bozai/Desktop/PixelDA/PixelDA/Data/tfdata"
file_tail = "22"
data_path = get_data_paths(data_folder,file_tail)
commands = commands_iterator(data_path)
while (not done):
attemption = commands.__next__()
for action_with_quaternion in attemption:
quaternion = action_with_quaternion[0][3:]
euler = p.getEulerFromQuaternion(quaternion)#[yaw,pitch,roll]
action = action_with_quaternion[0][:3].tolist()
action.append(euler[0])
action.append(euler[2])
if step%snapshot_interval==0:
#get cameta image
print("Saving image... Current image count: {}".format(img_serial_num))
img_arr = p.getCameraImage(640,512,viewMatrix=viewMat,projectionMatrix=projMatrix)#640*512*3
write_from_imgarr(img_arr, img_serial_num)
img_serial_num +=1
step +=1
state, reward, done, info = environment.step(action)#state: (256, 341, 4), info: empty dict
print("step: {} done: {} reward: {}".format(step, done, reward))
if done:
environment._reset()
randomObjs = add_random_objs_to_scene(num_of_objects+random.choice(range(-num_of_objects_var,num_of_objects_var+1)))
viewMat = get_randomized_ViewMat(sigma = 0.0001)#change view per try, not per image，0.003
done =False
print("Environment reseted!")
with open("sim_images/serial_num_log.txt","w") as f:
f.write(str(img_serial_num))
def get_camera_pos(self):
camera_params = p.getDebugVisualizerCamera(physicsClientId=self.sim.id)
camera_forward = torch.tensor(camera_params[5])
camera_left = -torch.tensor(camera_params[6])
camera_left /= camera_left.norm()
camera_up = torch.tensor(camera_params[7])
camera_up /= camera_up.norm()
R = torch.stack([camera_forward, camera_left, camera_up], dim=1)
camera_target = torch.tensor(camera_params[-1])
target_dist = camera_params[-2]
camera_pos_world = camera_target - target_dist * camera_forward
return camera_pos_world, R
def get_debug_visualizer_image() -> typing.Tuple[
np.ndarray, np.ndarray, np.ndarray
]:
import pybullet
width, height, *_ = pybullet.getDebugVisualizerCamera()
_, _, rgba, depth, segm = pybullet.getCameraImage(
width=width, height=height
)
rgb = rgba[:, :, :3]
depth[segm == -1] = np.nan
return rgb, depth, segm
def L_handviewMat(self):
L_hand = p.getLinkState(self._robonyan.robonyanUid, 6)
# L_handのカメラ座標取得
L_handPos, L_handOrn = L_hand[0], L_hand[1]
L_handEuler = p.getEulerFromQuaternion(L_handOrn)
L_handYaw = L_handEuler[2] * 360 / (2. * math.pi) - 90
L_camera = np.array([0.02, 0, 0])
L_handOrn = list(L_handOrn)
# 回転行列の生成
Lx = self.rotate_x(L_handOrn[0])
Ly = self.rotate_y(L_handOrn[1])
Lz = self.rotate_z(L_handOrn[2])
# 回転後のベクトルを計算
L1 = np.dot(Ly, Lx)
L2 = np.dot(L1, Lz)
L3 = np.dot(L2, L_camera)
# 途中経過
L4 = np.dot(Lz, L_camera)
La = np.dot(Lx, Lz)
L5 = np.dot(La, L_camera)
# 整数型に変換
b = np.array(L3, dtype=np.float32)
L_handPos = np.array(L_handPos) + b
L_handPos = L_handPos.tolist()
L_handPos = L_handPos[0]
#print(R_handPos)
L_handPos = tuple(L_handPos)
#print(R_handPos)
L4 = np.array(L4, dtype=np.float32)
L5 = np.array(L5, dtype=np.float32)
L_handOrn = tuple(L_handOrn)
camInfo = p.getDebugVisualizerCamera()
L_handMat = p.getMatrixFromQuaternion(L_handOrn)
upVector = [0, 0, 1]
forwardVec = [L_handMat[0], L_handMat[3], L_handMat[6]]
#sideVec = [camMat[1],camMat[4],camMat[7]]
L_handUpVec = [L_handMat[2], L_handMat[5], L_handMat[8]]
blockPos, blockOrn = p.getBasePositionAndOrientation(self.blockUid)
L_handTarget = self.blockPos
L_handUpTarget = [
L_handPos[0] + L_handUpVec[0], L_handPos[1] + L_handUpVec[1],
L_handPos[2] + L_handUpVec[2]
]
L_handviewMat = p.computeViewMatrix(L_handPos, L_handTarget,
L_handUpVec)
return L_handviewMat
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 _step(self, a):
self.nframe += 1
if not self.scene.multiplayer: # if multiplayer, action first applied to all robots, then global step() called, then _step() for all robots with the same actions
self.robot.apply_action(a)
self.scene.global_step()
# self.rewards = self._rewards(a)
done = self._termination()
# debugmode = False
# if debugmode:
# print("rewards=")
# print(self.rewards)
# print("sum rewards")
# print(sum(self.rewards))
#
# self.reward += sum(self.rewards)
# self.eps_reward += sum(self.rewards)
# debugmode = False
# if debugmode:
# print("Eps frame {} reward {}".format(self.nframe, self.reward))
# print("position", self.robot.get_position())
if self.gui:
pos = self.robot._get_scaled_position()
orn = self.robot.get_orientation()
pos = (pos[0], pos[1], pos[2] + self.tracking_camera['z_offset'])
pos = np.array(pos)
dist = self.tracking_camera['distance'] / self.robot.mjcf_scaling
[yaw, pitch] = p.getDebugVisualizerCamera()[8:10]
p.resetDebugVisualizerCamera(dist, yaw, pitch, pos)
eye_pos, eye_quat = self.get_eye_pos_orientation()
pose = [eye_pos, eye_quat]
observations = self.render_observations(pose)
# debugmode = 0
# if (debugmode):
# print("Camera env eye position", eye_pos)
# print("episode rewards", sum(self.rewards), "steps", self.nframe)
episode = None
if done:
episode = {'r': self.reward,
'l': self.nframe}
debugmode = 0
if debugmode:
print("return episode:", episode)
return observations, 0, bool(done), dict(eye_pos=eye_pos, eye_quat=eye_quat, episode=episode)
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 get_position():
"""
Get position of the 3D OpenGL debug visualizer camera.
Outputs:
target (vec3): Target focus point in Cartesian world coordinates.
distance (float): Distance from eye to focus point.
yaw (float): Yaw angle in degrees left / right around up-axis.
pitch (float): Pitch in degrees up / down.
"""
data = pb.getDebugVisualizerCamera()
yaw = data[8]
pitch = data[9]
distance = data[10]
target = data[11]
return target, distance, yaw, pitch
def compute_cam_vals(cam_dist, cam_tgt, cam_yaws=None, cam_pitches=None):
print('====== ATTENTION: if default cam_dist, cam_tgt are changed:')
print('copy/paste the output below to PanCamera.CAM_VALS')
if cam_yaws is None: cam_yaws = PanCamera.CAM_YAWS
if cam_pitches is None: cam_pitches = PanCamera.CAM_PITCHES
for i in range(len(cam_yaws)):
pybullet.resetDebugVisualizerCamera(cameraDistance=cam_dist,
cameraYaw=cam_yaws[i],
cameraPitch=cam_pitches[i],
cameraTargetPosition=cam_tgt)
_, _, view_matrix, proj_matrix, _, cam_forward, cam_horiz, cam_vert, \
_, _, cam_dist, cam_tgt = pybullet.getDebugVisualizerCamera()
print('# CAM_VALS for yaw', cam_yaws[i], 'pitch', cam_pitches[i])
print('[', view_matrix, ',', proj_matrix, ',', cam_forward, ',',
cam_horiz, ',', cam_vert, ',', cam_dist, ',', cam_tgt, '],')
def follow_waypts_sim(waypts):
'''Follow waypts with simulated robot
'''
robot = RobotArm('sim', headless=False, realtime=False)
# move GUI camera to get better view
yaw, pitch, _, target = pb.getDebugVisualizerCamera()[-4:]
pb.resetDebugVisualizerCamera(0.5, yaw, pitch, target)
pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
robot.move_hand_to(waypts[0])
for i in range(1, len(waypts)):
pb.addUserDebugLine(waypts[i - 1], waypts[i], [0.8, 0.3, 0.2], 2)
robot.move_hand_to(waypts[i])
time.sleep(1)
def getObservation(self):
# kinectv2のカメラ座標取得
kinect = p.getLinkState(self.robonyanUid, 56)
kinectPos, kinectOrn = kinect[0], kinect[1]
kinectPos = list(kinectPos)
kinectPos[0] += 0.2
kinectPos[1] += 0.002
kinectPos[2] += 0.05
#kinectPos = [-0.112 + 0.03153696, 0, 1.304 + 0.03153696]
kinectPos = tuple(kinectPos)
#kinectEuler = p.getEulerFromQuaternion(kinectOrn)
#kinectYaw = kinectEuler[2]*360/(2.*math.pi)-90
camInfo = p.getDebugVisualizerCamera()
kinectMat = p.getMatrixFromQuaternion(kinectOrn)
upVector = [0, 0, 1]
forwardVec = [kinectMat[0], kinectMat[3], kinectMat[6]]
#sideVec = [camMat[1],camMat[4],camMat[7]]
kinectUpVec = [kinectMat[2], kinectMat[5], kinectMat[8]]
kinectTarget = [
kinectPos[0] + forwardVec[0] * 100,
kinectPos[1] + forwardVec[1] * 100,
kinectPos[2] + forwardVec[2] * 100
]
kinectUpTarget = [
kinectPos[0] + kinectUpVec[0], kinectPos[1] + kinectUpVec[1],
kinectPos[2] + kinectUpVec[2]
]
kinectviewMat = p.computeViewMatrix(kinectPos, kinectTarget,
kinectUpVec)
kinectprojMat = camInfo[3]
#p.getCameraImage(320,200,viewMatrix=viewMat,projectionMatrix=projMat, flags=p.ER_NO_SEGMENTATION_MASK, renderer=p.ER_BULLET_HARDWARE_OPENGL)
#p.getCameraImage(width,height,viewMatrix=kinectviewMat,projectionMatrix=kinectprojMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
kinect_img_arr = p.getCameraImage(width=self.kinect_rgb_width,
height=self.kinect_rgb_height,
viewMatrix=kinectviewMat,
projectionMatrix=kinectprojMat)
kinect_rgb = kinect_img_arr[2]
kinect_np_img_arr = np.reshape(
kinect_rgb, (self.kinect_rgb_height, self.kinect_rgb_width, 4))
self._kinect_observation = kinect_np_img_arr
return self._kinect_observation
def checkkeyboardinput(): # Put keyboard control here
global quitApp, anchorPos, guiDisp, guiClock, jointCurPos
keys = p.getKeyboardEvents()
if p.B3G_BACKSPACE in keys:
quitApp = True
if ord('m') in keys and time.clock() - guiClock > guiMinTime:
p.configureDebugVisualizer(p.COV_ENABLE_GUI, guiDisp)
guiDisp = not guiDisp
guiClock = time.clock()
if p.B3G_LEFT_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
-x * speed
for x in np.cross(list(p.getDebugVisualizerCamera()[5]),
list(p.getDebugVisualizerCamera()[4]))
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_RIGHT_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
x * speed
for x in np.cross(list(p.getDebugVisualizerCamera()[5]),
list(p.getDebugVisualizerCamera()[4]))
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_DOWN_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos, [
-x * speed for x in list(p.getDebugVisualizerCamera()[5])
])))
p.changeConstraint(anchor, anchorPos)
if p.B3G_UP_ARROW in keys:
anchorPos = list(
map(
sum,
zip(anchorPos,
[x * speed
for x in list(p.getDebugVisualizerCamera()[5])])))
p.changeConstraint(anchor, anchorPos)
if ord('p') in keys:
jointCurPos = [0] * 19
for i, ji in enumerate(jointIndices):
if ord(jointKeys[i]) in keys and p.B3G_SHIFT not in keys:
jointCurPos[i] += jointSpeed * pi
elif ord(jointKeys[i]) in keys and p.B3G_SHIFT in keys:
jointCurPos[i] -= jointSpeed * pi
p.setJointMotorControl2(hand, ji, p.POSITION_CONTROL, jointCurPos[i])
def debugparameter(self, start):
motorsIds = []
pos = self.get_pos_robot()
# print('obser : ',obser)
xyz = pos[:3]
ori = p.getEulerFromQuaternion(pos[3:])
# ori = obser[4:]
xin = xyz[0]
yin = xyz[1]
zin = xyz[2]
rin = ori[0]
pitchin = ori[1]
yawin = ori[2]
abs_distance = 1.0
p.addUserDebugParameter("Camera X", -3, 3, 0.75)
p.addUserDebugParameter("Camera Y", -3, 3, 0.2)
p.addUserDebugParameter("Camera Z", -3, 3, 0.2)
motorsIds.append(p.addUserDebugParameter("X", 0, 1.5, xin))
motorsIds.append(p.addUserDebugParameter("Y", 0, abs_distance, yin))
motorsIds.append(p.addUserDebugParameter("Z", 0, abs_distance, zin))
motorsIds.append(
p.addUserDebugParameter("roll", -math.pi, math.pi, rin))
motorsIds.append(
p.addUserDebugParameter("pitch", -math.pi, math.pi, pitchin))
motorsIds.append(
p.addUserDebugParameter("yaw", -math.pi, math.pi, yawin))
motorsIds.append(p.addUserDebugParameter("fingerAngle", 0, 0.1, .04))
while (True):
action = []
for motorId in motorsIds:
action.append(p.readUserDebugParameter(motorId))
goal_pos = action[0:3]
goal_orien = p.getQuaternionFromEuler(action[3:6])
fingers = action[6]
camera = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(camera[-2], camera[-4], camera[-3], [
p.readUserDebugParameter(0),
p.readUserDebugParameter(1),
p.readUserDebugParameter(2)
])
panda_position, reward, done, observation = self.step(
goal_pos, goal_orien, fingers)
def moveCameraToPosition(position: Vec3, orientation: Optional[Quaternion] = None) -> None:
if PybulletAPI.gui():
camera_info = p.getDebugVisualizerCamera()
if orientation:
orn = orientation.to_euler()
yaw = - orn[YAW] * 360 / (2 * math.pi) - 90
pitch = orn[ROLL] * 360 / (2 * math.pi) - 20
else:
yaw = camera_info[8]
pitch = camera_info[9]
p.resetDebugVisualizerCamera(
cameraDistance=camera_info[10],
cameraYaw=yaw,
cameraPitch=pitch,
cameraTargetPosition=position.as_nwu()
)
def main_usingEnvOnly():
environment = KukaGymEnv(renders=True, isDiscrete=False, maxSteps=10000000)
randomObjs = add_random_objs_to_scene(10)
motorsIds = []
motorsIds.append(
environment._p.addUserDebugParameter("posX", 0.4, 0.75, 0.537))
motorsIds.append(
environment._p.addUserDebugParameter("posY", -.22, .3, 0.0))
motorsIds.append(environment._p.addUserDebugParameter("posZ", 0.1, 1, 0.2))
motorsIds.append(
environment._p.addUserDebugParameter("yaw", -3.14, 3.14, 0))
motorsIds.append(
environment._p.addUserDebugParameter("fingerAngle", 0, 0.3, .3))
# dv = 0.01
# motorsIds.append(environment._p.addUserDebugParameter("posX",-dv,dv,0))
# motorsIds.append(environment._p.addUserDebugParameter("posY",-dv,dv,0))
# motorsIds.append(environment._p.addUserDebugParameter("posZ",-dv,dv,0))
# motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
# motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
#According to the hand-eye coordination paper, the camera is mounted over the shoulder of the arm
camEyePos = [0.03, 0.236, 0.54]
targetPos = [0.640000, 0.075000, -0.190000]
cameraUp = [0, 0, 1]
viewMat = p.computeViewMatrix(camEyePos, targetPos, cameraUp)
camInfo = p.getDebugVisualizerCamera()
projMatrix = camInfo[3]
# viewMat = [-0.5120397806167603, 0.7171027660369873, -0.47284144163131714, 0.0, -0.8589617609977722, -0.42747554183006287, 0.28186774253845215, 0.0, 0.0, 0.5504802465438843, 0.8348482847213745, 0.0, 0.1925382763147354, -0.24935829639434814, -0.4401884973049164, 1.0]
# projMatrix = [0.75, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
img_arr = p.getCameraImage(640,
512,
viewMatrix=viewMat,
projectionMatrix=projMatrix) #640*512*3
# write_from_imgarr(img_arr, 1)
while (not done):
action = []
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step2(action)
obs = environment.getExtendedObservation()
def step(self):
if not p.isConnected(self.client_id):
return
# get keyboard events if gui is active
single_step = False
if self.gui:
# reset if R-key was pressed
r_key = ord('r')
n_key = ord('n')
s_key = ord('s')
f_key = ord('f')
space_key = p.B3G_SPACE
keys = p.getKeyboardEvents()
if r_key in keys and keys[r_key] & p.KEY_WAS_TRIGGERED:
self.reset()
if space_key in keys and keys[space_key] & p.KEY_WAS_TRIGGERED:
self.paused = not self.paused
if s_key in keys and keys[s_key] & p.KEY_IS_DOWN:
single_step = True
if n_key in keys and keys[n_key] & p.KEY_WAS_TRIGGERED:
if self.gravity:
p.setGravity(0, 0, 0)
else:
p.setGravity(0, 0, -9.81)
self.gravity = not self.gravity
if f_key in keys and keys[f_key] and p.KEY_WAS_TRIGGERED:
self.follow_camera = not self.follow_camera
backspace_key = p.B3G_BACKSPACE
if backspace_key in keys and keys[backspace_key] & p.KEY_WAS_TRIGGERED:
p.disconnect(self.client_id)
return
# check if simulation should continue currently
if not self.paused or single_step:
self.time += self.time_step
p.stepSimulation()
if self.follow_camera:
camera = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(cameraDistance=camera[10], cameraYaw=camera[8], cameraPitch=camera[9],
cameraTargetPosition=p.getBasePositionAndOrientation(self.robot_index)[0])
def get_mouse_events(self):
events = p.getMouseEvents(physicsClientId=self.sim.id)
for event in events:
if event[4] == 3:
_, x, y, _, _ = event
camera_params = p.getDebugVisualizerCamera(
physicsClientId=self.sim.id)
width = camera_params[0]
height = camera_params[1]
aspect = width / height
pos_view = torch.tensor([
1.0, aspect * (1.0 - (x / width) * 2),
1.0 - (y / height) * 2
])
camera_forward = torch.tensor(camera_params[5])
camera_left = -torch.tensor(camera_params[6])
camera_left /= camera_left.norm()
camera_up = torch.tensor(camera_params[7])
camera_up /= camera_up.norm()
R = torch.stack([camera_forward, camera_left, camera_up],
dim=1)
camera_target = torch.tensor(camera_params[-1])
target_dist = camera_params[-2]
camera_pos_world = camera_target - target_dist * camera_forward
pos_world = R @ pos_view + camera_pos_world
vec = pos_world - camera_pos_world
vec = vec / vec.norm()
ray = p.rayTest(rayFromPosition=camera_pos_world.tolist(),
rayToPosition=(100.0 * vec +
camera_pos_world).tolist(),
physicsClientId=self.sim.id)
hit_pos = torch.tensor(ray[0][3])
results = {}
results['camera_pos'] = camera_pos_world
results['target_pos'] = torch.tensor(ray[0][3])
results['target_obj'] = self.object_dict.get(ray[0][0], None)
results['target_normal'] = torch.tensor(ray[0][4])
return results
def step(self, action):
yaw = 0
while True:
yaw += -0.75
p.resetDebugVisualizerCamera(cameraDistance=1.2,
cameraYaw=yaw,
cameraPitch=-20,
cameraTargetPosition=[0, 0, 1.0],
physicsClientId=self.id)
indices = [4, 5, 6]
# indices = [14, 15, 16]
deltas = [0.01, 0.01, -0.01]
indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
deltas = [0, 0, 0, 0, 0.01, 0.01, -0.01, 0, 0, 0]
# indices = []
# deltas = []
# indices = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]) + 10
# deltas = [0, 0, 0, 0, -0.01, 0.01, -0.01, 0, 0, 0]
for i, d in zip(indices, deltas):
joint_position = p.getJointState(self.human,
jointIndex=i,
physicsClientId=self.id)[0]
if joint_position + d > self.human_lower_limits[
i] and joint_position + d < self.human_upper_limits[i]:
p.resetJointState(self.human,
jointIndex=i,
targetValue=joint_position + d,
targetVelocity=0,
physicsClientId=self.id)
p.stepSimulation(physicsClientId=self.id)
print('cameraYaw=%.2f, cameraPitch=%.2f, distance=%.2f' %
p.getDebugVisualizerCamera(physicsClientId=self.id)[-4:-1])
self.enforce_realistic_human_joint_limits()
time.sleep(0.05)
return [], None, None, None
def getCarYaw(car):
carPos,carOrn = p.getBasePositionAndOrientation(car)
carEuler = p.getEulerFromQuaternion(carOrn)
carYaw = carEuler[2]*360/(2.*math.pi)-90
return carYaw
prevCarYaw = getCarYaw(car)
frame = 0
lineId = p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
lineId2 = p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
lineId3= p.addUserDebugLine([0,0,0],[0,0,1],[1,0,0])
print("lineId=",lineId)
camInfo = p.getDebugVisualizerCamera()
lastTime = time.time()
lastControlTime = time.time()
lastLidarTime = time.time()
frame=0
while (True):
nowTime = time.time()
#render Camera at 10Hertz
if (nowTime-lastTime>.1):
ls = p.getLinkState(car,zed_camera_joint, computeForwardKinematics=True)
camPos = ls[0]
camOrn = ls[1]
camMat = p.getMatrixFromQuaternion(camOrn)
upVector = [0,0,1]
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
p.setGravity(0,0,0)
else:
p.stepSimulation()
def get_camera():
return CameraInfo(*p.getDebugVisualizerCamera())
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
targetVelocity = -10
p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)
def get_camera():
return CameraInfo(*p.getDebugVisualizerCamera(physicsClientId=CLIENT))
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True)
p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000])
while (1):
p.stepSimulation()
time.sleep(0.01)
#p.saveWorld("test.py")
viewMat = p.getDebugVisualizerCamera()[2]
#projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = p.getDebugVisualizerCamera()[3]
width=640
height=480
img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = -0.5
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
turn = 0.5
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
turn = 0
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
)
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
]
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
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]]
ortho=[- 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
- 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
- 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
rayTo = [rayFrom[0]+rayForward[0] +ortho[0],
rayFrom[1]+rayForward[1] +ortho[1],
rayFrom[2]+rayForward[2] +ortho[2]]
lenOrtho = math.sqrt(ortho[0]*ortho[0]+ortho[1]*ortho[1]+ortho[2]*ortho[2])
alpha = math.atan(lenOrtho/farPlane)
return rayFrom,rayTo, alpha
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera()
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
farPlane = 10000
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])]
lenFwd = math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])
oneOverWidth = float(1)/float(width)
oneOverHeight = float(1)/float(height)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
lendHor = math.sqrt(dHor[0]*dHor[0]+dHor[1]*dHor[1]+dHor[2]*dHor[2])
lendVer = math.sqrt(dVer[0]*dVer[0]+dVer[1]*dVer[1]+dVer[2]*dVer[2])
cornersX = [0,width,width,0]
cornersY = [0,0,height,height]
corners3D=[]