int contactConstraintsBV(RigidBodyManipulator *r, int nc, std::vector<double> support_mus, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv, MatrixXd &normals, double terrain_height)
{
int j, k=0, nq = r->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 (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator 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++) {
auto contact_pos_gradientvar = r->forwardKin(*pt_iter, iter->body_idx, 0, 0, 1);
contact_pos = contact_pos_gradientvar.value();
J = contact_pos_gradientvar.gradient().value();
if (iter->use_support_surface) {
normal = iter->support_surface.head(3);
}
else {
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
}
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);
auto Jpdotv_grad = r->forwardJacDotTimesV(pt,iter->body_idx,0,0,0);
Jpdotv.block(3*k,0,3,1) = Jpdotv_grad.value();
normals.col(k) = normal;
k++;
}
}
}
return k;
}
int contactConstraintsBV(RigidBodyManipulator *r, int nc, double mu, std::vector<SupportStateElement>& supp, void *map_ptr, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, MatrixXd &Jpdot, MatrixXd &normals, double terrain_height)
{
int j, k=0, nq = r->num_dof;
B.resize(3,nc*2*m_surface_tangents);
JB.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdot.resize(3*nc,nq);
normals.resize(3, nc);
Vector3d contact_pos,pos,posB,normal; Vector4d tmp;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
double norm = sqrt(1+mu*mu); // because normals and ds are orthogonal, the norm has a simple form
for (std::vector<SupportStateElement>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
std::unique_ptr<RigidBody>& b = r->bodies[iter->body_idx];
if (nc>0) {
for (std::set<int>::iterator pt_iter=iter->contact_pt_inds.begin(); pt_iter!=iter->contact_pt_inds.end(); pt_iter++) {
if (*pt_iter<0 || *pt_iter>=b->contact_pts.cols()) mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","requesting contact pt %d but body only has %d pts",*pt_iter,b->contact_pts.cols());
tmp = b->contact_pts.col(*pt_iter);
r->forwardKin(iter->body_idx,tmp,0,contact_pos);
r->forwardJac(iter->body_idx,tmp,0,J);
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
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 Jpdot
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
r->forwardJacDot(iter->body_idx,tmp,0,J);
Jpdot.block(3*k,0,3,nq) = J;
normals.col(k) = normal;
k++;
}
}
}
return k;
}
int contactConstraintsBV(RigidBodyManipulator *r, int nc, double mu, std::vector<SupportStateElement>& supp, void *map_ptr, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, MatrixXd &Jpdot, MatrixXd &normals, double terrain_height)
{
int j, k=0, nq = r->num_positions;
B.resize(3,nc*2*m_surface_tangents);
JB.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdot.resize(3*nc,nq);
normals.resize(3, nc);
Vector3d contact_pos,pos,posB,normal;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
double norm = sqrt(1+mu*mu); // because normals and ds are orthogonal, the norm has a simple form
for (std::vector<SupportStateElement>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
if (nc>0) {
for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
r->forwardKin(iter->body_idx,*pt_iter,0,contact_pos);
r->forwardJac(iter->body_idx,*pt_iter,0,J);
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
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 Jpdot
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
r->forwardJacDot(iter->body_idx,*pt_iter,0,J);
Jpdot.block(3*k,0,3,nq) = J;
normals.col(k) = normal;
k++;
}
}
}
return k;
}
int contactConstraints(RigidBodyManipulator *r, int nc, std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& supp, void *map_ptr, MatrixXd &n, MatrixXd &D, MatrixXd &Jp, MatrixXd &Jpdot,double terrain_height)
{
int j, k=0, nq = r->num_positions;
n.resize(nc,nq);
D.resize(nq,nc*2*m_surface_tangents);
Jp.resize(3*nc,nq);
Jpdot.resize(3*nc,nq);
Vector3d contact_pos,pos,normal;
MatrixXd J(3,nq);
Matrix<double,3,m_surface_tangents> d;
for (std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = supp.begin(); iter!=supp.end(); iter++) {
if (nc>0) {
for (std::vector<Vector4d,aligned_allocator<Vector4d>>::iterator pt_iter=iter->contact_pts.begin(); pt_iter!=iter->contact_pts.end(); pt_iter++) {
r->forwardKin(iter->body_idx,*pt_iter,0,contact_pos);
r->forwardJac(iter->body_idx,*pt_iter,0,J);
collisionDetect(map_ptr,contact_pos,pos,&normal,terrain_height);
surfaceTangents(normal,d);
n.row(k) = normal.transpose()*J;
for (j=0; j<m_surface_tangents; j++) {
D.col(2*k*m_surface_tangents+j) = J.transpose()*d.col(j);
D.col((2*k+1)*m_surface_tangents+j) = -D.col(2*k*m_surface_tangents+j);
}
// store away kin sols into Jp and Jpdot
// NOTE: I'm cheating and using a slightly different ordering of J and Jdot here
Jp.block(3*k,0,3,nq) = J;
r->forwardJacDot(iter->body_idx,*pt_iter,0,J);
Jpdot.block(3*k,0,3,nq) = J;
k++;
}
}
}
return k;
}
