def plotWorld(self):
self.flushWorld()
self.robot3d()
scene = Scene()
tf(self.world, Rx(-np.pi / 2))
key_light = [
DirectionalLight(color='#ffffff',
intensity=0.6,
position=[10, 10, 10])
]
ambient_light = [AmbientLight(color='#777777')]
scene.add(self.world)
scene.add(key_light)
scene.add(ambient_light)
c = PerspectiveCamera(position=[2, 2, 2])
renderer = Renderer(camera=c,
background='black',
background_opacity=1,
scene=scene,
controls=[OrbitControls(controlling=c)],
width=800,
height=800)
display(renderer)
def putPlane(H, size=12, color='#e0e0e0'):
plane = Mesh(geometry=PlaneGeometry(width=size, height=size),
material=MeshLambertMaterial(color=color),
position=[0, 0, 0])
tf(plane, H * Tx(size / 2 - 1) * Ty(size / 2 - 1))
return plane
def state(self, x, y, theta, steerAngle):
z = self.rWheel
H = Tx(x) * Ty(y) * Tz(z) * Rz(theta)
tf(self.baselink, H)
if (steerAngle != 0):
r = self.getRadius(steerAngle)
alphaInner, alphaOuter = self.getSteerAngles(steerAngle)
ri = self.getRadiusInner(steerAngle) / np.cos(alphaInner)
ro = self.getRadiusOuter(steerAngle) / np.cos(alphaOuter)
tf(self.wheelLeftFront, Rz(alphaInner))
tf(self.wheelRightFront, Rz(alphaOuter))
Hicr = Ty(r)
tf(self.ICR, Hicr)
updateLine(self.rLine, r)
updateLine(self.lfLine, ri)
updateLine(self.rfLine, ro)
def getI(length=0.2):
k = getJ(length=length, color='red')
return tf(k, Rz(-np.pi / 2))
def getK(length=0.2):
k = getJ(length=length, color='blue')
return tf(k, Rx(np.pi / 2))
def robot3d(self):
Hbaselink = Tx(0)
self.baselink = rf(Hbaselink)
HbaseEnd = Tx(self.wheelBase)
baseEnd = tf(getOrigin(), HbaseEnd)
self.baselink.add(baseEnd)
HwheelLeftFrontHinge = Ty(self.track / 2)
wheelLeftFrontHinge = tf(getOrigin(), HwheelLeftFrontHinge)
baseEnd.add(wheelLeftFrontHinge)
HwheelRightFrontHinge = Ty(-self.track / 2)
wheelRightFrontHinge = tf(getOrigin(), HwheelRightFrontHinge)
baseEnd.add(wheelRightFrontHinge)
alphaInner = 0
alphaOuter = 0
steerAngle = 0
HwheelLeftFront = Rz(alphaInner)
self.wheelLeftFront = rf(HwheelLeftFront)
wheelLeftFrontHinge.add(self.wheelLeftFront)
##self.baselink.add(tf(getOrigin(),HwheelLeftFront))
# HwheelCentralFront = HbaseEnd * Rz(steerAngle)
# self.baselink.add(rf(HwheelCentralFront))
# self.baselink.add(getWheelLine(HwheelCentralFront, getRadius(steerAngle)/np.cos(steerAngle)))
##self.baselink.add(tf(getOrigin(),HwheelRightFront))
HwheelRightFront = Rz(alphaOuter)
self.wheelRightFront = rf(HwheelRightFront)
wheelRightFrontHinge.add(self.wheelRightFront)
##self.baselink.add(tf(getOrigin(),HwheelRightFront))
r = 0
Hicr = Ty(r)
self.ICR = getICR(Hicr)
self.baselink.add(self.ICR)
self.rLine = getWheelLine(Tx(0), r)
self.baselink.add(self.rLine)
self.lfLine = getWheelLine(Tx(0), r)
self.wheelLeftFront.add(self.lfLine)
self.rfLine = getWheelLine(Tx(0), r)
self.wheelRightFront.add(self.rfLine)
HwheelLeftRearHinge = Ty(self.track / 2)
# self.baselink.add(HwheelLeftRearHinge)
HwheelRightRearHinge = Ty(-self.track / 2)
# self.baselink.add(HwheelLeftRearHinge)
HwheelLeftRear = HwheelLeftRearHinge
wheelLeftRear = tf(getOrigin(), HwheelLeftRear)
##self.baselink.add(rf(HwheelLeftRear))
self.baselink.add(wheelLeftRear)
HwheelRightRear = HwheelRightRearHinge
wheelRightRear = tf(getOrigin(), HwheelRightRear)
##self.baselink.add(rf(HwheelRightRear))
self.baselink.add(wheelRightRear)
self.wheelLeftFront.add(self.putWheel(Tx(0)))
self.wheelRightFront.add(self.putWheel(Tx(0)))
wheelLeftRear.add(self.putWheel(Tx(0)))
wheelRightRear.add(self.putWheel(Tx(0)))
# self.baselink.add(putWheel(HwheelCentralFront))
self.baselink.add(putCylinder(length=self.track, H=Tx(0)))
baseEnd.add(putCylinder(length=self.track, H=Tx(0)))
self.baselink.add(
putCylinder(r1=0.02,
r2=0.01,
length=self.wheelBase,
H=Tx(self.wheelBase / 2) * Rz(np.pi / 2)))
# plot3D(self.baselink)
return self.baselink
def putWall(self, H):
wall = self.getWall()
tf(wall, H * Tz(0.2) * Rx(np.pi / 2))
return wall
def putWheel(self, H):
wheel = self.getWheel()
tf(wheel, H)
return wheel
def getICR(H):
point = getOrigin(r=0.015)
tf(point, H)
return point
def rf(H):
origin = getCoordinateFrame()
tf(origin, H)
return origin
def putCylinder(length, H, r1=0.01, r2=0.01, color='orange'):
cylinder = getCylinder(length, r1, r2, color)
tf(cylinder, H)
return cylinder
