1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
import pybullet as p
import time
import math
from datetime import datetime
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3], useFixedBase=True)
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
p.loadURDF("cube.urdf", [2, 2, 5])
p.loadURDF("cube.urdf", [-2, -2, 5])
p.loadURDF("cube.urdf", [2, -2, 5])
#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
count = 0
useOrientation = 1
useSimulation = 1
useRealTimeSimulation = 1
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
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 range(5):
print("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.1
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)]
#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:
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos)
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=0.03,
velocityGain=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
|
