Commits

committed 7b3808d Merge

Merge with dev

demos/CentrifugalForceTest.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.30`
`-#`
`-# @author: german`
`-`
`-"""`
`-Demo of a test of ODE's and ARS's capability of simulating correctly a system were`
`-inertia and centrifugal force intervene. The theoretical angle (theta) of the cable that`
`+"""Demo of a test of ODE's and ARS's capability of simulating correctly a`
`+system where inertia and centrifugal force intervene. The theoretical angle`
`+(theta) of the cable that`
` holds the ball can be obtained with this equation:`
` `
` 	tan(theta) / (d + l*sin(theta)) = omega^2 / g`
` 		omega: angular velocity of pole and ball`
` 		g: gravity`
` `
`-However, theta must be calculated with a numerical solver since no simple analytical`
`-solution exists.`
`+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`
` from ars.model.simulator import signals`
` `
` #View point`
`-#===============================================================================`
`+#==============================================================================`
` # vp_hpr = (90.0, 0.0, 0.0) # orientation [degrees]`
` # `
` # QUICKSTEP_ITERS = 20 # # of iterations for the QuickStep function`
` # GRAVITY = -9.81`
` # GLOBAL_CFM = 1e-10 # default for ODE with double precision`
` # GLOBAL_ERP = 0.8`
`-#===============================================================================`
`+#==============================================================================`
`+`
` `
` 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, signals.SIM_PRE_FRAME)`
`-		`
`+`
` 		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()`

demos/ControlledSimpleArm.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.12.01`
`-#`
`-# @author: german`
`+"""Runs a simulation of a controlled (PD) simple arm, with 2 links and 2 rotary`
`+joints.`
`+`
`+There's friction proportional to angle speed, for both joints.`
` `
` """`
`-Runs a simulation of a controlled (PD) simple arm, with 2 links and 2 rotary joints.`
`-There's friction proportional to angle speed, for both joints.`
`-"""`
`-`
` 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 signals`
` `
`+`
` 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`
` 		return self.sim.get_joint('r2').joint.angle_rate`
` `
` 	def get_compensation(self, sp, q, time_step):`
`-		"""PD controller"""`
`+		"""PD controller."""`
` 		error = (sp - q)`
` 		error_p = (error - self.previous_error) / time_step`
` 		torque = self.R2_KP * error + self.R2_KD * error_p`
` if __name__ == '__main__':`
` 	print('Python file being executed: %s' % __file__)`
` 	sim_program = ControlledSimpleArm()`
`-	sim_program.start()`
`+	sim_program.start()`

demos/FallingBall.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.12.09`
`-#`
`-# @author: german`
`+"""Runs the simplest simulation ever: a falling ball impacts the floor.`
` `
` """`
`-Runs the simplest simulation ever: a falling ball impacts the floor`
`-"""`
`-`
` 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()`

demos/FallingBalls.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.12.09`
`-#`
`-# @author: german`
`+"""Runs a very simple simulation: three falling balls impact the floor.`
` `
` """`
`-Runs a very simple simulation: three falling balls impact the floor`
`-"""`
`-`
` 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()`

demos/IROS/example1_bouncing_ball.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.03.08`
`-#`
`-# @author: german`
`+"""Example #1. To achieve the same results reported in the paper, the contact`
`+joint bounce parameter (in :func:`ars.model.simulator.collision near_callback`)`
`+must be set to 0.9, instead of the default 0.2 value.`
` `
` """`
`-Example #1. To achieve the same results reported in the paper,`
`-the contact joint bounce parameter (in ars.model.simulator.collision near_callback function)`
`-must be set to 0.9, instead of the default 0.2 value.`
`-"""`
`-`
` from ars.app import Program`
` from ars.lib.pydispatch import dispatcher`
` from ars.model.simulator import signals`
` `
`+`
` 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, signals.SIM_PRE_STEP)`
`-	`
`+`
` 	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()`

demos/IROS/example1_bouncing_balls-no_data.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.03.08`
`-#`
`-# @author: german`
`+"""Example #1, with 3 balls (different colors) and no data output.`
` `
` """`
`-Example #1, with 3 balls (different colors) and no data output`
`-"""`
`-`
` 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()`

demos/IROS/example2_conical_pendulum.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.03.09`
`-#`
`-# @author: german`
`+"""Example #2.`
` `
` """`
`-Example #2`
`-"""`
`-`
` from ars.app import Program, Simulation`
` from ars.lib.pydispatch import dispatcher`
` from ars.model.simulator import signals`
` `
` `
` 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, signals.SIM_PRE_STEP)`
`-		`
`+`
` 		self.q1p_prev = 0.0`
` 		self.q2p_prev = 0.0`
`-		`
`+`
` 	def create_simulation(self, *args, **kwargs):`
` 		# 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())`

