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

data/block.urdf

@@ -3,10 +3,7 @@
   <link name="block_2_base_link">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="0.0"/>
+      <spinning_friction value=".001"/>
-      <inertia_scaling value="3.0"/>
-      <contact_cfm value="0.0"/>
-      <contact_erp value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/kuka_iiwa/kuka_with_gripper2.sdf

@@ -401,7 +401,7 @@
       <pose frame=''>0 0 1.261 0 0 0</pose>
       <inertial>
         <pose frame=''>0 0 0.02 0 -0 0</pose>
-        <mass>0.3</mass>
+        <mass>1.3</mass>
         <inertia>
           <ixx>0.001</ixx>
           <ixy>0</ixy>
@@ -462,12 +462,6 @@
       <child>base_link</child>
       <axis>
         <xyz>0 0 1</xyz>
-        <dynamics>
-          <damping>0.5</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
       </axis>
     </joint>
 
@@ -477,7 +471,7 @@
       <pose frame=''>0 0 1.305 0 -0 0</pose>
       <inertial>
         <pose frame=''>0 0 0 0 -0 0</pose>
-        <mass>1.2</mass>
+        <mass>0.2</mass>
         <inertia>
           <ixx>1</ixx>
           <ixy>0</ixy>
@@ -501,6 +495,20 @@
           <emissive>0 0 0 0</emissive>
         </material>
       </visual>
+        <collision name='base_link_collision'>
+        <pose frame=''>0 0 0 0 0 0</pose>
+        <geometry>
+		  <box>
+		     <size>0.05 0.05 0.1 </size>
+		  </box>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0.6 0.6 0.6 1</diffuse>
+          <specular>0.5 0.5 0.5 1</specular>
+          <emissive>0 0 0 0</emissive>
+        </material>
+      </collision>
     </link>
 
     <joint name='base_left_finger_joint' type='revolute'>
@@ -526,7 +534,7 @@
       <pose frame=''>0 0.024 1.35 0 -0.05 0</pose>
       <inertial>
         <pose frame=''>0 0 0.04 0 0 0</pose>
-        <mass>0.1</mass>
+        <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
           <ixy>0</ixy>
@@ -550,12 +558,25 @@
           <emissive>0 0 0 0</emissive>
         </material>
       </visual>
+       <collision name='left_finger_collision'>
+        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.08</size>
+          </box>
+        </geometry>
+        
+      </collision>
     </link>
     <joint name='left_finger_base_joint' type='fixed'>
       <parent>left_finger</parent>
       <child>left_finger_base</child>
     </joint>
     <link name='left_finger_base'>
+    <contact>
+        <lateral_friction>0.8</lateral_friction>
+        <spinning_friction>.1</spinning_friction>
+      </contact>
       <pose frame=''>-0.005 0.024 1.43 0 -0.3 0</pose>
       <inertial>
         <pose frame=''>-0.003 0 0.04 0 0 0 </pose>
@@ -616,7 +637,7 @@
     <link name='left_finger_tip'>
       <contact>
         <lateral_friction>0.8</lateral_friction>
-        <spinning_friction>1.0</spinning_friction>
+        <spinning_friction>.1</spinning_friction>
       </contact>
       <pose frame=''>-0.02 0.024 1.49 0 0.2 0</pose>
       <inertial>
@@ -676,10 +697,14 @@
       </axis>
     </joint>
     <link name='right_finger'>
+    <contact>
+        <lateral_friction>0.8</lateral_friction>
+        <spinning_friction>.1</spinning_friction>
+      </contact>
       <pose frame=''>0 0.024 1.35 0 0.05 0</pose>
       <inertial>
         <pose frame=''>0 0 0.04 0 0 0</pose>
-        <mass>0.1</mass>
+        <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
           <ixy>0</ixy>
@@ -703,12 +728,30 @@
           <emissive>0 0 0 0</emissive>
         </material>
       </visual>
+      <collision name='right_finger_collision'>
+        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.01 0.01 0.08</size>
+          </box>
+        </geometry>
+        <material>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0.6 0.6 0.6 1</diffuse>
+          <specular>0.5 0.5 0.5 1</specular>
+          <emissive>0 0 0 0</emissive>
+        </material>
+      </collision>
     </link>
     <joint name='right_finger_base_joint' type='fixed'>
       <parent>right_finger</parent>
       <child>right_finger_base</child>
     </joint>
     <link name='right_finger_base'>
+    <contact>
+        <lateral_friction>0.8</lateral_friction>
+        <spinning_friction>.1</spinning_friction>
+      </contact>
       <pose frame=''>0.005 0.024 1.43 0 0.3 0</pose>
       <inertial>
         <pose frame=''>0.003 0 0.04 0 0 0 </pose>
@@ -769,7 +812,7 @@
     <link name='right_finger_tip'>
       <contact>
         <lateral_friction>0.8</lateral_friction>
-        <spinning_friction>1.0</spinning_friction>
+        <spinning_friction>.1</spinning_friction>
       </contact>
       <pose frame=''>0.02 0.024 1.49 0 -0.2 0</pose>
       <inertial>

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):
+      if (self.endEffectorPos[0]>0.75):
-        self.endEffectorPos[0]=0.7
+        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):
+      if (self.endEffectorPos[1]<-0.22):
-        self.endEffectorPos[1]=-0.2
+        self.endEffectorPos[1]=-0.22
-      if (self.endEffectorPos[1]>0.3):
+      if (self.endEffectorPos[1]>0.22):
-        self.endEffectorPos[1]=0.3
+        self.endEffectorPos[1]=0.22
       
+      #print ("self.endEffectorPos[2]")
       #print (self.endEffectorPos[2])
-      self.endEffectorPos[2] = self.endEffectorPos[2]+dz
+      #print("actualEndEffectorPos[2]")
-      if (self.endEffectorPos[2]<0.1):
+      #print(actualEndEffectorPos[2])
-        self.endEffectorPos[2] = 0.1
+      if (dz>0 or actualEndEffectorPos[2]>0.10):
-      if (self.endEffectorPos[2]>0.5):
+        self.endEffectorPos[2] = self.endEffectorPos[2]+dz
-        self.endEffectorPos[2] = 0.5
+      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()
+    xpos = 0.5 +0.05*random.random()
-    ypos = 0 +0.22*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):
+    state = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaEndEffectorIndex)
-      for i in range (1000):
+    actualEndEffectorPos = state[0]
-        p.stepSimulation()
+      
+    #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):
-      for i in range (5000):
+        graspAction = [0,0,0.001,0,fingerAngle]
-        graspAction = [0,0,0,0,fingerAngle]
         self._kuka.applyAction(graspAction)
         p.stepSimulation()
-        fingerAngle = fingerAngle-(0.3/5000.)
+        fingerAngle = fingerAngle-(0.3/100.)
-        
+        if (fingerAngle<0):
-      for i in range (5000):
+          fingerAngle=0
-        graspAction = [0,0,0.0001,0,0]
-        self._kuka.applyAction(graspAction)
-        p.stepSimulation()
-        fingerAngle = fingerAngle-(0.3/10000.)
         
       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)
+    blockPos,blockOrn=p.getBasePositionAndOrientation(self.blockUid)
-    reward = pos[2]
+    closestPoints = p.getClosestPoints(self.blockUid,self._kuka.kukaUid,1000) 
-    if (reward>0.2):
+
-    	print("reward")
+    reward = -1000    
-    	print(reward)
+    numPt = len(closestPoints)
-    return reward
+    #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