mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-04 17:40:06 +00:00
tune kuka grasp gym env (make it a bit too easy)
This commit is contained in:
erwincoumans
parent
16f439d774
commit
c903bd8a49
@ -3,10 +3,7 @@
|
||||
<link name="block_2_base_link">
|
||||
<contact>
|
||||
<lateral_friction value="1.0"/>
|
||||
<rolling_friction value="0.0"/>
|
||||
<inertia_scaling value="3.0"/>
|
||||
<contact_cfm value="0.0"/>
|
||||
<contact_erp value="1.0"/>
|
||||
<spinning_friction value=".001"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user