demos/IROS/example3_speed_profile.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.11`
`-#`
`-# @author: german`
`+"""Example #3.`
` `
` """`
`-Example #3`
`-"""`
`-`
` import os`
` import sys`
` `
` `
` from VehicleWithArm import VehicleWithArm, mut`
` `
`+`
` class Example3(VehicleWithArm):`
` `
` 	#WINDOW_SIZE = (1024,630)`
` 	speed_i = 0`
` `
` 	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
`+		"""Constructor, calls the superclass constructor first."""`
` 		VehicleWithArm.__init__(self)`
` 		self.sim.get_object(self.chassis).actor.set_color((0.8,0,0))`
` `
` 	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())`

demos/IROS/example4_sinusoidal_terrain.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.16`
`-#`
`-# @author: german`
`+"""Example #4.`
` `
` """`
`-Example #4`
`-"""`
` import os`
` import sys`
` from random import random`
` `
` from VehicleWithArm import VehicleWithArm`
` `
`+`
` def random_heightfield(num_x, num_z, scale=1.0):`
`-	"""A height field where values are completely random"""`
`+	"""A heightfield where values are completely random."""`
` 	# that x and z are integers, not floats, does not matter`
` 	verts=[]`
` 	for x in range(num_x):`
` 			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`
`-	frequency_x its frequency"""`
`+	"""A sinusoidal heightfield along the X axis.`
`+`
`+	:param height_scale: controls the amplitude of the wave`
`+	:param frequency_x: controls the frequency of the wave`
`+`
`+	"""`
` 	# TODO: fix the frequency units`
` 	verts=[]`
` 	for x in range(num_x):`
` 			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"""`
`+	"""A heightfield where all the values are the same."""`
` 	# that x and z are integers, not floats, does not matter`
` 	verts=[]`
` 	for x in range(num_x):`
` 			verts.append((x, height, z))`
` 	return verts`
` `
`+`
` class Example4(VehicleWithArm):`
` `
` 	FPS = 50`
` 	R2_KD = 15.0`
` `
` 	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
`+		"""Constructor, calls the superclass constructor first."""`
` 		VehicleWithArm.__init__(self)`
` 		dispatcher.connect(self.on_pre_step, signals.SIM_PRE_STEP)`
` `
` 			print('Exception when executing on_pre_step: %s' % str(e))`
` `
` 	def get_arm_compensation(self, q1, q2):`
`-		"""Calculate the control torque with a PD controller"""`
`+		"""Calculate the control torque with a PD controller."""`
` 		time_step = self.sim.time_step`
` 		error_q1 = (self.q1_SP - q1)`
` 		error_q2 = (self.q2_SP - q2)`
` 	sim_program = Example4()`
` 	sim_program.start()`
` `
`-#===============================================================================`
`+#==============================================================================`
` # def shrink_XZ_heightfield(vertices, factor=(1.0,1.0)):`
` #	"""`
` #	test`
` #	for vertex in vertices:`
` #		new_vertices.append((vertex[0]/factor[0], vertex[1], vertex[2]/factor[1]))`
` #	return new_vertices`
`-#===============================================================================`
`+#==============================================================================`

demos/IROS/example4b_sinusoidal_terrain_with_screenshot_recorder.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.16`
`-#`
`-# @author: german`
`+"""Example #4 with a screenshot recorder.`
` `
` """`
`-Example #4 with a screenshot recorder`
`-"""`
`+from example4_sinusoidal_terrain import Example4`
` `
`-from example4_sinusoidal_terrain import Example4`
` `
` class Example4SR(Example4):`
` `
` 	RECORD_PERIODICALLY = True`
` `
` 	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
` 		Example4.__init__(self)`
` 		self.create_screenshot_recorder(`
` 			self.RECORDER_BASE_FILENAME, self.RECORD_PERIODICALLY)`
` `
` if __name__ == '__main__':`
` 	sim_program = Example4SR()`
`-	sim_program.start()`
`+	sim_program.start()`

demos/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 :func:`ars.model.simulator.collision near_callback`)`
`+must be set to 50, instead of the default 500 value.`
` `
` """`
`-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`
` `
` `
` from VehicleWithArm import VehicleWithArm`
` `
`+`
` class Example5(VehicleWithArm):`
` `
` 	FPS = 50`
` 	R2_KD = 3.0 # controller derivative action`
` `
` 	def __init__(self, use_capsule_wheels=False, frictionless_arm=True):`
`-		"""Constructor, calls the superclass constructor first"""`
` 		VehicleWithArm.__init__(self, use_capsule_wheels, frictionless_arm)`
` `
` 		self.key_press_functions.add('d', self.increase_sp)`
` 		self.torque_w2 = torque2`
` `
` 	def increase_sp(self):`
`-		"""Increase angle set point"""`
`+		"""Increase angle set point."""`
` 		self.sp += self.SP_STEP`
` `
` 	def decrease_sp(self):`
`-		"""Decrease angle set point"""`
`+		"""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"""`
`+		"""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`
` `
` if __name__ == '__main__':`
` 	sim_program = Example5(False, False)`
`-	sim_program.start()`
`+	sim_program.start()`

