tune kuka grasp gym env (make it a bit too easy)

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

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
+    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