Commits

German Larrain committed 3b68885

bin.RSS1 demo:
-new attributes r1_angle_rate_prev, r2_angle_rate_prev, to store previous angular velocities, allowing the calculation of the average acceleration.
-new get_kinematic_data method, which calculates and returns all the desired bodies' kimatic data for any instant

  • Participants
  • Parent commits 06efcb4

Comments (0)

Files changed (1)

 		""" 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:
 			torque = self.compensate(sp, cv)
 			self.apply_torque_to_wheels(torque)
 			
-			output_data(time, self.speed_i, sp, cv, sp - cv, 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 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()
+		end_effector_pos = self.sim.get_object(self.link2).get_position()
+		
+		return (r1_angle, r1_angle_rate, r1_angle_accel, end_effector_pos)
+		
 
 if __name__ == '__main__':
 	a = RSS1()