demos/SimpleArm.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.12.01`
`-#`
`-# @author: german`
`+"""Runs a simulation of a simple arm, with 2 links and 2 rotary joints.`
` `
`-"""Runs a simulation of a simple arm, with 2 links and 2 rotary joints"""`
`-`
`+"""`
` from ars.app import Program`
` 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()`

demos/Vehicle1.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.11.16`
`-#`
`-# @author: german`
`+"""Runs a simulation of a simple vehicle.`
` `
`+"""`
` 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()`

demos/Vehicle2.py

` #!/usr/bin/env python`
` `
`-# Created on 2011.11.16`
`-#`
`-# @author: german`
`+"""Runs a simulation of a vehicle with two powered wheels and one`
`+free-rotating spherical wheel.`
` `
` """`
`-Runs a simulation of a vehicle with two powered wheels and one`
`-free-rotating spherical wheel.`
`-"""`
`-`
` from ars.app import Program`
` import ars.constants as cts`
` `
`+`
` class Vehicle2(Program):`
` `
` 	TORQUE = 30`
` 	FLOOR_BOX_SIZE = (20,0.01,20)`
` `
` 	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
`+		"""Constructor, calls the superclass constructor first."""`
` 		Program.__init__(self)`
` 		self.key_press_functions.add('up', self.go_forwards, True)`
` 		self.key_press_functions.add('down', self.go_backwards, True)`
` 		self.key_press_functions.add('right', self.turn_right, True)`
` `
` 	def create_sim_objects(self):`
`-		"""Implementation of the required method. Creates and sets up all the`
`-		objects of the simulation."""`
`+		"""Implementation of the required method.`
` `
`+		Creates and sets up all the objects of the simulation.`
`+`
`+		"""`
` 		offset = self.OFFSET`
` `
` 		wheelR = self.sim.add_cylinder(0.4, 0.3, (0,0,-0.5), density=1) # length, radius, center, density`
` 		# 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"""`
`+		"""Rotate both powered wheels in the same direction, forwards."""`
` 		self.sim.get_joint('r1').add_torque(self.TORQUE)`
` 		self.sim.get_joint('r2').add_torque(self.TORQUE)`
` `
` 	def go_backwards(self):`
`-		"""Rotate both powered wheels in the same direction, backwards"""`
`+		"""Rotate both powered wheels in the same direction, backwards."""`
` 		self.sim.get_joint('r1').add_torque(-self.TORQUE)`
` 		self.sim.get_joint('r2').add_torque(-self.TORQUE)`
` `
` 	def turn_left(self):`
`-		"""Rotate both powered wheels in different directions, with a global`
`-		counter-clockwise rotation (from above)"""`
`+		"""Rotate vehicle counter-clockwise (from above)."""`
` 		self.sim.get_joint('r1').add_torque(-self.TORQUE)`
` 		self.sim.get_joint('r2').add_torque(self.TORQUE)`
` `
` 	def turn_right(self):`
`-		"""Rotate both powered wheels in different directions, with a global`
`-		clockwise rotation (from above)"""`
`+		"""Rotate vehicle clockwise (from above)."""`
` 		self.sim.get_joint('r1').add_torque(self.TORQUE)`
` 		self.sim.get_joint('r2').add_torque(-self.TORQUE)`
` `
` if __name__ == '__main__':`
` 	sim_program = Vehicle2()`
`-	sim_program.start()`
`+	sim_program.start()`

demos/Vehicle2WithScreenshots.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.02.07`
`-#`
`-# @author: german`
`+"""Runs a simulation of a vehicle with two powered wheels and one`
`+free-rotating spherical wheel.`
` `
`-"""Runs a simulation of a vehicle with two powered wheels and one`
`-free-rotating spherical wheel"""`
`+"""`
`+from Vehicle2 import Vehicle2`
` `
`-from Vehicle2 import Vehicle2`
` `
` class Vehicle2WithScreenshots(Vehicle2):`
` 	FPS = 100 # this is the recording frequency`
` `
` if __name__ == '__main__':`
` 	sim_program = Vehicle2WithScreenshots()`
`-	sim_program.start()`
`+	sim_program.start()`

