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