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
#include
#include
#include
#include
#include
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)
}
↧