# Commits

committed f893701

backport of da1a26abe175

• Participants
• Parent commits 23f9220
• Branches 0.3.x

# File bin/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`
`-holds the ball can be obtained with this equation:`
`-`
`-	tan(theta) / (d + l*sin(theta)) = omega^2 / g`
`-		d: distance from pole to anchor`
`-		l: "cable" length`
`-		omega: angular velocity of pole and ball`
`-		g: gravity`
`-`
`-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.lib.pydispatch import dispatcher`
`-from ars.model.simulator import SIM_PRE_FRAME_SIGNAL`
`-`
`-#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`
`-	STEPS_PER_FRAME = 20 #200 #STEP_SIZE = 1e-5 # 0.01 ms`
`-`
`-	POLE_SPEED_STEP = 0.01`
`-	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('z', self.dec_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`
`-		pole = self.sim.add_cylinder(self.POLE_HEIGHT, self.POLE_VISUAL_RADIUS, self.POLE_INITIAL_POS, density=1.0)`
`-		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), `
`-							mut.add3(self.OFFSET, self.JOINT2_ANCHOR), self.JOINT2_AXIS)`
`-		self.box = box`
`-		self.pole = pole`
`-		self.ball = ball`
`-`
`-	def on_pre_frame(self):`
`-		try:`
`-			#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()`
`-			z_top = self.JOINT2_ANCHOR[1] # JOINT2_ANCHOR[2] in C++ example`
`-			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()`

# File bin/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.`
`-"""`
`-`
`-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`
`-`
`-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`
`-`
`-	OFFSET = (2.5,1,2.5)`
`-`
`-	BOX_PARAMS = (((3,0.5,3),(0,-0.75,0)),{'density':1}) # ((size, center), density)`
`-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'density':1}) # ((length, radius, center), density)`
`-	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'density':1})`
`-`
`-	SP_STEP = 0.1`
`-	q2_INITIAL_SP = 0.0 # mut.pi/3 # set point`
`-	R2_KP = 1.0 # controller proportional action`
`-	R2_KD = 0.5 # controller derivative action`
`-`
`-	Q1_FRICTION_COEFF = 0.01`
`-	Q2_FRICTION_COEFF = 0.01`
`-`
`-	def __init__(self):`
`-		Program.__init__(self)`
`-		self.key_press_functions.add('a', self.rotate_clockwise)`
`-		self.key_press_functions.add('z', self.rotate_counterlockwise)`
`-		self.key_press_functions.add('d', self.increase_sp)`
`-		self.key_press_functions.add('c', self.decrease_sp)`
`-`
`-		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)`
`-`
`-		self.sp = self.q2_INITIAL_SP`
`-		self.previous_error = 0.0`
`-`
`-	def create_sim_objects(self):`
`-`
`-		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])`
`-		link1 = self.sim.add_capsule(*self.LINK1_PARAMS[0], **self.LINK1_PARAMS[1])`
`-		link2 = self.sim.add_capsule(*self.LINK2_PARAMS[0], **self.LINK2_PARAMS[1])`
`-`
`-		# bodies are rotated before attaching themselves through joints`
`-		self.sim.get_object(link1).rotate(cts.X_AXIS, mut.pi/2)`
`-		self.sim.get_object(link2).rotate(cts.X_AXIS, mut.pi/2)`
`-`
`-		self.sim.get_object(box).offset_by_position(self.OFFSET)`
`-		self.sim.get_object(link1).offset_by_position(self.OFFSET)`
`-		self.sim.get_object(link2).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(link1),`
`-							None, cts.Y_AXIS)`
`-		r2_anchor = mut.sub3(self.sim.get_object(link2).get_position(),`
`-							(0,self.LINK2_PARAMS[0][0]/2,0)) # (0, length/2, 0)`
`-		self.sim.add_rotary_joint('r2',`
`-							self.sim.get_object(link1),`
`-							self.sim.get_object(link2),`
`-							r2_anchor, cts.Z_AXIS)`
`-`
`-	def on_pre_step(self):`
`-		try:`
`-			time = self.sim.sim_time`
`-			time_step = self.sim.time_step`
`-			cv = self.get_q2()`
`-			q1p = self.get_q1p()`
`-			q2p = self.get_q2p()`
`-`
`-			mv = self.get_compensation(self.sp, cv, time_step)`
`-			self.apply_torque_to_joints(0, mv)`
`-			self.apply_friction(q1p, q2p)`
`-`
`-			output_data(time, self.sp, cv, self.sp - cv, mv)`
`-			#print('q1p: %f, q2p: %f' % (q1p, q2p))`
`-`
`-		except Exception as e:`
`-			print('Exception when executing on_pre_step: %s' % str(e))`
`-`
`-	def rotate_clockwise(self):`
`-		self.apply_torque_to_joints(self.R1_TORQUE, 0)`
`-`
`-	def rotate_counterlockwise(self):`
`-		self.apply_torque_to_joints(-self.R1_TORQUE, 0)`
`-`
`-	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 increase_sp(self):`
`-		self.sp += self.SP_STEP`
`-`
`-	def decrease_sp(self):`
`-		self.sp -= self.SP_STEP`
`-`
`-	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 get_compensation(self, sp, q, time_step):`
`-		"""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`
`-`
`-	def apply_friction(self, q1p, q2p):`
`-		self.apply_torque_to_joints(-q1p * self.Q1_FRICTION_COEFF, -q2p * self.Q2_FRICTION_COEFF)`
`-`
`-if __name__ == '__main__':`
`-	print('Python file being executed: %s' % __file__)`
`-	sim_program = ControlledSimpleArm()`
`-	sim_program.start()`

