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;
}
};