void JointPath::calcJacobian(Eigen::MatrixXd& out_J) const
{
const int n = joints.size();
out_J.resize(6, n);
if(n > 0){
//! \todo compare the efficiency for the following codes
if(false){
setJacobian<0x3f, 0, 0>(*this, linkPath.endLink(), out_J);
} else {
Link* targetLink = linkPath.endLink();
for(int i=0; i < n; ++i){
Link* link = joints[i];
switch(link->jointType()){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega = link->R() * link->a();
const Vector3 arm = targetLink->p() - link->p();
if(!isJointDownward(i)){
omega = -omega;
}
out_J.col(i) << omega.cross(arm), omega;
}
break;
case Link::SLIDE_JOINT:
{
Vector3 dp = link->R() * link->d();
if(!isJointDownward(i)){
dp = -dp;
}
out_J.col(i) << dp, Vector3::Zero();
}
break;
default:
out_J.col(i).setZero();
}
}
}
}
}
/**
@brief compute CoM Jacobian
@param base link fixed to the environment
@param J CoM Jacobian
@note Link::wc must be computed by calcCM() before calling
*/
void calcCMJacobian(const BodyPtr& body, Link* base, Eigen::MatrixXd& J)
{
// prepare subm, submwc
const int nj = body->numJoints();
vector<SubMass> subMasses(body->numLinks());
Link* rootLink = body->rootLink();
JointPath path;
if(!base) {
calcSubMass(rootLink, subMasses);
J.resize(3, nj + 6);
} else {
path.setPath(rootLink, base);
Link* skip = path.joint(0);
SubMass& sub = subMasses[skip->index()];
sub.m = rootLink->m();
sub.mwc = rootLink->m() * rootLink->wc();
for(Link* child = rootLink->child(); child; child = child->sibling()) {
if(child != skip) {
calcSubMass(child, subMasses);
subMasses[skip->index()] += subMasses[child->index()];
}
}
// assuming there is no branch between base and root
for(int i=1; i < path.numJoints(); i++) {
Link* joint = path.joint(i);
const Link* parent = joint->parent();
SubMass& sub = subMasses[joint->index()];
sub.m = parent->m();
sub.mwc = parent->m() * parent->wc();
sub += subMasses[parent->index()];
}
J.resize(3, nj);
}
// compute Jacobian
std::vector<int> sgn(nj, 1);
for(int i=0; i < path.numJoints(); i++) {
sgn[path.joint(i)->jointId()] = -1;
}
for(int i=0; i < nj; i++) {
Link* joint = body->joint(i);
if(joint->isRotationalJoint()) {
const Vector3 omega = sgn[joint->jointId()] * joint->R() * joint->a();
const SubMass& sub = subMasses[joint->index()];
const Vector3 arm = (sub.mwc - sub.m * joint->p()) / body->mass();
const Vector3 dp = omega.cross(arm);
J.col(joint->jointId()) = dp;
} else {
std::cerr << "calcCMJacobian() : unsupported jointType("
<< joint->jointType() << std::endl;
}
}
if(!base) {
const int c = nj;
J.block(0, c, 3, 3).setIdentity();
const Vector3 dp = subMasses[0].mwc / body->mass() - rootLink->p();
J.block(0, c + 3, 3, 3) <<
0.0, dp(2), -dp(1),
-dp(2), 0.0, dp(0),
dp(1), -dp(0), 0.0;
}
}
static void nearCallback(void* data, dGeomID g1, dGeomID g2)
{
if(dGeomIsSpace(g1) || dGeomIsSpace(g2)) {
dSpaceCollide2(g1, g2, data, &nearCallback);
if(false) { // Currently just skip same body link pairs.
if(dGeomIsSpace(g1)){
dSpaceCollide((dSpaceID)g1, data, &nearCallback);
}
if(dGeomIsSpace(g2)){
dSpaceCollide((dSpaceID)g2, data, &nearCallback);
}
}
} else {
ODESimulatorItemImpl* impl = (ODESimulatorItemImpl*)data;
static const int MaxNumContacts = 100;
dContact contacts[MaxNumContacts];
int numContacts = dCollide(g1, g2, MaxNumContacts, &contacts[0].geom, sizeof(dContact));
if(numContacts > 0){
dBodyID body1ID = dGeomGetBody(g1);
dBodyID body2ID = dGeomGetBody(g2);
Link* crawlerlink = 0;
double sign = 1.0;
if(!impl->crawlerLinks.empty()){
CrawlerLinkMap::iterator p = impl->crawlerLinks.find(body1ID);
if(p != impl->crawlerLinks.end()){
crawlerlink = p->second;
}
p = impl->crawlerLinks.find(body2ID);
if(p != impl->crawlerLinks.end()){
crawlerlink = p->second;
sign = -1.0;
}
}
for(int i=0; i < numContacts; ++i){
dSurfaceParameters& surface = contacts[i].surface;
if(!crawlerlink){
//surface.mode = dContactApprox1 | dContactBounce;
//surface.bounce = 0.0;
//surface.bounce_vel = 1.0;
surface.mode = dContactApprox1;
surface.mu = impl->friction;
} else {
if(contacts[i].geom.depth > 0.001){
continue;
}
surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2 | dContactApprox1_2;
const Vector3 axis = crawlerlink->R() * crawlerlink->a();
const Vector3 n(contacts[i].geom.normal);
Vector3 dir = axis.cross(n);
if(dir.norm() < 1.0e-5){
surface.mode = dContactApprox1;
surface.mu = impl->friction;
} else {
dir *= sign;
dir.normalize();
contacts[i].fdir1[0] = dir[0];
contacts[i].fdir1[1] = dir[1];
contacts[i].fdir1[2] = dir[2];
//dVector3& dpos = contacts[i].geom.pos;
//Vector3 pos(dpos[0], dpos[1], dpos[2]);
//Vector3 v = crawlerlink->v + crawlerlink->w.cross(pos-crawlerlink->p);
//surface.motion1 = dir.dot(v) + crawlerlink->u;
surface.motion1 = crawlerlink->u();
surface.mu = impl->friction;
surface.mu2 = 0.5;
}
}
dJointID jointID = dJointCreateContact(impl->worldID, impl->contactJointGroupID, &contacts[i]);
dJointAttach(jointID, body1ID, body2ID);
}
}
}
}
void ODESimulatorItemImpl::collisionCallback(const CollisionPair& collisionPair)
{
ODELink* link1 = geometryIdToLink[collisionPair.geometryId[0]];
ODELink* link2 = geometryIdToLink[collisionPair.geometryId[1]];
const vector<Collision>& collisions = collisionPair.collisions;
dBodyID body1ID = link1->bodyID;
dBodyID body2ID = link2->bodyID;
Link* crawlerlink = 0;
double sign = 1.0;
if(!crawlerLinks.empty()){
CrawlerLinkMap::iterator p = crawlerLinks.find(body1ID);
if(p != crawlerLinks.end()){
crawlerlink = p->second;
}
p = crawlerLinks.find(body2ID);
if(p != crawlerLinks.end()){
crawlerlink = p->second;
sign = -1.0;
}
}
int numContacts = collisions.size();
for(int i=0; i < numContacts; ++i){
dContact contact;
contact.geom.pos[0] = collisions[i].point[0];
contact.geom.pos[1] = collisions[i].point[1];
contact.geom.pos[2] = collisions[i].point[2];
contact.geom.normal[0] = -collisions[i].normal[0];
contact.geom.normal[1] = -collisions[i].normal[1];
contact.geom.normal[2] = -collisions[i].normal[2];
contact.geom.depth = collisions[i].depth;
dSurfaceParameters& surface = contact.surface;
if(!crawlerlink){
surface.mode = dContactApprox1;
surface.mu = friction;
} else {
if(contact.geom.depth > 0.001){
continue;
}
surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2 | dContactApprox1_2;
const Vector3 axis = crawlerlink->R() * crawlerlink->a();
const Vector3 n(contact.geom.normal);
Vector3 dir = axis.cross(n);
if(dir.norm() < 1.0e-5){
surface.mode = dContactApprox1;
surface.mu = friction;
} else {
dir *= sign;
dir.normalize();
contact.fdir1[0] = dir[0];
contact.fdir1[1] = dir[1];
contact.fdir1[2] = dir[2];
surface.motion1 = crawlerlink->u();
surface.mu = friction;
surface.mu2 = 0.5;
}
}
dJointID jointID = dJointCreateContact(worldID, contactJointGroupID, &contact);
dJointAttach(jointID, body1ID, body2ID);
}
}
void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
{
Vector3 arm;
int i;
for(i=1; i <= numUpwardConnections; ++i){
Link* link = links[i];
const Link* child = links[i-1];
switch(child->jointType()){
case Link::ROTATIONAL_JOINT:
link->R().noalias() = child->R() * AngleAxisd(child->q(), child->a()).inverse();
arm.noalias() = link->R() * child->b();
link->p().noalias() = child->p() - arm;
if(calcVelocity){
const Vector3 sw(link->R() * child->a());
link->w() = child->w() - child->dq() * sw;
link->v() = child->v() - link->w().cross(arm);
if(calcAcceleration){
link->dw() = child->dw() - child->dq() * child->w().cross(sw) - (child->ddq() * sw);
link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);
}
}
break;
case Link::SLIDE_JOINT:
link->R() = child->R();
arm.noalias() = link->R() * (child->b() + child->q() * child->d());
link->p().noalias() = child->p() - arm;
if(calcVelocity){
const Vector3 sv(link->R() * child->d());
link->w() = child->w();
link->v().noalias() = child->v() - child->dq() * sv;
if(calcAcceleration){
link->dw() = child->dw();
link->dv().noalias() =
child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm)
- 2.0 * child->dq() * child->w().cross(sv) - child->ddq() * sv;
}
}
break;
case Link::FIXED_JOINT:
default:
arm.noalias() = link->R() * child->b();
link->R() = child->R();
link->p().noalias() = child->p() - arm;
if(calcVelocity){
link->w() = child->w();
link->v() = child->v() - link->w().cross(arm);
if(calcAcceleration){
link->dw() = child->dw();
link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);;
}
}
break;
}
}
const int n = links.size();
for( ; i < n; ++i){
Link* link = links[i];
const Link* parent = link->parent();
switch(link->jointType()){
case Link::ROTATIONAL_JOINT:
link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
arm.noalias() = parent->R() * link->b();
link->p().noalias() = parent->p() + arm;
if(calcVelocity){
const Vector3 sw(parent->R() * link->a());
link->w().noalias() = parent->w() + sw * link->dq();
link->v().noalias() = parent->v() + parent->w().cross(arm);
if(calcAcceleration){
link->dw().noalias() = parent->dw() + link->dq() * parent->w().cross(sw) + (link->ddq() * sw);
link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
}
}
break;
case Link::SLIDE_JOINT:
link->R() = parent->R();
arm.noalias() = parent->R() * (link->b() + link->q() * link->d());
link->p() = parent->p() + arm;
if(calcVelocity){
const Vector3 sv(parent->R() * link->d());
link->w() = parent->w();
link->v().noalias() = parent->v() + sv * link->dq();
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm)
+ 2.0 * link->dq() * parent->w().cross(sv) + link->ddq() * sv;
}
}
break;
case Link::FIXED_JOINT:
default:
arm.noalias() = parent->R() * link->b();
link->R() = parent->R();
link->p().noalias() = arm + parent->p();
if(calcVelocity){
link->w() = parent->w();
link->v() = parent->v() + parent->w().cross(arm);;
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() +
parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
}
}
break;
}
}
}
