示例#1
0
void normalizeVec(
    const Eigen::MatrixBase<Derived>& x,
    typename Derived::PlainObject& x_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 1>::type* dx_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 2>::type* ddx_norm) {

  typename Derived::Scalar xdotx = x.squaredNorm();
  typename Derived::Scalar norm_x = std::sqrt(xdotx);
  x_norm = x / norm_x;

  if (dx_norm) {
    dx_norm->setIdentity(x.rows(), x.rows());
    (*dx_norm) -= x * x.transpose() / xdotx;
    (*dx_norm) /= norm_x;

    if (ddx_norm) {
      auto dx_norm_transpose = transposeGrad(*dx_norm, x.rows());
      auto ddx_norm_times_norm = -matGradMultMat(x_norm, x_norm.transpose(), (*dx_norm), dx_norm_transpose);
      auto dnorm_inv = -x.transpose() / (xdotx * norm_x);
      (*ddx_norm) = ddx_norm_times_norm / norm_x;
      auto temp = (*dx_norm) * norm_x;
      int n = x.rows();
      for (int col = 0; col < n; col++) {
        auto column_as_matrix = (dnorm_inv(0, col) * temp);
        for (int row_block = 0; row_block < n; row_block++) {
          ddx_norm->block(row_block * n, col, n, 1) += column_as_matrix.col(row_block);
        }
      }
    }
  }
}
示例#2
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);
}