Commits

German Larrain committed 7b791a7

IROS example 3 and 4: comments cleanup

  • Participants
  • Parent commits d0b1e49
  • Branches dev-IROS

Comments (0)

Files changed (2)

File bin/IROS/example3_speed_profile.py

 
 	VEHICLE_OFFSET = (-4,0.5,4)
 
-	LINK1_PARAMS = ((0.8, 0.1,(0,0,0)),{'mass':1}) # ((length, radius, center), density)
+	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
 			self.apply_torque_to_wheels(torque)
 			self.apply_friction(q1p, q2p)
 			
-			#===================================================================
-			# print('time: %f, speed_i: %d, sp: %f, cv: %f, error: %f, torque: %f' % 
-			#	(time, self.speed_i, sp, cv, error, torque))
-			#===================================================================
-			
 			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))
 			

File bin/IROS/example4_sinusoidal_terrain.py

 
 def random_heightfield(num_x, num_z, scale=1.0):
 	"""
-	Based on:
-	Edward Dale - Snowballs: An experiment in Winter frivolity (2006).
-	http://scompt.com/files/cg2/final.pdf
-		
 	A height field where values are completely random.
 	"""
 	# that x and z are integers, not floats, does not matter
 			verts.append( (x, mut.sin(x * frequency_x)*height_scale, z) )
 	return verts
 
-#===============================================================================
-# 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
-#===============================================================================
-
 def constant_heightfield(num_x, num_z, height=0.0):
 	"""
 	A height field where all the values are the same.
 	WHEELS_TORQUE = 4.0
 	MAX_SPEED = 2.0
 	
-	q1_SP = 0.0 # set point # mut.pi/3
+	q1_SP = 0.0 # set point
 	R1_KP = 20.0 # controller proportional action
 	R1_KD = 15.0 # controller derivative action
 	
-	q2_SP = 0.0 #-mut.pi/2
+	q2_SP = 0.0
 	R2_KP = 20.0
 	R2_KD = 15.0
 	
 	
 	def create_simulation(self):
 		
-		#tm_resolution = 1.0
 		tm_x, tm_z = self.TM_X, self.TM_Z
-		#shrink_factor = (1.0,1.0)
 		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)
 		
-		faces = Trimesh.get_heightfield_faces(tm_x, tm_z)
-		
 		# set up the simulation parameters
 		self.sim = Simulation(self.FPS, self.STEPS_PER_FRAME)
 		self.sim.add_basic_simulation_objects()
 			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],cv,sp,q1,q1p,q2,q2p,torque))
-			#===================================================================
-			
 			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))
 			
 
 if __name__ == '__main__':
 	sim_program = Example4()
-	sim_program.start()
+	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
+#===============================================================================