void AISTSimulatorItemImpl::addBody(AISTSimBody* simBody) { DyBody* body = static_cast<DyBody*>(simBody->body()); DyLink* rootLink = body->rootLink(); rootLink->v().setZero(); rootLink->dv().setZero(); rootLink->w().setZero(); rootLink->dw().setZero(); rootLink->vo().setZero(); rootLink->dvo().setZero(); for(int i=0; i < body->numLinks(); ++i){ Link* link = body->link(i); link->u() = 0.0; link->dq() = 0.0; link->ddq() = 0.0; } body->clearExternalForces(); body->calcForwardKinematics(true, true); int bodyIndex; if(dynamicsMode.is(AISTSimulatorItem::HG_DYNAMICS)){ ForwardDynamicsCBMPtr cbm = make_shared_aligned<ForwardDynamicsCBM>(body); cbm->setHighGainModeForAllJoints(); bodyIndex = world.addBody(body, cbm); } else { bodyIndex = world.addBody(body); } bodyIndexMap[body] = bodyIndex; }
void Body::initializeState() { rootLink_->T() = rootLink_->Tb(); rootLink_->v().setZero(); rootLink_->w().setZero(); rootLink_->dv().setZero(); rootLink_->dw().setZero(); const int n = linkTraverse_.numLinks(); for(int i=0; i < n; ++i){ Link* link = linkTraverse_[i]; link->u() = 0.0; link->q() = 0.0; link->dq() = 0.0; link->ddq() = 0.0; } calcForwardKinematics(true, true); clearExternalForces(); initializeDeviceStates(); }
void ODESimulatorItemImpl::addBody(ODEBody* odeBody) { Body& body = *odeBody->body(); Link* rootLink = body.rootLink(); rootLink->v().setZero(); rootLink->dv().setZero(); rootLink->w().setZero(); rootLink->dw().setZero(); for(int i=0; i < body.numJoints(); ++i){ Link* joint = body.joint(i); joint->u() = 0.0; joint->dq() = 0.0; joint->ddq() = 0.0; } body.clearExternalForces(); body.calcForwardKinematics(true, true); odeBody->createBody(this); }
/** calculate the mass matrix using the unit vector method \todo replace the unit vector method here with a more efficient method The motion equation (dv != dvo) | | | dv | | | | fext | | out_M | * | dw | + | b1 | = | tauext | | | |ddq | | | | u | */ void calcMassMatrix(Body* body, const Vector3& g, Eigen::MatrixXd& out_M) { const int nj = body->numJoints(); Link* rootLink = body->rootLink(); const int totaldof = rootLink->isFixedJoint() ? nj : nj + 6; out_M.resize(totaldof, totaldof); // preserve and clear the joint accelerations VectorXd ddqorg(nj); VectorXd uorg(nj); for(int i = 0; i < nj; ++i){ Link* joint = body->joint(i); ddqorg[i] = joint->ddq(); uorg [i] = joint->u(); joint->ddq() = 0.0; } // preserve and clear the root link acceleration const Vector3 dvorg = rootLink->dv(); const Vector3 dworg = rootLink->dw(); //const Vector3 root_w_x_v = rootLink->w().cross(rootLink->vo() + rootLink->w().cross(rootLink->p())); rootLink->dv() = g; rootLink->dw().setZero(); MatrixXd b1(totaldof, 1); setColumnOfMassMatrix(body, b1, 0); if(!rootLink->isFixedJoint()){ for(int i=0; i < 3; ++i){ rootLink->dv()[i] += 1.0; setColumnOfMassMatrix(body, out_M, i); rootLink->dv()[i] -= 0.0; } for(int i=0; i < 3; ++i){ rootLink->dw()[i] = 1.0; setColumnOfMassMatrix(body, out_M, i + 3); rootLink->dw()[i] = 0.0; } } for(int i = 0; i < nj; ++i){ Link* joint = body->joint(i); joint->ddq() = 1.0; int j = i + 6; setColumnOfMassMatrix(body, out_M, j); out_M(j, j) += joint->Jm2(); // motor inertia joint->ddq() = 0.0; } // subtract the constant term for(int i = 0; i < out_M.cols(); ++i){ out_M.col(i) -= b1; } // recover state for(int i = 0; i < nj; ++i){ Link* joint = body->joint(i); joint->ddq() = ddqorg[i]; joint->u() = uorg [i]; } rootLink->dv() = dvorg; rootLink->dw() = dworg; }
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; } } }