Pose of Collision entity

edit

Hi,

I am trying to retrieve the current Pose of all Collision entities. However I just can retrieve the initial Pose before starting the physics engine. The complete plugin code is pasted below.

include <boost bind.hpp=""> include <gazebo.hh> include <physics physics.hh=""> include <common common.hh=""> include <stdio.h>

using namespace std;

namespace gazebo
{   
  class ModelPush : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
      // Store the pointer to the model
      this->model = _parent;

      //frame
      frame=0;

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&ModelPush::OnUpdate, this, _1));

    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      // Apply a small linear velocity to the model.
      std::cout << "collision test" << std::endl;





      system("clear");
      cout << "frame\t" << frame << endl;
      frame = frame +1;

      physics::Link_V links = model->GetLinks();
      physics::Collision_V colls;
      for (unsigned int i = 0; i < links.size(); i++) {

          cout << links[i]->GetName() << "\t\t" <<links[i]->GetWorldPose() << endl;
          colls = links[i]->GetCollisions();
          for(unsigned i=0; i<colls.size(); i++)
          {
              colls[i]->Update();
              cout << colls[i]->GetName() << "\t\t" << colls[i]->GetWorldPose() << endl;
          }

      }

    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    // Frame counter
    private: unsigned int frame;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(ModelPush)
}

Can anyone help? What am I doing wrong?