60 def __init__(self, offset, thickness, width, joint_radius, wallType):
63 self.
width = width + joint_radius
73 def checkJoint(self, v):
102 def __init__(self, links, obstacles=0, extra=1):
103 super(ChainConstraint, self).__init__(3 * links, links + extra)
105 self.length = ChainConstraint.LINK_LENGTH
106 self.
width = ChainConstraint.WALL_WIDTH
107 self.radius = links - 2
108 self.jointRadius = ChainConstraint.JOINT_RADIUS
109 self.obstacles = obstacles
111 step = 2. * self.radius / (obstacles + 1.)
113 self.
width, self.jointRadius, i % 2)
114 for i
in range(obstacles)]
116 def function(self, x, out):
118 for i
in range(self.links):
119 joint2 = x[(3 * i):(3 * i + 3)]
120 out[i] = np.linalg.norm(joint1 - joint2) - self.length
124 out[self.links] = np.linalg.norm(x[-3:]) - self.radius
129 out[self.links + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2]
131 out[self.links + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0]
133 out[self.links + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2]
135 def jacobian(self, x, out):
136 out[:, :] = np.zeros(
137 (self.getCoDimension(), self.getAmbientDimension()), dtype=np.float64)
139 plus = np.zeros(3 * (self.links + 1))
140 plus[:(3 * self.links)] = x[:(3 * self.links)]
142 minus = np.zeros(3 * (self.links + 1))
143 minus[-(3 * self.links):] = x[:(3 * self.links)]
145 diagonal = plus - minus
147 for i
in range(self.links):
148 out[i, (3 * i):(3 * i + 3)] = normalize(diagonal[(3 * i):(3 * i + 3)])
149 out[1:self.links, 0:(3 * self.links - 3)
150 ] -= out[1:self.links, 3:(3 * self.links)]
153 out[self.links, -3:] = -normalize(diagonal[-3:])
158 out[self.links + 1, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
160 out[self.links + 2, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
162 out[self.links + 3, (o * 3 + 2):(o * 3 + 5)] = [1, -1]
166 def isValid(self, state):
167 x = np.array([state[i]
for i
in range(self.getAmbientDimension())])
168 for i
in range(self.links):
169 joint = x[(3 * i):(3 * i + 3)]
172 if np.linalg.norm(joint) >= self.radius - self.jointRadius:
173 for wall
in self.walls:
174 if not wall.checkJoint(joint):
177 for i
in range(self.links):
178 joint1 = x[(3 * i):(3 * i + 3)]
179 if np.max(np.absolute(joint1)) < self.jointRadius:
182 for j
in range(i + 1, self.links):
183 joint2 = x[(3 * j):(3 * j + 3)]
184 if np.max(np.absolute(joint1 - joint2)) < self.jointRadius:
189 def createSpace(self):
193 for i
in range(self.links):
194 bounds.setLow(3 * i + 0, -i - 1)
195 bounds.setHigh(3 * i + 0, i + 1)
197 bounds.setLow(3 * i + 1, -i - 1)
198 bounds.setHigh(3 * i + 1, i + 1)
200 bounds.setLow(3 * i + 2, -i - 1)
201 bounds.setHigh(3 * i + 2, i + 1)
203 rvss.setBounds(bounds)
206 def getStartAndGoalStates(self):
207 start = np.zeros(3 * self.links)
208 goal = np.zeros(3 * self.links)
210 for i
in range(self.links - 3):
215 goal[3 * i] = -(i + 1)
222 start[3 * i + 1] = -1
232 start[3 * i + 1] = -1
245 goal[3 * i] = -(i - 1)
254 def getProjection(self, space):
257 def __init__(self, space, links, radius):
258 super(ChainProjection, self).__init__(space)
262 self.defaultCellSizes()
264 def getDimension(self):
267 def defaultCellSizes(self):
268 self.cellSizes_ = list2vec([.1, .1])
270 def project(self, state, projection):
271 s = 3 * (self.links - 1)
272 projection[0] = math.atan2(state[s + 1], state[s])
273 projection[1] = math.acos(state[s + 2] / self.radius)
275 return ChainProjection(space, self.links, self.radius)
277 def dump(self, outfile):
278 print(self.links, file=outfile)
279 print(self.obstacles, file=outfile)
280 print(self.extra, file=outfile)
281 print(self.jointRadius, file=outfile)
282 print(self.length, file=outfile)
283 print(self.radius, file=outfile)
284 print(self.
width, file=outfile)
286 def addBenchmarkParameters(self, bench):
287 bench.addExperimentParameter(
"links",
"INTEGER", str(self.links))
288 bench.addExperimentParameter(
289 "obstacles",
"INTEGER", str(self.obstacles))
290 bench.addExperimentParameter(
"extra",
"INTEGER", str(self.extra))