From d2888f0884baa08c23d2f83803e70894373caa34 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 14 Jun 2017 19:34:33 -0700 Subject: [PATCH 1/3] add nicer meshes to kuka_with_gripper.sdf and add kuka_with_gripper2.sdf that can rotate without messing up IK fix tray/tray_textured4.obj and tray/tray.urdf fix kuka_with_cube.py allow both IK /end-effector control and joint-space control in kuka environment, use 1./240. sec. step and 150 solver iter bump up pybullet to 1.1.7 --- data/kuka_iiwa/kuka_with_gripper.sdf | 104 ++- data/kuka_iiwa/kuka_with_gripper2.sdf | 814 ++++++++++++++++++ data/tray/tray_textured4.mtl | 2 +- data/tray/tray_textured4.obj | 2 +- examples/pybullet/examples/kuka_with_cube.py | 4 +- examples/pybullet/gym/envs/bullet/kuka.py | 93 +- .../pybullet/gym/envs/bullet/kukaGymEnv.py | 4 +- examples/pybullet/gym/kukaGymEnvTest.py | 13 +- .../pybullet/gym/kukaJointSpaceGymEnvTest.py | 23 + setup.py | 2 +- 10 files changed, 1004 insertions(+), 57 deletions(-) create mode 100644 data/kuka_iiwa/kuka_with_gripper2.sdf create mode 100644 examples/pybullet/gym/kukaJointSpaceGymEnvTest.py 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..e04fba230 --- /dev/null +++ b/data/kuka_iiwa/kuka_with_gripper2.sdf @@ -0,0 +1,814 @@ + + + + + + + 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 + 0.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.5 + 0 + 0 + 0 + + + + + + + + 0 0 1.305 0 -0 0 + + 0 0 0 0 -0 0 + 1.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 + + + + + + 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.1 + + 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 + + + + + left_finger + left_finger_base + + + -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 + + -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 0.024 1.35 0 0.05 0 + + 0 0 0.04 0 0 0 + 0.1 + + 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 + + + + + right_finger + right_finger_base + + + 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 + + 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/envs/bullet/kuka.py b/examples/pybullet/gym/envs/bullet/kuka.py index 3f74d87ff..6c25dd25d 100644 --- a/examples/pybullet/gym/envs/bullet/kuka.py +++ b/examples/pybullet/gym/envs/bullet/kuka.py @@ -9,34 +9,59 @@ class Kuka: self.urdfRootPath = urdfRootPath self.timeStep = timeStep self.reset() - self.maxForce = 100 - + self.maxForce = 90 + self.fingerAForce = 12 + self.fingerBForce = 10 + self.fingerTipForce = 3 + + 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] + def reset(self): - objects = p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") + 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.numJoints = p.getNumJoints(self.kukaUid) + for jointIndex in range (self.numJoints): p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex]) 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) - + p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.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()) @@ -51,8 +76,48 @@ class Kuka: return observation def applyAction(self, motorCommands): + #print ("self.numJoints") + #print (self.numJoints) + if (self.useInverseKinematics): + pos = [motorCommands[0],motorCommands[1],motorCommands[2]] + yaw = motorCommands[3] + fingerAngle = motorCommands[4] + #roll = motorCommands[5] + 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) - 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("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=yaw,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..6ee08a7b6 100644 --- a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py @@ -20,7 +20,7 @@ class KukaGymEnv(gym.Env): isEnableSelfCollision=True, renders=True): print("init") - self._timeStep = 0.01 + self._timeStep = 1./240. self._urdfRoot = urdfRoot self._actionRepeat = actionRepeat self._isEnableSelfCollision = isEnableSelfCollision @@ -45,7 +45,7 @@ class KukaGymEnv(gym.Env): def _reset(self): p.resetSimulation() - #p.setPhysicsEngineParameter(numSolverIterations=300) + p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) p.loadURDF("%splane.urdf" % self._urdfRoot,[0,0,-1]) diff --git a/examples/pybullet/gym/kukaGymEnvTest.py b/examples/pybullet/gym/kukaGymEnvTest.py index 46bde180a..e58ade822 100644 --- a/examples/pybullet/gym/kukaGymEnvTest.py +++ b/examples/pybullet/gym/kukaGymEnvTest.py @@ -1,14 +1,17 @@ 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): 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/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', From a547c8f3fcf0c5170307c6707a6e45e63a71815b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 14 Jun 2017 23:42:14 -0700 Subject: [PATCH 2/3] hook up kuka grasping to OpenAI dqn --- examples/pybullet/gym/enjoy_kuka_grasping.py | 26 +++++++ examples/pybullet/gym/envs/bullet/kuka.py | 53 ++++++++++---- .../pybullet/gym/envs/bullet/kukaGymEnv.py | 73 +++++++++++++++---- examples/pybullet/gym/kukaGymEnvTest.py | 20 +++-- examples/pybullet/gym/train_kuka_grasping.py | 40 ++++++++++ 5 files changed, 179 insertions(+), 33 deletions(-) create mode 100644 examples/pybullet/gym/enjoy_kuka_grasping.py create mode 100644 examples/pybullet/gym/train_kuka_grasping.py 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 6c25dd25d..e86bb2a0a 100644 --- a/examples/pybullet/gym/envs/bullet/kuka.py +++ b/examples/pybullet/gym/envs/bullet/kuka.py @@ -10,8 +10,8 @@ class Kuka: self.timeStep = timeStep self.reset() self.maxForce = 90 - self.fingerAForce = 12 - self.fingerBForce = 10 + self.fingerAForce = 6 + self.fingerBForce = 5 self.fingerTipForce = 3 self.useInverseKinematics = 1 @@ -34,17 +34,18 @@ class Kuka: 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)) + #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 ] + 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]) 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) - p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0) + self.endEffectorPos = [0.537,0.0,0.5] + self.endEffectorAngle = 0 + self.motorNames = [] self.motorIndices = [] @@ -68,21 +69,47 @@ class Kuka: 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)) return observation def applyAction(self, motorCommands): + #print ("self.numJoints") #print (self.numJoints) if (self.useInverseKinematics): - pos = [motorCommands[0],motorCommands[1],motorCommands[2]] - yaw = motorCommands[3] + + dx = motorCommands[0] + dy = motorCommands[1] + dz = motorCommands[2] + da = motorCommands[3] fingerAngle = motorCommands[4] - #roll = motorCommands[5] + + self.endEffectorPos[0] = self.endEffectorPos[0]+dx + if (self.endEffectorPos[0]>0.7): + self.endEffectorPos[0]=0.7 + if (self.endEffectorPos[0]<0.45): + self.endEffectorPos[0]=0.45 + self.endEffectorPos[1] = self.endEffectorPos[1]+dy + if (self.endEffectorPos[1]<-0.2): + self.endEffectorPos[1]=-0.2 + if (self.endEffectorPos[1]>0.3): + self.endEffectorPos[1]=0.3 + + #print (self.endEffectorPos[2]) + self.endEffectorPos[2] = self.endEffectorPos[2]+dz + if (self.endEffectorPos[2]<0.1): + self.endEffectorPos[2] = 0.1 + if (self.endEffectorPos[2]>0.5): + self.endEffectorPos[2] = 0.5 + + 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): @@ -108,7 +135,7 @@ class Kuka: for i in range (self.numJoints): p.resetJointState(self.kukaUid,i,jointPoses[i]) #fingers - p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=yaw,force=self.maxForce) + 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) diff --git a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py index 6ee08a7b6..0baa0f3ca 100644 --- a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py @@ -16,7 +16,7 @@ class KukaGymEnv(gym.Env): def __init__(self, urdfRoot="", - actionRepeat=50, + actionRepeat=1, isEnableSelfCollision=True, renders=True): print("init") @@ -32,6 +32,7 @@ class KukaGymEnv(gym.Env): 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,7 +40,7 @@ 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 @@ -48,14 +49,16 @@ class KukaGymEnv(gym.Env): 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.25*random.random() + ypos = 0 +0.22*random.random() + ang = 3.1415925438*random.random() + orn = p.getQuaternionFromEuler([0,0,ang]) + self.blockUid =p.loadURDF("block.urdf", xpos,ypos,0.02,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 +75,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.005 + dx = [0,-dv,dv,0,0,0,0][action] + dy = [0,0,0,-dv,dv,0,0][action] + da = [0,0,0,0,0,-dv,dv][action] + f = 0.3 + realAction = [dx,dy,-0.1,da,f] + return self.step2( realAction) + + def step2(self, action): self._kuka.applyAction(action) for i in range(self._actionRepeat): p.stepSimulation() @@ -84,8 +100,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 +113,36 @@ class KukaGymEnv(gym.Env): return def _termination(self): - return self._envStepCounter>1000 + #print (self._kuka.endEffectorPos[2]) + if (self._envStepCounter>1000): + for i in range (1000): + p.stepSimulation() + #start grasp and terminate + fingerAngle = 0.3 + + + for i in range (5000): + graspAction = [0,0,0,0,fingerAngle] + self._kuka.applyAction(graspAction) + p.stepSimulation() + fingerAngle = fingerAngle-(0.3/5000.) + + for i in range (5000): + graspAction = [0,0,0.0001,0,0] + self._kuka.applyAction(graspAction) + p.stepSimulation() + fingerAngle = fingerAngle-(0.3/10000.) + + self._observation = self.getExtendedObservation() + return True + return False def _reward(self): - #todo - reward=0 + #rewards is height of target object + pos,orn=p.getBasePositionAndOrientation(self.blockUid) + reward = pos[2] + if (reward>0.2): + print("reward") + print(reward) return reward \ No newline at end of file diff --git a/examples/pybullet/gym/kukaGymEnvTest.py b/examples/pybullet/gym/kukaGymEnvTest.py index e58ade822..05ca9497f 100644 --- a/examples/pybullet/gym/kukaGymEnvTest.py +++ b/examples/pybullet/gym/kukaGymEnvTest.py @@ -7,18 +7,26 @@ 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("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)) -while (True): +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/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() From c903bd8a4907e7c808eba89a32fcb4920cfe16de Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 15 Jun 2017 11:18:08 -0700 Subject: [PATCH 3/3] tune kuka grasp gym env (make it a bit too easy) --- data/block.urdf | 5 +- data/kuka_iiwa/kuka_with_gripper2.sdf | 67 +++++++++++++---- examples/pybullet/gym/envs/bullet/kuka.py | 44 +++++++----- .../pybullet/gym/envs/bullet/kukaGymEnv.py | 71 ++++++++++++------- 4 files changed, 129 insertions(+), 58 deletions(-) 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_gripper2.sdf b/data/kuka_iiwa/kuka_with_gripper2.sdf index e04fba230..9c3787bcb 100644 --- a/data/kuka_iiwa/kuka_with_gripper2.sdf +++ b/data/kuka_iiwa/kuka_with_gripper2.sdf @@ -401,7 +401,7 @@ 0 0 1.261 0 0 0 0 0 0.02 0 -0 0 - 0.3 + 1.3 0.001 0 @@ -462,12 +462,6 @@ base_link 0 0 1 - - 0.5 - 0 - 0 - 0 - @@ -477,7 +471,7 @@ 0 0 1.305 0 -0 0 0 0 0 0 -0 0 - 1.2 + 0.2 1 0 @@ -501,6 +495,20 @@ 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 + + @@ -526,7 +534,7 @@ 0 0.024 1.35 0 -0.05 0 0 0 0.04 0 0 0 - 0.1 + 0.2 0.1 0 @@ -550,12 +558,25 @@ 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 @@ -616,7 +637,7 @@ 0.8 - 1.0 + .1 -0.02 0.024 1.49 0 0.2 0 @@ -676,10 +697,14 @@ + + 0.8 + .1 + 0 0.024 1.35 0 0.05 0 0 0 0.04 0 0 0 - 0.1 + 0.2 0.1 0 @@ -703,12 +728,30 @@ 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 @@ -769,7 +812,7 @@ 0.8 - 1.0 + .1 0.02 0.024 1.49 0 -0.2 0 diff --git a/examples/pybullet/gym/envs/bullet/kuka.py b/examples/pybullet/gym/envs/bullet/kuka.py index e86bb2a0a..da6b53444 100644 --- a/examples/pybullet/gym/envs/bullet/kuka.py +++ b/examples/pybullet/gym/envs/bullet/kuka.py @@ -8,12 +8,11 @@ class Kuka: def __init__(self, urdfRootPath='', timeStep=0.01): self.urdfRootPath = urdfRootPath self.timeStep = timeStep - self.reset() - self.maxForce = 90 - self.fingerAForce = 6 - self.fingerBForce = 5 - self.fingerTipForce = 3 + self.maxForce = 200. + self.fingerAForce = 6 + self.fingerBForce = 5.5 + self.fingerTipForce = 6 self.useInverseKinematics = 1 self.useSimulation = 1 self.useNullSpace = 1 @@ -29,9 +28,9 @@ class Kuka: 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)): @@ -41,6 +40,7 @@ class Kuka: 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.endEffectorPos = [0.537,0.0,0.5] @@ -90,23 +90,33 @@ class Kuka: 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.7): - self.endEffectorPos[0]=0.7 + 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.2): - self.endEffectorPos[1]=-0.2 - if (self.endEffectorPos[1]>0.3): - self.endEffectorPos[1]=0.3 + 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]) - self.endEffectorPos[2] = self.endEffectorPos[2]+dz - if (self.endEffectorPos[2]<0.1): - self.endEffectorPos[2] = 0.1 - if (self.endEffectorPos[2]>0.5): - self.endEffectorPos[2] = 0.5 + #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 diff --git a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py index 0baa0f3ca..c76d69044 100644 --- a/examples/pybullet/gym/envs/bullet/kukaGymEnv.py +++ b/examples/pybullet/gym/envs/bullet/kukaGymEnv.py @@ -27,6 +27,7 @@ class KukaGymEnv(gym.Env): self._observation = [] self._envStepCounter = 0 self._renders = renders + self.terminated = 0 self._p = p if self._renders: p.connect(p.GUI) @@ -45,6 +46,8 @@ class KukaGymEnv(gym.Env): self.viewer = None def _reset(self): + print("reset") + self.terminated = 0 p.resetSimulation() p.setPhysicsEngineParameter(numSolverIterations=150) p.setTimeStep(self._timeStep) @@ -53,11 +56,11 @@ class KukaGymEnv(gym.Env): 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) - xpos = 0.5 +0.25*random.random() - ypos = 0 +0.22*random.random() + 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.02,orn[0],orn[1],orn[2],orn[3]) + 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) @@ -82,12 +85,12 @@ class KukaGymEnv(gym.Env): return self._observation def _step(self, action): - dv = 0.005 + 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,-dv,dv][action] + da = [0,0,0,0,0,-0.1,0.1][action] f = 0.3 - realAction = [dx,dy,-0.1,da,f] + realAction = [dx,dy,-0.002,da,f] return self.step2( realAction) def step2(self, action): @@ -114,24 +117,29 @@ class KukaGymEnv(gym.Env): def _termination(self): #print (self._kuka.endEffectorPos[2]) - if (self._envStepCounter>1000): - for i in range (1000): - p.stepSimulation() + 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 (5000): - graspAction = [0,0,0,0,fingerAngle] + for i in range (1000): + graspAction = [0,0,0.001,0,fingerAngle] self._kuka.applyAction(graspAction) p.stepSimulation() - fingerAngle = fingerAngle-(0.3/5000.) - - for i in range (5000): - graspAction = [0,0,0.0001,0,0] - self._kuka.applyAction(graspAction) - p.stepSimulation() - fingerAngle = fingerAngle-(0.3/10000.) + fingerAngle = fingerAngle-(0.3/100.) + if (fingerAngle<0): + fingerAngle=0 self._observation = self.getExtendedObservation() return True @@ -140,9 +148,22 @@ class KukaGymEnv(gym.Env): def _reward(self): #rewards is height of target object - pos,orn=p.getBasePositionAndOrientation(self.blockUid) - reward = pos[2] - if (reward>0.2): - print("reward") - print(reward) - return reward \ No newline at end of file + 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