170 lines
5.9 KiB
Python
170 lines
5.9 KiB
Python
import pybullet as p
|
|
import time
|
|
import math
|
|
from datetime import datetime
|
|
from datetime import datetime
|
|
|
|
clid = p.connect(p.SHARED_MEMORY)
|
|
if (clid<0):
|
|
p.connect(p.GUI)
|
|
p.loadURDF("plane.urdf",[0,0,-0.3])
|
|
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
|
|
for i in range (p.getNumJoints(husky)):
|
|
print(p.getJointInfo(husky,i))
|
|
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
|
|
ob = kukaId
|
|
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
|
|
for jointIndex in range (p.getNumJoints(ob)):
|
|
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
|
|
|
|
|
|
#put kuka on top of husky
|
|
|
|
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
|
|
|
|
|
|
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
|
|
baseorn = [0,0,0,1]
|
|
#[0, 0, 0.707, 0.707]
|
|
|
|
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
|
|
kukaEndEffectorIndex = 6
|
|
numJoints = p.getNumJoints(kukaId)
|
|
if (numJoints!=7):
|
|
exit()
|
|
|
|
|
|
#lower limits for null space
|
|
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
|
|
#upper limits for null space
|
|
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
|
|
#joint ranges for null space
|
|
jr=[5.8,4,5.8,4,5.8,4,6]
|
|
#restposes for null space
|
|
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
|
|
#joint damping coefficents
|
|
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
|
|
|
|
for i in range (numJoints):
|
|
p.resetJointState(kukaId,i,rp[i])
|
|
|
|
p.setGravity(0,0,-10)
|
|
t=0.
|
|
prevPose=[0,0,0]
|
|
prevPose1=[0,0,0]
|
|
hasPrevPose = 0
|
|
useNullSpace = 0
|
|
|
|
useOrientation =0
|
|
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
|
|
#This can be used to test the IK result accuracy.
|
|
useSimulation = 0
|
|
useRealTimeSimulation = 1
|
|
p.setRealTimeSimulation(useRealTimeSimulation)
|
|
#trailDuration is duration (in seconds) after debug lines will be removed automatically
|
|
#use 0 for no-removal
|
|
trailDuration = 15
|
|
basepos =[0,0,0]
|
|
ang = 0
|
|
ang=0
|
|
|
|
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
|
|
closeEnough = False
|
|
iter = 0
|
|
dist2 = 1e30
|
|
while (not closeEnough and iter<maxIter):
|
|
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
|
|
for i in range (numJoints):
|
|
p.resetJointState(kukaId,i,jointPoses[i])
|
|
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
|
|
newPos = ls[4]
|
|
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
|
|
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
|
|
closeEnough = (dist2 < threshold)
|
|
iter=iter+1
|
|
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
|
|
return jointPoses
|
|
|
|
|
|
wheels=[2,3,4,5]
|
|
#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
|
|
#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
|
|
#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
|
|
#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
|
|
wheelVelocities=[0,0,0,0]
|
|
wheelDeltasTurn=[1,-1,1,-1]
|
|
wheelDeltasFwd=[1,1,1,1]
|
|
while 1:
|
|
keys = p.getKeyboardEvents()
|
|
shift = 0.01
|
|
wheelVelocities=[0,0,0,0]
|
|
speed = 1.0
|
|
for k in keys:
|
|
if ord('s') in keys:
|
|
p.saveWorld("state.py")
|
|
if ord('a') in keys:
|
|
basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]]
|
|
if ord('d') in keys:
|
|
basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]]
|
|
|
|
if p.B3G_LEFT_ARROW in keys:
|
|
for i in range(len(wheels)):
|
|
wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i]
|
|
if p.B3G_RIGHT_ARROW in keys:
|
|
for i in range(len(wheels)):
|
|
wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i]
|
|
if p.B3G_UP_ARROW in keys:
|
|
for i in range(len(wheels)):
|
|
wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i]
|
|
if p.B3G_DOWN_ARROW in keys:
|
|
for i in range(len(wheels)):
|
|
wheelVelocities[i] = wheelVelocities[i] -speed*wheelDeltasFwd[i]
|
|
|
|
baseorn = p.getQuaternionFromEuler([0,0,ang])
|
|
for i in range(len(wheels)):
|
|
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000)
|
|
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
|
|
if (useRealTimeSimulation):
|
|
t = time.time()#(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
|
|
#t = (dt.second/60.)*2.*math.pi
|
|
else:
|
|
t=t+0.001
|
|
|
|
if (useSimulation and useRealTimeSimulation==0):
|
|
p.stepSimulation()
|
|
|
|
for i in range (1):
|
|
#pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
|
|
pos = [0.2*math.cos(t),0,0.+0.2*math.sin(t)+0.7]
|
|
#end effector points down, not up (in case useOrientation==1)
|
|
orn = p.getQuaternionFromEuler([0,-math.pi,0])
|
|
|
|
if (useNullSpace==1):
|
|
if (useOrientation==1):
|
|
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp)
|
|
else:
|
|
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
|
|
else:
|
|
if (useOrientation==1):
|
|
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
|
|
else:
|
|
threshold =0.001
|
|
maxIter = 100
|
|
jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter)
|
|
|
|
if (useSimulation):
|
|
for i in range (numJoints):
|
|
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=1,velocityGain=0.1)
|
|
else:
|
|
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
|
|
for i in range (numJoints):
|
|
p.resetJointState(kukaId,i,jointPoses[i])
|
|
|
|
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
|
|
if (hasPrevPose):
|
|
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
|
|
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration)
|
|
prevPose=pos
|
|
prevPose1=ls[4]
|
|
hasPrevPose = 1
|