Commits

German Larrain  committed 129905e

model.robot.sensors: new class 'TotalEnergy'

  • Participants
  • Parent commits dd3f21e
  • Branches dev-sensors

Comments (0)

Files changed (1)

File ars/model/robot/sensors.py

 		kwargs = {'potential energy': potential_e, }
 		return SensorData(**kwargs)
 
+
+class TotalEnergy(BodySensor):
+	r"""Calculate and retrieve body's potential and kinetic (both due to
+	translation and rotation) energy.
+
+	.. math::
+		E_p &= m \cdot g \cdot h = - m \cdot g^\top x \\
+		E_k &= \frac{1}{2} m \cdot v^\top v
+		  + \frac{1}{2} \omega^\top \mathbf{I} \omega
+
+	"""
+	def __init__(self, body, gravity, disaggregate=False):
+		super(TotalEnergy, self).__init__(body)
+		self._gravity = gravity
+		self._disaggregate = disaggregate
+		
+	def _build_data(self):
+		potential_e = self.body.calc_potential_energy(self._gravity)
+		linear_ke = self.body.calc_translation_kinetic_energy()
+		angular_ke = self.body.calc_rotation_kinetic_energy()
+
+		if self._disaggregate:
+			kwargs = {
+				'potential energy': potential_e,
+				'kinetic energy': linear_ke + angular_ke, }
+		else:
+			kwargs = {'total energy': potential_e + linear_ke + angular_ke, }
+		return SensorData(**kwargs)
+
 #==============================================================================
 # aux classes
 #==============================================================================