C++ (Cpp) BodyPtr::calcForwardKinematicsの例

コード例 #1

ファイル: JointPathExC.cpp プロジェクト: ManviG/hrpsys-base

 int _setJointAngles (double* ja)
 {
     for ( int i = 0; i < m_robot->numJoints(); i++ ) {
         m_robot->joint(i)->q = ja[i];
     }
     m_robot->calcForwardKinematics();
     return 0;
 }

コード例 #2

ファイル: JointPathEx.cpp プロジェクト: k-okada/hrpsys-base

void hrp::calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _f_ans, hrp::Vector3& _t_ans){
  for(int i=0;i<_m_robot->numJoints();i++){
    _m_robot->joint(i)->dq = _idsb.dq(i);
    _m_robot->joint(i)->ddq = _idsb.ddq(i);
  }
  _m_robot->rootLink()->vo = _idsb.base_v - _idsb.base_w.cross(_idsb.base_p);
  _m_robot->rootLink()->dvo = _idsb.base_dv - _idsb.base_dw.cross(_idsb.base_p) - _idsb.base_w.cross(_idsb.base_v); // calc in differential way
  _m_robot->rootLink()->w = _idsb.base_w;
  _m_robot->rootLink()->dw = _idsb.base_dw;
  _m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel
  _m_robot->calcInverseDynamics(_m_robot->rootLink(), _f_ans, _t_ans);// this returns f,t at the coordinate origin (not at base link pos)
};