Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

Looking at the OnUpdate() method in your code...

public: void OnUpdate()
{
  this->model->GetJoint("joint1")->SetPosition(0, pos[count%4]);
  this->model->GetJoint("joint1")->>Update();
  this->model->GetJoint("joint2")->SetPosition(0, pos[count%4]);
  this->model->GetJoint("joint2")->>Update();

  cout << fixed << this->model->GetJoint("joint1")->GetAngle(0).Radian() << "\t";
  cout << fixed << this->model->GetJoint("joint2")->GetAngle(0).Radian() << "\t" << count << endl;

  count++;
  usleep(500000);
}

1)
You are sleeping for 0.5s inside this callback method which is connected to the world update event
(this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelPush::OnUpdate, this));)

Note that, according to this answer, OnUpdate()is blocking the entire physics thread.

Thus, instead of ~1000 iterations a second, you are looking at ~1 iteration every 2 seconds and whatever wonky behavior comes with that.

Solution:
Instead of sleeping inside OnUpdate(), use this->world->GetSimTime().Double() to check the elapsed simulated since the last count increment. See this answer and this answer.


2)
I'm not a fan of using SetPosition(). If I remember correctly, SetPosition() moves everything to the desired position in a single time step and leaves the physics engine to clean up the mess caused by things magically teleporting.

Solution:
Check out SetForce() as explained in this answer and this answer.


3)
Just a musing, but depending on your system's physical properties, 0.5s may not be enough time to move between positions in a "friendly" manner.

Looking at the OnUpdate() OnUpdate() method in your code...

public: void OnUpdate()
{
  this->model->GetJoint("joint1")->SetPosition(0, pos[count%4]);
  this->model->GetJoint("joint1")->>Update();
  this->model->GetJoint("joint2")->SetPosition(0, pos[count%4]);
  this->model->GetJoint("joint2")->>Update();

  cout << fixed << this->model->GetJoint("joint1")->GetAngle(0).Radian() << "\t";
  cout << fixed << this->model->GetJoint("joint2")->GetAngle(0).Radian() << "\t" << count << endl;

  count++;
  usleep(500000);
}

1)
You are sleeping for 0.5s inside this callback method which is connected to the world update event
(this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelPush::OnUpdate, this));)

Note that, according to this answer, OnUpdate()is blocking the entire physics thread.

Thus, instead of ~1000 iterations a second, you are looking at ~1 iteration every 2 seconds and whatever wonky behavior comes with that.

Solution:
Instead of sleeping inside OnUpdate(), use this->world->GetSimTime().Double() to check the elapsed simulated since the last count increment. See this answer and this answer.


2)
I'm not a fan of using SetPosition(). If I remember correctly, SetPosition() moves everything to the desired position in a single time step and leaves the physics engine to clean up the mess caused by things magically teleporting.

Solution:
Check out SetForce() with a PID controller as explained in this answer and this answer.


3)
Just a musing, but depending on your system's physical properties, 0.5s may not be enough time to move between positions in a "friendly" manner.