# File bin/FallingBall.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2011.12.09`
`-#`
`-# @author: german`
`-`
`-"""`
`-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()`

# File bin/FallingBalls.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2011.12.09`
`-#`
`-# @author: german`
`-`
`-"""`
`-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)`
`-		self.sim.add_sphere(0.2, (3,2,3), mass=5) # radius, center, mass`
`-`
`-if __name__ == '__main__':`
`-	sim_program = FallingBalls()`
`-	sim_program.start()`

# File bin/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 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 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()`

# File bin/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`
`-"""`
`-`
`-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()`

# File bin/IROS/example2_conical_pendulum.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2012.03.09`
`-#`
`-# @author: german`
`-`
`-"""`
`-Example #2`
`-"""`
`-`
`-from ars.app import Program, Simulation`
`-from ars.lib.pydispatch import dispatcher`
`-import ars.model.simulator`
`-import ars.utils.mathematical as mut`
`-import ars.constants as cts`
`-`
`-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`
`-`
`-	# control`
`-	MAX_TORQUE = 20`
`-	SATURATION_TIME = 1`
`-`
`-	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`
`-`
`-	def on_pre_step(self):`
`-		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())`

# File bin/IROS/example3_speed_profile.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2012.01.11`
`-#`
`-# @author: german`
`-`
`-"""`
`-Example #3`
`-"""`
`-`
`-import os`
`-import sys`
`-`
`-import ars.exceptions as exc`
`-`
`-# 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 Example3(VehicleWithArm):`
`-`
`-	#WINDOW_SIZE = (1024,630)`
`-	CAMERA_POSITION = (0,8,25) # (0,8,15)`
`-`
`-	FPS = 50`
`-	STEPS_PER_FRAME = 80`
`-`
`-	VEHICLE_OFFSET = (-4,0.5,4)`
`-`
`-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'mass':1}) # ((length, radius, center), mass)`
`-	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'mass':1})`
`-`
`-	Q1_FRICTION_COEFF = 0.02`
`-	Q2_FRICTION_COEFF = 0.02`
`-`
`-	KP = 10 # controller proportional action`
`-`
`-	# speed profile setup`
`-	speeds = ((0,0), (1,0), (5,1), (9,1), (13,0), (14,0)) # (time,speed)`
`-	speed_i = 0`
`-`
`-	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
`-		VehicleWithArm.__init__(self)`
`-		self.sim.get_object(self.chassis).actor.set_color((0.8,0,0))`
`-`
`-		self.r1_angle_rate_prev = 0.0`
`-		self.r2_angle_rate_prev = 0.0`
`-`
`-	def on_pre_step(self):`
`-		try:`
`-			time = self.sim.sim_time`
`-`
`-			if self.speed_i < len(self.speeds) - 1:`
`-				if time > self.speeds[self.speed_i + 1][0]:`
`-					self.speed_i += 1`
`-			elif self.speed_i == len(self.speeds) - 1:`
`-				pass`
`-`
`-			pos = self.sim.get_object(self.chassis).get_position()`
`-			q1 = self.sim.get_joint('r1').joint.angle`
`-			q1p = self.sim.get_joint('r1').joint.angle_rate`
`-			q2 = self.sim.get_joint('r2').joint.angle`
`-			q2p = self.sim.get_joint('r2').joint.angle_rate`
`-`
`-			linear_vel = self.sim.get_object(self.chassis).get_linear_velocity()`
`-			linear_vel_XZ = (linear_vel[0], linear_vel[2])`
`-			cv = mut.length2(linear_vel_XZ) * mut.sign(linear_vel[0])`
`-`
`-			sp = self.calc_desired_speed(time)`
`-			torque = self.compensate(sp, cv)`
`-			self.apply_torque_to_wheels(torque, torque)`
`-			self.apply_friction(q1p, q2p)`
`-`
`-			print('%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e' %`
`-				(time,pos[0],cv,sp,q1,q1p,q2,q2p,torque))`
`-`
`-		except Exception as e:`
`-			print('Exception when executing on_pre_step: %s' % str(e))`
`-`
`-	def calc_desired_speed(self, time):`
`-`
`-		if self.speed_i == len(self.speeds) - 1:`
`-			return float(self.speeds[self.speed_i][1])`
`-`
`-		elif 0 <= self.speed_i < len(self.speeds) - 1:`
`-			time_diff = time - self.speeds[self.speed_i][0]`
`-			time_period = self.speeds[self.speed_i + 1][0] - self.speeds[self.speed_i][0]`
`-			prev_speed = float(self.speeds[self.speed_i][1])`
`-			next_speed = float(self.speeds[self.speed_i + 1][1] )`
`-			return (next_speed - prev_speed) * (time_diff / time_period) + prev_speed`
`-		else:`
`-			raise exc.ArsError('invalid speed_i value: %d' % self.speed_i)`
`-`
`-	def compensate(self, sp, cv):`
`-		return (sp - cv) * self.KP`
`-`
`-if __name__ == '__main__':`
`-	sim_program = Example3()`
`-	sim_program.start()`
`-`
`-	# print mass of bodies defined with a density value`
`-	ball_body = sim_program.sim.get_object(sim_program.ball).body`
`-	chassis_body = sim_program.sim.get_object(sim_program.chassis).body`
`-	wheelR_body = sim_program.sim.get_object(sim_program.wheelR).body`
`-	wheelL_body = sim_program.sim.get_object(sim_program.wheelL).body`
`-	print(ball_body.get_mass())`
`-	print(chassis_body.get_mass())`
`-	print(wheelR_body.get_mass())`
`-	print(wheelL_body.get_mass())`

