0
$\begingroup$

I am trying to initialize the positions of my robot using Gazebo Plugins. SetPosition() function of the Joint class is supposed to set the joint angle (all my joints are revolute joints). However, the function doesn't do anything. The function returns false indicating the action is unsuccessful. I have attached a simplified version of my code. I don't understand why it returns false always. Any help is appreciated.

#include <map>
#include <functional>

#include <ignition/math/Vector3.hh>

#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

namespace gazebo {
class RobotInitialization : public ModelPlugin {
 public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr) {
    this->model = _parent;

    auto joint = this->model->GetJoint("revolute-joint");
    joint->SetPosition(0, 1.0, true);
    std::cout << joint->Position(0) << std::endl;
  }

 private:
  physics::ModelPtr model;
};
GZ_REGISTER_MODEL_PLUGIN(CassieInitialization)
}  // namespace gazebo  

Edit:
Thanks for the suggestion. I checked the joint limits and this helped for 6 out of 16 joints on my robot. I set velocities for all joints using SetVelocity() and that works for all the joints but SetPosition() doesn't. I also checked DOF of the joints using joint.DOF() and it returns 1 for all my joints.

Edit: I am using bullet as the solver.

$\endgroup$
0
$\begingroup$

It's returning false because it's failing to set the joint angle. I would try to troubleshoot this by stepping through the different reasons it could fail.

The first thing that I think of, especially when I see a Get<something>() that has a string input, is that the name changed, isn't spelled correctly, etc. I would check first if you're actually getting a valid joint from your GetJoint() command:

if(joint==nullptr)
{
    std::cout<<"Joint not found!"<<std::endl;
}
else
{
    <your code here>
}

The next thing I would check is whether your joint has any degrees of freedom to actually move - that it's not fully constrained:

if(joint.DOF()==0)
{
    std::cout<<"Joint has zero degrees of freedom!"<<std::endl;
}
else
{
    <your code here>
}

I can't see in the documentation what units the inputs to SetPosition() are, but Position() returns radians and I'm assuming that SetPosition() is also expecting radians. You're inputting 1.0 radians, which is about 60 degrees, so I don't know if there are some kinematic constraints that are preventing that rotation.

Please do these checks and edit your question to post the results.

:EDIT:

It looks like there's an open bug for setting joint states when using SimBody, but it looks like maybe using ODE as the solver instead works.

Also, there's another post that suggests using a joint controller and setting the joint position target instead of trying to set the joint position directly.

$\endgroup$
  • $\begingroup$ Will the method using PID controller be instantaneous? I want to start my simulator paused. Also, I was wondering if modifying the sdf initial_position help. $\endgroup$ – NKV Jul 12 at 17:08
  • $\begingroup$ @NKV - I don't think the PID controller will be instantaneous, or it wouldn't be a controller. In your edit you said the joint limits... something... for 6 of 16 joints. You said SetVelocity() works but SetPosition() doesn't, so the edit I made above collects the results of me searching for that problem - that you can set velocity but not position. Ultimately I don't have your model so it's really hard to troubleshoot what's going on, but it sounds like some kind of a configuration issue, especially if not all joints are responding the same. $\endgroup$ – Chuck Jul 12 at 18:04

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.