Commits

German Larrain  committed 62929b4

demos: backport of 11686975d30a

  • Participants
  • Parent commits c34faae
  • Branches 0.3.x

Comments (0)

Files changed (17)

File demos/CentrifugalForceTest.py

 However, theta must be calculated with a numerical solver since no simple analytical
 solution exists.
 """
-
 from ars.app import Program
 import ars.utils.mathematical as mut
 import ars.constants as cts
 def get_sphere_volume(radius):
 	return 4.0 / 3.0 * mut.pi * (radius ** 3)
 
+
 class CentrifugalForceTest(Program):
-		
+
 	OFFSET = (2,0.5,2)
 	BOX_PARAMS = (((5,0.5,5),(0,-0.25,0)),{'density':1}) # ((size, center), density)
-	
+
 	WINDOW_SIZE = (900,600)
 	CAMERA_POSITION = (2,5,10) # (0,-4,1) in C++ example #vp_xyz = (0.0,-4.0,1.0) # position [meters]
 	FPS = 50
 	POLE_VISUAL_RADIUS = 0.05 # 5 cm. how it will be displayed
 	POLE_HEIGHT = 2 # 2 m
 	POLE_INITIAL_POS = (0.0,1.0,0.0) # (0.0,0.0,1.0) in C++ example
-	
+
 	BALL_MASS = 1.0 # 1kg
 	BALL_RADIUS = 0.01 # 1 cm
 	BALL_VISUAL_RADIUS = 0.1 # 10 cm
 	BALL_INITIAL_POS = (0.0, 1.0, 1.0)
-	
+
 	JOINT1_ANCHOR = (0.0,0.0,0.0)
 	JOINT1_AXIS = (0.0,1.0,0.0) # (0.0,0.0,1.0) Z-axis in C++ example
 	JOINT1_FMAX = 100
 	JOINT2_ANCHOR = (0.0,2.0,1.0) # (0.0,2.0,1.0) # (0.0,1.0,2.0) in C++ example
 	JOINT2_AXIS = (1.0,0.0,0.0) # X-axis
-	
+
 	CABLE_LENGTH = mut.length3(mut.sub3(BALL_INITIAL_POS, JOINT2_ANCHOR))
-		
+
 	JOINT2_ANGLE_RATE_CONTROLLER_KP = 500.0
 	JOINT1_ANGLE_RATE_INITIAL = 3.0
-	
+
 	def __init__(self):
 		Program.__init__(self)
 		self.key_press_functions.add('a', self.inc_joint1_vel)
 		#self.key_press_functions.add('f', self.rotate_clockwise)
 
 		dispatcher.connect(self.on_pre_frame, SIM_PRE_FRAME_SIGNAL)
-		
+
 		self.joint1_vel_user = self.JOINT1_ANGLE_RATE_INITIAL
 		self.large_speed_steps = True
 		#TODO: set ERP, CFM
-		
-	
+
 	def create_sim_objects(self):
 		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])
 		# FIXME: pole should have no mass! => does not really matter, since speed is set
 		ball_density = self.BALL_MASS / get_sphere_volume(self.BALL_RADIUS)
 		# FIXME: visual radius => did not affect the results noticeably
 		ball = self.sim.add_sphere(self.BALL_RADIUS, self.BALL_INITIAL_POS, density=ball_density)
-		
+
 		# bodies are rotated before attaching themselves through joints
 		self.sim.get_object(pole).rotate(cts.X_AXIS, mut.pi/2)
-		
+
 		self.sim.get_object(box).offset_by_position(self.OFFSET)
 		self.sim.get_object(pole).offset_by_position(self.OFFSET)
 		self.sim.get_object(ball).offset_by_position(self.OFFSET)
-		
+
 		self.sim.add_rotary_joint('r1',					# name, obj1, obj2, anchor, axis
 							self.sim.get_object(box), 
 							self.sim.get_object(pole),
 							None, self.JOINT1_AXIS)
-		
+
 		self.sim.add_rotary_joint('r2', 
 							self.sim.get_object(pole), 
 							self.sim.get_object(ball), 
 			#time = self.sim.sim_time
 			self.set_joint1_speed()
 			self.apply_friction()
-			
+
 			ball_pos = self.sim.get_object(self.ball).get_position()
 			ball_vel = self.sim.get_object(self.ball).get_linear_velocity()
 			ball_omega = self.sim.get_object(self.ball).get_angular_velocity()
 			theta_sim = mut.acos((z_top - ball_pos[1] + self.OFFSET[1]) / self.CABLE_LENGTH) # ball_pos[2] in C++ example
 
 			print((ball_pos, ball_vel, ball_omega, theta_sim))
-			
+
 		except Exception as e:
 			print('Exception when executing on_pre_frame: %s' % str(e))
-		
+
 	def inc_joint1_vel(self):
 		self.joint1_vel_user += self.POLE_SPEED_STEP
-	
+
 	def dec_joint1_vel(self):
 		self.joint1_vel_user -= self.POLE_SPEED_STEP
-	
+
 	def set_joint1_speed(self):
 		self.sim.get_joint('r1').joint.set_speed(self.joint1_vel_user, self.JOINT1_FMAX)
-	
+
 	def apply_friction(self):
 		torque = -self.JOINT2_ANGLE_RATE_CONTROLLER_KP * self.sim.get_joint('r2').joint.angle_rate
 		self.sim.get_joint('r2').joint.add_torque(torque)
 
 if __name__ == '__main__':
 	sim_program = CentrifugalForceTest()
-	sim_program.start()
+	sim_program.start()

File demos/ControlledSimpleArm.py

 from ars.lib.pydispatch import dispatcher
 from ars.model.simulator import SIM_PRE_STEP_SIGNAL
 
+
 def output_data(time, sp, cv, error, torque):
 
 	print('time: %f, sp: %f, cv: %f, error: %f, torque: %f' %
 		(time, sp, cv, error, torque))
 
+
 class ControlledSimpleArm(Program):
 
 	R1_TORQUE = 3
 if __name__ == '__main__':
 	print('Python file being executed: %s' % __file__)
 	sim_program = ControlledSimpleArm()
-	sim_program.start()
+	sim_program.start()

File demos/FallingBall.py

 
 from ars.app import Program
 
+
 class FallingBall(Program):
-	
+
 	def create_sim_objects(self):
 		self.sim.add_sphere(0.5, (1,10,1), density=1) # radius, center, density
 
 if __name__ == '__main__':
 	sim_program = FallingBall()
-	sim_program.start()
+	sim_program.start()

File demos/FallingBalls.py

 
 from ars.app import Program
 
+
 class FallingBalls(Program):
-	
+
 	def create_sim_objects(self):
 		self.sim.add_sphere(0.3, (1,6,1), density=1) # radius, center, density
 		self.sim.add_sphere(0.5, (2,4,2), density=0.5)
 
 if __name__ == '__main__':
 	sim_program = FallingBalls()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example1_bouncing_ball.py

 from ars.lib.pydispatch import dispatcher
 from ars.model.simulator import SIM_PRE_STEP_SIGNAL
 
+
 class Example1(Program):
-	
+
 	FPS = 50
 	STEPS_PER_FRAME = 80
-	
+
 	HEIGHT = 1
 	RADIUS = 0.01
 	CENTER = (1,HEIGHT + RADIUS,1)
 	MASS = 1
-	
+
 	def __init__(self):
 		Program.__init__(self)
 		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)
-	
+
 	def create_sim_objects(self):
 		self.sphere = self.sim.add_sphere(self.RADIUS, self.CENTER, mass=self.MASS)
-	
+
 	def on_pre_step(self):
 		try:
 			time = self.sim.sim_time
 			sim_ball = self.sim.get_object(self.sphere)
 			pos = sim_ball.get_position()
 			vel = sim_ball.get_linear_velocity()
-			
+
 			#print('time: %f, pos: %f, vel: %f' % (time, pos[1], vel[1]))
 			print('%.7e\t%.7e\t%.7e' % (time, pos[1], vel[1]))
-			
+
 		except Exception:
 			print('Exception when executing on_pre_step')
 
 if __name__ == '__main__':
 	sim_program = Example1()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example1_bouncing_balls-no_data.py

 
 from ars.app import Program
 
+
 class Example1NoData(Program):
-	
+
 	FPS = 50
 	STEPS_PER_FRAME = 80
-	
+
 	def create_sim_objects(self):
 		# (radius, center, mass)
 		ball1 = self.sim.add_sphere(0.1, (1,1 + 0.1,1), 1.0)
 		ball2 = self.sim.add_sphere(0.1, (2,1.5 + 0.1,2), 1.0)
 		ball3 = self.sim.add_sphere(0.1, (3,2 + 0.1,3), 1.0)
-		
+
 		print(self.sim.get_object(ball1).actor.set_color((1,1,1)))
 		print(self.sim.get_object(ball2).actor.set_color((0,0.8,0.8)))
 		print(self.sim.get_object(ball3).actor.set_color((0.7,0.5,0)))
 
 if __name__ == '__main__':
 	sim_program = Example1NoData()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example2_conical_pendulum.py

 SIM_PRE_STEP_SIGNAL = ars.model.simulator.SIM_PRE_STEP_SIGNAL
 
 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)
-	
+
 	POLE_RADIUS = 0.141421 # 1/(5*sqrt(2))
 	POLE_HEIGHT = 1
 	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,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.0 + DELTA,0.1)
 	JOINT2_AXIS = cts.X_AXIS
-	
+
 	Q1_FRICTION_COEFF = 50e-3 * 100
 	Q2_FRICTION_COEFF = 50e-3 * 100
 
 	def __init__(self):
 		Program.__init__(self)
 		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)
-		
+
 		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.actors.values())
 		self.sim.update_actors()
-	
+
 	def create_sim_objects(self):
 		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])
-		
+
 		pole = self.sim.add_cylinder(self.POLE_HEIGHT, self.POLE_RADIUS, 
 									self.POLE_INITIAL_POS, mass=self.POLE_MASS)
 		arm = self.sim.add_cylinder(self.ARM_LENGTH, self.ARM_RADIUS, 
 									self.ARM_INITIAL_POS, mass=self.ARM_MASS)
-		
+
 		# bodies are rotated before attaching themselves through joints
 		self.sim.get_object(pole).rotate(cts.X_AXIS, mut.pi/2)
 		self.sim.get_object(arm).rotate(cts.X_AXIS, mut.pi/2)
-		
+
 		self.sim.get_object(box).offset_by_position(self.OFFSET)
 		self.sim.get_object(pole).offset_by_position(self.OFFSET)
 		self.sim.get_object(arm).offset_by_position(self.OFFSET)
-		
+
 		self.sim.add_rotary_joint('r1',         # name, obj1, obj2, anchor, axis
 							self.sim.get_object(box), 
 							self.sim.get_object(pole),
 							None,
 							self.JOINT1_AXIS)
-		
+
 		self.sim.add_rotary_joint('r2', 
 							self.sim.get_object(pole), 
 							self.sim.get_object(arm), 
 							mut.add3(self.OFFSET, self.JOINT2_ANCHOR),
 							self.JOINT2_AXIS)
-		
+
 		#self.sim.get_object(box).actor.set_color(cts.COLOR_RED)
 		self.sim.get_object(pole).actor.set_color(cts.COLOR_YELLOW)
 		self.sim.get_object(arm).actor.set_color(cts.COLOR_NAVY)
-		
+
 		self.box = box
 		self.pole = pole
 		self.arm = arm
 		try:
 			time = self.sim.sim_time
 			torque1 = self.get_torque_to_apply(time)
-			
+
 			self.apply_torque_to_joints(torque1, None)
 			self.apply_friction(self.q1p_prev, self.q2p_prev)
-			
+
 			q1 = self.get_q1()
 			q2 = self.get_q2()
 			q1p = self.get_q1p()
 			q2p = self.get_q2p()
-		
+
 			self.q1p_prev = q1p
 			self.q2p_prev = q2p
-		
+
 			print('%.7e\t%.7e\t%.7e\t%.7e\t%.7e' % (time,q1,q1p,q2,q2p))
-			
+
 		except Exception as e:
 			print('Exception when executing on_pre_step: %s' % str(e))
-	
+
 	def get_torque_to_apply(self, time):
 		if time < self.SATURATION_TIME:
 			torque = time * self.MAX_TORQUE
 		else:
 			torque = self.MAX_TORQUE
 		return torque
-	
+
 	def get_q1(self):
 		return self.sim.get_joint('r1').joint.angle
-	
+
 	def get_q2(self):
 		return self.sim.get_joint('r2').joint.angle
-	
+
 	def get_q1p(self):
 		return self.sim.get_joint('r1').joint.angle_rate
-	
+
 	def get_q2p(self):
 		return self.sim.get_joint('r2').joint.angle_rate
-	
+
 	def apply_torque_to_joints(self, torque1, torque2):
 		if torque1 is not None:
 			self.sim.get_joint('r1').add_torque(torque1)
 		if torque2 is not None:
 			self.sim.get_joint('r2').add_torque(torque2)
-	
+
 	def apply_friction(self, q1p, q2p):
 		self.apply_torque_to_joints(-q1p * self.Q1_FRICTION_COEFF,
 								-q2p * self.Q2_FRICTION_COEFF)
 if __name__ == '__main__':
 	sim_program = Example2()
 	sim_program.start()
-	
+
 	# print arm links' inertia matrices
 	pole_body = sim_program.sim.get_object(sim_program.pole).body
 	arm_body = sim_program.sim.get_object(sim_program.arm).body
 	print(pole_body.get_inertia_tensor())
-	print(arm_body.get_inertia_tensor())
+	print(arm_body.get_inertia_tensor())

File demos/IROS/example3_speed_profile.py

 
 from VehicleWithArm import VehicleWithArm, mut
 
+
 class Example3(VehicleWithArm):
 
 	#WINDOW_SIZE = (1024,630)
 	print(ball_body.get_mass())
 	print(chassis_body.get_mass())
 	print(wheelR_body.get_mass())
-	print(wheelL_body.get_mass())
+	print(wheelL_body.get_mass())

File demos/IROS/example4_sinusoidal_terrain.py

 
 from VehicleWithArm import VehicleWithArm
 
+
 def random_heightfield(num_x, num_z, scale=1.0):
 	"""A height field where values are completely random"""
 	# that x and z are integers, not floats, does not matter
 			verts.append( (x,random()*scale,z) )
 	return verts
 
+
 def sinusoidal_heightfield(num_x, num_z, height_scale=1.0, frequency_x=1.0):
 	"""Creates the vertices corresponding to a sinusoidal heightfield along the
 	X axis. The height_scale controls the amplitude of the wave, and
 			verts.append( (x, mut.sin(x * frequency_x)*height_scale, z) )
 	return verts
 
