fix indexing bug in torque calculation in ContactForceTorqueSensor