void Motion::debugPlots()
{
// some basic plots
// plotting sensor data
PLOT("Motion:GyrometerData:data:x", getGyrometerData().data.x);
PLOT("Motion:GyrometerData:data:y", getGyrometerData().data.y);
PLOT("Motion:GyrometerData:rawData:x", getGyrometerData().rawData.x);
PLOT("Motion:GyrometerData:rawData:y", getGyrometerData().rawData.y);
PLOT("Motion:GyrometerData:ref", getGyrometerData().ref);
PLOT("Motion:AccelerometerData:x", getAccelerometerData().data.x);
PLOT("Motion:AccelerometerData:y", getAccelerometerData().data.y);
PLOT("Motion:AccelerometerData:z", getAccelerometerData().data.z);
PLOT("Motion:InertialSensorData:x", sin(getInertialSensorData().data.x));
PLOT("Motion:InertialSensorData:y", (getInertialSensorData().data.y));
PLOT("Motion:KinematicChain:oriantation:model:x",
getKinematicChainMotor().theLinks[KinematicChain::Hip].R.getXAngle()
);
PLOT("Motion:KinematicChain:oriantation:model:y",
getKinematicChainMotor().theLinks[KinematicChain::Hip].R.getYAngle()
);
DEBUG_REQUEST("Motion:KinematicChain:orientation_test",
RotationMatrix calculatedRotation =
Kinematics::ForwardKinematics::calcChestFeetRotation(getKinematicChainSensor());
// calculate expected acceleration sensor reading
Vector2d inertialExpected(calculatedRotation.getXAngle(), calculatedRotation.getYAngle());
PLOT("Motion:KinematicChain:oriantation:sensor:x", Math::toDegrees(inertialExpected.x) );
PLOT("Motion:KinematicChain:oriantation:sensor:y", Math::toDegrees(inertialExpected.y) );
);
GTEST_TEST(RotationMatrix, getYAngle)
{
float angle = 0_deg;
RotationMatrix r = RotationMatrix::aroundY(angle);
float yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, angle));
angle = 90_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, angle));
angle = 180_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(Angle::normalize(yAngle), Angle::normalize(angle)));
angle = -180_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(Angle::normalize(yAngle), Angle::normalize(angle)));
angle = 360_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, Angle::normalize(angle)));
for(int i = 0; i < RUNS; ++i)
{
const float angle = Random::uniform() * pi;
const RotationMatrix r = RotationMatrix::aroundY(angle);
float yAngle = r.getYAngle();
if(!Approx::isEqual(yAngle, angle, 1e-3f))
EXPECT_TRUE(Approx::isEqual(yAngle, angle, 1e-3f))
<< "yAngle:\n"
<< yAngle << "\n"
<< "angle\n"
<< angle << "\n";
}
}
