def main():
environment = KukaGymEnv(renders=True)
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.001
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, -dv))
motorsIds.append(environment._p.addUserDebugParameter("yaw", -dv, dv, 0))
motorsIds.append(
environment._p.addUserDebugParameter("fingerAngle", 0, 0.3, .3))
done = False
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 main():
environment = KukaGymEnv(renders=True, isDiscrete=False, maxSteps=10000000)
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
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 main(control="gym-like"):
env = KukaGymEnv(renders=True, isDiscrete=False, maxSteps=10000000)
motorsIds = []
if control == "gym-like":
dv = 1
motorsIds.append(env._p.addUserDebugParameter("posX", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("posY", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("yaw", -dv, dv, 0))
else:
dv = 0.01
motorsIds.append(env._p.addUserDebugParameter("posX", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("posY", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("posZ", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("yaw", -dv, dv, 0))
motorsIds.append(env._p.addUserDebugParameter("fingerAngle", 0, 0.3, 0.3))
done = False
i = 0
while not done:
time.sleep(0.01)
action = []
for motorId in motorsIds:
action.append(env._p.readUserDebugParameter(motorId))
if control == "gym-like":
state, reward, done, info = env.step(action)
else:
state, reward, done, info = env.step2(action)
obs = env.getExtendedObservation()
print(i)
print(f"Action: {action}")
print(f"Observation: {obs}")
i += 1
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()
import time
environment = KukaGymEnv(renders=True)
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.001
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,-dv))
motorsIds.append(environment._p.addUserDebugParameter("yaw",-dv,dv,0))
motorsIds.append(environment._p.addUserDebugParameter("fingerAngle",0,0.3,.3))
done = False
while (not done):
action=[]
for motorId in motorsIds:
action.append(environment._p.readUserDebugParameter(motorId))
state, reward, done, info = environment.step2(action)
obs = environment.getExtendedObservation()