# File bin/IROS/example4_sinusoidal_terrain.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2012.01.16`
`-#`
`-# @author: german`
`-`
`-"""`
`-Example #4`
`-"""`
`-import os`
`-import sys`
`-from random import random`
`-`
`-from ars.lib.pydispatch import dispatcher`
`-from ars.model.collision.base import Trimesh`
`-from ars.model.simulator import Simulation, SIM_PRE_STEP_SIGNAL`
`-import ars.utils.mathematical as mut`
`-`
`-# 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`
`-`
`-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=[]`
`-	for x in range(num_x):`
`-		for z in range(num_z):`
`-			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"""`
`-	# TODO: fix the frequency units`
`-	verts=[]`
`-	for x in range(num_x):`
`-		for z in range(num_z):`
`-			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=[]`
`-	for x in range(num_x):`
`-		for z in range(num_z):`
`-			verts.append((x, height, z))`
`-	return verts`
`-`
`-class Example4(VehicleWithArm):`
`-`
`-	FPS = 50`
`-	STEPS_PER_FRAME = 80`
`-	CAMERA_POSITION = (0,8,30)`
`-`
`-	VEHICLE_OFFSET = (-1.05,-0.35,5)`
`-	TM_X, TM_Z = (40,20)`
`-`
`-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'mass':1}) # ((length, radius, center), mass)`
`-	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'mass':1})`
`-`
`-	Q1_FRICTION_COEFF = 0.02`
`-	Q2_FRICTION_COEFF = 0.02`
`-`
`-	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`
`-`
`-	q2_SP = 0.0`
`-	R2_KP = 20.0`
`-	R2_KD = 15.0`
`-`
`-	def __init__(self):`
`-		"""Constructor, calls the superclass constructor first"""`
`-		VehicleWithArm.__init__(self)`
`-		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)`
`-`
`-		self.q1_previous_error = 0.0`
`-		self.q2_previous_error = 0.0`
`-`
`-	def create_simulation(self):`
`-`
`-		tm_x, tm_z = self.TM_X, self.TM_Z`
`-		vertices = sinusoidal_heightfield(tm_x, tm_z, height_scale=0.7, frequency_x=0.5)`
`-		faces = Trimesh.get_heightfield_faces(tm_x, tm_z)`
`-		#vertices = constant_heightfield(tm_x, tm_z, height=0.0)`
`-		#vertices = random_heightfield(tm_x, tm_z, 0.5)`
`-		#shrink_factor = (1.0,1.0)`
`-		#vertices = shrink_XZ_heightfield(vertices, shrink_factor)`
`-`
`-		# 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_trimesh_floor(vertices, faces, center=(-10,0,-10), color=(0.7,0.7,0.7))`
`-`
`-		self.create_sim_objects()`
`-`
`-		# add the graphic objects`
`-		self.gAdapter.add_objects_list(self.sim.actors.values())`
`-		self.sim.update_actors()`
`-`
`-	def on_pre_step(self):`
`-		try:`
`-			time = self.sim.sim_time`
`-`
`-			pos = self.sim.get_object(self.chassis).get_position()`
`-			vel = self.sim.get_object(self.chassis).get_linear_velocity()`
`-			q1 = self.sim.get_joint('r1').joint.angle`
`-			q1p = self.sim.get_joint('r1').joint.angle_rate`
`-			q2 = self.sim.get_joint('r2').joint.angle`
`-			q2p = self.sim.get_joint('r2').joint.angle_rate`
`-`
`-			if mut.length3(vel) < self.MAX_SPEED:`
`-				wheels_torque = self.WHEELS_TORQUE`
`-			else:`
`-				wheels_torque = 0.0`
`-`
`-			self.apply_torque_to_wheels(wheels_torque, wheels_torque)`
`-			self.apply_friction(q1p, q2p)`
`-			torque1, torque2 = self.get_arm_compensation(q1, q2)`
`-			self.apply_torque_to_joints(torque1, torque2)`
`-`
`-			print('%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e\t%.7e' %`
`-				(time,pos[0],pos[1],pos[2],q1,torque1,q2,torque2,wheels_torque))`
`-`
`-		except Exception as e:`
`-			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"""`
`-		time_step = self.sim.time_step`
`-		error_q1 = (self.q1_SP - q1)`
`-		error_q2 = (self.q2_SP - q2)`
`-`
`-		error_q1_p = (error_q1 - self.q1_previous_error) / time_step`
`-		error_q2_p = (error_q2 - self.q2_previous_error) / time_step`
`-`
`-		torque1 = self.R1_KP * error_q1 + self.R1_KD * error_q1_p`
`-		torque2 = self.R2_KP * error_q2 + self.R2_KD * error_q2_p`
`-`
`-		self.q1_previous_error = error_q1`
`-		self.q2_previous_error = error_q2`
`-`
`-		return torque1, torque2`
`-`
`-if __name__ == '__main__':`
`-	sim_program = Example4()`
`-	sim_program.start()`
`-`
`-#===============================================================================`
`-# def shrink_XZ_heightfield(vertices, factor=(1.0,1.0)):`
`-#	"""`
`-#	test`
`-#	"""`
`-#	new_vertices = []`
`-#	for vertex in vertices:`
`-#		new_vertices.append((vertex[0]/factor[0], vertex[1], vertex[2]/factor[1]))`
`-#	return new_vertices`
`-#===============================================================================`

