Commits

German Larrain  committed a5efa06

old RSS1 demo is now IROS third example: now it is removed from bin dir

  • Participants
  • Parent commits 20c4b5a
  • Branches dev-IROS

Comments (0)

Files changed (1)

File bin/RSS1.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()