Commits

German Larrain committed 5687195

third example: some code refactor and names changed. A little trick was necessary to import a module in the parent folder.

Comments (0)

Files changed (1)

bin/IROS/example3_speed_profile.py

 # @author: german
 
 """
-Experiment #1 for a paper submitted to the 
-2012 Robotics Science and Systems conference
+Example #3
 """
 
+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, mut
 
-
-def output_data(time, speed_i, sp, cv, error, torque):
-	
-	print('time: %f, speed_i: %d, sp: %f, cv: %f, error: %f, torque: %f' % 
-		(time, speed_i, sp, cv, error, torque))
-
-class RSS1(VehicleWithArm):
+class Example3(VehicleWithArm):
 	
 	WINDOW_SIZE = (1024,630)
 	CAMERA_POSITION = (0,8,15)
 			
 			sp = self.calc_desired_speed(time)
 			linear_vel = self.sim.get_object(self.chassis).get_linear_velocity()
-			linear_vel_plane = (linear_vel[0], linear_vel[2])
-			cv = mut.length2(linear_vel_plane) * mut.sign(linear_vel[0])
+			linear_vel_XZ = (linear_vel[0], linear_vel[2])
+			cv = mut.length2(linear_vel_XZ) * mut.sign(linear_vel[0])
+			error = sp - cv
 			torque = self.compensate(sp, cv)
 			self.apply_torque_to_wheels(torque)
 			
-			output_data(time, self.speed_i, sp, cv, sp - cv, torque)
-			#print(self.get_kinematic_data())
+			print('time: %f, speed_i: %d, sp: %f, cv: %f, error: %f, torque: %f' % 
+				(time, self.speed_i, sp, cv, error, torque))
 			
 		except:
 			print('Exception when executing on_pre_step')
 	def compensate(self, sp, cv):
 		return (sp - cv) * self.kp
 	
-	def get_kinematic_data(self):
-		r1_angle = self.sim.get_joint('r1').get_joint().get_angle()
-		r1_angle_rate = self.sim.get_joint('r1').get_joint().get_angle_rate()
-		r1_angle_accel = (self.r1_angle_rate_prev - r1_angle_rate) / self.sim.get_time_step()
-		
-		r2_angle = self.sim.get_joint('r2').get_joint().get_angle()
-		r2_angle_rate = self.sim.get_joint('r2').get_joint().get_angle_rate()
-		r2_angle_accel = (self.r2_angle_rate_prev - r2_angle_rate) / self.sim.get_time_step()
-		
-		end_effector_pos = self.sim.get_object(self.link2).get_position()
-		
-		return (r1_angle, r1_angle_rate, r1_angle_accel, r2_angle, 
-			r2_angle_rate, r2_angle_accel, end_effector_pos)
+	#===========================================================================
+	# def get_kinematic_data(self):
+	#	r1_angle = self.sim.get_joint('r1').get_joint().get_angle()
+	#	r1_angle_rate = self.sim.get_joint('r1').get_joint().get_angle_rate()
+	#	r1_angle_accel = (self.r1_angle_rate_prev - r1_angle_rate) / self.sim.get_time_step()
+	#	
+	#	r2_angle = self.sim.get_joint('r2').get_joint().get_angle()
+	#	r2_angle_rate = self.sim.get_joint('r2').get_joint().get_angle_rate()
+	#	r2_angle_accel = (self.r2_angle_rate_prev - r2_angle_rate) / self.sim.get_time_step()
+	#	
+	#	end_effector_pos = self.sim.get_object(self.link2).get_position()
+	#	
+	#	return (r1_angle, r1_angle_rate, r1_angle_accel, r2_angle, 
+	#		r2_angle_rate, r2_angle_accel, end_effector_pos)
+	#===========================================================================
 
 if __name__ == '__main__':
-	sim_program = RSS1()
+	sim_program = Example3()
 	sim_program.start()