German Larrain avatar German Larrain committed c77c51d

fifth example: based on VehicleWithControlledArm

Comments (0)

Files changed (1)

bin/IROS/example5_vehicle_with_user_input.py

+#!/usr/bin/env python
+
+# Created on 2012.01.30
+#
+# @author: german
+
+
+#===============================================================================
+# """
+# Runs a simulation of a vehicle with two powered wheels and one
+# free-rotating spherical wheel. It has a 2-link robotic arm attached,
+# with joints either friction-free or with friction proportional to
+# joint speed. The second joint has a PD controller.
+# """
+#===============================================================================
+
+"""
+Example #5
+"""
+import os
+import sys
+
+# we need to add the parent directory to the Python path (PYTHONPATH)
+CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))
+sys.path.append(os.path.dirname(CURRENT_DIR))
+
+from VehicleWithArm import VehicleWithArm
+
+class Example5(VehicleWithArm):
+	
+	WHEEL_TORQUE = 3
+		
+	WHEEL_R_PARAMS = ((0.4,0.3,(0,0,-0.5)),{'mass':1}) # ((length, radius, center), mass)
+	WHEEL_L_PARAMS = ((0.4,0.3,(0,0,0.5)),{'mass':1})
+	
+	SP_STEP = 0.1
+	q2_SP = 0.0 # mut.pi/3 # 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=True):
+		""" Constructor, which calls the superclass constructor. """
+		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_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.get_time_step()
+			pos = self.sim.get_object(self.chassis).get_position()
+			q1 = self.sim.get_joint('r1').get_joint().get_angle()
+			q2 = self.sim.get_joint('r2').get_joint().get_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' % 
+				(time,pos[0],pos[1],q1,q2,mv,self.torque_w1,self.torque_w2))
+			
+			self.torque_w1 = 0.0
+			self.torque_w2 = 0.0
+			
+		except Exception as e:
+			print('Exception when executing on_pre_step: %s' % str(e))
+	
+	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
+
+if __name__ == '__main__':
+	sim_program = Example5(False, False)
+	sim_program.start()
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.