def getSphere(dimX, dimY, dimZ, pos=[0, 0, 0, 0, 0, 0]):
sphere = Geometry3D()
sphere.loadFile("sphere.off")
sphere.scale(dimX, dimY, dimZ)
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
sphere.transform(rotMat, [pos[0], pos[1], pos[2]])
return sphere
def getCube(dimX=0.35, dimY=0.35, dimZ=0.35, pos=[0, 0, 0, 0, 0, 0]):
cube = Geometry3D()
cube.loadFile("cube.off")
cube.scale(dimX, dimY, dimZ)
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
cube.transform(rotMat, [pos[0], pos[1], pos[2]])
return cube, pos, [dimX, dimY, dimZ]
def getSpheres(self):
for i in range(len(self.obsList)):
obsDim = self.obsDimList[i]
pos = self.obsList[i]
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
obsSphere = so3.apply(
rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2])
obsSphere = np.array(obsSphere)
obsSphere = obsSphere + [pos[0], pos[1], pos[2]]
self.spherePosList.append(obsSphere)
return self.spherePosList
def checkCollision(self, q):
collisionFlag = False
# Hardcoded robot dimensions
robDim = [0.05, 0.05, 0.05]
robSphereRad = self.getRadius(robDim)
rotMat = mathUtils.euler_zyx_mat([q[3], q[4], q[5]])
# position is half of the robots dimension - Center of the approximated robot when placed at [0,0,0]
robSphere = so3.apply(rotMat,
[robDim[0] / 2, robDim[1] / 2, robDim[2] / 2])
robSphere = np.array(robSphere)
robSphere = robSphere + [q[0], q[1], q[2]]
if len(self.spherePosList) == len(self.obsList):
for i in range(len(self.spherePosList)):
obsDim = self.obsDimList[i]
obsSphereRad = self.getRadius(obsDim)
obsSphere = self.spherePosList[i]
dist = np.sqrt((obsSphere[0] - robSphere[0])**2 +
(obsSphere[1] - robSphere[1])**2 +
(obsSphere[2] - robSphere[2])**2)
if (dist <= robSphereRad + obsSphereRad + self.epsilon):
collisionFlag = True
break
return collisionFlag
for i in range(len(self.obsList)):
obsDim = self.obsDimList[i]
obsSphereRad = self.getRadius(obsDim)
pos = self.obsList[i]
rotMat = mathUtils.euler_zyx_mat([pos[3], pos[4], pos[5]])
obsSphere = so3.apply(
rotMat, [obsDim[0] / 2, obsDim[1] / 2, obsDim[2] / 2])
obsSphere = np.array(obsSphere)
obsSphere = obsSphere + [pos[0], pos[1], pos[2]]
dist = np.sqrt((obsSphere[0] - robSphere[0])**2 +
(obsSphere[1] - robSphere[1])**2 +
(obsSphere[2] - robSphere[2])**2)
if (dist <= robSphereRad + obsSphereRad):
collisionFlag = True
break
return collisionFlag
def setConfig(self, x, y, z, sc, tht):
self.currConfig = [x, y, z, sc, tht]
cosTht = math.cos(tht)
sinTht = math.sin(tht)
vis.lock()
pt = [x, y, z]
rotMat = mathUtils.euler_zyx_mat([tht, 0, 0])
vis.add(self.robotSystemName, [rotMat, pt])
for iR in range(self.n):
q = self.robots[iR].getConfig()
scSj = vectorops.mul(self.sj[iR], sc)
q[0] = self.currConfig[0] + cosTht * scSj[0] - sinTht * scSj[1]
q[1] = self.currConfig[1] + sinTht * scSj[0] + cosTht * scSj[1]
q[2] = self.currConfig[2] + scSj[2]
self.robots[iR].setConfig(q)
vis.unlock()
self.checkCollision()
def getTransform(self):
q = self.robot.getConfig()
theta = [q[3], q[4], q[5]]
rotMat = mathUtils.euler_zyx_mat(theta)
return [rotMat, [q[0], q[1], q[2]]]
def orientation_dist(orient1, orient2):
rotMat1 = mathUtils.euler_zyx_mat(orient1)
rotMat2 = mathUtils.euler_zyx_mat(orient2)
dist = so3.distance(rotMat1, rotMat2)
return dist