# File bin/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`
`-"""`
`-`
`-from example4_sinusoidal_terrain import Example4`
`-`
`-class Example4SR(Example4):`
`-`
`-	# screenshot recorder`
`-	RECORDER_BASE_FILENAME = 'sin'`
`-	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()`

# 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`
`-`
`-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, calls the superclass constructor first"""`
`-		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.time_step`
`-			pos = self.sim.get_object(self.chassis).get_position()`
`-			q1 = self.sim.get_joint('r1').joint.angle`
`-			q2 = self.sim.get_joint('r2').joint.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/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"""`
`-`
`-from ars.app import Program`
`-import ars.utils.mathematical as mut`
`-import ars.constants as cts`
`-`
`-class SimpleArm(Program):`
`-`
`-	TORQUE = 3`
`-`
`-	OFFSET = (2.5,1,2.5)`
`-`
`-	BOX_PARAMS = (((3,0.5,3),(0,-0.75,0)),{'density':1}) # ((size, center), density)`
`-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'density':1}) # ((length, radius, center), density)`
`-	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'density':1})`
`-`
`-	def __init__(self):`
`-		Program.__init__(self)`
`-		self.key_press_functions.add('a', self.rotate_clockwise)`
`-		self.key_press_functions.add('z', self.rotate_counterlockwise)`
`-`
`-	def create_sim_objects(self):`
`-`
`-		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])`
`-		link1 = self.sim.add_capsule(*self.LINK1_PARAMS[0], **self.LINK1_PARAMS[1])`
`-		link2 = self.sim.add_capsule(*self.LINK2_PARAMS[0], **self.LINK2_PARAMS[1])`
`-`
`-		# bodies are rotated before attaching themselves through joints`
`-		self.sim.get_object(link1).rotate(cts.X_AXIS, mut.pi/2)`
`-		self.sim.get_object(link2).rotate(cts.X_AXIS, mut.pi/2)`
`-`
`-		self.sim.get_object(box).offset_by_position(self.OFFSET)`
`-		self.sim.get_object(link1).offset_by_position(self.OFFSET)`
`-		self.sim.get_object(link2).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(link1),`
`-							None, cts.Y_AXIS)`
`-		r2_anchor = mut.sub3(self.sim.get_object(link2).get_position(),`
`-							(0,self.LINK2_PARAMS[0][0]/2,0)) # (0, length/2, 0)`
`-		self.sim.add_rotary_joint('r2',`
`-							self.sim.get_object(link1),`
`-							self.sim.get_object(link2),`
`-							r2_anchor, cts.Z_AXIS)`
`-`
`-	def rotate_clockwise(self):`
`-		self.sim.get_joint('r1').add_torque(self.TORQUE)`
`-`
`-	def rotate_counterlockwise(self):`
`-		self.sim.get_joint('r1').add_torque(-self.TORQUE)`
`-`
`-if __name__ == '__main__':`
`-	print('Python file being executed: %s' % __file__)`
`-	sim_program = SimpleArm()`
`-	sim_program.start()`

