Source

ars / demos / IROS / example5_vehicle_with_user_input.py

Full commit
"""Example #5. To achieve the same results reported in the paper, the contact
joint mu parameter (in :func:`ars.model.simulator.collision near_callback`)
must be set to 50, instead of the default 500 value.

"""
from ..VehicleWithArm import VehicleWithArm, logger


class Example5(VehicleWithArm):

	FPS = 50
	STEPS_PER_FRAME = 80
	CAMERA_POSITION = (15, 10, 15)
	PRINT_KEY_INFO = False

	WHEEL_TORQUE = 3

	# ((length, radius, center), mass)
	WHEEL_R_PARAMS = ((0.4, 0.3, (0, 0, -0.5)), {'mass': 1})
	WHEEL_L_PARAMS = ((0.4, 0.3, (0, 0, 0.5)), {'mass': 1})

	# joint 2 controller params
	SP_STEP = 0.1  # set point step
	q2_INITIAL_SP = 0.0  # initial set point
	R2_KP = 3.0  # controller proportional action
	R2_KD = 3.0  # controller derivative action

	def __init__(self, use_capsule_wheels=False, frictionless_arm=False):
		VehicleWithArm.__init__(self, use_capsule_wheels, frictionless_arm)

		self.key_press_functions.add('d', self.increase_sp)
		self.key_press_functions.add('c', self.decrease_sp)

		self.sp = self.q2_INITIAL_SP
		self.previous_error = 0.0
		self.torque_w1 = 0.0
		self.torque_w2 = 0.0

	def on_pre_step(self):
		try:
			time = self.sim.sim_time
			time_step = self.sim.time_step
			pos = self.sim.get_object(self.chassis).get_position()
			q1 = self.sim.get_joint('r1').joint.angle
			q2 = self.sim.get_joint('r2').joint.angle

			mv = self.get_compensation(self.sp, q2, time_step)
			self.apply_torque_to_joints(0, mv)  # torque1, torque2

			print('%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e' %
			      (time, pos[0], pos[2], q1, q2, self.sp, mv,
			       self.torque_w1, self.torque_w2))

			self.torque_w1 = 0.0
			self.torque_w2 = 0.0

		except Exception:
			logger.exception("Exception when executing on_pre_step")

	def apply_torque_to_wheels(self, torque1, torque2):
		VehicleWithArm.apply_torque_to_wheels(self, torque1, torque2)
		self.torque_w1 = torque1
		self.torque_w2 = torque2

	def increase_sp(self):
		"""Increase angle set point."""
		self.sp += self.SP_STEP

	def decrease_sp(self):
		"""Decrease angle set point."""
		self.sp -= self.SP_STEP

	def get_compensation(self, sp, q, time_step):
		"""Calculate the control torque with a PD controller."""
		error = (sp - q)
		error_p = (error - self.previous_error) / time_step
		torque = self.R2_KP * error + self.R2_KD * error_p
		self.previous_error = error
		return torque