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()