def __init__ (self, obj, parent=None):
""" Constructor method. """
# Call the constructor of the parent class
PhysicsWheelRobot.__init__(self, obj, parent)
# get wheel references and ID's
self.get_wheels()
self._max_steering_angle = math.radians(self._max_steering_angle)
# construct the vehicle
self.build_vehicle()
self._axle_distance = self.get_distance_axle()
logger.warn("Using wheel separation of %.4f" % self._trackWidth)
self.pid_v = PIDController(kp =self._vkp, kd =self._vkd,
ki =self._vki, limits_integrator =self._vki_limits)
self.pid_w = PIDController(kp =self._wkp, kd =self._wkd,
ki = self._wki, limits_integrator = self._wki_limits)
# Force speed at 0.0 at startup
self.apply_vw_wheels(0.0, 0.0)
class PhysicsAckermannRobot(PhysicsWheelRobot):
"""
Base class for mobile robots following the Ackermann steering principle
This base class handles the simulation of the physical interactions
between Ackermann-like vehicle and the ground.
It assumes the vehicle has 4 wheels.
To ensure proper (v, w) enforcement, the model relies on some
internal PID controller, hence the different properties.
"""
add_property('_max_steering_angle', 45.0, 'max_steering_angle', 'double',
'The bigger angle possible the vehicle is able to turn \
its front wheel (in degree)')
add_property('_vkp', 1.0, 'velocity_p_gain', 'double',
'the proportional gain for linear velocity')
add_property('_vkd', 1.0, 'velocity_d_gain', 'double',
'the differiential gain for linear velocity')
add_property('_vki', 1.0, 'velocity_i_gain', 'double',
'the integral gain for linear velocity')
add_property('_vki_limits', 1.0, 'velocity_integral_limits', 'double',
'limits of the integral term for velocity')
add_property('_wkp', 1.0, 'angular_velocity_p_gain', 'double',
'the proportional gain for angular velocity')
add_property('_wkd', 1.0, 'angular_velocity_d_gain', 'double',
'the differiential gain for angular velocity')
add_property('_wki', 1.0, 'angular_velociy_i_gain', 'double',
'the integral gain for angular velocity')
add_property('_wki_limits', 1.0, 'angular_velocity_integral_limits', 'double',
'limits of the integral term for velocity')
def __init__ (self, obj, parent=None):
""" Constructor method. """
# Call the constructor of the parent class
PhysicsWheelRobot.__init__(self, obj, parent)
# get wheel references and ID's
self.get_wheels()
self._max_steering_angle = math.radians(self._max_steering_angle)
# construct the vehicle
self.build_vehicle()
self._axle_distance = self.get_distance_axle()
logger.warn("Using wheel separation of %.4f" % self._trackWidth)
self.pid_v = PIDController(kp =self._vkp, kd =self._vkd,
ki =self._vki, limits_integrator =self._vki_limits)
self.pid_w = PIDController(kp =self._wkp, kd =self._wkd,
ki = self._wki, limits_integrator = self._wki_limits)
# Force speed at 0.0 at startup
self.apply_vw_wheels(0.0, 0.0)
def build_vehicle(self):
""" Apply the constraints to the vehicle parts. """
# get track width
self._trackWidth = self.get_track_width()
# set up wheel constraints
# add wheels to either suspension arms or vehicle chassis
if self._has_suspension:
self.build_model_with_suspension()
else:
self.build_model_without_suspension()
def build_model_without_suspension(self):
""" Add all the constraints to attach the wheels to the body """
for index in ['FR', 'FL']:
self._wheel_joints[index] = self.attach_front_wheel_to_body(
self._wheels[index], self.bge_object)
for index in ['RR', 'RL']:
self._wheel_joints[index] = self.attach_rear_wheel_to_body(
self._wheels[index], self.bge_object)
def attach_front_wheel_to_body(self, wheel, parent):
""" Attaches the wheel to the given parent using a 6DOF constraint
Set the wheel positions relative to the robot in case the
chassis was moved by the builder script or manually in blender
"""
joint = Joint6DoF(wheel, parent)
joint.limit_rotation_dof('Y', -self._max_steering_angle, self._max_steering_angle)
joint.free_rotation_dof('Z')
return joint # return a reference to the constraint
def attach_rear_wheel_to_body(self, wheel, parent):
""" Attaches the wheel to the given parent using a 6DOF constraint
Set the wheel positions relative to the robot in case the
chassis was moved by the builder script or manually in blender
"""
joint = Joint6DoF(wheel, parent)
joint.free_rotation_dof('Z')
return joint # return a reference to the constraint
def apply_vw_wheels(self, vx, vw):
"""
Apply (v, w) on the parent robot.
We cannot rely on the theoric ackermann model due to important
friction generation by front wheel. So, use a simple PID to
guarantee the constraints
"""
self.pid_v.setpoint = vx
vel = self.bge_object.localLinearVelocity
computed_vx = self.pid_v.update(vel[0])
self.pid_w.setpoint = vw
angular_vel = self.bge_object.localAngularVelocity
computed_vw = self.pid_w.update(angular_vel[2])
self._apply_vw_wheels(computed_vx, computed_vw)
def _apply_vw_wheels(self, vx, vw):
""" Apply (v, w) to the parent robot.
Implement theoric Ackermann model
"""
velocity_control = 'Z'
steering_control = 'Y'
# calculate desired wheel speeds and set them
if abs(vx) < 0.001 and abs(vw) < 0.001:
# stop the wheel when velocity is below a given threshold
for index in ['RL', 'RR']:
self._wheel_joints[index].angular_velocity(velocity_control, 0)
for index in ['FR', 'FL']:
self._wheel_joints[index].angular_velocity(steering_control, 0)
self._stopped = True
else:
# this is need to "wake up" the physic objects if they have
# gone to sleep apply a tiny impulse straight down on the
# object
if self._stopped:
self.bge_object.applyImpulse(
self.bge_object.position, (0.0, 0.0, 0.000001))
# no longer stopped
self._stopped = False
# speed of rear wheels
wx = -1.0 * vx / self._wheel_radius
for index in ['RL', 'RR']:
self._wheel_joints[index].angular_velocity(velocity_control, wx)
logger.debug("Rear wheel speeds set to %.4f" % wx)
vel = self.bge_object.localLinearVelocity
# Compute angle of steering wheels
if abs(vw) > 0.01:
radius = vel[0] / vw
if abs(radius) < (self._trackWidth / 2):
l_angle = math.copysign(self._max_steering_angle, radius)
r_angle = math.copysign(self._max_steering_angle, radius)
else:
cot_l_angle = self._axle_distance / (radius + (self._trackWidth / 2))
cot_r_angle = self._axle_distance / (radius - (self._trackWidth / 2))
l_angle = math.atan(cot_l_angle)
r_angle = math.atan(cot_r_angle)
logger.debug('virtual angle %f' % (math.atan(self._axle_distance / radius)))
else:
l_angle = r_angle = 0.0
logger.info("l_angle %f r_angle %f" % (l_angle, r_angle))
if abs(l_angle) >= self._max_steering_angle or \
abs(r_angle) >= self._max_steering_angle:
logger.warning("wz = %f is not applicable at current speed %f\
due to physical limitation" % (vw, vel[0]))
current_l_angle = self._wheel_joints['FL'].euler_angle(steering_control)
current_r_angle = self._wheel_joints['FR'].euler_angle(steering_control)
diff_l_angle = l_angle - current_l_angle
diff_r_angle = r_angle - current_r_angle
self._wheel_joints['FL'].angular_velocity(steering_control, diff_l_angle)
self._wheel_joints['FR'].angular_velocity(steering_control, diff_r_angle)
logger.debug("Angle left w %f right w %f" % (diff_l_angle, diff_r_angle))