Commits

German Larrain committed 804db70 Merge
  • Participants
  • Parent commits d668b4a, 42a4b4f

Comments (0)

Files changed (8)

File ars/app/__init__.py

 
 	WRITE_DATA_FILES = False
 	DEBUG = False
+	PRINT_KEY_INFO = True
 	
 	WINDOW_TITLE = "Autonomous Robot Simulator"
 	WINDOW_POSITION = (0,0)
 		if self.WRITE_DATA_FILES: self.close_files(self.sim.data_files)
 	
 	def reset_simulation(self):
-		print("reset simulation")
+		if self.PRINT_KEY_INFO:
+			print("reset simulation")
 		self.do_create_window = True
 		self.gAdapter.reset()
 		self.create_simulation()
 		# TODO: add 'toggle use of QuickStep function'
 
 	def on_action_selection(self, key):
-		print(key)
+		if self.PRINT_KEY_INFO:
+			print(key)
 		try:
 			if self.key_press_functions.has_key(key):
 				if self.key_press_functions.is_repeat(key):
 				else:
 					self.key_press_functions.call(key)
 			else:
-				print('unregistered key: %s' % key)
+				if self.PRINT_KEY_INFO:
+					print('unregistered key: %s' % key)
 		except Exception as ex:
 			print(ex)
 
 		add torque to an already selected joint
 		'''
 		print('add_torque has not been fully implemented')
-		self.sim.get_joint('r1').add_torque(1000) # FIXME: use member or global variable
+		#self.sim.get_joint('r1').add_torque(1000) # FIXME: use member or global variable
 	
 	def inc_joint_vel(self):
 		'''

File ars/model/simulator/collision.py

 	Callback function for the collide() method (in ODE). This function checks if the given geoms
 	do collide and creates contact joints (c_joint) if they do, except if they are connected.
 	"""
-	c_joint_bounce = 0.2
-	c_joint_mu = 500 # 0-5 = very slippery, 50-500 = normal, 5000 = very sticky
+	c_joint_bounce = 0.2 # default: 0.2
+	c_joint_mu = 500 # default: 500. 0-5 = very slippery, 50-500 = normal, 5000 = very sticky
 
 	if (ode.areConnected(geom1.getBody(), geom2.getBody())):
 		return

File bin/ControlledSimpleArm.py

 	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'density':1})
 	
 	SP_STEP = 0.1
-	q2_SP = 0.0 # mut.pi/3 # set point
+	q2_INITIAL_SP = 0.0 # mut.pi/3 # set point
 	R2_KP = 1.0 # controller proportional action
 	R2_KD = 0.5 # controller derivative action
 	
 		
 		self.set_pre_step_callback(self.on_pre_step)
 		
-		self.sp = self.q2_SP
+		self.sp = self.q2_INITIAL_SP
 		self.previous_error = 0.0
 	
 	def create_sim_objects(self):

File bin/IROS/example2_conical_pendulum.py

 Example #2
 """
 
-from ars.app import Program
+from ars.app import Program, Simulation
 import ars.utilities.mathematical as mut
 import ars.constants as cts
 
-class Example2(Program):	
-
-	OFFSET = (2,0.5,2)
+class Example2(Program):
+	
+	# simulation & window parameters
+	CAMERA_POSITION = (6,3,6)
+	FPS = 50
+	STEPS_PER_FRAME = 80
+	
+	# bodies' parameters
+	DELTA = 0.01 # to prevent the collision of the 2nd link with the floor
+	OFFSET = (1,0,2)
 	BOX_PARAMS = (((10,0.5,10),(0,-0.25,0)),{'density':100}) # ((size, center), density)
 	
-	WINDOW_SIZE = (900,600)
-	# (0,-4,1) in C++ example #vp_xyz = (0.0,-4.0,1.0) # position [meters]
-	CAMERA_POSITION = (2,5,10)
-	FPS = 50
-	STEPS_PER_FRAME = 80
-
 	POLE_RADIUS = 0.141421 # 1/(5*sqrt(2))
 	POLE_HEIGHT = 1
-	POLE_INITIAL_POS = (0.0,1.0,0.0)
+	POLE_INITIAL_POS = (0.0,0.5 + DELTA,0.0)
 	POLE_MASS = 10.0
 	
 	ARM_RADIUS = 0.141421
 	ARM_LENGTH = 1.0
-	ARM_INITIAL_POS = (0.0,1.0,0.1)
+	ARM_INITIAL_POS = (0.0,0.5 + DELTA,0.1)
 	ARM_MASS = 10.0
 	
 	JOINT1_ANCHOR = (0.0,0.0,0.0)
 	JOINT1_AXIS = cts.Y_AXIS
-	JOINT2_ANCHOR = (0.0,1.5,0.1)
+	JOINT2_ANCHOR = (0.0,1.0 + DELTA,0.1)
 	JOINT2_AXIS = cts.X_AXIS
 	
 	Q1_FRICTION_COEFF = (50e-3)*100
 	Q2_FRICTION_COEFF = (50e-3)*100
 
