c10::List forward_kinematics()

in polymetis/src/torchscript_operators/pinocchio.cpp [90:116]

• 22 lines of code
• 2 McCabe index (conditional complexity)

  c10::List<torch::Tensor> forward_kinematics(torch::Tensor joint_positions) {
    c10::List<torch::Tensor> result;
    torch::Tensor pos_result = torch::zeros(3, torch::kFloat32);
    torch::Tensor quat_result = torch::zeros(4, torch::kFloat32);

    joint_positions = validTensor(joint_positions);
    pinocchio::forwardKinematics(
        model_, model_data_,
        matrixToVector(dtt::libtorch2eigen<double>(joint_positions)));
    pinocchio::updateFramePlacement(model_, model_data_, ee_idx_);

    auto pos_data = model_data_.oMf[ee_idx_].translation().transpose();
    auto quat_data = Eigen::Quaterniond(model_data_.oMf[ee_idx_].rotation());

    for (int i = 0; i < 3; i++) {
      pos_result[i] = pos_data[i];
    }
    quat_result[0] = quat_data.x();
    quat_result[1] = quat_data.y();
    quat_result[2] = quat_data.z();
    quat_result[3] = quat_data.w();

    result.push_back(pos_result);
    result.push_back(quat_result);

    return result;
  }