Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add nicer meshes to kuka_with_gripper.sdf and add kuka_with_gripper2.sdf that can rotate without messing up IK

Browse Source 
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
This commit is contained in:
erwincoumans
2017-06-14 19:34:33 -07:00
parent cc34ebab25
commit d2888f0884
10 changed files with 1004 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/pybullet/examples/kuka_with_cube.py
View File

@@ -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):

93
examples/pybullet/gym/envs/bullet/kuka.py
View File

@@ -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)
#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)

4
examples/pybullet/gym/envs/bullet/kukaGymEnv.py
View File

@@ -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])

13
examples/pybullet/gym/kukaGymEnvTest.py
View File

@@ -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):

23
examples/pybullet/gym/kukaJointSpaceGymEnvTest.py Normal file
View File

@@ -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)