Commits

German Larrain committed 5a4206d

new demo CentrifugalForceTest, which tests the accuracy of a simple simulation

Comments (0)

Files changed (1)

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.utilities.mathematical as mut
+import ars.constants as cts
+
+#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 = (1,(5,0.5,5),(0,-0.25,0)) # (density, size, center)
+	
+	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,0.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)
+		
+		self.set_pre_frame_callback(self.on_pre_frame)
+		
+		self.joint1_vel_user = self.JOINT1_ANGLE_RATE_INITIAL
+		self.large_speed_steps = True
+		#TODO: set ERP, CFM
+		
+	
+	def create_sim_objects(self):
+		box = self.sim.add_box(*self.BOX_PARAMS)
+		# FIXME: pole has no mass! => does not really matter, since speed is set
+		pole = self.sim.add_cylinder(1.0, self.POLE_HEIGHT, self.POLE_VISUAL_RADIUS, self.POLE_INITIAL_POS)
+		ball_density = self.BALL_MASS / get_sphere_volume(self.BALL_VISUAL_RADIUS)
+		# FIXME: visual radius => did not affect the results noticeably
+		ball = self.sim.add_sphere(ball_density, self.BALL_VISUAL_RADIUS, self.BALL_INITIAL_POS)
+		
+		# 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', 
+							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_step: %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').get_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').get_joint().get_angle_rate()
+		self.sim.get_joint('r2').get_joint().add_torque(torque)
+
+if __name__ == '__main__':
+	a = CentrifugalForceTest()
+	a.start()