void TorsoMatrixProvider::update(TorsoMatrixBH& torsoMatrix)
{
// remove the z-rotation from the orientation data rotation matrix
Vector3BH<> g(theOrientationDataBH.rotation.c1.z, -theOrientationDataBH.rotation.c0.z, 0.f);
float w = std::atan2(std::sqrt(g.x * g.x + g.y * g.y), theOrientationDataBH.rotation.c2.z);
RotationMatrixBH torsoRotation(g, w);
// calculate "center of hip" position from left foot
Pose3DBH fromLeftFoot(torsoRotation);
fromLeftFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footLeft]);
fromLeftFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
fromLeftFoot.translation *= -1.;
fromLeftFoot.rotation = torsoRotation;
// calculate "center of hip" position from right foot
Pose3DBH fromRightFoot(torsoRotation);
fromRightFoot.conc(theRobotModelBH.limbs[MassCalibrationBH::footRight]);
fromRightFoot.translate(0, 0, -theRobotDimensionsBH.heightLeg5Joint);
fromRightFoot.translation *= -1.;
fromRightFoot.rotation = torsoRotation;
// get foot z-rotations
const Pose3DBH& leftFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footLeft].invert());
const Pose3DBH& rightFootInverse(theRobotModelBH.limbs[MassCalibrationBH::footRight].invert());
const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
const float rightFootZRotation = rightFootInverse.rotation.getZAngle();
// determine used foot
const bool useLeft = fromLeftFoot.translation.z > fromRightFoot.translation.z;
torsoMatrix.leftSupportFoot = useLeft;
// calculate foot span
const Vector3BH<> newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);
// and construct the matrix
Pose3DBH newTorsoMatrix;
newTorsoMatrix.translate(newFootSpan.x / (useLeft ? 2.f : -2.f), newFootSpan.y / (useLeft ? 2.f : -2.f), 0);
newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);
// calculate torso offset
if(torsoMatrix.translation.z != 0) // the last torso matrix should be valid
{
Pose3DBH& torsoOffset(torsoMatrix.offset);
torsoOffset = torsoMatrix.invert();
torsoOffset.translate(lastFootSpan.x / (useLeft ? 2.f : -2.f), lastFootSpan.y / (useLeft ? 2.f : -2.f), 0);
torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
torsoOffset.translate(newFootSpan.x / (useLeft ? -2.f : 2.f), newFootSpan.y / (useLeft ? -2.f : 2.f), 0);
torsoOffset.conc(newTorsoMatrix);
}
// adopt new matrix and footSpan
(Pose3DBH&)torsoMatrix = newTorsoMatrix;
lastLeftFootZRotation = leftFootZRotation;
lastRightFootZRotation = rightFootZRotation;
lastFootSpan = newFootSpan;
// valid?
torsoMatrix.isValid = theGroundContactStateBH.contact;
//
PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x);
PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y);
PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z);
PLOT("module:TorsoMatrixProvider:torsoMatrixX", torsoMatrix.translation.x);
PLOT("module:TorsoMatrixProvider:torsoMatrixY", torsoMatrix.translation.y);
PLOT("module:TorsoMatrixProvider:torsoMatrixZ", torsoMatrix.translation.z);
}
void TorsoMatrixProvider::update(TorsoMatrixBH& torsoMatrix,
const FilteredSensorData& theFilteredSensorData,
const RobotDimensionsBH& theRobotDimensions,
const RobotModel& theRobotModel,
const GroundContactState& theGroundContactState,
const DamageConfiguration& theDamageConfiguration)
{
// generate rotation matrix from measured angleX and angleY
const Vector3BH<> axis((float) theFilteredSensorData.data[SensorData::angleX], (float) theFilteredSensorData.data[SensorData::angleY], 0);
RotationMatrixBH torsoRotation(axis);
// calculate "center of hip" position from left foot
Pose3DBH fromLeftFoot(torsoRotation);
fromLeftFoot.conc(theRobotModel.limbs[MassCalibrationBH::footLeft]);
fromLeftFoot.translate(0, 0, (float) -theRobotDimensions.heightLeg5Joint);
fromLeftFoot.translation *= -1.;
fromLeftFoot.rotation = torsoRotation;
// calculate "center of hip" position from right foot
Pose3DBH fromRightFoot(torsoRotation);
fromRightFoot.conc(theRobotModel.limbs[MassCalibrationBH::footRight]);
fromRightFoot.translate(0, 0, (float) -theRobotDimensions.heightLeg5Joint);
fromRightFoot.translation *= -1.;
fromRightFoot.rotation = torsoRotation;
// get foot z-rotations
const Pose3DBH& leftFootInverse(theRobotModel.limbs[MassCalibrationBH::footLeft].invert());
const Pose3DBH& rightFootInverse(theRobotModel.limbs[MassCalibrationBH::footRight].invert());
const float leftFootZRotation = leftFootInverse.rotation.getZAngle();
const float rightFootZRotation = rightFootInverse.rotation.getZAngle();
// determine used foot
const bool useLeft = fromLeftFoot.translation.z > fromRightFoot.translation.z;
// calculate foot span
const Vector3BH<> newFootSpan(fromRightFoot.translation - fromLeftFoot.translation);
// and construct the matrix
Pose3DBH newTorsoMatrix;
newTorsoMatrix.translate(newFootSpan.x / (useLeft ? 2.f : -2.f), newFootSpan.y / (useLeft ? 2.f : -2.f), 0);
newTorsoMatrix.conc(useLeft ? fromLeftFoot : fromRightFoot);
// calculate torso offset
if(torsoMatrix.translation.z != 0) // the last torso matrix should be valid
{
Pose3DBH& torsoOffset(torsoMatrix.offset);
torsoOffset = torsoMatrix.invert();
torsoOffset.translate(lastFootSpan.x / (useLeft ? 2.f : -2.f), lastFootSpan.y / (useLeft ? 2.f : -2.f), 0);
torsoOffset.rotateZ(useLeft ? float(leftFootZRotation - lastLeftFootZRotation) : float(rightFootZRotation - lastRightFootZRotation));
torsoOffset.translate(newFootSpan.x / (useLeft ? -2.f : 2.f), newFootSpan.y / (useLeft ? -2.f : 2.f), 0);
torsoOffset.conc(newTorsoMatrix);
}
// adopt new matrix and footSpan
(Pose3DBH&)torsoMatrix = newTorsoMatrix;
lastLeftFootZRotation = leftFootZRotation;
lastRightFootZRotation = rightFootZRotation;
lastFootSpan = newFootSpan;
// valid?
torsoMatrix.isValid = (theGroundContactState.contact || !theDamageConfiguration.useGroundContactDetection);
//
// PLOT("module:TorsoMatrixProvider:footSpanX", newFootSpan.x);
// PLOT("module:TorsoMatrixProvider:footSpanY", newFootSpan.y);
// PLOT("module:TorsoMatrixProvider:footSpanZ", newFootSpan.z);
//
// PLOT("module:TorsoMatrixProvider:torsoMatrixX", torsoMatrix.translation.x);
// PLOT("module:TorsoMatrixProvider:torsoMatrixY", torsoMatrix.translation.y);
// PLOT("module:TorsoMatrixProvider:torsoMatrixZ", torsoMatrix.translation.z);
}