# File bin/Vehicle1.py

`-#!/usr/bin/env python`
`-`
`-# Created on 2011.11.16`
`-#`
`-# @author: german`
`-`
`-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(wheel2), (1,1,2), (0,0,1))`
`-		self.sim.add_rotary_joint('r3', 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)`
`-		self.sim.get_joint('r2').add_torque(-self.TORQUE/2)`
`-		self.sim.get_joint('r4').add_torque(-self.TORQUE/2)`
`-`
`-if __name__ == '__main__':`
`-	sim_program = Vehicle1()`
`-	sim_program.start()`

# File bin/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.`
`-"""`
`-`
`-from ars.app import Program`
`-import ars.constants as cts`
`-`
`-class Vehicle2(Program):`
`-`
`-	TORQUE = 30`
`-`
`-	OFFSET = (3,1,3)`
`-`
`-	FLOOR_BOX_SIZE = (20,0.01,20)`
`-`
`-	def __init__(self):`
`-		"""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('left', self.turn_left, 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."""`
`-`
`-		offset = self.OFFSET`
`-`
`-		wheelR = self.sim.add_cylinder(0.4, 0.3, (0,0,-0.5), density=1) # length, radius, center, density`
`-		wheelL = self.sim.add_cylinder(0.4, 0.3, (0,0,0.5), density=1)`
`-		ball = self.sim.add_sphere(0.3, (1,0,0), density=1) # radius, center, density`
`-		chassis = self.sim.add_box((2,0.2,1.5), (0.5,0.45,0), density=10) # size, center, density`
`-`
`-		self.sim.add_rotary_joint('r1',				   # name, obj1, obj2, anchor, axis`
`-							self.sim.get_object(chassis),`
`-							self.sim.get_object(wheelR),`
`-							None, cts.Z_AXIS)`
`-		self.sim.add_rotary_joint('r2',`
`-							self.sim.get_object(chassis),`
`-							self.sim.get_object(wheelL),`
`-							None, cts.Z_AXIS)`
`-		self.sim.add_ball_socket_joint('bs',			  # name, obj1, obj2, anchor`
`-								self.sim.get_object(chassis),`
`-								self.sim.get_object(ball),`
`-								None)`
`-`
`-		self.sim.get_object(wheelR).offset_by_position(offset)`
`-		self.sim.get_object(wheelL).offset_by_position(offset)`
`-		self.sim.get_object(ball).offset_by_position(offset)`
`-		self.sim.get_object(chassis).offset_by_position(offset)`
`-`
`-		# 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)`
`-		self.sim.get_joint('r2').add_torque(self.TORQUE)`
`-`
`-	def go_backwards(self):`
`-		"""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)"""`
`-		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)"""`
`-		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()`

# File bin/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"""`
`-`
`-from Vehicle2 import Vehicle2`
`-`
`-class Vehicle2WithScreenshots(Vehicle2):`
`-	FPS = 100 # this is the recording frequency`
`-	RECORDER_BASE_FILENAME = 'test'`
`-	RECORD_PERIODICALLY = True`
`-`
`-	def __init__(self):`
`-		Vehicle2.__init__(self)`
`-		self.create_screenshot_recorder(`
`-			self.RECORDER_BASE_FILENAME, self.RECORD_PERIODICALLY)`
`-`
`-if __name__ == '__main__':`
`-	sim_program = Vehicle2WithScreenshots()`
`-	sim_program.start()`

