Exemple #1
0
int contactConstraintsBV(
    const RigidBodyTree<double>& r,
    const KinematicsCache<double>& cache, int nc,
    std::vector<double> support_mus,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    drake::eigen_aligned_std_vector<SupportStateElement>& supp, MatrixXd& B,
    // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
    MatrixXd& JB, MatrixXd& Jp, VectorXd& Jpdotv, MatrixXd& normals) {
  int j, k = 0, nq = r.get_num_positions();

  B.resize(3, nc * 2 * m_surface_tangents);
  JB.resize(nq, nc * 2 * m_surface_tangents);
  Jp.resize(3 * nc, nq);
  Jpdotv.resize(3 * nc);
  normals.resize(3, nc);

  Vector3d contact_pos, pos, normal;
  MatrixXd J(3, nq);
  Matrix<double, 3, m_surface_tangents> d;

  for (auto iter = supp.begin(); iter != supp.end(); iter++) {
    double mu = support_mus[iter - supp.begin()];
    double norm = sqrt(1 + mu * mu);  // because normals and ds are orthogonal,
                                      // the norm has a simple form
    if (nc > 0) {
      for (auto pt_iter = iter->contact_pts.begin();
           pt_iter != iter->contact_pts.end(); pt_iter++) {
        contact_pos = r.transformPoints(cache, *pt_iter, iter->body_idx, 0);
        J = r.transformPointsJacobian(cache, *pt_iter, iter->body_idx, 0, true);

        normal = iter->support_surface.head(3);
        surfaceTangents(normal, d);
        for (j = 0; j < m_surface_tangents; j++) {
          B.col(2 * k * m_surface_tangents + j) =
              (normal + mu * d.col(j)) / norm;
          B.col((2 * k + 1) * m_surface_tangents + j) =
              (normal - mu * d.col(j)) / norm;

          JB.col(2 * k * m_surface_tangents + j) =
              J.transpose() * B.col(2 * k * m_surface_tangents + j);
          JB.col((2 * k + 1) * m_surface_tangents + j) =
              J.transpose() * B.col((2 * k + 1) * m_surface_tangents + j);
        }

        // store away kin sols into Jp and Jpdotv
        // NOTE: I'm cheating and using a slightly different ordering of J and
        // Jdot here
        Jp.block(3 * k, 0, 3, nq) = J;
        Vector3d pt = (*pt_iter).head(3);
        Jpdotv.block(3 * k, 0, 3, 1) =
            r.transformPointsJacobianDotTimesV(cache, pt, iter->body_idx, 0);
        normals.col(k) = normal;

        k++;
      }
    }
  }

  return k;
}
void scenario1(const RigidBodyTree& model, KinematicsCache<Scalar>& cache,
               const vector<Matrix<Scalar, Dynamic, 1>>& qs,
               const map<int, Matrix3Xd>& body_fixed_points) {
  for (const auto& q : qs) {
    cache.initialize(q);
    model.doKinematics(cache, false);
    for (const auto& pair : body_fixed_points) {
      auto J = model.transformPointsJacobian(cache, pair.second, pair.first, 0,
                                             false);
      if (uniform(generator) < 1e-15) {
        // print with some probability to avoid optimizing away
        printMatrix<decltype(J)::RowsAtCompileTime,
                    decltype(J)::ColsAtCompileTime>(
            J);  // MSVC 2013 can't infer rows and cols (ICE)
      }
    }
  }
}
Matrix<Scalar, Dynamic, Dynamic> forwardKinJacobianTemp(
    const RigidBodyTree &tree, const KinematicsCache<Scalar> &cache,
    const MatrixBase<DerivedPoints> &points, int current_body_or_frame_ind,
    int new_body_or_frame_ind, int rotation_type, bool in_terms_of_qdot) {
    auto Jtrans =
        tree.transformPointsJacobian(cache, points, current_body_or_frame_ind,
                                     new_body_or_frame_ind, in_terms_of_qdot);
    if (rotation_type == 0)
        return Jtrans;
    else {
        Matrix<Scalar, Dynamic, Dynamic> Jrot(
            rotationRepresentationSize(rotation_type), Jtrans.cols());
        if (rotation_type == 1)
            Jrot = tree.relativeRollPitchYawJacobian(cache, current_body_or_frame_ind,
                    new_body_or_frame_ind,
                    in_terms_of_qdot);
        else if (rotation_type == 2)
            Jrot = tree.relativeQuaternionJacobian(cache, current_body_or_frame_ind,
                                                   new_body_or_frame_ind,
                                                   in_terms_of_qdot);
        else
            throw runtime_error("rotation_type not recognized");
        Matrix<Scalar, Dynamic, Dynamic> J((3 + Jrot.rows()) * points.cols(),
                                           Jtrans.cols());
        int row_start = 0;
        for (int i = 0; i < points.cols(); i++) {
            J.template middleRows<3>(row_start) =
                Jtrans.template middleRows<3>(3 * i);
            row_start += 3;

            J.middleRows(row_start, Jrot.rows()) = Jrot;
            row_start += Jrot.rows();
        }
        return J;
    }
};