Any support for 6d point Jacobian?

[shikui08]

point_jacobian returns 3 x nDof jacobian matrix, how to get 6D jacobian(i.e., similar to rbdl CalcPointJacobian6D)?

[shikui08]

If anyone is interested in getting 6d jacobian, I pasted some code below that might work. It is mainly modified from point_jacobian function..

template <typename Algebra>
typename Algebra::MatrixX point_jacobian6d(
    const MultiBody<Algebra> &mb, const typename Algebra::VectorX &q, int link_index,
    const typename Algebra::Vector3 &point, bool is_local_point) {
  using Scalar = typename Algebra::Scalar;
  using Vector3 = typename Algebra::Vector3;
  using Matrix3 = typename Algebra::Matrix3;
  using Matrix6x3 = typename Algebra::Matrix6x3;
  using Matrix3X = typename Algebra::Matrix3X;
  using MatrixX = typename Algebra::MatrixX;
  typedef tds::Transform<Algebra> Transform;
  typedef tds::MotionVector<Algebra> MotionVector;
  typedef tds::ForceVector<Algebra> ForceVector;
  typedef tds::Link<Algebra> Link;


  assert(Algebra::size(q)  == mb.dof());
  assert(link_index < static_cast<int>(mb.num_links()));
  MatrixX jac( 6, mb.dof_qd());
  Algebra::set_zero(jac);
  std::vector<Transform> links_X_world;
  std::vector<Transform> links_X_base;
  Transform base_X_world;
  forward_kinematics_q(mb, q, &base_X_world, &links_X_world, &links_X_base);
  Transform point_tf;
  point_tf.set_identity();
  point_tf.translation = point;
                         
  if (mb.is_floating()) {
      Vector3 base_point = is_local_point ? point : point - mb.base_X_world().translation;

    // see (Eq. 2.238) in
    // https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/FloatingBaseKinematics.pdf
#ifdef TDS_USE_LEFT_ASSOCIATIVE_TRANSFORMS
    // Matrix3 cr = Algebra::cross_matrix(point_tf.translation);
    Matrix3 cr = Algebra::transpose(Algebra::cross_matrix(base_point));
    Algebra::assign_block(jac, cr, 3, 0);
    jac(0, 0) = Algebra::one();
    jac(1, 1) = Algebra::one();
    jac(2, 2) = Algebra::one();
    jac(3, 3) = Algebra::one();
    jac(4, 4) = Algebra::one();
    jac(5, 5) = Algebra::one();
#else
    Matrix3 cr = Algebra::transpose(Algebra::cross_matrix(base_point));
    Algebra::assign_block(jac, cr, 3, 0);
    jac(0, 0) = Algebra::one();
    jac(1, 1) = Algebra::one();
    jac(2, 2) = Algebra::one();
    jac(3, 3) = Algebra::one();
    jac(4, 4) = Algebra::one();
    jac(5, 5) = Algebra::one();
#endif //TDS_USE_LEFT_ASSOCIATIVE_TRANSFORMS
    
  } else {
    point_tf.translation = point;
  }
  // loop over all links that lie on the path from the given link to world
  if (link_index >= 0) {
    const Link *body = &mb[link_index];
    while (true) {
      int i = body->index;
      if (body->joint_type == JOINT_SPHERICAL){
        Matrix6x3 st = is_local_point ? links_X_base[i].apply_inverse(body->S_3d, false) : links_X_world[i].apply_inverse(body->S_3d, false);
        Matrix6x3 xs = point_tf.apply(st, false);
        Algebra::assign_block(jac, xs, 0, body->qd_index);
      }
      else if (body->joint_type != JOINT_FIXED) {
        MotionVector st = is_local_point ? links_X_base[i].apply_inverse(body->S) : links_X_world[i].apply_inverse(body->S);
        MotionVector xs = point_tf.apply(st);
        Algebra::assign_column(jac, body->qd_index, xs);
      }
      if (body->parent_index < 0) break;
      body = &mb[body->parent_index];
    }
  }
  //jac.print("jac");
  return jac;
}

[erwincoumans]

Thanks for the contribution. It may become useful, in particular when we add non-contact constraints.