Incomplete KinematicsInformation

[dave992]

I have some tests written for a class that converts group joint states retrieved from the SRDF to tesseract_planning::JointWaypoint. As the order of the join_names matters for the JointWaypoint, I had to adjust the conversion code. I now retrieve the joint_names to tell me the correct order. (Related issue: https://github.com/tesseract-robotics/tesseract_planning/issues/454)

For the test, I used the URDF and SRDF of the ABB IRB2400 provided by tesseract_support as these are already available and contain all the info I need.

Below is a minimal example of the 'test' I have written. where I replaced the 'expect' and 'assert' statements with print statements:

// Expected data
std::string group_name("manipulator");
std::string state_name("all-zeros");
std::vector<std::string> expected_joint_names{ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
std::vector<double> expected_joint_values{ 0, 0, 0, 0, 0, 0 };

// Resource locator
auto locator = std::make_shared<tesseract_common::TesseractSupportResourceLocator>();
tesseract_common::fs::path urdf_path = locator->locateResource("package://tesseract_support/urdf/abb_irb2400.urdf")->getFilePath();
tesseract_common::fs::path srdf_path = locator->locateResource("package://tesseract_support/urdf/abb_irb2400.srdf")->getFilePath();

// Create environment with data
auto env = std::make_shared<tesseract_environment::Environment>();
env->init(urdf_path, srdf_path, locator));

// Get kinematics information:
auto kinematics_info = env->getKinematicsInformation();

// Print the kinematics information
std::cout << "Defined group names: " <<  std::endl;
for (auto& name : kinematics_info.group_names){
    std::cout << "  - '" << group_name_ << "'" <<  std::endl;
}

std::cout << "Joint groups:" <<  std::endl;
for (auto& [group_name, joint_group] : kinematics_info.joint_groups)
{
  std::cout << "  - Group name: '" << group_name << "', contains " << std::to_string(joint_group.size()) << "elements." <<  std::endl;
  for (auto& joint_name : joint_group)
  {
    std::cout << "    - Joint name: '" << joint_name << "'" <<  std::endl;
  }
}

std::cout << "Group joint states:" <<  std::endl;
for (auto& [group_name, groups_joint_states] : kinematics_info.group_states)
{
  std::cout << "  - Group name: '" << group_name << "', contains " << std::to_string(groups_joint_states.size()) << " elements." <<  std::endl;

  for (auto& [state_name, group_joint_state] : groups_joint_states)
  {
    std::cout << "    - State name: '" << state_name << "'" <<  std::endl;
    for (auto& [name, value] : group_joint_state)
    {
      std::cout << "      - Joint: '" << name << "', value: '" << std::to_string(value) << "'" <<  std::endl;
    }
  }
}

Received output:

Defined group names: 
  - 'manipulator'
Joint groups:
Group joint states:
  - Group name: 'manipulator', contains 1 elements.
    - State name: 'all-zeros'
      - Joint: 'joint_5', value: '0.000000'
      - Joint: 'joint_4', value: '0.000000'
      - Joint: 'joint_6', value: '0.000000'
      - Joint: 'joint_3', value: '0.000000'
      - Joint: 'joint_2', value: '0.000000'
      - Joint: 'joint_1', value: '0.000000'

Expected output:

Defined group names: 
  - 'manipulator'
Joint groups:
  - Group name: 'manipulator', contains 6 elements.
    - Joint name: 'joint_1' 
    - Joint name: 'joint_2' 
    - Joint name: 'joint_3' 
    - Joint name: 'joint_4' 
    - Joint name: 'joint_5' 
    - Joint name: 'joint_6' 
Group joint states:
  - Group name: 'manipulator', contains 1 elements.
    - State name: 'all-zeros'
      - Joint: 'joint_5', value: '0.000000'
      - Joint: 'joint_4', value: '0.000000'
      - Joint: 'joint_6', value: '0.000000'
      - Joint: 'joint_3', value: '0.000000'
      - Joint: 'joint_2', value: '0.000000'
      - Joint: 'joint_1', value: '0.000000'

Note: as the group joint states are saved in an std::unorderd_map, the order in which the print can change.