Robotics StackExchange | Archived questions

Clarification on moving joint with model plugin

This is my first week with Gazebo. The tutorials are clear (except for my dearth of C++ knowledge) but now that I'm working to move out on my own things are getting cloudy. I made a model comprising two boxes and a revolute joint. The file onertest.world loads this model. A plugin is "loaded" (?) in model.sdf and that plugin, ModelControl, comes from modelpush.cc in the "Model plugins" tutorial (http://gazebosim.org/tutorials?tut=pluginsmodel&cat=writeplugin), which uses SetLinearVel to move a box. I can get this same behavior out of modelcontrol.cc if I just copy the tutorial code (and change the plugin name as appropriate), but that's not what I want. I'm seeking to eventually simulate joint control of robotic manipulators and what's not working in this basic simulation is my attempt to move the model joint via the ModelControl plugin. It moves in the GUI if I set the velocity (or torque) that way. The model_control.cc code is pasted below in hopes that you can identify a problem.

I used SetParam based on this answer: http://answers.gazebosim.org/question/8371/kobukiturtlebot-doesnt-move-when-given-velocity-commands-in-gazebo-5/?answer=8377#post-id-8377.

model_control.cc

include "boost/bind.hpp"

include "gazebo/gazebo.hh"

include "gazebo/physics/physics.hh"

include "gazebo/common/common.hh"

include "stdio.h"

// In the real file these quotes are greater-than and less-than but I // don't know how to get that to show up in my question

namespace gazebo { class ModelControl : public ModelPlugin { public: void Load(physics::ModelPtr parent, sdf::ElementPtr /*sdf*/) { // Store the pointer to the model this->model = _parent;

  // Store the pointers to the joints
  this->jointR1_ = this->model->GetJoint("r1");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ModelControl::OnUpdate, this, _1));
}

// Called by the world update start event

public: void OnUpdate(const common::UpdateInfo & /_info/) { // Apply a small linear velocity to the model. //this->model->SetLinearVel(math::Vector3(.03, 0, 0));

  // Apply angular velocity to joint
  this->jointR1_->SetParam("vel", 0, 99);
  this->jointR1_->SetParam("max_force", 0, 9999999999);

  double currAngle = this->jointR1_->GetAngle(0).Radian();
  printf("Current angle is \t %f\n", currAngle);
}

// Maybe I want to keep track of time?
common::Time last_update_time_;

// Pointer to the model

private: physics::ModelPtr model;

// Pointer to the update event connection

private: event::ConnectionPtr updateConnection;

// Pointers to joints
physics::JointPtr jointR1_;

};

// Register this plugin with the simulator GZREGISTERMODEL_PLUGIN(ModelControl) }

edit: If I change

this->jointR1_->SetParam("vel", 0, 99);

to

this->jointR1_->SetVelocity(0, 99);

then the joint moves (yes, very, very quickly). What's wrong with SetParam vs SetVelocity?

Asked by raequin on 2016-01-15 12:44:19 UTC

Comments

Answers

After trying different ways of moving joints, like model->SetJointPositions(...) or joint->SetVelocity(...) they gave me very unexpected or none results... Apparently this part of the API has changed between versions.

I ended using force control + PID as explained in this answer: http://answers.gazebosim.org/question/2341/set-and-get-position-of-gazebo-model-using-ros-plugin/

Asked by gecastro on 2016-01-21 02:31:54 UTC

Comments

I'm glad to read your input. It seems like the force-control approach is less than ideal since the physics engine is supposed to be able to compute the forces necessary to obtain a prescribed motion. That is, having to tune a PID controller is doing work that could (better) be done by Open Dynamics Engine.

Asked by raequin on 2016-01-21 15:41:01 UTC