diff --git a/data/block.urdf b/data/block.urdf index 9ff64decf..d4be09aa0 100644 --- a/data/block.urdf +++ b/data/block.urdf @@ -3,10 +3,7 @@ - - - - + diff --git a/data/kuka_iiwa/kuka_with_gripper.sdf b/data/kuka_iiwa/kuka_with_gripper.sdf index 739eac349..fd8ba3554 100644 --- a/data/kuka_iiwa/kuka_with_gripper.sdf +++ b/data/kuka_iiwa/kuka_with_gripper.sdf @@ -38,13 +38,13 @@ 1 1 1 - meshes/link_0.stl + meshes/link_0.obj 1 0 0 1 - 0.5 0.5 0.5 1 - 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1.0 + 0.4 0.4 0.4 1 0 0 0 0 @@ -77,13 +77,13 @@ 1 1 1 - meshes/link_1.stl + meshes/link_1.obj 1 0 0 1 - 0.5 0.5 0.5 1 - 0.1 0.1 0.1 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -135,13 +135,13 @@ 1 1 1 - meshes/link_2.stl + meshes/link_2.obj 1 0 0 1 - 1.0 0.42 0.04 1 - 0.1 0.1 0.1 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -193,13 +193,13 @@ 1 1 1 - meshes/link_3.stl + meshes/link_3.obj 1 0 0 1 - 0.6 0.6 0.6 1 - 0.1 0.1 0.1 1 + 1.0 0.423529411765 0.0392156862745 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -251,13 +251,13 @@ 1 1 1 - meshes/link_4.stl + meshes/link_4.obj 1 0 0 1 - 0.6 0.6 0.6 1 - 0.1 0.1 0.1 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -309,13 +309,13 @@ 1 1 1 - meshes/link_5.stl + meshes/link_5.obj 1 0 0 1 - 0.6 0.6 0.6 1 - 0.1 0.1 0.1 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -367,13 +367,13 @@ 1 1 1 - meshes/link_6.stl + meshes/link_6.obj 1 0 0 1 - 1.0 0.42 0.04 1 - 0.1 0.1 0.1 1 + 1.0 0.423529411765 0.0392156862745 1.0 + 0.5 0.5 0.5 1 0 0 0 0 @@ -425,13 +425,13 @@ 1 1 1 - meshes/link_7.stl + meshes/link_7.obj 1 0 0 1 0.6 0.6 0.6 1 - 0.1 0.1 0.1 1 + 0.5 0.5 0.5 1 0 0 0 0 @@ -482,6 +482,12 @@ 0.05 0.05 0.1 + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + @@ -491,8 +497,8 @@ 0 1 0 - -0.4 - 0.01 + -10.4 + 10.01 100 0 @@ -525,6 +531,12 @@ 0.01 0.01 0.08 + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + @@ -553,6 +565,12 @@ meshes/finger_base_left.stl + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + 0 0 0 0 0 0 @@ -570,8 +588,8 @@ 0 1 0 - -0.1 - 0.3 + -10.1 + 10.3 0 0 @@ -609,6 +627,12 @@ meshes/finger_tip_left.stl + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + 0 0 0 0 0 0 @@ -626,8 +650,8 @@ 0 1 0 - -0.01 - 0.4 + -10.01 + 10.4 100 0 @@ -660,6 +684,12 @@ 0.01 0.01 0.08 + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + @@ -688,6 +718,12 @@ meshes/finger_base_right.stl + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + 0 0 0 0 0 0 @@ -705,8 +741,8 @@ 0 1 0 - -0.3 - 0.1 + -10.3 + 10.1 0 0 @@ -744,6 +780,12 @@ meshes/finger_tip_right.stl + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + 0 0 0 0 0 0 diff --git a/data/kuka_iiwa/kuka_with_gripper2.sdf b/data/kuka_iiwa/kuka_with_gripper2.sdf new file mode 100644 index 000000000..9c3787bcb --- /dev/null +++ b/data/kuka_iiwa/kuka_with_gripper2.sdf @@ -0,0 +1,857 @@ + + + + + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + /meshes/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.obj + + + + 1 0 0 1 + 0.2 0.2 0.2 1.0 + 0.4 0.4 0.4 1 + 0 0 0 0 + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.obj + + + + 1 0 0 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.obj + + + + 1 0 0 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.obj + + + + 1 0 0 1 + 1.0 0.423529411765 0.0392156862745 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.obj + + + + 1 0 0 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.obj + + + + 1 0 0 1 + 0.5 0.7 1.0 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.obj + + + + 1 0 0 1 + 1.0 0.423529411765 0.0392156862745 1.0 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 1.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.obj + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + + + + + + lbr_iiwa_link_7 + base_link + + 0 0 1 + + + + + + + 0 0 1.305 0 -0 0 + + 0 0 0 0 -0 0 + 0.2 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + 0 0 0 0 0 0 + + + 0.05 0.05 0.1 + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0 0 0 0 + + + 0.05 0.05 0.1 + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + + base_link + left_finger + + 0 1 0 + + -10.4 + 10.01 + 100 + 0 + + + 0 + 0 + 0 + 0 + + + + + 0 0.024 1.35 0 -0.05 0 + + 0 0 0.04 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0.04 0 0 0 + + + 0.01 0.01 0.08 + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0.04 0 0 0 + + + 0.01 0.01 0.08 + + + + + + + left_finger + left_finger_base + + + + 0.8 + .1 + + -0.005 0.024 1.43 0 -0.3 0 + + -0.003 0 0.04 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_base_left.stl + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_base_left.stl + + + + + + left_finger_base + left_finger_tip + + 0 1 0 + + -10.1 + 10.3 + 0 + 0 + + + 0 + 0 + 0 + 0 + + + + + + 0.8 + .1 + + -0.02 0.024 1.49 0 0.2 0 + + -0.005 0 0.026 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_tip_left.stl + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_tip_left.stl + + + + + + base_link + right_finger + + 0 1 0 + + -10.01 + 10.4 + 100 + 0 + + + 0 + 0 + 0 + 0 + + + + + + 0.8 + .1 + + 0 0.024 1.35 0 0.05 0 + + 0 0 0.04 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0.04 0 0 0 + + + 0.01 0.01 0.08 + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0.04 0 0 0 + + + 0.01 0.01 0.08 + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + + right_finger + right_finger_base + + + + 0.8 + .1 + + 0.005 0.024 1.43 0 0.3 0 + + 0.003 0 0.04 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_base_right.stl + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_base_right.stl + + + + + + right_finger_base + right_finger_tip + + 0 1 0 + + -10.3 + 10.1 + 0 + 0 + + + 0 + 0 + 0 + 0 + + + + + + 0.8 + .1 + + 0.02 0.024 1.49 0 -0.2 0 + + 0.005 0 0.026 0 0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_tip_right.stl + + + + 1 0 0 1 + 0.6 0.6 0.6 1 + 0.5 0.5 0.5 1 + 0 0 0 0 + + + + 0 0 0 0 0 0 + + + 1 1 1 + meshes/finger_tip_right.stl + + + + + + + diff --git a/data/tray/tray_textured4.mtl b/data/tray/tray_textured4.mtl index 1a2697882..e3b14fbaf 100755 --- a/data/tray/tray_textured4.mtl +++ b/data/tray/tray_textured4.mtl @@ -10,4 +10,4 @@ Ke 0.000000 0.000000 0.000000 Ni 1.000000 d 1.000000 illum 2 -#map_Kd tray.jpg +map_Kd tray.jpg diff --git a/data/tray/tray_textured4.obj b/data/tray/tray_textured4.obj index a367c02cb..a05f010d8 100755 --- a/data/tray/tray_textured4.obj +++ b/data/tray/tray_textured4.obj @@ -1,6 +1,6 @@ # Blender v2.77 (sub 0) OBJ File: '' # www.blender.org -mtllib tray_textured5.mtl +mtllib tray_textured4.mtl o edge_2_Cube v 0.593683 0.625721 0.255175 v 0.406317 0.459389 0.004580 diff --git a/examples/pybullet/examples/kuka_with_cube.py b/examples/pybullet/examples/kuka_with_cube.py index a4668dd49..4f52978e3 100644 --- a/examples/pybullet/examples/kuka_with_cube.py +++ b/examples/pybullet/examples/kuka_with_cube.py @@ -50,8 +50,8 @@ 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]) +for i in range(5): + print ("Body %d's name is %s." % (i, p.getBodyInfo(i)[1])) while 1: if (useRealTimeSimulation): diff --git a/examples/pybullet/gym/enjoy_kuka_grasping.py b/examples/pybullet/gym/enjoy_kuka_grasping.py new file mode 100644 index 000000000..2814e1b01 --- /dev/null +++ b/examples/pybullet/gym/enjoy_kuka_grasping.py @@ -0,0 +1,26 @@ +import gym +from envs.bullet.kukaGymEnv import KukaGymEnv + +from baselines import deepq + + +def main(): + + env = KukaGymEnv(renders=True) + act = deepq.load("kuka_model.pkl") + print(act) + while True: + obs, done = env.reset(), False + print("===================================") + print("obs") + print(obs) + episode_rew = 0 + while not done: + env.render() + obs, rew, done, _ = env.step(act(obs[None])[0]) + episode_rew += rew + print("Episode reward", episode_rew) + + +if __name__ == '__main__': + main() diff --git a/examples/pybullet/gym/envs/bullet/kuka.py b/examples/pybullet/gym/envs/bullet/kuka.py index 3f74d87ff..da6b53444 100644 --- a/examples/pybullet/gym/envs/bullet/kuka.py +++ b/examples/pybullet/gym/envs/bullet/kuka.py @@ -8,43 +8,71 @@ class Kuka: def __init__(self, urdfRootPath='', timeStep=0.01): self.urdfRootPath = urdfRootPath self.timeStep = timeStep - self.reset() - self.maxForce = 100 - - def reset(self): - objects = p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") + self.maxForce = 200. + self.fingerAForce = 6 + self.fingerBForce = 5.5 + self.fingerTipForce = 6 + self.useInverseKinematics = 1 + self.useSimulation = 1 + self.useNullSpace = 1 + self.useOrientation = 1 + self.kukaEndEffectorIndex = 6 + #lower limits for null space + self.ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] + #upper limits for null space + self.ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] + #joint ranges for null space + self.jr=[5.8,4,5.8,4,5.8,4,6] + #restposes for null space + self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] + #joint damping coefficents + self.jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] + self.reset() + + def reset(self): + objects = p.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf") self.kukaUid = objects[0] + #for i in range (p.getNumJoints(self.kukaUid)): + # print(p.getJointInfo(self.kukaUid,i)) p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000]) - self.jointPositions=[ -0.196884, 0.857586, -0.023543, -1.664977, 0.030403, 0.624786, -0.232294, 0.000000, -0.296450, 0.000000, 0.100002, 0.284399, 0.000000, -0.099999 ] - for jointIndex in range (p.getNumJoints(self.kukaUid)): + self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ] + self.numJoints = p.getNumJoints(self.kukaUid) + for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) + p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce) self.trayUid = p.loadURDF("tray/tray.urdf", 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000) - self.blockUid =p.loadURDF("block.urdf", 0.604746,0.080626,-0.180069,0.000050,-0.000859,-0.824149,0.566372) - + self.endEffectorPos = [0.537,0.0,0.5] + self.endEffectorAngle = 0 + + self.motorNames = [] self.motorIndices = [] - numJoints = p.getNumJoints(self.kukaUid) - for i in range (numJoints): + + for i in range (self.numJoints): jointInfo = p.getJointInfo(self.kukaUid,i) qIndex = jointInfo[3] if qIndex > -1: - print("motorname") - print(jointInfo[1]) + #print("motorname") + #print(jointInfo[1]) self.motorNames.append(str(jointInfo[1])) self.motorIndices.append(i) def getActionDimension(self): - return len(self.motorIndices) + if (self.useInverseKinematics): + return len(self.motorIndices) + return 6 #position x,y,z and roll/pitch/yaw euler angles of end effector def getObservationDimension(self): return len(self.getObservation()) def getObservation(self): observation = [] - pos,orn=p.getBasePositionAndOrientation(self.blockUid) - + state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex) + pos = state[0] + orn = state[1] + observation.extend(list(pos)) observation.extend(list(orn)) @@ -52,7 +80,81 @@ class Kuka: def applyAction(self, motorCommands): - for action in range (len(motorCommands)): - motor = self.motorIndices[action] - p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce) - \ No newline at end of file + #print ("self.numJoints") + #print (self.numJoints) + if (self.useInverseKinematics): + + dx = motorCommands[0] + dy = motorCommands[1] + dz = motorCommands[2] + da = motorCommands[3] + fingerAngle = motorCommands[4] + + state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex) + actualEndEffectorPos = state[0] + #print("pos[2] (getLinkState(kukaEndEffectorIndex)") + #print(actualEndEffectorPos[2]) + + + + self.endEffectorPos[0] = self.endEffectorPos[0]+dx + if (self.endEffectorPos[0]>0.75): + self.endEffectorPos[0]=0.75 + if (self.endEffectorPos[0]<0.45): + self.endEffectorPos[0]=0.45 + self.endEffectorPos[1] = self.endEffectorPos[1]+dy + if (self.endEffectorPos[1]<-0.22): + self.endEffectorPos[1]=-0.22 + if (self.endEffectorPos[1]>0.22): + self.endEffectorPos[1]=0.22 + + #print ("self.endEffectorPos[2]") + #print (self.endEffectorPos[2]) + #print("actualEndEffectorPos[2]") + #print(actualEndEffectorPos[2]) + if (dz>0 or actualEndEffectorPos[2]>0.10): + self.endEffectorPos[2] = self.endEffectorPos[2]+dz + if (actualEndEffectorPos[2]<0.10): + self.endEffectorPos[2] = self.endEffectorPos[2]+0.0001 + + + self.endEffectorAngle = self.endEffectorAngle + da + pos = self.endEffectorPos + orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw]) + if (self.useNullSpace==1): + if (self.useOrientation==1): + jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp) + else: + jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp) + else: + if (self.useOrientation==1): + jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd) + else: + jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos) + + #print("jointPoses") + #print(jointPoses) + #print("self.kukaEndEffectorIndex") + #print(self.kukaEndEffectorIndex) + if (self.useSimulation): + for i in range (self.kukaEndEffectorIndex+1): + #print(i) + p.setJointMotorControl2(bodyIndex=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,positionGain=0.03,velocityGain=1) + else: + #reset the joint state (ignoring all dynamics, not recommended to use during simulation) + for i in range (self.numJoints): + p.resetJointState(self.kukaUid,i,jointPoses[i]) + #fingers + p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce) + p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce) + p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce) + + p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) + p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce) + + + else: + for action in range (len(motorCommands)): + motor = self.motorIndices[action] + p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce) + diff --git a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py index ee0479c6c..c76d69044 100644 --- a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py @@ -16,22 +16,24 @@ class KukaGymEnv(gym.Env): def __init__(self, urdfRoot="", - actionRepeat=50, + actionRepeat=1, isEnableSelfCollision=True, renders=True): print("init") - self._timeStep = 0.01 + self._timeStep = 1./240. self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat self._isEnableSelfCollision = isEnableSelfCollision self._observation = [] self._envStepCounter = 0 self._renders = renders + self.terminated = 0 self._p = p if self._renders: p.connect(p.GUI) else: p.connect(p.DIRECT) + #timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "kukaTimings.json") self._seed() self.reset() observationDim = len(self.getExtendedObservation()) @@ -39,23 +41,27 @@ class KukaGymEnv(gym.Env): #print(observationDim) observation_high = np.array([np.finfo(np.float32).max] * observationDim) - self.action_space = spaces.Discrete(9) + self.action_space = spaces.Discrete(7) self.observation_space = spaces.Box(-observation_high, observation_high) self.viewer = None def _reset(self): + print("reset") + self.terminated = 0 p.resetSimulation() - #p.setPhysicsEngineParameter(numSolverIterations=300) + p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1]) + if self._renders: + p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33]) + p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) - dist = 5 +2.*random.random() - ang = 2.*3.1415925438*random.random() - - ballx = dist * math.sin(ang) - bally = dist * math.cos(ang) - ballz = 1 - + xpos = 0.5 +0.05*random.random() + ypos = 0 +0.05*random.random() + ang = 3.1415925438*random.random() + orn = p.getQuaternionFromEuler([0,0,ang]) + self.blockUid =p.loadURDF("block.urdf", xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3]) + p.setGravity(0,0,-10) self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep) self._envStepCounter = 0 @@ -72,9 +78,22 @@ class KukaGymEnv(gym.Env): def getExtendedObservation(self): self._observation = self._kuka.getObservation() + pos,orn = p.getBasePositionAndOrientation(self.blockUid) + self._observation.extend(list(pos)) + self._observation.extend(list(orn)) + return self._observation - + def _step(self, action): + dv = 0.002 + dx = [0,-dv,dv,0,0,0,0][action] + dy = [0,0,0,-dv,dv,0,0][action] + da = [0,0,0,0,0,-0.1,0.1][action] + f = 0.3 + realAction = [dx,dy,-0.002,da,f] + return self.step2( realAction) + + def step2(self, action): self._kuka.applyAction(action) for i in range(self._actionRepeat): p.stepSimulation() @@ -84,8 +103,11 @@ class KukaGymEnv(gym.Env): if self._termination(): break self._envStepCounter += 1 - reward = self._reward() + #print("self._envStepCounter") + #print(self._envStepCounter) + done = self._termination() + reward = self._reward() #print("len=%r" % len(self._observation)) return np.array(self._observation), reward, done, {} @@ -94,10 +116,54 @@ class KukaGymEnv(gym.Env): return def _termination(self): - return self._envStepCounter>1000 + #print (self._kuka.endEffectorPos[2]) + state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex) + actualEndEffectorPos = state[0] + + #print("self._envStepCounter") + #print(self._envStepCounter) + if (self.terminated or self._envStepCounter>1000): + self._observation = self.getExtendedObservation() + return True + + if (actualEndEffectorPos[2] <= 0.10): + self.terminated = 1 + + #print("closing gripper, attempting grasp") + #start grasp and terminate + fingerAngle = 0.3 + + for i in range (1000): + graspAction = [0,0,0.001,0,fingerAngle] + self._kuka.applyAction(graspAction) + p.stepSimulation() + fingerAngle = fingerAngle-(0.3/100.) + if (fingerAngle<0): + fingerAngle=0 + + self._observation = self.getExtendedObservation() + return True + return False def _reward(self): - #todo - reward=0 - return reward \ No newline at end of file + #rewards is height of target object + blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid) + closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000) + + reward = -1000 + numPt = len(closestPoints) + #print(numPt) + if (numPt>0): + #print("reward:") + reward = -closestPoints[0][8]*10 + + if (blockPos[2] >0.2): + print("grasped a block!!!") + print("self._envStepCounter") + print(self._envStepCounter) + reward = reward+1000 + + #print("reward") + #print(reward) + return reward diff --git a/examples/pybullet/gym/kukaGymEnvTest.py b/examples/pybullet/gym/kukaGymEnvTest.py index 46bde180a..05ca9497f 100644 --- a/examples/pybullet/gym/kukaGymEnvTest.py +++ b/examples/pybullet/gym/kukaGymEnvTest.py @@ -1,21 +1,32 @@ from envs.bullet.kukaGymEnv import KukaGymEnv -print ("hello") +import time + + environment = KukaGymEnv(renders=True) motorsIds=[] -for i in range (len(environment._kuka.motorNames)): - motor = environment._kuka.motorNames[i] - motorJointIndex = environment._kuka.motorIndices[i] - motorsIds.append(environment._p.addUserDebugParameter(motor,-3,3,environment._kuka.jointPositions[i])) +#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)) -while (True): +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.step(action) + state, reward, done, info = environment.step2(action) obs = environment.getExtendedObservation() diff --git a/examples/pybullet/gym/kukaJointSpaceGymEnvTest.py b/examples/pybullet/gym/kukaJointSpaceGymEnvTest.py new file mode 100644 index 000000000..ec442e1fd --- /dev/null +++ b/examples/pybullet/gym/kukaJointSpaceGymEnvTest.py @@ -0,0 +1,23 @@ + +from envs.bullet.kukaGymEnv import KukaGymEnv +import time + + +environment = KukaGymEnv(renders=True) +environment._kuka.useInverseKinematics=0 + +motorsIds=[] +for i in range (len(environment._kuka.motorNames)): + motor = environment._kuka.motorNames[i] + motorJointIndex = environment._kuka.motorIndices[i] + motorsIds.append(environment._p.addUserDebugParameter(motor,-3,3,environment._kuka.jointPositions[i])) + +while (True): + + action=[] + for motorId in motorsIds: + action.append(environment._p.readUserDebugParameter(motorId)) + + state, reward, done, info = environment.step(action) + obs = environment.getExtendedObservation() + time.sleep(0.01) diff --git a/examples/pybullet/gym/train_kuka_grasping.py b/examples/pybullet/gym/train_kuka_grasping.py new file mode 100644 index 000000000..10bb5c1ee --- /dev/null +++ b/examples/pybullet/gym/train_kuka_grasping.py @@ -0,0 +1,40 @@ +import gym +from envs.bullet.kukaGymEnv import KukaGymEnv + +from baselines import deepq + +import datetime + + + +def callback(lcl, glb): + # stop training if reward exceeds 199 + total = sum(lcl['episode_rewards'][-101:-1]) / 100 + totalt = lcl['t'] + #print("totalt") + #print(totalt) + is_solved = totalt > 2000 and total >= 10 + return is_solved + + +def main(): + + env = KukaGymEnv(renders=False) + model = deepq.models.mlp([64]) + act = deepq.learn( + env, + q_func=model, + lr=1e-3, + max_timesteps=10000000, + buffer_size=50000, + exploration_fraction=0.1, + exploration_final_eps=0.02, + print_freq=10, + callback=callback + ) + print("Saving model to kuka_model.pkl") + act.save("kuka_model.pkl") + + +if __name__ == '__main__': + main() diff --git a/setup.py b/setup.py index fecab1248..27865edf2 100644 --- a/setup.py +++ b/setup.py @@ -419,7 +419,7 @@ else: setup( name = 'pybullet', - version='1.1.6', + version='1.1.7', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',