Example #1
0
std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(const Eigen::MatrixBase<DerivedTorque> & torque, const Eigen::MatrixBase<DerivedForce> & force, const Eigen::MatrixBase<DerivedNormal> & normal, const Eigen::MatrixBase<DerivedPoint> & point_on_contact_plane)
{
    // TODO: implement multi-column version
    using namespace Eigen;

    if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
        throw std::runtime_error("Drake:resolveCenterOfPressure:BadInputs: normal should be a unit vector");
    }

    Vector3d cop;
    double normal_torque_at_cop;

    double fz = normal.dot(force);
    bool cop_exists = abs(fz) > 1e-12;

    if (cop_exists) {
        auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
        double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
        auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
        cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
        auto torque_at_cop = torque - cop.cross(force);
        normal_torque_at_cop = normal.dot(torque_at_cop);
    }
    else {
        cop.setConstant(std::numeric_limits<double>::quiet_NaN());
        normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
    }
    return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}
inline void makeCoordinateSystem(Eigen::MatrixBase<Derived>& v1,
                                 Eigen::MatrixBase<DerivedOther1>& v2,
                                 Eigen::MatrixBase<DerivedOther2>& v3)
{
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedOther1, 3);
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedOther2, 3);
    /*ASSERTMSG(  v1.rows() == 3 && v1.cols()==1 &&
               v2.rows() == 3 && v2.cols() == 1 &&
               v3.rows() == 3 && v3.cols() == 1
               , "IN: "<< v1.rows()<<","<<v1.cols()<<" OUT: "<<
       v2.rows()<<","<<v2.cols()<<"/"<<v3.rows()<<","<<v3.cols() );*/

    typedef typename Derived::Scalar PREC;

    using std::abs;
    using std::sqrt;

    v1.normalize();

    if (abs(v1(0)) > abs(v1(2)))
    {
        PREC invLen = 1.0 / sqrt(v1(0) * v1(0) + v1(2) * v1(2));
        v2          = typename MyMatrix<double>::Vector3(-v1(2) * invLen, 0, v1(0) * invLen);
    }
    else
    {
        PREC invLen = 1.0 / sqrt(v1(1) * v1(1) + v1(2) * v1(2));
        v2          = typename MyMatrix<double>::Vector3(0, v1(2) * invLen, -v1(1) * invLen);
    }
    v3 = v1.cross(v2);

    v2.normalize();
    v3.normalize();
};