Commits

German Larrain committed 7f91b33

old RSS1 demo is now IROS third example

  • Participants
  • Parent commits 023bdaf
  • Branches dev-IROS

Comments (0)

Files changed (1)

File bin/IROS/example3_speed_profile.py

+#!/usr/bin/env python
+
+# Created on 2012.01.11
+#
+# @author: german
+
+"""
+Experiment #1 for a paper submitted to the 
+2012 Robotics Science and Systems conference
+"""
+
+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):
+	
+	WINDOW_SIZE = (1024,630)
+	CAMERA_POSITION = (0,8,15)
+
+	VEHICLE_OFFSET = (-4,0.5,2)
+
+	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})
+	
+	kp = 10
+	speeds = ((0,0), (1,0), (5,1), (9,1), (13,0), (14,0)) # (time,speed)
+	speed_i = 0
+	
+	def __init__(self):
+		""" Constructor, calls first the superclass constructor. """
+		VehicleWithArm.__init__(self)
+		self.sim.get_object(self.chassis).get_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
+			
+			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])
+			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())
+			
+		except:
+			print('Exception when executing on_pre_step')
+	
+	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 Exception('invalid speed_i value: %d' % self.speed_i)
+	
+	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)
+
+if __name__ == '__main__':
+	sim_program = RSS1()
+	sim_program.start()