def move(self, period):
d_theta = self.state[1]*pi/180*period
d_x = self.state[0]*cos(d_theta)*period
d_y = self.state[0]*sin(d_theta)*period
if self.actuated:
self.actuated.x += (d_x*cos(self.actuated.yaw*pi/180)-
d_y*sin(self.actuated.yaw*pi/180))
self.actuated.y += (d_x*sin(self.actuated.yaw*pi/180)+
d_y*cos(self.actuated.yaw*pi/180))
self.actuated.yaw += d_theta*180/pi
for i in range(len(self.casterCranks)):
self.casterCranks[i].yaw += self.casterRates[i]*period
WheelDrive.move(self, period)
def move(self, period):
if abs(self.state[1]) >= self.epsilon:
radius = self.wheelBase/tan(self.state[1]*pi/180)
d_theta = self.state[0]*period/radius
d_x = radius*sin(d_theta)
d_y = radius*(1-cos(d_theta))
else:
d_theta = 0
d_x = self.state[0]*period
d_y = 0
if self.actuated:
self.actuated.x += (d_x*cos(self.actuated.yaw*pi/180)-
d_y*sin(self.actuated.yaw*pi/180))
self.actuated.y += (d_x*sin(self.actuated.yaw*pi/180)+
d_y*cos(self.actuated.yaw*pi/180))
self.actuated.yaw += d_theta*180/pi
for i in [0, 1]:
self.wheels[i].yaw += self.wheelSteeringRates[i]*period
WheelDrive.move(self, period)