+	# control
 	MAX_TORQUE = 20
 	SATURATION_TIME = 1
 
 		
 		self.q1p_prev = 0.0
 		self.q2p_prev = 0.0
+		
+	def create_simulation(self):
+		# we didn't need to code this method
+		# but if we want to modify the floor, we have to
+		
+		# set up the simulation parameters
+		self.sim = Simulation(self.FPS, self.STEPS_PER_FRAME)
+		self.sim.add_basic_simulation_objects()
+		self.sim.add_axes()
+		self.sim.add_floor(normal=(0,1,0), box_size=self.FLOOR_BOX_SIZE, 
+						color=(0.7,0.7,0.7), dist=-0.5, box_center=(0,-0.5,0))
+		
+		self.create_sim_objects()
+		
+		# add the graphic objects
+		self.gAdapter.add_objects_list(self.sim.get_actors().values())
+		self.sim.update_actors()
 	
 	def create_sim_objects(self):
 		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])
 							self.sim.get_object(arm), 
 							mut.add3(self.OFFSET, self.JOINT2_ANCHOR),
 							self.JOINT2_AXIS)
+		
+		#self.sim.get_object(box).get_actor().set_color(cts.COLOR_RED)
+		self.sim.get_object(pole).get_actor().set_color(cts.COLOR_YELLOW)
+		self.sim.get_object(arm).get_actor().set_color(cts.COLOR_NAVY)
+		
 		self.box = box
 		self.pole = pole
 		self.arm = arm
 	sim_program = Example2()
 	sim_program.start()
 	
+	# print arm links' inertia matrices
 	pole_body = sim_program.sim.get_object(sim_program.pole).get_body()
 	arm_body = sim_program.sim.get_object(sim_program.arm).get_body()
 	print(pole_body.get_inertia_tensor())

File bin/IROS/example3_speed_profile.py

 
 class Example3(VehicleWithArm):
 	
-	WINDOW_SIZE = (1024,630)
-	CAMERA_POSITION = (0,8,15)
+	#WINDOW_SIZE = (1024,630)
+	CAMERA_POSITION = (0,8,25) # (0,8,15)
 	
 	FPS = 50
 	STEPS_PER_FRAME = 80

File bin/IROS/example4_sinusoidal_terrain.py

 # @author: german
 
 """
-Experiment #2 for a paper submitted to the 
-2012 Robotics Science and Systems conference
+Example #4
 """
 import os
 import sys
 	
 	FPS = 50
 	STEPS_PER_FRAME = 80
+	CAMERA_POSITION = (0,8,30)
 
 	VEHICLE_OFFSET = (-1.05,-0.35,5)
 	TM_X, TM_Z = (40,20)
 	WHEELS_TORQUE = 4.0
 	MAX_SPEED = 2.0
 	
+	# arm controller
 	q1_SP = 0.0 # set point
 	R1_KP = 20.0 # controller proportional action
 	R1_KD = 15.0 # controller derivative action
 	R2_KP = 20.0
 	R2_KD = 15.0
 	
+	# screenshot recorder
+	RECORDER_BASE_FILENAME = 'sin'
+	RECORD_PERIODICALLY = True
+	
 	def __init__(self):
 		""" Constructor, calls first the superclass constructor. """
 		VehicleWithArm.__init__(self)
 		self.set_pre_step_callback(self.on_pre_step)
+		self.create_screenshot_recorder(
+			self.RECORDER_BASE_FILENAME, self.RECORD_PERIODICALLY)
 		
 		self.q1_previous_error = 0.0
 		self.q2_previous_error = 0.0

File bin/IROS/example5_vehicle_with_user_input.py

+#!/usr/bin/env python
+
+# Created on 2012.01.30
+#
+# @author: german
+
+"""
+Example #5. To achieve the same results reported in the paper,
+the contact joint mu parameter (in ars.model.simulator.collision near_callback function)
+must be set to 50, instead of the default 500 value.
+"""
+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, 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})
+	
+	# 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=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_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.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\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 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()

File bin/VehicleWithArm.py

 	BACKGROUND_COLOR = (0.8,0.8,0.8)
 	FLOOR_BOX_SIZE = (20,0.01,20)
 	
-	WHEEL_TORQUE = 400
+	WHEEL_TORQUE = 3
 	VEHICLE_OFFSET = (2,0.35,4)
 	
 	BALL_PARAMS = ((0.3,(1,0,0)),{'density':1}) # ((radius, center), density)
 		self._frictionless_arm = frictionless_arm
 		
 		Program.__init__(self)
-		self.key_press_functions.add('up', self.go_forwards)
-		self.key_press_functions.add('down', self.go_backwards)
-		self.key_press_functions.add('left', self.turn_left)
-		self.key_press_functions.add('right', self.turn_right)
+		self.key_press_functions.add('up', self.go_forwards, repeat=True)
+		self.key_press_functions.add('down', self.go_backwards, repeat=True)
+		self.key_press_functions.add('left', self.turn_left, repeat=True)
+		self.key_press_functions.add('right', self.turn_right, repeat=True)
 		
 		self.key_press_functions.add('a', self.rotate_clockwise)
 		self.key_press_functions.add('z', self.rotate_counterlockwise)