# File bin/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`
`-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`
`-	BACKGROUND_COLOR = (0.8,0.8,0.8)`
`-	FLOOR_BOX_SIZE = (20,0.01,20)`
`-`
`-	WHEEL_TORQUE = 3`
`-	VEHICLE_OFFSET = (2,0.35,4)`
`-`
`-	BALL_PARAMS = ((0.3,(1,0,0)),{'density':1}) # ((radius, center), density)`
`-	CHASSIS_PARAMS = (((2,0.2,1.5),(0.5,0.45,0)),{'mass':6}) # ((size, center), mass)`
`-`
`-	WHEEL_R_PARAMS = ((0.4,0.3,(0,0,-0.5)),{'density':1}) # ((length, radius, center), density)`
`-	WHEEL_L_PARAMS = ((0.4,0.3,(0,0,0.5)),{'density':1})`
`-`
`-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'density':1}) # ((length, radius, center), density)`
`-	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'density':1})`
`-`
`-	R1_TORQUE = 3`
`-`
`-	Q1_FRICTION_COEFF = 0.01`
`-	Q2_FRICTION_COEFF = 0.01`
`-`
`-	def __init__(self, use_capsule_wheels=False, frictionless_arm=True):`
`-		"""Constructor, calls the superclass constructor first"""`
`-		self._use_capsule_wheels = use_capsule_wheels`
`-		self._frictionless_arm = frictionless_arm`
`-`
`-		Program.__init__(self)`
`-		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)`
`-`
`-		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)`
`-`
`-	def create_sim_objects(self):`
`-		"""Implementation of the required method. Creates and sets up all the`
`-		objects of the simulation."""`
`-`
`-		arm_offset = (0,0.5,0)`
`-`
`-		#=======================================================================`
`-		# VEHICLE`
`-		#=======================================================================`
`-`
`-		if self._use_capsule_wheels:`
`-			wheelR = self.sim.add_capsule(*self.WHEEL_R_PARAMS[0], **self.WHEEL_R_PARAMS[1])`
`-			wheelL = self.sim.add_capsule(*self.WHEEL_L_PARAMS[0], **self.WHEEL_L_PARAMS[1])`
`-		else:`
`-			wheelR = self.sim.add_cylinder(*self.WHEEL_R_PARAMS[0], **self.WHEEL_R_PARAMS[1])`
`-			wheelL = self.sim.add_cylinder(*self.WHEEL_L_PARAMS[0], **self.WHEEL_L_PARAMS[1])`
`-`
`-		ball = self.sim.add_sphere(*self.BALL_PARAMS[0], **self.BALL_PARAMS[1])`
`-		chassis = self.sim.add_box(*self.CHASSIS_PARAMS[0], **self.CHASSIS_PARAMS[1])`
`-`
`-		self.sim.add_rotary_joint('w1',                   # name, obj1, obj2, anchor, axis`
`-							self.sim.get_object(chassis),`
`-							self.sim.get_object(wheelR),`
`-							None, cts.Z_AXIS)`
`-		self.sim.add_rotary_joint('w2',`
`-							self.sim.get_object(chassis),`
`-							self.sim.get_object(wheelL),`
`-							None, cts.Z_AXIS)`
`-		self.sim.add_ball_socket_joint('bs',              # name, obj1, obj2, anchor`
`-								self.sim.get_object(chassis),`
`-								self.sim.get_object(ball),`
`-								None)`
`-`
`-		self.sim.get_object(wheelR).offset_by_position(self.VEHICLE_OFFSET)`
`-		self.sim.get_object(wheelL).offset_by_position(self.VEHICLE_OFFSET)`
`-		self.sim.get_object(ball).offset_by_position(self.VEHICLE_OFFSET)`
`-		self.sim.get_object(chassis).offset_by_position(self.VEHICLE_OFFSET)`
`-`
`-		#=======================================================================`
`-		# ROBOTIC ARM`
`-		#=======================================================================`
`-		link1 = self.sim.add_capsule(*self.LINK1_PARAMS[0], **self.LINK1_PARAMS[1])`
`-		link2 = self.sim.add_capsule(*self.LINK2_PARAMS[0], **self.LINK2_PARAMS[1])`
`-`
`-		# bodies are rotated before attaching themselves through joints`
`-		self.sim.get_object(link1).rotate(cts.X_AXIS, mut.pi/2)`
`-		self.sim.get_object(link2).rotate(cts.X_AXIS, mut.pi/2)`
`-`
`-		self.sim.get_object(link1).offset_by_object(self.sim.get_object(chassis))`
`-		self.sim.get_object(link1).offset_by_position(arm_offset)`
`-		self.sim.get_object(link2).offset_by_object(self.sim.get_object(link1))`
`-`
`-		self.sim.add_rotary_joint('r1',				   # name, obj1, obj2, anchor, axis`
`-							self.sim.get_object(chassis),`
`-							self.sim.get_object(link1),`
`-							None, cts.Y_AXIS)`
`-		r2_anchor = mut.sub3(self.sim.get_object(link2).get_position(),`
`-							(0,self.LINK2_PARAMS[0][0]/2,0)) # (0,length/2,0)`
`-		self.sim.add_rotary_joint('r2',`
`-							self.sim.get_object(link1),`
`-							self.sim.get_object(link2),`
`-							r2_anchor, cts.Z_AXIS)`
`-`
`-		self.sim.get_object(chassis).actor.set_color(cts.COLOR_RED)`
`-		self.sim.get_object(link1).actor.set_color(cts.COLOR_YELLOW)`
`-		self.sim.get_object(link2).actor.set_color(cts.COLOR_NAVY)`
`-`
`-		self.chassis = chassis`
`-		self.wheelR = wheelR`
`-		self.wheelL = wheelL`
`-		self.ball = ball`
`-`
`-		self.link1 = link1`
`-		self.link2 = link2`
`-`
`-	def go_forwards(self):`
`-		"""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"""`
`-		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)"""`
`-		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)"""`
`-		self.apply_torque_to_wheels(self.WHEEL_TORQUE, -self.WHEEL_TORQUE)`
`-`
`-	def on_pre_step(self):`
`-		#print(self.sim.get_object(self.chassis).get_position())`
`-		try:`
`-			#time = self.sim.sim_time`
`-			q1p = self.get_q1p()`
`-			q2p = self.get_q2p()`
`-`
`-			if not self._frictionless_arm:`
`-				self.apply_friction(q1p, q2p)`
`-`
`-			print('q1p: %f, q2p: %f' % (q1p, q2p))`
`-`
`-		except Exception as e:`
`-			print('Exception when executing on_pre_step: %s' % str(e))`
`-`
`-	def apply_torque_to_wheels(self, torque1, torque2):`
`-		if torque1 is not None:`
`-			self.sim.get_joint('w1').add_torque(torque1)`
`-		if torque2 is not None:`
`-			self.sim.get_joint('w2').add_torque(torque2)`
`-`
`-	def rotate_clockwise(self):`
`-		self.apply_torque_to_joints(self.R1_TORQUE, 0)`
`-`
`-	def rotate_counterlockwise(self):`
`-		self.apply_torque_to_joints(-self.R1_TORQUE, 0)`
`-`
`-	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 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_friction(self, q1p, q2p):`
`-		self.apply_torque_to_joints(-q1p * self.Q1_FRICTION_COEFF, -q2p * self.Q2_FRICTION_COEFF)`
`-`
`-if __name__ == '__main__':`
`-	sim_program = VehicleWithArm()`
`-	sim_program.start()`

