Problem with SetPositionTarget

edit

Hi guys, I'm new with Gazebo. I'm trying to make the robot handles, thanks to a plugin, a link in a certain position through a prismatic joint. The robot's structure is composed by a fixed part and a mobile one; the fixed part is under the mobile one. The two parts have the same shape and they are linked through 3 prismatic joints that allow the translation over the z axis. I want to rise the mobile part in a certain position through the z axis. Now I'm using the method SetPositionTarget but it doesn't work. Anyone has some suggestions on how to fix it? The code of the plugin is pasted below in hopes that you can identify a problem.

namespace gazebo{

class ModelPush: public ModelPlugin{


    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
        this->model = _model;
        this->sdf = _sdf;
        if(this->model->GetJointCount() == 0){
            std::cerr << "Invalid joint count\n";
            return;
        }

        this->centerJoint = this->model->GetJoint("center_chassis_joint");

        this->leftJoint = this->model->GetJoint("left_chassis_joint");

        this->rightJoint = this->model->GetJoint("right_chassis_joint");

        this->pid_right = common::PID(0.1, 0, 0);
        this->pid_left = common::PID(0.1, 0, 0);
        this->pid_center = common::PID(0.1, 0, 0);

        this->model->GetJointController()->SetPositionPID(this->rightJoint->GetScopedName(), this->pid_right);
        this->model->GetJointController()->SetPositionPID(this->centerJoint->GetScopedName(), this->pid_center);
        this->model->GetJointController()->SetPositionPID(this->leftJoint->GetScopedName(), this->pid_left);

        this->model->GetJointController()->SetPositionTarget(this->rightJoint->GetScopedName(), 2);
        this->model->GetJointController()->SetPositionTarget(this->centerJoint->GetScopedName(), 2);
        this->model->GetJointController()->SetPositionTarget(this->leftJoint->GetScopedName(), 2);
    }

public: physics::JointPtr centerJoint;
    public: physics::JointPtr leftJoint;
    public: physics::JointPtr rightJoint;
    public: physics::JointPtr backJoint;
    public: physics::ModelPtr model;
    public: sdf::ElementPtr sdf;
    public: common::PID pid_right,pid_left,pid_center;
};
GZ_REGISTER_MODEL_PLUGIN(ModelPush);
}