C++ Vector3f::head方法代码示例

bool ZMPFallDetector::getStabilityInidcator(SupportPhase phase,
                                            const Eigen::Matrix4f& leftFootPose,
                                            const Eigen::Matrix4f& rightFootPose)
{
    // If not foot touches the ground, we are fucked.
    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;
}