# File bin/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`
`-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`
`-`
`-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)`
`-	WHEEL_L_PARAMS = ((0.4,0.3,(0,0,0.5)),{'density':1})`
`-`
`-	SP_STEP = 0.1`
`-	q2_SP = 0.0 # mut.pi/3 # set point`
`-	R2_KP = 1.0 # controller proportional action`
`-	R2_KD = 0.5 # 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.key_press_functions.add('c', self.decrease_sp)`
`-`
`-		self.sp = self.q2_SP`
`-		self.previous_error = 0.0`
`-`
`-	def on_pre_step(self):`
`-		try:`
`-			time = self.sim.sim_time`
`-			time_step = self.sim.time_step`
`-			cv = self.get_q2()`
`-`
`-			mv = self.get_compensation(self.sp, cv, time_step)`
`-			self.apply_torque_to_joints(0, mv) # torque1, torque2`
`-`
`-			output_data(time, self.sp, cv, self.sp - cv, mv)`
`-`
`-		except Exception as e:`
`-			print('Exception when executing on_pre_step: %s' % str(e))`
`-`
`-	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_q2(self):`
`-		"""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"""`
`-		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 = VehicleWithControlledArm(False, False)`
`-	sim_program.start()`

# File 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`
`+holds the ball can be obtained with this equation:`
`+`
`+	tan(theta) / (d + l*sin(theta)) = omega^2 / g`
`+		d: distance from pole to anchor`
`+		l: "cable" length`
`+		omega: angular velocity of pole and ball`
`+		g: gravity`
`+`
`+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.lib.pydispatch import dispatcher`
`+from ars.model.simulator import SIM_PRE_FRAME_SIGNAL`
`+`
`+#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`
`+	STEPS_PER_FRAME = 20 #200 #STEP_SIZE = 1e-5 # 0.01 ms`
`+`
`+	POLE_SPEED_STEP = 0.01`
`+	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('z', self.dec_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`
`+		pole = self.sim.add_cylinder(self.POLE_HEIGHT, self.POLE_VISUAL_RADIUS, self.POLE_INITIAL_POS, density=1.0)`
`+		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), `
`+							mut.add3(self.OFFSET, self.JOINT2_ANCHOR), self.JOINT2_AXIS)`
`+		self.box = box`
`+		self.pole = pole`
`+		self.ball = ball`
`+`
`+	def on_pre_frame(self):`
`+		try:`
`+			#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()`
`+			z_top = self.JOINT2_ANCHOR[1] # JOINT2_ANCHOR[2] in C++ example`
`+			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()`