demos/VehicleWithArm.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.03`
`-#`
`-# @author: german`
`-`
`-"""`
`-Runs a simulation of a vehicle with two powered wheels and one`
`+"""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.`
`+`
` """`
`-`
` 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 signals`
` `
`+`
` class VehicleWithArm(Program):`
` `
` 	STEPS_PER_FRAME = 200`
` 	Q2_FRICTION_COEFF = 0.01`
` `
` 	def __init__(self, use_capsule_wheels=False, frictionless_arm=True):`
`-		"""Constructor, calls the superclass constructor first"""`
`+		"""Constructor, calls the superclass constructor first."""`
` 		self._use_capsule_wheels = use_capsule_wheels`
` 		self._frictionless_arm = frictionless_arm`
` `
` 		dispatcher.connect(self.on_pre_step, signals.SIM_PRE_STEP)`
` `
` 	def create_sim_objects(self):`
`-		"""Implementation of the required method. Creates and sets up all the`
`-		objects of the simulation."""`
`+		"""Implementation of the required method.`
` `
`+		Creates and sets up all the objects of the simulation.`
`+`
`+		"""`
` 		arm_offset = (0,0.5,0)`
` `
` 		#=======================================================================`
` 		self.link2 = link2`
` `
` 	def go_forwards(self):`
`-		"""Rotate both powered wheels in the same direction, forwards"""`
`+		"""Rotate both powered wheels in the same direction, forwards."""`
` 		self.apply_torque_to_wheels(self.WHEEL_TORQUE, self.WHEEL_TORQUE)`
` `
` 	def go_backwards(self):`
`-		"""Rotate both powered wheels in the same direction, backwards"""`
`+		"""Rotate both powered wheels in the same direction, backwards."""`
` 		self.apply_torque_to_wheels(-self.WHEEL_TORQUE, -self.WHEEL_TORQUE)`
` `
` 	def turn_left(self):`
`-		"""Rotate both powered wheels in different directions, with a global`
`-		counter-clockwise rotation (from above)"""`
`+		"""Rotate vehicle counter-clockwise (from above)."""`
` 		self.apply_torque_to_wheels(-self.WHEEL_TORQUE, self.WHEEL_TORQUE)`
` `
` 	def turn_right(self):`
`-		"""Rotate both powered wheels in different directions, with a global`
`-		clockwise rotation (from above)"""`
`+		"""Rotate vehicle clockwise (from above)."""`
` 		self.apply_torque_to_wheels(self.WHEEL_TORQUE, -self.WHEEL_TORQUE)`
` `
` 	def on_pre_step(self):`
` `
` if __name__ == '__main__':`
` 	sim_program = VehicleWithArm()`
`-	sim_program.start()`
`+	sim_program.start()`

demos/VehicleWithControlledArm.py

` #!/usr/bin/env python`
` `
`-# Created on 2012.01.30`
`-#`
`-# @author: german`
`-`
`-"""`
`-Runs a simulation of a vehicle with two powered wheels and one`
`+"""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.`
`+`
` """`
`+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)`
` 	R2_KD = 0.5 # controller derivative action`
` `
` 	def __init__(self, use_capsule_wheels=False, frictionless_arm=True):`
`-		"""Constructor, calls the superclass constructor first"""`
`+		"""Constructor, calls the superclass constructor first."""`
` 		VehicleWithArm.__init__(self, use_capsule_wheels, frictionless_arm)`
` `
` 		self.key_press_functions.add('d', self.increase_sp)`
` 			print('Exception when executing on_pre_step: %s' % str(e))`
` `
` 	def increase_sp(self):`
`-		"""Increase angle set point"""`
`+		"""Increase angle set point."""`
` 		self.sp += self.SP_STEP`
` `
` 	def decrease_sp(self):`
`-		"""Decrease angle set point"""`
`+		"""Decrease angle set point."""`
` 		self.sp -= self.SP_STEP`
` `
` 	def get_q2(self):`
`-		"""Get the current angle of the second rotary joint"""`
`+		"""Get the current angle of the second rotary joint."""`
` 		return self.sim.get_joint('r2').joint.angle`
` `
` 	def get_compensation(self, sp, q, time_step):`
`-		"""Calculate the control torque with a PD controller"""`
`+		"""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`
` `
` if __name__ == '__main__':`
` 	sim_program = VehicleWithControlledArm(False, False)`
`-	sim_program.start()`
`+	sim_program.start()`

docs/sphinx/source/releases.rst

` =======	==========	============`
` version	date		revision`
` =======	==========	============`
`+0.3a1	2012.10.17	`b9190db2b909 <https://bitbucket.org/glarrain/ars/changeset/b9190db2b909>`_`
` 0.3dev3	2012.08.28	`626f6c657103 <https://bitbucket.org/glarrain/ars/changeset/626f6c657103>`_`
` 0.3dev2	2012.08.12	`83e4b0f82342 <https://bitbucket.org/glarrain/ars/changeset/83e4b0f82342>`_`
` 0.3dev1	2012.08.01	`ef537d62f3c4 <https://bitbucket.org/glarrain/ars/changeset/ef537d62f3c4>`_`