def angle(a,b):
return arccos(dot(a,b) / (l2norm(a) * l2norm(b)))
def setupIMU(id, joint):
# Set up an ideal IMU with the trajectory of the supplied joint, which samples at samplingPeriod.
imu = IdealIMU()
imu.simulation = sim
imu.trajectory = joint
def handleSample(behaviour):
pass
behaviour = BasicIMUBehaviour(imu, samplingPeriod, sampleCallback=handleSample, initialTime=sim.time)
return behaviour
joint = simModel.getJoint('rfoot')
#original_quaternions = joint.rotationKeyFrames
behaviour = setupIMU(1, joint)
#show()
sim.run(simModel.endTime)
# get the sensor reading from the IMU at each sample time.
accel = behaviour.imu.accelerometer.rawMeasurements
mag = behaviour.imu.magnetometer.rawMeasurements
gyro = behaviour.imu.gyroscope.rawMeasurements
sample_timestamps = accel.timestamps
accel_values = accel.values
mag_values = mag.values
gyro_values = gyro.values
