Model plugin resets model pose

edit

Hi,

I am trying to change the position of my catapult model dynamically. I am able to change the pose but when I send a message to the plugin the model gets bugged. Any idea how to fix the following code so that the plugin does not affect the model pose? Full plugin available here .

public: void OnRosMsg(const catapult_control::CatapultConstPtr &_msg)
{
  this->SetVelocity(_msg->velocity);
  this->SetUpperLimit(_msg->upper_limit);
}

public: void SetVelocity(const double &_vel)
{
  this->model->GetJointController()->SetVelocityTarget(
      this->joint->GetScopedName(), _vel);
}

public: void SetUpperLimit(const double &_upper_limit)
{
  this->joint->SetHighStop(0,_upper_limit);
}