+
 def constant_heightfield(num_x, num_z, height=0.0):
 	"""A height field where all the values are the same"""
 	# that x and z are integers, not floats, does not matter
 			verts.append((x, height, z))
 	return verts
 
+
 class Example4(VehicleWithArm):
 
 	FPS = 50

File demos/IROS/example4b_sinusoidal_terrain_with_screenshot_recorder.py

 
 if __name__ == '__main__':
 	sim_program = Example4SR()
-	sim_program.start()
+	sim_program.start()

File demos/IROS/example5_vehicle_with_user_input.py

 
 from VehicleWithArm import VehicleWithArm
 
+
 class Example5(VehicleWithArm):
 
 	FPS = 50
 
 if __name__ == '__main__':
 	sim_program = Example5(False, False)
-	sim_program.start()
+	sim_program.start()

File demos/SimpleArm.py

 import ars.utils.mathematical as mut
 import ars.constants as cts
 
+
 class SimpleArm(Program):
 
 	TORQUE = 3
 if __name__ == '__main__':
 	print('Python file being executed: %s' % __file__)
 	sim_program = SimpleArm()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle1.py

 
 from ars.app import Program
 
+
 class Vehicle1(Program):
-	
+
 	TORQUE = 500
-	
+
 	def __init__(self):
 		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)
