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)