Commits

German Larrain committed da1a26a

reorganization: renamed 'bin' subdirectory to 'demos'

Comments (0)

Files changed (34)

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()

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()

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()

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()

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()

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()

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())

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())

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
-#===============================================================================

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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)
+