-	
+
 	def create_sim_objects(self):
 		# POR: point of reference
 		wheel1 = self.sim.add_cylinder(0.1, 0.2, (1,1,1), density=1) # length, radius, center, density
-		
+
 		wheel2 = self.sim.add_cylinder(0.1, 0.2, (0,0,1), density=1)
 		wheel3 = self.sim.add_cylinder(0.1, 0.2, (1,0,0), density=1)
 		wheel4 = self.sim.add_cylinder(0.1, 0.2, (1,0,1), density=1)
-		
+
 		chassis = self.sim.add_box((1.3,0.2,0.6), (0.5,0,0.5), density=10) # size, center, density
-		
+
 		self.sim.get_object(wheel2).offset_by_object(self.sim.get_object(wheel1))
 		self.sim.get_object(wheel3).offset_by_object(self.sim.get_object(wheel1))
 		self.sim.get_object(wheel4).offset_by_object(self.sim.get_object(wheel1))
 		self.sim.get_object(chassis).offset_by_object(self.sim.get_object(wheel1))
-		
+
 		self.sim.add_rotary_joint('r1', self.sim.get_object(chassis), 
 								self.sim.get_object(wheel1), (1,1,1), (0,0,1)) # name, obj1, obj2, anchor, axis
 		self.sim.add_rotary_joint('r2', self.sim.get_object(chassis), 
 								self.sim.get_object(wheel3), (2,1,1), (0,0,1))
 		self.sim.add_rotary_joint('r4', self.sim.get_object(chassis), 
 								self.sim.get_object(wheel4), (2,1,2), (0,0,1))
