217 lines
8.5 KiB
Python
217 lines
8.5 KiB
Python
import os, inspect
|
|
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
|
|
parentdir = os.path.dirname(os.path.dirname(currentdir))
|
|
os.sys.path.insert(0, parentdir)
|
|
|
|
import pybullet as p
|
|
import numpy as np
|
|
import copy
|
|
import math
|
|
import pybullet_data
|
|
|
|
|
|
class Kuka:
|
|
|
|
def __init__(self, urdfRootPath=pybullet_data.getDataPath(), timeStep=0.01):
|
|
self.urdfRootPath = urdfRootPath
|
|
self.timeStep = timeStep
|
|
self.maxVelocity = .35
|
|
self.maxForce = 200.
|
|
self.fingerAForce = 2
|
|
self.fingerBForce = 2.5
|
|
self.fingerTipForce = 2
|
|
self.useInverseKinematics = 1
|
|
self.useSimulation = 1
|
|
self.useNullSpace = 21
|
|
self.useOrientation = 1
|
|
self.kukaEndEffectorIndex = 6
|
|
self.kukaGripperIndex = 7
|
|
#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.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
|
|
0.00001, 0.00001, 0.00001, 0.00001
|
|
]
|
|
self.reset()
|
|
|
|
def reset(self):
|
|
objects = p.loadSDF(os.path.join(self.urdfRootPath, "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.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048,
|
|
-0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200
|
|
]
|
|
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(os.path.join(self.urdfRootPath, "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]
|
|
self.endEffectorAngle = 0
|
|
|
|
self.motorNames = []
|
|
self.motorIndices = []
|
|
|
|
for i in range(self.numJoints):
|
|
jointInfo = p.getJointInfo(self.kukaUid, i)
|
|
qIndex = jointInfo[3]
|
|
if qIndex > -1:
|
|
#print("motorname")
|
|
#print(jointInfo[1])
|
|
self.motorNames.append(str(jointInfo[1]))
|
|
self.motorIndices.append(i)
|
|
|
|
def getActionDimension(self):
|
|
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())
|
|
|
|
def getObservation(self):
|
|
observation = []
|
|
state = p.getLinkState(self.kukaUid, self.kukaGripperIndex)
|
|
pos = state[0]
|
|
orn = state[1]
|
|
euler = p.getEulerFromQuaternion(orn)
|
|
|
|
observation.extend(list(pos))
|
|
observation.extend(list(euler))
|
|
|
|
return observation
|
|
|
|
def applyAction(self, motorCommands):
|
|
|
|
#print ("self.numJoints")
|
|
#print (self.numJoints)
|
|
if (self.useInverseKinematics):
|
|
|
|
dx = motorCommands[0]
|
|
dy = motorCommands[1]
|
|
dz = motorCommands[2]
|
|
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.65):
|
|
self.endEffectorPos[0] = 0.65
|
|
if (self.endEffectorPos[0] < 0.50):
|
|
self.endEffectorPos[0] = 0.50
|
|
self.endEffectorPos[1] = self.endEffectorPos[1] + dy
|
|
if (self.endEffectorPos[1] < -0.17):
|
|
self.endEffectorPos[1] = -0.17
|
|
if (self.endEffectorPos[1] > 0.22):
|
|
self.endEffectorPos[1] = 0.22
|
|
|
|
#print ("self.endEffectorPos[2]")
|
|
#print (self.endEffectorPos[2])
|
|
#print("actualEndEffectorPos[2]")
|
|
#print(actualEndEffectorPos[2])
|
|
#if (dz<0 or actualEndEffectorPos[2]<0.5):
|
|
self.endEffectorPos[2] = self.endEffectorPos[2] + dz
|
|
|
|
self.endEffectorAngle = self.endEffectorAngle + da
|
|
pos = self.endEffectorPos
|
|
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)
|
|
|
|
#print("jointPoses")
|
|
#print(jointPoses)
|
|
#print("self.kukaEndEffectorIndex")
|
|
#print(self.kukaEndEffectorIndex)
|
|
if (self.useSimulation):
|
|
for i in range(self.kukaEndEffectorIndex + 1):
|
|
#print(i)
|
|
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,
|
|
jointIndex=i,
|
|
controlMode=p.POSITION_CONTROL,
|
|
targetPosition=jointPoses[i],
|
|
targetVelocity=0,
|
|
force=self.maxForce,
|
|
maxVelocity=self.maxVelocity,
|
|
positionGain=0.3,
|
|
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=self.endEffectorAngle,
|
|
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)
|