text: /**
     \file
     \author Shin'ichiro Nakaoka
  */
  
  #ifndef UCNOID_BODY_FORWARD_DYNAMICS_ABM_CPP_H
  #define UCNOID_BODY_FORWARD_DYNAMICS_ABM_CPP_H
  
  #include "ForwardDynamicsABM.h"
  #include "DyBody.h"
  #include "LinkTraverse.h"
  #include <ucnoid/EigenUtil>
  
  namespace cnoid {
  inline namespace ucnoid {
  
  namespace detail::forward_dynamics_abm {
  
  static const bool debugMode = false;
  static const bool rootAttitudeNormalizationEnabled = false;
  
  }   // namespace detail::forward_dynamics_abm
  
  inline ForwardDynamicsABM::ForwardDynamicsABM(DyBody* body) :
      ForwardDynamics(body),
      q0(body->numLinks()),
      dq0(body->numLinks()),
      dq(body->numLinks()),
      ddq(body->numLinks())
  {
  
  }
  
  
  inline ForwardDynamicsABM::~ForwardDynamicsABM()
  {
  
  }
  
  
  inline void ForwardDynamicsABM::initialize()
  {
      DyLink* rootLink = body->rootLink();
      rootLink->sw().setZero();
      rootLink->sv().setZero();
      rootLink->cv().setZero();
      rootLink->cw().setZero();
      rootLink->hhv().setZero();
      rootLink->hhw().setZero();
      rootLink->uu() = 0.0;
      rootLink->dd() = 0.0;
  
      initializeSensors();
      calcABMFirstHalf();
  }
  
  
  inline void ForwardDynamicsABM::calcABMFirstHalf()
  {
      calcABMPhase1(true);
      calcABMPhase2Part1();
  }
  
  
  inline void ForwardDynamicsABM::calcABMLastHalf()
  {
      calcABMPhase2Part2();
      calcABMPhase3();
  }
  
  
  inline void ForwardDynamicsABM::calcNextState()
  {
      switch(integrationMode){
  
      case EULER_METHOD:
          calcMotionWithEulerMethod();
          break;
  		
      case RUNGEKUTTA_METHOD:
          calcMotionWithRungeKuttaMethod();
          break;
      }
  
      if(detail::forward_dynamics_abm::rootAttitudeNormalizationEnabled){
          normalizeRotation(body->rootLink()->T());
      }
  
      calcABMFirstHalf();
  
      if(sensorsEnabled){
          sensorHelper.updateGyroAndAccelerationSensors();
      }
  }
  
  
  inline 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;
      }
  }
  
  
  inline 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();
      }
  }
  
  
  inline 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(false);
      calcABMPhase2();
      calcABMPhase3();
  
      integrateRungeKuttaOneStep(2.0 / 6.0, timeStep / 2.0);
      calcABMPhase1(false);
      calcABMPhase2();
      calcABMPhase3();
  
      integrateRungeKuttaOneStep(2.0 / 6.0, timeStep);
      calcABMPhase1(false);
      calcABMPhase2();
      calcABMPhase3();
  
      if(!root->isFixedJoint()){
          root->dvo() = dvo + root->dvo() / 6.0;
          root->dw() = dw  + root->dw()  / 6.0;
          root->dv() =
              root->dvo() - T0.translation().cross(root->dw()) + w0.cross(vo0 + w0.cross(T0.translation()));
          root->vo() = vo0 + root->dvo() * timeStep;
          root->w()  = w0  + root->dw() * timeStep;
          SE3exp(root->T(), T0, w0, vo0, 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;
      }
  }
  
  
  /**
     \note v, dv, dw are not used in the forward dynamics, but are calculated
     for forward dynamics users.
  */
  inline void ForwardDynamicsABM::calcABMPhase1(bool updateNonSpatialVariables)
  {
      const LinkTraverse& traverse = body->linkTraverse();
      const int n = traverse.numLinks();
  
      for(int i=0; i < n; ++i){
          DyLink* link = static_cast<DyLink*>(traverse[i]);
          const DyLink* parent = link->parent();
  
          if(parent){
              
              switch(link->jointType()){
                  
              case Link::ROTATIONAL_JOINT:
              {
                  const Vector3 arm = parent->R() * link->b();
                  link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
                  link->p().noalias() = arm + parent->p();
                  link->sw().noalias() = parent->R() * link->a();
                  link->sv().noalias() = link->p().cross(link->sw());
                  link->w().noalias() = link->dq() * link->sw() + parent->w();
                  if(updateNonSpatialVariables){
                      link->dw().noalias() =
                          parent->dw() + link->dq() * parent->w().cross(link->sw()) + (link->ddq() * link->sw());
                      link->dv().noalias() =
                          parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                  }
                  break;
              }
                  
              case Link::SLIDE_JOINT:
                  link->p().noalias() = parent->R() * (link->b() + link->q() * link->d()) + parent->p();
                  link->R() = parent->R();
                  link->sw().setZero();
                  link->sv().noalias() = parent->R() * link->d();
                  link->w() = parent->w();
                  if(updateNonSpatialVariables){
                      link->dw() = parent->dw();
                      const Vector3 arm = parent->R() * link->b();
                      link->dv().noalias() =
                          parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm)
                          + 2.0 * link->dq() * parent->w().cross(link->sv()) + link->ddq() * link->sv();
                  }
                  break;
                  
              case Link::FIXED_JOINT:
              default:
                  link->p().noalias() = parent->R() * link->b() + parent->p();
                  link->R() = parent->R();
                  link->w() = parent->w();
                  link->vo() = parent->vo();
                  link->sw().setZero();
                  link->sv().setZero();
                  link->cv().setZero();
                  link->cw().setZero();
                  if(updateNonSpatialVariables){
                      link->dw() = parent->dw();
                      const Vector3 arm = parent->R() * link->b();
                      link->dv().noalias() = parent->dv() +
                          parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                  }
                  
                  goto COMMON_CALCS_FOR_ALL_JOINT_TYPES;
              }
              
              // Common for ROTATE and SLIDE
              link->vo().noalias() = link->dq() * link->sv() + parent->vo();
              const Vector3 dsv = parent->w().cross(link->sv()) + parent->vo().cross(link->sw());
              const Vector3 dsw = parent->w().cross(link->sw());
              link->cv() = link->dq() * dsv;
              link->cw() = link->dq() * dsw;
          }
          
  COMMON_CALCS_FOR_ALL_JOINT_TYPES:
  
          if(updateNonSpatialVariables){
              link->v().noalias() = link->vo() + link->w().cross(link->p());
          }
          link->wc().noalias() = link->R() * link->c() + link->p();
          
          // compute I^s (Eq.(6.24) of Kajita's textbook))
          const Matrix3 Iw = link->R() * link->I() * link->R().transpose();
          
          const double m = link->m();
          const Matrix3 c_hat = hat(link->wc());
          link->Iww().noalias() = m * c_hat * c_hat.transpose() + Iw;
  
          link->Ivv() <<
              m,  0.0, 0.0,
              0.0,  m,  0.0,
              0.0, 0.0,  m;
          
          link->Iwv() = m * c_hat;
          
          // compute P and L (Eq.(6.25) of Kajita's textbook)
          const Vector3 P = m * (link->vo() + link->w().cross(link->wc()));
          const Vector3 L = link->Iww() * link->w() + m * link->wc().cross(link->vo());
          
          link->pf().noalias() = link->w().cross(P);
          link->ptau().noalias() = link->vo().cross(P) + link->w().cross(L);
          
          const Vector3 fg = m * g;
          const Vector3 tg = link->wc().cross(fg);
          
          link->pf() -= fg;
          link->ptau() -= tg;
      }
  }
  
  
  inline void ForwardDynamicsABM::calcABMPhase2()
  {
      const LinkTraverse& traverse = body->linkTraverse();
      const int n = traverse.numLinks();
  
      for(int i = n-1; i >= 0; --i){
          DyLink* link = static_cast<DyLink*>(traverse[i]);
  
          link->pf()   -= link->f_ext();
          link->ptau() -= link->tau_ext();
  
          // compute articulated inertia (Eq.(6.48) of Kajita's textbook)
          for(DyLink* child = link->child(); child; child = child->sibling()){
  
              if(child->isFixedJoint()){
                  link->Ivv() += child->Ivv();
                  link->Iwv() += child->Iwv();
                  link->Iww() += child->Iww();
  
              }else{
                  const Vector3 hhv_dd = child->hhv() / child->dd();
                  link->Ivv().noalias() += child->Ivv() - child->hhv() * hhv_dd.transpose();
                  link->Iwv().noalias() += child->Iwv() - child->hhw() * hhv_dd.transpose();
                  link->Iww().noalias() += child->Iww() - child->hhw() * (child->hhw() / child->dd()).transpose();
              }
  
              link->pf()  .noalias() += child->Ivv() * child->cv() + child->Iwv().transpose() * child->cw() + child->pf();
              link->ptau().noalias() += child->Iwv() * child->cv() + child->Iww() * child->cw() + child->ptau();
  
              if(!child->isFixedJoint()){
                  const double uu_dd = child->uu() / child->dd();
                  link->pf()   += uu_dd * child->hhv();
                  link->ptau() += uu_dd * child->hhw();
              }
          }
  
          if(i > 0){
              if(!link->isFixedJoint()){
                  // hh = Ia * s
                  link->hhv().noalias() = link->Ivv() * link->sv() + link->Iwv().transpose() * link->sw();
                  link->hhw().noalias() = link->Iwv() * link->sv() + link->Iww() * link->sw();
                  // dd = Ia * s * s^T
                  link->dd() = link->sv().dot(link->hhv()) + link->sw().dot(link->hhw()) + link->Jm2();
                  // uu = u - hh^T*c + s^T*pp
                  link->uu() = link->u() -
                      (link->hhv().dot(link->cv()) + link->hhw().dot(link->cw()) +
                       link->sv().dot(link->pf()) + link->sw().dot(link->ptau()));
              }
          }
      }
  }
  
  
  // A part of phase 2 (inbound loop) that can be calculated before external forces are given
  inline void ForwardDynamicsABM::calcABMPhase2Part1()
  {
      const LinkTraverse& traverse = body->linkTraverse();
      const int n = traverse.numLinks();
  
      for(int i = n-1; i >= 0; --i){
          DyLink* link = static_cast<DyLink*>(traverse[i]);
  
          for(DyLink* child = link->child(); child; child = child->sibling()){
  
              if(child->isFixedJoint()){
                  link->Ivv() += child->Ivv();
                  link->Iwv() += child->Iwv();
                  link->Iww() += child->Iww();
  
              }else{
                  const Vector3 hhv_dd = child->hhv() / child->dd();
                  link->Ivv().noalias() += child->Ivv() - child->hhv() * hhv_dd.transpose();
                  link->Iwv().noalias() += child->Iwv() - child->hhw() * hhv_dd.transpose();
                  link->Iww().noalias() += child->Iww() - child->hhw() * (child->hhw() / child->dd()).transpose();
              }
  
              link->pf()  .noalias() += child->Ivv() * child->cv() + child->Iwv().transpose() * child->cw();
              link->ptau().noalias() += child->Iwv() * child->cv() + child->Iww() * child->cw();
          }
  
          if(i > 0){
              if(!link->isFixedJoint()){
                  link->hhv().noalias() = link->Ivv() * link->sv() + link->Iwv().transpose() * link->sw();
                  link->hhw().noalias() = link->Iwv() * link->sv() + link->Iww() * link->sw();
                  link->dd() = link->sv().dot(link->hhv()) + link->sw().dot(link->hhw()) + link->Jm2();
                  link->uu() = -(link->hhv().dot(link->cv()) + link->hhw().dot(link->cw()));
              }
          }
      }
  }
  
  
  // A remaining part of phase 2 that requires external forces
  inline void ForwardDynamicsABM::calcABMPhase2Part2()
  {
      const LinkTraverse& traverse = body->linkTraverse();
      const int n = traverse.numLinks();
  
      for(int i = n-1; i >= 0; --i){
          DyLink* link = static_cast<DyLink*>(traverse[i]);
  
          link->pf()   -= link->f_ext();
          link->ptau() -= link->tau_ext();
  
          for(DyLink* child = link->child(); child; child = child->sibling()){
              link->pf()   += child->pf();
              link->ptau() += child->ptau();
  
              if(!child->isFixedJoint()){
                  const double uu_dd = child->uu() / child->dd();
                  link->pf()   += uu_dd * child->hhv();
                  link->ptau() += uu_dd * child->hhw();
              }
          }
  
          if(i > 0){
              if(!link->isFixedJoint()){
                  link->uu() += link->u() - (link->sv().dot(link->pf()) + link->sw().dot(link->ptau()));
              }
          }
      }
  }
  
  
  inline 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(); 
          }
      }
  }
  
  
  inline void ForwardDynamicsABM::updateForceSensors()
  {
      const DeviceList<ForceSensor>& sensors = sensorHelper.forceSensors();
      for(size_t i=0; i < sensors.size(); ++i){
          ForceSensor* sensor = sensors[i];
          const DyLink* link = static_cast<DyLink*>(sensor->link());
  
          //    | f   | = | Ivv  trans(Iwv) | * | dvo | + | pf   |
          //    | tau |   | Iwv     Iww     |   | dw  |   | ptau |
          const Vector3 f = -(link->Ivv() * link->dvo() + link->Iwv().transpose() * link->dw() + link->pf());
          const Vector3 tau = -(link->Iwv() * link->dvo() + link->Iww() * link->dw() + link->ptau());
          
          const Matrix3 R = link->R() * sensor->R_local();
          const Vector3 p = link->p() + link->R() * sensor->p_local();
          sensor->f().noalias()   = R.transpose() * f;
          sensor->tau().noalias() = R.transpose() * (tau - p.cross(f));
          sensor->notifyStateChange();
      }
  }
  
  }   // inline namespace ucnoid
  }   // namespace cnoid
  
  #endif  // UCNOID_BODY_FORWARD_DYNAMICS_ABM_CPP_H
  