Understanding Actor Pose vs. World Pose in Gazebo Sim with Trajectory Movement

Help
,
1

Note: I posted a similar question to robotic.stackexchange

I am using Gazebo Harmonic. Can anyone help me figure out why:

  1. My actor’s pose does not change, even though the world pose changes and the actor with a trajectory moves? I’m aware that, according to the Gazebo Sim documentation, “If the actor has a trajectory, this will only return the origin pose of the trajectory and not the actual world pose of the actor.” However, this leads me to my second question.

    image
    image1028×911 83.3 KB

  2. I tried to read the world pose, but I can’t seem to find it, as shown below. Also find my PostUpdate implementation below.

[Dbg] [MonitorMovingActorPlugin.cc:109] Box position: (0, 0, 0.5)
[Dbg] [MonitorMovingActorPlugin.cc:128] WorldPose component not found for entity: 4
[Dbg] [MonitorMovingActorPlugin.cc:135] Box position (using wrapper): (0, 0, 0.5)
[Dbg] [MonitorMovingActorPlugin.cc:153] Could not get world pose using Actor wrapper
[Dbg] [MonitorMovingActorPlugin.cc:166] TrajectoryPose component not found for entity: 4
[Dbg] [MonitorMovingActorPlugin.cc:171] [TrajectoryPose] present (wrapper): 0

if (_info.paused) 
        return;

    // Increment the counter
    this->iterationCount++;

    // Only print every 1000 iterations
    if (this->iterationCount % 1000 == 0) {
        
        // Use the ECM directly to get the pose component
        auto poseComp = _ecm.Component<gz::sim::components::Pose>(this->actorEntity);
        if (poseComp) {
            const gz::math::Pose3d &pose = poseComp->Data();
            // Print the actor's position (X, Y, Z) to console
            gzdbg << "Box position: (" 
                        << pose.Pos().X() << ", " 
                        << pose.Pos().Y() << ", " 
                        << pose.Pos().Z() << ")\n";
        }
        else {
            gzdbg << "Pose component not found for entity: " << this->actorEntity << std::endl;
        }

        // Get position using WorldPose
        auto worldPoseComp = _ecm.Component<gz::sim::components::WorldPose>(this->actorEntity);
        if (worldPoseComp) {
            const gz::math::Pose3d &worldPose = worldPoseComp->Data();
            gzdbg << "Box world position: (" 
                  << worldPose.Pos().X() << ", "
                  << worldPose.Pos().Y() << ", " 
                  << worldPose.Pos().Z() << ")\n";
        }
        else {
            gzdbg << "WorldPose component not found for entity: " << this->actorEntity << std::endl;
        }

        // Or using the wrapper class Actor 
        // Get position using Actor wrapper Pose() method
        auto pose = this->actorWrapper.Pose(_ecm);
        if (pose) {
            gzdbg << "Box position (using wrapper): (" 
                  << pose->Pos().X() << ", "
                  << pose->Pos().Y() << ", " 
                  << pose->Pos().Z() << ")\n";
        }
        else {
            gzdbg << "Could not get pose using Actor wrapper" << std::endl;
        }

        // Get position using Actor wrapper WorldPose() method
        auto worldPose = this->actorWrapper.WorldPose(_ecm);
        if (worldPose) {
            gzdbg << "Box world position (using wrapper): (" 
                  << worldPose->Pos().X() << ", "
                  << worldPose->Pos().Y() << ", " 
                  << worldPose->Pos().Z() << ")\n";
        }
        else {
            gzdbg << "Could not get world pose using Actor wrapper" << std::endl;
        }

        // Check for TrajectoryPose using Component template
        auto trajComp = _ecm.Component<gz::sim::components::TrajectoryPose>(this->actorEntity);
        if (trajComp) {
            const gz::math::Pose3d &trajPose = trajComp->Data();
            gzdbg << "Box trajectory position: (" 
                  << trajPose.Pos().X() << ", "
                  << trajPose.Pos().Y() << ", " 
                  << trajPose.Pos().Z() << ")\n";
        }
        else {
            gzdbg << "TrajectoryPose component not found for entity: " << this->actorEntity << std::endl;
        }

        // Check for TrajectoryPose using wrapper
        auto traj = this->actorWrapper.TrajectoryPose(_ecm);
        gzdbg << "[TrajectoryPose] present (wrapper): " << bool(traj) << "\n";

    }

Here is my SDF file:

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="moving_box_world">

    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>

    <actor name="moving_box">
        <pose>0 0 0.5 0 0 0</pose>
        <link name="moving_box_link">
            <visual name="moving_box_visual">
                <geometry>
                    <box>
                        <size>1 1 1</size>
                    </box>
                </geometry>
            </visual>
        </link>

        <script>
            <loop>true</loop>
            <delay_start>1.0</delay_start>
            <auto_start>true</auto_start>
            <trajectory id="0" type="moving_box_trajectory">
              <!-- start at X=0 -->
              <waypoint>
                <time>0.0</time>
                <pose>0 0 0.5 0 0 0</pose>
              </waypoint>
              <!-- move to X=5 over 10 s -->
              <waypoint>
                <time>10.0</time>
                <pose>5 0 0.5 0 0 0</pose>
              </waypoint>
            </trajectory>
        
          </script>

          <plugin 
            filename="MonitorMovingActorPlugin"
            name="monitor::MonitorMovingActorPlugin">
            <speed>1.0</speed>
          </plugin>

    </actor>

  </world>
</sdf>