From d2888f0884baa08c23d2f83803e70894373caa34 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 14 Jun 2017 19:34:33 -0700 Subject: [PATCH] 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',