/
IKPlannerFunctions.py
229 lines (172 loc) · 7.46 KB
/
IKPlannerFunctions.py
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
#from brett2.PR2 import Arm
import kinematics.kinematics_utils as ku
import rospy
import numpy as np
from numpy.linalg import norm
class IKInterpolationPlanner(object):
"""
Class which plans based on OpenRAVE's IK solver
"""
def __init__(self, _arm, _rl, _filter_options=1 ):
self.rl = _rl
#If useful
self.rl_long = {'l':'left', 'r':'right'}[self.rl]
self.arm = _arm
self.filter_options = _filter_options
def setFilterOptions (self, filter_options):
"""
Filter options indicate basic options while planning:
IKFO_CheckEnvCollisions = 1
IKFO_IgnoreSelfCollisions = 2
IKFO_IgnoreJointLimits = 4
IKFO_IgnoreCustomFilters = 8
IKFO_IgnoreEndEffectorCollisions = 16
"""
self.filter_options = filter_options
def plan(self, transforms):
"""
Basic plan function which just solves the IK for each transform given.
List of transforms along path -> transforms
"""
if len(transforms) < 1:
rospy.ERROR('Not enough transforms!')
if len(transforms) == 1:
firstTransform = self.arm.manip.GetEndEffectorTransform()
transforms = [firstTransform, transforms[0]]
trajectory = []
for transform in transforms:
joints = self.arm.manip.FindIKSolution(transform, self.filter_options)
if joints is None:
rospy.logwarn('IK Failed on ' + self.rl_long + 'arm.')
return joints
trajectory.append(joints)
return trajectory
#Assuming all the Joint DOFs are numpy arrays
def smoothPlan(self, transforms):
"""
Smooth plan function which solves for all IK solutions for each transform given.
It then chooses the joint DOF values which are closest to current DOF values.
List of transforms along path -> transforms
"""
if len(transforms) < 1:
rospy.ERROR('Not enough transforms!')
if len(transforms) == 1:
initTransform = self.arm.manip.GetEndEffectorTransform()
transforms = [initTransform, transforms[0]]
trajectory = []
armIndices = self.arm.manip.GetArmIndices()
currJoints = self.arm.manip.GetRobot().GetDOFValues(armIndices)
# currJoints = self.arm.get_joint_positions()
for transform in transforms:
allJoints = self.arm.manip.FindIKSolutions(transform, self.filter_options)
if not allJoints.size:
rospy.logwarn('IK Failed on ' + self.rl_long + 'arm.')
return None
allJoints = [ku.closer_joint_angles(joints, currJoints) for joints in allJoints]
normDifferences = [norm(currJoints-joints,2) for joints in allJoints]
minIndex = normDifferences.index(min(normDifferences))
trajectory.append(allJoints[minIndex])
return trajectory
#Not sure about scale here. Treating everything as meters.
#I don't think I need to worry about scale here anyway
def goInDirection (self, d, dist, steps=10):
"""
Moves the tool tip in the specified direction in the gripper frame.
Direction of movement -> d
f -> forward (along the tip of the gripper)
b -> backward
u -> up
d -> down
l -> left
r -> right
Distance traveled by tool tip -> dist
Number of points of linear interpolation -> steps
"""
initTransform = self.arm.manip.GetEndEffectorTransform()
initOrigin = initTransform[0:3,3]
if d == 'f':
dirVec = initTransform[0:3,2]
elif d == 'b':
dirVec = -1*initTransform[0:3,2]
elif d == 'u':
dirVec = initTransform[0:3,1]
elif d == 'd':
dirVec = -1*initTransform[0:3,1]
elif d == 'l':
dirVec = initTransform[0:3,0]
elif d == 'r':
dirVec = -1*initTransform[0:3,0]
else:
rospy.ERROR("Invalid direction: " + d)
endOffset = dirVec*float(dist)
transforms = []
for step in range(steps+1):
currVec = initOrigin + float(step)/steps*endOffset
newTfm = initTransform.copy()
newTfm[0:3,3] = currVec
transforms.append(newTfm)
return self.smoothPlan(transforms)
#Not sure about scale here. Treating everything as meters.
#I don't think I need to worry about scale here anyway
def goInWorldDirection (self, d, dist, steps=10):
"""
Moves the tool tip in the specified direction in the base_ frame.
Direction of movement -> d
f -> forward
b -> backward
u -> up
d -> down
l -> left
r -> right
Distance traveled by tool tip -> dist
Number of points of linear interpolation -> steps
"""
initTransform = self.arm.manip.GetEndEffectorTransform()
initOrigin = initTransform[0:3,3]
baseTransform = self.arm.pr2.robot.GetLink('base_footprint').GetTransform()
if d == 'u':
dirVec = baseTransform[0:3,2]
elif d == 'd':
dirVec = -1*baseTransform[0:3,2]
elif d == 'l':
dirVec = baseTransform[0:3,1]
elif d == 'r':
dirVec = -1*baseTransform[0:3,1]
elif d == 'f':
dirVec = baseTransform[0:3,0]
elif d == 'b':
dirVec = -1*baseTransform[0:3,0]
else:
rospy.ERROR("Invalid direction: " + d)
endOffset = dirVec*float(dist)
transforms = []
for step in range(steps+1):
currVec = initOrigin + endOffset*(float(step)/steps)
newTfm = initTransform.copy()
newTfm[0:3,3] = currVec
transforms.append(newTfm)
return self.smoothPlan(transforms)
def circleAroundRadius (self, d, t, rad, finAng, steps=10):
"""
Moves the gripper in a circle.
Direction of circle (either inner or outer) -> d
Direction of turn along circle -> t
Radius of circle -> rad
Final angle covered by circle -> finAng
Number of points of linear interpolation of angle -> steps
"""
WorldFromEETfm = self.arm.manip.GetEndEffectorTransform()
initTfm = np.eye(4)
initOrigin = d*rad*np.array([0,1,0])
initTfm[0:3,3] = initOrigin
transforms = []
for step in range(steps+1):
ang = float(finAng)*step/steps
rotMat = np.eye(4)
rotMat[0,0] = np.cos(t*ang)
rotMat[0,1] = -np.sin(t*ang)
rotMat[1,0] = np.sin(t*ang)
rotMat[1,1] = np.cos(t*ang)
rotMat[0:3,3] = -1*initOrigin
transforms.append(WorldFromEETfm.dot(rotMat.dot(initTfm)))
return self.smoothPlan(transforms)