Commits

German Larrain committed f0f1e22

second examples: change in visualization via a custom floor creation, necessary to create the video. Also, class parameter reordered and some explaining comments added.

  • Participants
  • Parent commits 1ea8cf0
  • Branches dev-IROS

Comments (0)

Files changed (1)

File bin/IROS/example2_conical_pendulum.py

 Example #2
 """
 
-from ars.app import Program
+from ars.app import Program, Simulation
 import ars.utilities.mathematical as mut
 import ars.constants as cts
 
-class Example2(Program):	
-
-	OFFSET = (2,0.5,2)
+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)
 	
-	WINDOW_SIZE = (900,600)
-	# (0,-4,1) in C++ example #vp_xyz = (0.0,-4.0,1.0) # position [meters]
-	CAMERA_POSITION = (2,5,10)
-	FPS = 50
-	STEPS_PER_FRAME = 80
-
 	POLE_RADIUS = 0.141421 # 1/(5*sqrt(2))
 	POLE_HEIGHT = 1
-	POLE_INITIAL_POS = (0.0,1.0,0.0)
+	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,1.0,0.1)
+	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.5,0.1)
+	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
 
 		
 		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.get_actors().values())
+		self.sim.update_actors()
 	
 	def create_sim_objects(self):
 		box = self.sim.add_box(*self.BOX_PARAMS[0], **self.BOX_PARAMS[1])
 							self.sim.get_object(arm), 
 							mut.add3(self.OFFSET, self.JOINT2_ANCHOR),
 							self.JOINT2_AXIS)
+		
+		#self.sim.get_object(box).get_actor().set_color(cts.COLOR_RED)
+		self.sim.get_object(pole).get_actor().set_color(cts.COLOR_YELLOW)
+		self.sim.get_object(arm).get_actor().set_color(cts.COLOR_NAVY)
+		
 		self.box = box
 		self.pole = pole
 		self.arm = arm
 	sim_program = Example2()
 	sim_program.start()
 	
+	# print arm links' inertia matrices
 	pole_body = sim_program.sim.get_object(sim_program.pole).get_body()
 	arm_body = sim_program.sim.get_object(sim_program.arm).get_body()
 	print(pole_body.get_inertia_tensor())