-	
+
 	def go_forwards(self):
 		self.sim.get_joint('r1').add_torque(self.TORQUE)
 		self.sim.get_joint('r2').add_torque(self.TORQUE)
-	
+
 	def go_backwards(self):
 		self.sim.get_joint('r1').add_torque(-self.TORQUE)
 		self.sim.get_joint('r2').add_torque(-self.TORQUE)
-		
+
 	def turn_left(self):
 		self.sim.get_joint('r1').add_torque(-self.TORQUE/2)
 		self.sim.get_joint('r3').add_torque(-self.TORQUE/2)
 		self.sim.get_joint('r2').add_torque(self.TORQUE/2)
 		self.sim.get_joint('r4').add_torque(self.TORQUE/2)
-		
+
 	def turn_right(self):
 		self.sim.get_joint('r1').add_torque(self.TORQUE/2)
 		self.sim.get_joint('r3').add_torque(self.TORQUE/2)
 
 if __name__ == '__main__':
 	sim_program = Vehicle1()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle2.py

 from ars.app import Program
 import ars.constants as cts
 
+
 class Vehicle2(Program):
 
 	TORQUE = 30
 		# test
 		#print(self.sim.get_object(wheelR).actor.set_color((0.8,0,0)))
 
-
 	def go_forwards(self):
 		"""Rotate both powered wheels in the same direction, forwards"""
 		self.sim.get_joint('r1').add_torque(self.TORQUE)
 
 if __name__ == '__main__':
 	sim_program = Vehicle2()