# File 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.`
`+"""`
`+`
`+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`
`+`
`+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`
`+`
`+	OFFSET = (2.5,1,2.5)`
`+`
`+	BOX_PARAMS = (((3,0.5,3),(0,-0.75,0)),{'density':1}) # ((size, center), density)`
`+	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'density':1}) # ((length, radius, center), density)`
`+	LINK2_PARAMS = ((0.6,0.1,(0,0.7,0.2)),{'density':1})`
`+`
`+	SP_STEP = 0.1`
`+	q2_INITIAL_SP = 0.0 # mut.pi/3 # set point`
`+	R2_KP = 1.0 # controller proportional action`
`+	R2_KD = 0.5 # controller derivative action`
`+`
`+	Q1_FRICTION_COEFF = 0.01`
`+	Q2_FRICTION_COEFF = 0.01`
`+`
`+	def __init__(self):`
`+		Program.__init__(self)`
`+		self.key_press_functions.add('a', self.rotate_clockwise)`
`+		self.key_press_functions.add('z', self.rotate_counterlockwise)`
`+		self.key_press_functions.add('d', self.increase_sp)`
`+		self.key_press_functions.add('c', self.decrease_sp)`
`+`
`+		dispatcher.connect(self.on_pre_step, SIM_PRE_STEP_SIGNAL)`
`+`
`+		self.sp = self.q2_INITIAL_SP`
`+		self.previous_error = 0.0`
`+`
`+	def create_sim_objects(self):`
`+`
`+		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])`
`+		link1 = self.sim.add_capsule(*self.LINK1_PARAMS[0], **self.LINK1_PARAMS[1])`
`+		link2 = self.sim.add_capsule(*self.LINK2_PARAMS[0], **self.LINK2_PARAMS[1])`
`+`
`+		# bodies are rotated before attaching themselves through joints`
`+		self.sim.get_object(link1).rotate(cts.X_AXIS, mut.pi/2)`
`+		self.sim.get_object(link2).rotate(cts.X_AXIS, mut.pi/2)`
`+`
`+		self.sim.get_object(box).offset_by_position(self.OFFSET)`
`+		self.sim.get_object(link1).offset_by_position(self.OFFSET)`
`+		self.sim.get_object(link2).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(link1),`
`+							None, cts.Y_AXIS)`
`+		r2_anchor = mut.sub3(self.sim.get_object(link2).get_position(),`
`+							(0,self.LINK2_PARAMS[0][0]/2,0)) # (0, length/2, 0)`
`+		self.sim.add_rotary_joint('r2',`
`+							self.sim.get_object(link1),`
`+							self.sim.get_object(link2),`
`+							r2_anchor, cts.Z_AXIS)`
`+`
`+	def on_pre_step(self):`
`+		try:`
`+			time = self.sim.sim_time`
`+			time_step = self.sim.time_step`
`+			cv = self.get_q2()`
`+			q1p = self.get_q1p()`
`+			q2p = self.get_q2p()`
`+`
`+			mv = self.get_compensation(self.sp, cv, time_step)`
`+			self.apply_torque_to_joints(0, mv)`
`+			self.apply_friction(q1p, q2p)`
`+`
`+			output_data(time, self.sp, cv, self.sp - cv, mv)`
`+			#print('q1p: %f, q2p: %f' % (q1p, q2p))`
`+`
`+		except Exception as e:`
`+			print('Exception when executing on_pre_step: %s' % str(e))`
`+`
`+	def rotate_clockwise(self):`
`+		self.apply_torque_to_joints(self.R1_TORQUE, 0)`
`+`
`+	def rotate_counterlockwise(self):`
`+		self.apply_torque_to_joints(-self.R1_TORQUE, 0)`
`+`
`+	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 increase_sp(self):`
`+		self.sp += self.SP_STEP`
`+`
`+	def decrease_sp(self):`
`+		self.sp -= self.SP_STEP`
`+`
`+	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 get_compensation(self, sp, q, time_step):`
`+		"""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`
`+`
`+	def apply_friction(self, q1p, q2p):`
`+		self.apply_torque_to_joints(-q1p * self.Q1_FRICTION_COEFF, -q2p * self.Q2_FRICTION_COEFF)`
`+`
`+if __name__ == '__main__':`
`+	print('Python file being executed: %s' % __file__)`
`+	sim_program = ControlledSimpleArm()`
`+	sim_program.start()`

# File 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`
`+"""`
`+`
`+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()`

# File 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`
`+"""`
`+`
`+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)`
`+		self.sim.add_sphere(0.2, (3,2,3), mass=5) # radius, center, mass`
`+`
`+if __name__ == '__main__':`
`+	sim_program = FallingBalls()`
`+	sim_program.start()`

# File 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 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 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()`

# File 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`
`+"""`
`+`
`+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()`

# File demos/IROS/example2_conical_pendulum.py

`+#!/usr/bin/env python`
`+`
`+# Created on 2012.03.09`
`+#`
`+# @author: german`
`+`
`+"""`
`+Example #2`
`+"""`
`+`
`+from ars.app import Program, Simulation`
`+from ars.lib.pydispatch import dispatcher`
`+import ars.model.simulator`
`+import ars.utils.mathematical as mut`
`+import ars.constants as cts`
`+`
`+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`
`+`
`+	# control`
`+	MAX_TORQUE = 20`
`+	SATURATION_TIME = 1`
`+`
`+	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)`