当前位置: 首页>>代码示例>>C++>>正文


C++ Transform3f::transformInv方法代码示例

本文整理汇总了C++中Transform3f::transformInv方法的典型用法代码示例。如果您正苦于以下问题:C++ Transform3f::transformInv方法的具体用法?C++ Transform3f::transformInv怎么用?C++ Transform3f::transformInv使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在Transform3f的用法示例。


在下文中一共展示了Transform3f::transformInv方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: computeWorldPointFootDK

CMUK_ERROR_CODE cmuk::computeWorldPointFootDK( LegIndex leg,
                                               JointOffset link,
                                               const KState& state,
                                               const vec3f& pworld,
                                               mat3f* J_trans,
                                               mat3f* J_rot,
                                               float* det,
                                               float det_tol,
                                               float lambda ) const {

  if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
    return CMUK_BAD_LEG_INDEX;
  }
  if (!J_trans || !J_rot) {
    return CMUK_INSUFFICIENT_ARGUMENTS;
  }
    
  Transform3f xform = state.xform();
  vec3f pbody = xform.transformInv(pworld);
  vec3f fbody;

  mat3f J;

  CMUK_ERROR_CODE err = 
    computePointFootDK( leg, link, state.leg_rot[leg], pbody, &J,
                        &fbody, det, det_tol, lambda );

  if (err != CMUK_OKAY && err != CMUK_SINGULAR_MATRIX) {
    return err;
  }

  const mat3f& R = xform.rotFwd();
  const mat3f& Rinv = xform.rotInv();

  *J_trans = mat3f::identity() - R * J * Rinv;
  //*J_rot = R * (-mat3f::cross(pbody) + J * mat3f::cross(fbody));
  *J_rot = R * ( J * mat3f::cross(fbody) - mat3f::cross(pbody) ) * Rinv;

  return err;

}
开发者ID:swatbotics,项目名称:darwin,代码行数:41,代码来源:cmuk.cpp

示例2: computeFootIK


//.........这里部分代码省略.........

  if (swap) {
    std::swap(hip_rx_angles[0], hip_rx_angles[1]);
    std::swap(badness[0], badness[1]);  
    std::swap(flags[0], flags[1]);
  }
  
  int hip_solution_cnt = 2;

  if (badness[0] == 0 && badness[1] != 0) {
    hip_solution_cnt = 1;
  } 

  debug << "hip_rx_angles[0]=" << hip_rx_angles[0] 
        << ", badness=" << badness[0]
        << ", flags=" << flags[0] << "\n";

  debug << "hip_rx_angles[1]=" << hip_rx_angles[1] 
        << ", badness=" << badness[1]
        << ", flags=" << flags[1] << "\n";
  
  debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";

  vec3f qfwd[2], qrear[2];
  
  for (int i=0; i<hip_solution_cnt; ++i) {

    debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";

    float hip_rx = hip_rx_angles[i];
    
    // now make inv. transform to get rid of hip rotation
    Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
    vec3f ptx = tx.transformInv(orig);

    debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
    
    // calculate lengths for cosine law
    float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
    float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
    float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
    float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
    
    float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
    float k = sqrt(ksqr);

    debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
    
    // check triangle inequality
    if (k > l1 + l2) { 
      debug << "oops, violated the triangle inequality for lower segments: "
            << "k = " << k << ", "
            << "l1 + l2 = " << l1 + l2 << "\n";
      if (k - (l1 + l2) > 1e-4) {
        flags[i] = flags[i] | IK_LOWER_DISTANCE;
      }
      k = l1 + l2;
      ksqr = k * k;
    }
    
    // 2*theta is the acute angle formed by the spread
    // of the two hip rotations... 
    float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
    if (fabs(costheta) > 1) {
      debug << "costheta = " << costheta << " > 1\n";
      if (fabs(costheta) - 1 > 1e-4) {
开发者ID:swatbotics,项目名称:darwin,代码行数:67,代码来源:cmuk.cpp


注:本文中的Transform3f::transformInv方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。