Gazebo: SetPosition() function in the Joint class doesn't set the joint position and returns false sometimes

Question

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.

Answer 1

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.