correct format for Plan2Proto?

Hi all,

I am trying to create a Plan2Proto as input for Isaac.Navigation.FollowPath and believe I have successfully crafted the proto but cannot seem to input any values that do not return an error generated by
packages/planner/GlobalPlanner.cpp@205: Unknown pose

For quick reference, here’s a rough breakdown of the structure of a Plan2Proto (from https://docs.nvidia.com/isaac/isaac/doc/message_api.html):

Plan2Proto {
  # List of poses the robot need to go through
  poses @0: List(Pose2dProto) <i>list</i>{
    # unit complex number (cos(a), sin(a)) for rotation angle a
    rotation @0: SO2dProto {
      q @0: Vector2dProto {
        x @0: Float64;
        y @1: Float64;
      }
    }
    translation @1: Vector2dProto {
      x @0: Float64;
      y @1: Float64:
    }
  }
  # name of the frame the plan is.
  planFrame @1: text;
}

Here’s my C++ code to create and publish the proto:

//create Plan2Proto builder
auto plan = tx_plan().initProto();

//create List<Pose2dProto> builder with n list entries
auto poses = plan.initPoses(1);

//create SO2dProto builder
auto rotation = poses[0].initRotation();

//create Vector2dProto builder
auto q = rotation.initQ();

//set "rotation" x and y values (cos(a), sin(a) for given angle a per documentation)
q.setX(cos(PI));
q.setY(sin(PI));

//create Vector2dProto builder
auto translation = poses[0].initTranslation();

//set "translation" x and y values
translation.setX(5.0);
translation.setY(10.0);

//set "planFrame" (?)
plan.setPlanFrame("follow_path");

//publish Plan2Proto to isaac.navigation.FollowPath
tx_plan().publish();

I suspect that the proto is at least being created and published generally, because I get a distinct and different error if I set the X and Y values (of q) of the rotation both to 0:
./engine/core/math/so2.hpp@47: Direction vector must not be 0

My inclination is to still be suspicious of this strict and obscure format for rotation causing the “Unknown Pose” error, but truthfully it might be anything about my formatting of this proto. Beyond the given documentation above there is not much to go on, or any examples that I can find of its use.

I would appreciate if anyone has any helpful information, for example what purpose “planFrame” is and how this should be used (anything other than the goal_frame I set in isaac.navigation.FollowPath, “follow_path”, yields errors). I have tried a handful of things but can’t get any feedback other than Unknown Pose (and the 2 errors above).

Thanks in advance for any ideas on how to proceed!

  • Travis

Hi Travis,

Let me try to help here.

First the error you get when you set both X and Y to 0 come from the fact that internally we represent the SO2 (2d rotation) by the normalized direction vector rather than the angle of the rotation. The reason is simple, it’s for performance reason as most of the time we use to apply transformation, and storing the angle would require constantly computing cos/sin of the angle. To simplify the serialization and avoid calling atan2, we store directly the cos and sin of the angle. So if you provide a vector null, then it becomes impossible to recover the rotation angle.

Note that you should not need to manually serialize/de-serialize Pose2/Pose3, you should be able to directly call:

  • ToProto(goal_pose, poses[0]);
  • goal_pose = FromProto(poses[0]);

If you do not have the pose goal_pose, you should probably construct it first (Pose2::FromXYA(5.0, 10.0, Pi);) rather than building the capn’proto first, as it would simplify the code.

Regarding the planFrame attribute, whenever there is a Pose in a capn’proto, we need to know in which coordinate frame it is specified, this is what the field is for. For example you might want to set the target in different way:

  • Absolute position in the map, you would use the frame “map” or “world” with the coordinate of the target in the map/world
  • Relative position to the robot, for example 5m ahead of the robot, you would set the frame to “robot”.
  • Set the position relative to some sensors (for example you detect it using the lidar, rather than converting the target in the robot frame, you can use the position relative to the lidar and set the frame to “lidar”

In general a Pose in itself does not carry enough information, you always needs to know what reference frame it refers to, we could have decided a common frame (for example “world”) for every pose however it would require to constantly query the PoseTree and apply transformation, so we decided it would be more efficient to allow the provider decides the coordinate frame that minimize the number of computation.

If you look at the list of messages of the navigation stack you will see many times that the frame needs to be provided. It should corresponds to the origin of the transformation provided in the message, and the name of the frame should be present in PoseTree (you can use sight to see how the different coordinate frames are linked together).

I hope it helps, please let me know if you have any other issue relating to the Plan2Proto

  • Benjamin