Gazebo joint->SetForce() call applies forces to the whole model. How to circumvent that?
The following code applies a force to the joint in an update method call. The problem is that the force seems to dissipate / is applied consecutively to other parts of the model, specifically the chassis, which holds the rotating laser. How can I circumvent that?
The chassis shold be a moving vehicle so I can't just fix it to the ground plane, using a fixed joint.
This is my onUpdate() from within the gazebo plugin. Essentially its making the joint rotate back and forth on a specified axis.
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
rotation = this->joint->GetAngle(0);
double degree = rotation.Degree();
if (degree <= -90) {
this->joint->SetForce(rotationAxis,effort*2);
}
else if (degree >= 90) {
this->joint->SetForce(rotationAxis,-effort*2);
}
std::cerr << degree << "\n";
}
The definition from the model.sdf is this:
<joint name="back_and_forth_joint" type="revolute">
<child>laser</child>
<parent>chassis</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1.57079633</lower>
<upper>1.57079633</upper>
</limit>
</axis>
</joint>
Thanks.
Update:
I have asked exactly the same question on http://robotics.stackexchange.com/questions/11642/gazebo-joint-setforce-call-applies-forces-to-the-whole-model-how-to-circumve so feel free. I'll update both with the best solution I have found.
One of the possibilities is to simply add mass to the chassis like so:
<link name='chassis'>
<pose>0 0 .1 0 0 0</pose>
<inertial>
<mass>10</mass>
</inertial>
Asked by pijemcolu on 2017-02-17 06:35:52 UTC
Comments
What would you expect happens to the chassis? When you apply force to a joint, you add momentum to a link. That link's momentum will affect whatever is connected to it. Maybe your chassis is too light? A video might help understand the problem.
Asked by chapulina on 2017-02-17 12:41:19 UTC