194 lines
7.3 KiB
Python
194 lines
7.3 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
|