Rotate joint around given axis (steer car)
Hello,
I have a car whose front wheel are connected to chassis in this manner :
<joint name="wheel_front_left_steer_spin" type="universal">
<parent>chassis</parent>
<child>wheel_front_left</child>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.5235987755982988</lower>
<upper>0.5235987755982988</upper>
</limit>
</axis>
<axis2>
<xyz>0 0 1</xyz>
</axis2>
</joint>
In order to steer my car, I want to set the position around axis2 to 30 degrees. So I tried this (inside my plugin):
this->joint_ = model_->GetJoint("car::wheel_front_left_steer_spin");
this->steer_pid_ = common::PID(0.1, 0, 0);
this->model_->GetJointController()->SetPositionPID(joint_->GetScopedName(), steer_pid_);
double position = -3.14159265 / 6;
this->model_->GetJointController()->SetVelocityTarget(joint_->GetScopedName(), position);
But it sends my car bouncing and jumping uncontrollably. So how do I set the angle of my wheels? I haven't found clear examples and couldn't figure it out from the doc.
Thanks in advance,
EDIT: So I split my joint into two revolute joints, but can only get one moving at a time. How can I link the two so I keep the 2 degrees of freedom?
<joint name="wheel_front_right_steer" type="revolute">
<parent>chassis</parent>
<child>wheel_front_right_steer_link</child>
<axis>
<xyz>0 -1 0</xyz>
<limit>
<lower>-0.5235987755982988</lower>
<upper>0.5235987755982988</upper>
</limit>
</axis>
</joint>
<link name="wheel_front_right_steer_link">
<!-- Not sure what to put here -->
</link>
<joint name="wheel_front_right_spin" type="revolute">
<parent>wheel_front_right_steer_link</parent>
<child>wheel_front_right</child>
<axis>
<xyz>0 0 1</xyz>
</axis>
</joint>
Asked by kaldo on 2017-01-23 14:37:39 UTC
Answers
It looks like the joint controller doesn't have good support for multi-axis joints (such as universal). I ticketed an issue for this.
I would suggest you change to a revolute joint. If you need the 2 DOF, you could add 2 revolute joints with a dummy link in between them.
Also, when things start bouncing uncontrollably like this, it's a good idea to run Gazebo in verbose mode to see if any error messages are being printed.
I'd also inspect the model by visualizing the inertias and masses for each link, to make sure there are no tiny or huge links in the model making the physics engine crazy. Finally, you can try playing with different PID gains to see if things become more stable.
Asked by chapulina on 2017-01-23 16:41:29 UTC
Comments
Thank you :) Gazebo is already verbose and said nothing. And I'm using an adapted version of the "standard" cart with front steering, so I don't expect to have any extravagant link. I split my joint as you suggested, but can get only one to move at a time. Could you please elaborate on the dummy link?
Asked by kaldo on 2017-01-24 04:20:48 UTC
Comments
I was stuck with exactly the same problem. I "solved" it by adapting the working example from https://github.com/osrf/car_demo rather than designing my own model.
Asked by fhwedel-hoe on 2020-09-01 06:56:49 UTC