Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
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)
};
Exemplo n.º 3
0
void hrp::calcAccelerationsForInverseDynamics(const hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb){
  for(int i=0;i<_m_robot->numJoints();i++)_idsb.q(i) = _m_robot->joint(i)->q;
  _idsb.dq = (_idsb.q - _idsb.q_old) / _idsb.DT;
  _idsb.ddq = (_idsb.q - 2 * _idsb.q_old + _idsb.q_oldold) / (_idsb.DT * _idsb.DT);
  const hrp::Vector3 g(0, 0, 9.80665);
  _idsb.base_p = _m_robot->rootLink()->p;
  _idsb.base_v = (_idsb.base_p - _idsb.base_p_old) / _idsb.DT;
  _idsb.base_dv = g + (_idsb.base_p - 2 * _idsb.base_p_old + _idsb.base_p_oldold) / (_idsb.DT * _idsb.DT);
  _idsb.base_R =  _m_robot->rootLink()->R;
  _idsb.base_dR = (_idsb.base_R - _idsb.base_R_old) / _idsb.DT;
  _idsb.base_w_hat = _idsb.base_dR * _idsb.base_R.transpose();
  _idsb.base_w = hrp::Vector3(_idsb.base_w_hat(2,1), - _idsb.base_w_hat(0,2), _idsb.base_w_hat(1,0));
  _idsb.base_dw = (_idsb.base_w - _idsb.base_w_old) / _idsb.DT;
};
Exemplo n.º 4
0
bool getJointList(hrp::BodyPtr body, const std::vector<std::string> &elements,
                  std::vector<hrp::Link *> &joints)
{
    if (elements.size() == 0){
        for (int i=0; i<body->numJoints(); i++){
            joints.push_back(body->joint(i));
        }
    }else{
        for (size_t i=0; i<elements.size(); i++){
            hrp::Link *j = body->link(elements[i]);
            if (j){
                joints.push_back(j);
            }else{
                std::cerr << "can't find a joint(" << elements[i] << ")"
                          << std::endl;
                return false;
            }
        }
    }
    return true;
}
Exemplo n.º 5
0
 int _getJointAngles (double* ja)
 {
     for ( int i = 0; i < m_robot->numJoints(); i++ ) {
         ja[i] = m_robot->joint(i)->q;
     }
 }