void ForwardDynamicsABM::calcMotionWithEulerMethod()
{
calcABMLastHalf();
if(!sensorHelper.forceSensors().empty()){
updateForceSensors();
}
DyLink* root = body->rootLink();
if(!root->isFixedJoint()){
root->dv() =
root->dvo() - root->p().cross(root->dw())
+ root->w().cross(root->vo() + root->w().cross(root->p()));
Position T;
SE3exp(T, root->T(), root->w(), root->vo(), timeStep);
root->T() = T;
root->vo() += root->dvo() * timeStep;
root->w() += root->dw() * timeStep;
}
int n = body->numLinks();
for(int i=1; i < n; ++i){
DyLink* link = body->link(i);
link->q() += link->dq() * timeStep;
link->dq() += link->ddq() * timeStep;
}
}
void ForwardDynamicsABM::integrateRungeKuttaOneStep(double r, double dt)
{
DyLink* root = body->rootLink();
if(!root->isFixedJoint()){
SE3exp(root->T(), T0, root->w(), root->vo(), dt);
root->vo().noalias() = vo0 + root->dvo() * dt;
root->w().noalias() = w0 + root->dw() * dt;
vo += r * root->vo();
w += r * root->w();
dvo += r * root->dvo();
dw += r * root->dw();
}
int n = body->numLinks();
for(int i=1; i < n; ++i){
DyLink* link = body->link(i);
link->q() = q0[i] + dt * link->dq();
link->dq() = dq0[i] + dt * link->ddq();
dq[i] += r * link->dq();
ddq[i] += r * link->ddq();
}
}
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 ForwardDynamicsABM::calcABMPhase3()
{
const LinkTraverse& traverse = body->linkTraverse();
DyLink* root = static_cast<DyLink*>(traverse[0]);
if(root->isFreeJoint()){
// - | Ivv trans(Iwv) | * | dvo | = | pf |
// | Iwv Iww | | dw | | ptau |
Eigen::Matrix<double, 6, 6> M;
M << root->Ivv(), root->Iwv().transpose(),
root->Iwv(), root->Iww();
Eigen::Matrix<double, 6, 1> f;
f << root->pf(),
root->ptau();
f *= -1.0;
Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));
root->dvo() = a.head<3>();
root->dw() = a.tail<3>();
} else {
root->dvo().setZero();
root->dw().setZero();
}
const int n = traverse.numLinks();
for(int i=1; i < n; ++i){
DyLink* link = static_cast<DyLink*>(traverse[i]);
const DyLink* parent = link->parent();
if(!link->isFixedJoint()){
link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();
link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();
link->dw().noalias() = parent->dw() + link->cw() + link->sw() * link->ddq();
}else{
link->ddq() = 0.0;
link->dvo() = parent->dvo();
link->dw() = parent->dw();
}
}
}
void ForwardDynamicsABM::calcMotionWithRungeKuttaMethod()
{
DyLink* root = body->rootLink();
if(!root->isFixedJoint()){
T0 = root->T();
vo0 = root->vo();
w0 = root->w();
}
vo.setZero();
w.setZero();
dvo.setZero();
dw.setZero();
int n = body->numLinks();
for(int i=1; i < n; ++i){
DyLink* link = body->link(i);
q0[i] = link->q();
dq0[i] = link->dq();
dq[i] = 0.0;
ddq[i] = 0.0;
}
calcABMLastHalf();
if(!sensorHelper.forceSensors().empty()){
updateForceSensors();
}
integrateRungeKuttaOneStep(1.0 / 6.0, timeStep / 2.0);
calcABMPhase1();
calcABMPhase2();
calcABMPhase3();
integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
calcABMPhase1();
calcABMPhase2();
calcABMPhase3();
integrateRungeKuttaOneStep(2.0 / 6.0, timeStep);
calcABMPhase1();
calcABMPhase2();
calcABMPhase3();
if(!root->isFixedJoint()){
SE3exp(root->T(), T0, w0, vo0, timeStep);
root->vo() = vo0 + (dvo + root->dvo() / 6.0) * timeStep;
root->w() = w0 + (dw + root->dw() / 6.0) * timeStep;
}
for(int i=1; i < n; ++i){
DyLink* link = body->link(i);
link->q() = q0[i] + ( dq[i] + link->dq() / 6.0) * timeStep;
link->dq() = dq0[i] + (ddq[i] + link->ddq() / 6.0) * timeStep;
}
}