-	sim_program.start()
+	sim_program.start()

File demos/Vehicle2WithScreenshots.py

 
 if __name__ == '__main__':
 	sim_program = Vehicle2WithScreenshots()
-	sim_program.start()
+	sim_program.start()

File demos/VehicleWithArm.py

 free-rotating spherical wheel. It has a 2-link robotic arm attached,
 with joints either friction-free or with friction proportional to
 joint speed.
+
 """
-
 from ars.app import Program
 import ars.utils.mathematical as mut
 import ars.constants as cts
 from ars.lib.pydispatch import dispatcher
 from ars.model.simulator import SIM_PRE_STEP_SIGNAL
 
+
 class VehicleWithArm(Program):
 
 	STEPS_PER_FRAME = 200
 
 if __name__ == '__main__':
 	sim_program = VehicleWithArm()
-	sim_program.start()
+	sim_program.start()

File demos/VehicleWithControlledArm.py

 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.
+
 """
+from VehicleWithArm import VehicleWithArm
 
-from VehicleWithArm import VehicleWithArm
 
 def output_data(time, sp, cv, error, torque):
 
 	print('time: %f, sp: %f, cv: %f, error: %f, torque: %f' %
 		(time, sp, cv, error, torque))
 
+
 class VehicleWithControlledArm(VehicleWithArm):
 
 	WHEEL_R_PARAMS = ((0.4,0.3,(0,0,-0.5)),{'density':1}) # ((length, radius, center), density)
 
 if __name__ == '__main__':
 	sim_program = VehicleWithControlledArm(False, False)
-	sim_program.start()
+	sim_program.start()