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;
}
Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> forwardKinTemp(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) {
Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> ret(3 + rotationRepresentationSize(rotation_type), points.cols());
ret.template topRows<3>() = tree.transformPoints(cache, points, current_body_or_frame_ind, new_body_or_frame_ind);
if (rotation_type == 1) {
ret.template bottomRows<3>().colwise() = tree.relativeRollPitchYaw(cache, current_body_or_frame_ind, new_body_or_frame_ind);
}
else if (rotation_type == 2) {
ret.template bottomRows<4>().colwise() = tree.relativeQuaternion(cache, current_body_or_frame_ind, new_body_or_frame_ind);
}
return ret;
};
int main(int argc, char* argv[]) {
if (argc < 2) {
cerr << "Usage: urdfKinTest urdf_filename" << endl;
exit(-1);
}
RigidBodyTree* model = new RigidBodyTree(argv[1]);
cout << "=======" << endl;
// run kinematics with second derivatives 100 times
Eigen::VectorXd q = model->getZeroConfiguration();
Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
int i;
if (argc >= 2 + model->num_positions) {
for (i = 0; i < model->num_positions; i++)
sscanf(argv[2 + i], "%lf", &q(i));
}
// for (i=0; i<model->num_dof; i++)
// q(i)=(double)rand() / RAND_MAX;
KinematicsCache<double> cache = model->doKinematics(q, v);
// }
// const Vector4d zero(0,0,0,1);
Eigen::Vector3d zero = Eigen::Vector3d::Zero();
Eigen::Matrix<double, 6, 1> pt;
for (i = 0; i < model->bodies.size(); i++) {
// model->forwardKin(i,zero,1,pt);
auto pt = model->transformPoints(cache, zero, i, 0);
auto rpy = model->relativeRollPitchYaw(cache, i, 0);
Eigen::Matrix<double, 6, 1> x;
x << pt, rpy;
// cout << i << ": forward kin: " << model->bodies[i].linkname << " is at
// " << pt.transpose() << endl;
cout << model->bodies[i]->linkname << " ";
cout << x.transpose() << endl;
// for (int j=0; j<pt.size(); j++)
// cout << pt(j) << " ";
}
auto phi = model->positionConstraints<double>(cache);
cout << "phi = " << phi.transpose() << endl;
delete model;
return 0;
}
int contactPhi(const RigidBodyTree &r, const KinematicsCache<double> &cache,
SupportStateElement &supp, VectorXd &phi) {
int nc = static_cast<int>(supp.contact_pts.size());
phi.resize(nc);
if (nc < 1) return nc;
int i = 0;
for (auto pt_iter = supp.contact_pts.begin();
pt_iter != supp.contact_pts.end(); pt_iter++) {
Vector3d contact_pos = r.transformPoints(cache, *pt_iter, supp.body_idx, 0);
phi(i) = supp.support_surface.head<3>().dot(contact_pos) +
supp.support_surface(3);
i++;
}
return nc;
}
MatrixXd individualSupportCOPs(
const RigidBodyTree &r, const KinematicsCache<double> &cache,
const std::vector<SupportStateElement,
Eigen::aligned_allocator<SupportStateElement>> &
active_supports,
const MatrixXd &normals, const MatrixXd &B, const VectorXd &beta) {
const int n_basis_vectors_per_contact =
static_cast<int>(B.cols() / normals.cols());
const int n = static_cast<int>(active_supports.size());
int normals_start = 0;
int beta_start = 0;
MatrixXd individual_cops(3, n);
individual_cops.fill(std::numeric_limits<double>::quiet_NaN());
for (size_t j = 0; j < active_supports.size(); j++) {
auto active_support = active_supports[j];
auto contact_pts = active_support.contact_pts;
int ncj = static_cast<int>(contact_pts.size());
int active_support_length = n_basis_vectors_per_contact * ncj;
auto normalsj = normals.middleCols(normals_start, ncj);
Vector3d normal = normalsj.col(0);
bool normals_identical =
(normalsj.colwise().operator-(normal)).squaredNorm() < 1e-15;
if (normals_identical) { // otherwise computing a COP doesn't make sense
const auto &Bj = B.middleCols(beta_start, active_support_length);
const auto &betaj = beta.segment(beta_start, active_support_length);
const auto &contact_positions =
r.bodies[active_support.body_idx]->contact_pts;
Vector3d force = Vector3d::Zero();
Vector3d torque = Vector3d::Zero();
for (size_t k = 0; k < contact_pts.size(); k++) {
// for (auto k = contact_pts.begin(); k!= contact_pts.end(); k++) {
const auto &Bblock = Bj.middleCols(k * n_basis_vectors_per_contact,
n_basis_vectors_per_contact);
const auto &betablock = betaj.segment(k * n_basis_vectors_per_contact,
n_basis_vectors_per_contact);
Vector3d point_force = Bblock * betablock;
force += point_force;
Vector3d contact_pt = contact_pts[k].head(3);
auto torquejk = contact_pt.cross(point_force);
torque += torquejk;
}
Vector3d point_on_contact_plane = contact_positions.col(0);
std::pair<Vector3d, double> cop_and_normal_torque =
resolveCenterOfPressure(torque, force, normal,
point_on_contact_plane);
Vector3d cop_world = r.transformPoints(cache, cop_and_normal_torque.first,
active_support.body_idx, 0);
individual_cops.col(j) = cop_world;
}
normals_start += ncj;
beta_start += active_support_length;
}
return individual_cops;
}