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