Simulating Actuated Joint Stiffness
Hello.
I am trying to simulate a joint which actuates a lift using Gazebo 7 and ROS Indigo. The URDF definition for the joint is as follows:
<joint name="lift_joint" type="prismatic">
<origin xyz="0 0 0.177" rpy="1.5707963267949 0 0" />
<parent link="chassis_link" />
<child link="lift_link" />
<axis xyz="0 1 0" />
<limit lower="0" upper="0.08" effort="20000.0" velocity="0.02" />
<dynamics damping="100.0" />
</joint>
The lift is driven by a position controller with these parameters:
<rosparam>
gazebo_ros_control:
pid_gains:
left_wheel_joint: { p: 1.0, i: 0.0, d: 0.0 }
right_wheel_joint: { p: 1.0, i: 0.0, d: 0.0 }
lift_joint: { p: 100000.0, i: 1000.0, i_clamp: 1000, d: 1000.0 }
</rosparam>
The problem is that the lift can be easily depressed by another object. Say the lift is given a command to raise. It does so, but when it comes into contact with the object it is meant to actually lift, it "bounces" back do to a lower position. It will then continually attempt to reach the commanded position, and continually get bounced back down before it can do so.
Is there a way to specify a stiffness for the joint, so that it only moves when commanded to move?
Asked by JLiviero on 2016-08-15 07:59:43 UTC
Answers
If you are using effort control, you can try adding some joint friction to the screw joint (<dynamics damping="100.0" friction="200.0" />
), which may help with holding the weight.
EDIT:
I don't think joint friction is working right now for screw joints. You could dynamically set the joint limits to act as a brake, but that would circumvent effort limits. I'm still thinking about it...
Asked by scpeters on 2016-08-16 18:54:21 UTC
Comments
Hm...this might not work for screw joints...
Asked by scpeters on 2016-08-16 18:55:58 UTC
We aren't actually using a screw joint, at least for now it's a simple prismatic joint. I suppose we could maybe paper over the problem with an extremely large amount of friction?
Asked by mikepurvis on 2016-08-16 19:07:14 UTC
As Mike mentioned, the actual joint is a screw, but we're using a prismatic joint as a stand-in.
Joint friction was something I attempted also. However, I have friction set to an unrealistically high value, and we are still getting backdrive. Also, the "effort" value and p term of the PID need to be set correspondingly high so that the lift can still move, which means that the lift has a hilarious-but-unfortunate tendency to fling its payload across the map when compensating for the backdrive.
Asked by JLiviero on 2016-08-16 21:53:12 UTC
Joint friction has units of effort (N for prismatic joint) and will only work as a form of gravity compensation if it is larger than the weight of the object you are lifting. Is this the case?
Asked by scpeters on 2016-08-17 02:19:25 UTC
This is starting to sound like a control problem. From a controls perspective and ignoring joint friction, it sounds like you want gravity compensation, but if the load is unknown, you need to adapt to the unknown parameter. The integral term can provide that adaptation, but I would expect a transient while it adapts to the unknown parameter.
Asked by scpeters on 2016-08-17 02:21:33 UTC
But the real system doesn't need to compensate for gravity— it's just not backdriveable.
Asked by mikepurvis on 2016-08-17 20:50:31 UTC
Comments