Commits

German Larrain committed 42a4b4f

fifth example: more sim parameters customized; bugfix in printed variables; comments cleanup.

Comments (0)

Files changed (1)

bin/IROS/example5_vehicle_with_user_input.py

 #
 # @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. To achieve the same results reported in the paper,
 the contact joint mu parameter (in ars.model.simulator.collision near_callback function)
 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))
 sys.path.append(os.path.dirname(CURRENT_DIR))
 
-from VehicleWithArm import VehicleWithArm
+from VehicleWithArm import VehicleWithArm, mut
 
 class Example5(VehicleWithArm):
 	
+	FPS = 50
+	STEPS_PER_FRAME = 80
+	CAMERA_POSITION = (15,10,15)
+	PRINT_KEY_INFO = False
+	
 	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
+	# 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
 	
 		self.key_press_functions.add('d', self.increase_sp)
 		self.key_press_functions.add('c', self.decrease_sp)
 		
-		self.sp = self.q2_SP
+		self.sp = self.q2_INITIAL_SP
 		self.previous_error = 0.0
 		self.torque_w1 = 0.0
 		self.torque_w2 = 0.0
 			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))
+			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