Home | Tutorials | Wiki | Issues
Ask Your Question
0

Array elements are skipped

asked 2018-07-27 07:10:29 -0600

jwchang gravatar image

updated 2018-07-27 07:36:08 -0600

I want to move the joints of my robot at 0, 90, 180, 270 degrees, but it only displays the state of 90 and 270 degrees on gazebo repeatedly. I output the results to the terminal, and actually four angles show periodically, and counts are formally 0,1,2,3,4,5,6.... However I do the different tests, the robot always skips one element between two array elements on gazebo. Does anyone know how to solve this problem? Here is my plugin code.

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <gazebo/physics/physics.hh>
#include <stdio.h>

using namespace std;

namespace gazebo
{
  int count = 0;
  float pos[4] = {0, 1.57, 3.14, 4.71};

  class ModelPush : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      this->model = _parent;
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelPush::OnUpdate, this));
    }

    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);
    }

    private: physics::ModelPtr model;
    private: event::ConnectionPtr updateConnection;
  };
  GZ_REGISTER_MODEL_PLUGIN(ModelPush)
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-07-27 08:45:32 -0600

updated 2018-07-27 08:46:52 -0600

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() 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.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-07-27 07:10:29 -0600

Seen: 49 times

Last updated: Jul 27