bool ZMPFallDetector::getStabilityInidcator(SupportPhase phase,
const Eigen::Matrix4f& leftFootPose,
const Eigen::Matrix4f& rightFootPose)
{
// If not foot touches the ground, we are f****d.
bool stillFalling = phase == SUPPORT_NONE;
if (stillFalling)
return true;
const auto& supportHull = [this, phase, &leftFootPose, &rightFootPose]() {
if (phase == Bipedal::SUPPORT_LEFT)
return leftSupportHull;
if (phase == Bipedal::SUPPORT_RIGHT)
return rightSupportHull;
BOOST_ASSERT(phase == Bipedal::SUPPORT_BOTH);
// Foot positions changed
//if (lastSupportPhase != phase)
{
dualSupportHull = computeSupportPolygone(leftFootPose,
rightFootPose,
leftSupportHull,
rightSupportHull,
phase);
}
lastSupportPhase = phase;
return dualSupportHull;
}();
// center of convex hull and orientation should correspond with ground frame
const auto& groundFrame = Bipedal::computeGroundFrame(leftFootPose, rightFootPose, phase);
Eigen::Vector3f zmp = Eigen::Vector3f::Zero();
zmp.head(2) = zmpEstimator.estimation * 1000;
Eigen::Vector3f zmpConvexHull = VirtualRobot::MathTools::transformPosition(zmp, groundFrame.inverse());
// if zmp is inside of the CH, we are not falling
if (VirtualRobot::MathTools::isInside(zmpConvexHull.head(2), supportHull))
{
stillFalling = false;
//std::cout << "(" << phase << ") ZMP is inside the CH. " << std::endl;
}
else
{
Eigen::Vector3f contact = Eigen::Vector3f::Zero();
contact.head(2) = Bipedal::computeHullContactPoint(zmpConvexHull.head(2), supportHull);
contactPoint = VirtualRobot::MathTools::transformPosition(contact, groundFrame);
double dist = (contact-zmpConvexHull).norm();
//std::cout << "(" << phase << ") ZMP is outside the CH: " << dist << std::endl;
stillFalling = dist > maxHullDist;
}
return stillFalling;
}
/**
* Warning: This only works if we have a position without a slope!
*/
double ZMPDistributor::computeAlpha(const Eigen::Matrix4f& groundPoseLeft,
const Eigen::Matrix4f& groundPoseRight,
const Eigen::Vector3f& refZMP,
const Eigen::Vector2f& relZMPLeft,
const Eigen::Vector2f& relZMPRight,
Bipedal::SupportPhase phase)
{
double alpha;
Eigen::Vector2f leftContact, rightContact;
Eigen::Vector2f pAlpha;
switch (phase)
{
case Bipedal::SUPPORT_LEFT:
alpha = 0.0;
break;
case Bipedal::SUPPORT_RIGHT:
alpha = 1.0;
break;
case Bipedal::SUPPORT_BOTH:
// get contact points in world coordinates
leftContact = VirtualRobot::MathTools::transformPosition(Bipedal::computeHullContactPoint(relZMPLeft, leftConvexHull), groundPoseLeft);
rightContact = VirtualRobot::MathTools::transformPosition(Bipedal::computeHullContactPoint(relZMPRight, rightConvexHull), groundPoseRight);
pAlpha = VirtualRobot::MathTools::nearestPointOnSegment(
leftContact, rightContact,
Eigen::Vector2f(refZMP.head(2))
);
alpha = (leftContact - pAlpha).norm() / (leftContact - rightContact).norm();
break;
}
return alpha;
}
IGL_INLINE void igl::viewer::ViewerCore::get_scale_and_shift_to_fit_mesh(
const Eigen::MatrixXd& V,
float& zoom,
Eigen::Vector3f& shift)
{
if (V.rows() == 0)
return;
auto min_point = V.colwise().minCoeff();
auto max_point = V.colwise().maxCoeff();
auto centroid = (0.5*(min_point + max_point)).eval();
shift.setConstant(0);
shift.head(centroid.size()) = -centroid.cast<float>();
zoom = 2.0 / (max_point-min_point).array().abs().maxCoeff();
}
/**
* @brief Computes and applies the translation transformations to the trackball given new position.
* @param pos New mouse position in normalized trackball system
*/
void translateCamera (const Eigen::Vector2f& pos)
{
Eigen::Vector2f normalized_pos = normalizePosition(pos);
if (!translating)
{
translating = true;
initialTranslationPosition = normalized_pos;
}
else if (pos != initialPosition.head(2))
{
finalTranslationPosition = normalized_pos;
computeTranslationVector();
updateViewMatrix();
initialTranslationPosition = finalTranslationPosition;
}
}
/**
* @brief Computes and applies the rotation transformations to the trackball given new position.
* @param pos New mouse position in normalized trackball system
*/
void rotateCamera (const Eigen::Vector2f& pos)
{
Eigen::Vector3f normalized_pos = computeSpherePosition(normalizePosition(pos));
if (!rotating)
{
rotating = true;
initialPosition = normalized_pos;
}
else if ( pos != initialPosition.head(2))
{
finalPosition = normalized_pos;
computeRotationAngle();
updateViewMatrix();
initialPosition = finalPosition;
}
}
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
const Eigen::Vector3f& localAnkleRight,
const Eigen::Matrix4f& leftFootPoseGroundFrame,
const Eigen::Matrix4f& rightFootPoseGroundFrame,
const Eigen::Vector3f& zmpRefGroundFrame,
Bipedal::SupportPhase phase)
{
Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());
double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);
//std::cout << "########## " << alpha << " ###########" << std::endl;
ForceTorque ft;
// kg*m/s^2 = N
ft.leftForce = -(1-alpha) * mass * gravity;
ft.rightForce = -alpha * mass * gravity;
// Note we need force as kg*mm/s^2
ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000);
ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);
// ZMP not contained in convex polygone
if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
&& std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
{
Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque;
Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);
//std::cout << "Tau0World: " << tau0.transpose() << std::endl;
//std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl;
//std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;
// Note: Our coordinate system is different than in the paper!
// Also this is not the same as the ground frame.
Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
- localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
xAxis /= xAxis.norm();
Eigen::Vector3f zAxis(0, 0, 1);
Eigen::Vector3f yAxis = zAxis.cross(xAxis);
yAxis /= yAxis.norm();
Eigen::Matrix3f centerFrame;
centerFrame.block(0, 0, 3, 1) = xAxis;
centerFrame.block(0, 1, 3, 1) = yAxis;
centerFrame.block(0, 2, 3, 1) = zAxis;
//std::cout << "Center frame:\n" << centerFrame << std::endl;
Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
Eigen::Vector3f leftTorqueCenter;
leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
leftTorqueCenter.z() = 0;
Eigen::Vector3f rightTorqueCenter;
rightTorqueCenter.x() = alpha*centerTau0.x();
rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
rightTorqueCenter.z() = 0;
//std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
//std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl;
//std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;
// tcp <--- ground frame <--- center frame
ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter;
ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
}
// Torque depends on timestep, we need a way to do this correctly.
const double torqueFactor = 1;
// convert to Nm
ft.leftTorque *= torqueFactor / 1000.0 / 1000.0;
ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;
//std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl;
//std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;
return ft;
}