I have a model plugin, in which on each update, I set a z axis position of a floating link to zero.

math::Pose pose = this->link_ptr->GetRelativePose();
pose.pos.z=0.0;
this->link_ptr->SetRelativePose(pose);

When I run the simulation, the link slowly sinks under the ground and continues slowly falling down. What is happening? How can I fix it?

---

EDIT

I found out, that the link is not sinking if I turn the gravity off for the individual link. What is happening? That is definitively not how this ought to work. Is it a bug?

Moreover I can't set the gravity to false from the URDF description, as it gets reset after the conversion to SDF (viz this issue). Anybody faced this issue before?

---

EDIT 2

the source file of the model plugin:

#include <ros/callback_queue.h>
#include "CoverPlugin.hh"

namespace gazebo
{
    // Constructor
    CoverPlugin::CoverPlugin()
    {}

    // Default plugin init call.
    void CoverPlugin::Init()
    {}

    void CoverPlugin::Load( physics::ModelPtr _parent,
                                sdf::ElementPtr _sdf)
    {
        // Get the world name.
        this->world = _parent->GetWorld();
        this->model = _parent;

        // Get link
        std::string link_ptr_name = this->model->GetName() + "::"
            + "cover_link";
        this->link_ptr = this->model->GetLink(link_ptr_name);

        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
            boost::bind(&CoverPlugin::OnUpdate, this));

        ros::AsyncSpinner spinner(2); // Use 2 threads
        spinner.start(); // spin() will not return until the node has been shutdown  

    }

    void CoverPlugin::OnUpdate()
    {
        math::Pose pose = this->link_ptr->GetRelativePose();

        if( this->link_ptr->GetRelativePose().pos.z < 0.0 )
        {
            pose.pos.z=0.0;
        }

        if( this->link_ptr->GetRelativePose().pos.x > 0.028 )
        {
            pose.pos.x=0.028;
        }

        if( this->link_ptr->GetRelativePose().pos.y > 0.014 )
        {
            pose.pos.y=0.014;
        }

        if( this->link_ptr->GetRelativePose().pos.z > 0.01 )
        {
            pose.pos.z=0.01;
        }

        if( this->link_ptr->GetRelativePose().pos.x < -0.028 )
        {
            pose.pos.x=-0.028;
        }

        if( this->link_ptr->GetRelativePose().pos.y < -0.014 )
        {
            pose.pos.y=-0.014;
        }


        if( this->link_ptr->GetRelativePose().rot.x > 0.05 )
        {
            pose.rot.x=0.05;
        }

        if( this->link_ptr->GetRelativePose().rot.y > 0.05 )
        {
            pose.rot.y=0.05;
        }

        if( this->link_ptr->GetRelativePose().rot.z > 0.05 )
        {
            pose.rot.z=0.05;
        }

        if( this->link_ptr->GetRelativePose().rot.x < -0.05)
        {
            pose.rot.x=-0.05;
        }

        if( this->link_ptr->GetRelativePose().rot.y < -0.05 )
        {
            pose.rot.y=-0.05;
        }

        if( this->link_ptr->GetRelativePose().rot.z < -0.05 )
        {
            pose.rot.z=-0.05;
        }

        this->link_ptr->SetRelativePose(pose);

        math::Vector3 force;
        math::Vector3 torque;

        force.x = -pose.pos.x*10000;
        force.y = -pose.pos.y*10000;
        force.z = -pose.pos.z*50000;

        torque.x = -pose.rot.x*100;
        torque.y = -pose.rot.y*100;
        torque.z = -pose.rot.z*100;

        this->link_ptr->SetForce(force);

    }

;

}

header file:

#ifndef COVER_PLUGIN_HH
#define COVER_PLUGIN_HH

#include <gazebo/gazebo.hh>
#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include <gazebo/physics/physics.hh>
#include <gazebo/math/Pose.hh>
#include <gazebo/math/Vector3.hh>
#include <gazebo/common/common.hh>


namespace gazebo
{
    class CoverPlugin : public ModelPlugin
    {

        // Constructor. 
        public: CoverPlugin();

        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);

        private: physics::WorldPtr world;
        private: physics::ModelPtr model;

        private: physics::LinkPtr link_ptr;

        private: event::ConnectionPtr updateConnection;

        public: virtual void Init();

        private: void OnUpdate();

        private: std::unique_ptr<ros::NodeHandle> rosNode;

    };

    GZ_REGISTER_MODEL_PLUGIN(CoverPlugin)

}

#endif