I apply some torques to the joint of a robot arm and output the current time, angle, velocity and torque value to terminal everytime when the simulation is updated.
Every two lines are the same. Why? Can I let it just display once?
Below are my terminal and code snippet.
namespace gazebo
{
class ModelPush : public ModelPlugin
{
private: int count = 0;
private: float tor[10] = {1,2,3,4,5,6,7,8,9,10};
private: physics::ModelPtr model;
private: physics::JointPtr joint;
private: event::ConnectionPtr updateConnection;
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
this->model = _parent;
this->joint = this->model->GetJoint("arm_shoulder_pan_joint");
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&ModelPush::OnUpdate, this));
}
public: void OnUpdate()
{
this->joint->SetForce(0, tor[count++%10]);
cout << fixed << this->model->GetWorld()->GetSimTime() << "\t";
cout << fixed << "pos=" << this->joint->GetAngle(0).Radian() << "\t";
cout << fixed << "vel=" << this->joint->GetVelocity(0) << "\t";
cout << fixed << tor[count%10] << endl;
}
};
GZ_REGISTER_MODEL_PLUGIN(ModelPush) }
↧