Position-controlled actuators

[blandry]

Drake should have a way of modeling actuators that take position as input (like servos) directly from URDFs. This would of course be an abstraction for the controller inside the servo but most people assume those controllers to be "ideal" anyway.

A temporary workaround I found is to build a PD controller around a RigidBodyManipulator yourself.

                        +-------+         +--------+
                        |       |         |        |
        u +--------+--->|    T  +-------->|   RBM  +---+-----> x
                   ^    +-------+         +--------+   |
                   |                                   |
                   |                                   |
                   |                                   |
                   |            +---------+            |
                   |            |         |            |
                   +------------+    LS   |<-----------+
                                |         |
                                +---------+

The T is the mechanicalReduction in your URDF (which is technically inside RBM but is easier to see that way), RBM is your RigidBodyManipulator and LS is a LinearSystem (the PD controller). Here is an example:

options.floating = true;
sys1 = RigidBodyManipulator('Sbach.urdf',options);

Kp = -500;
Kd = -400;
D = zeros(5,20);
D(1,7) = Kp; 
D(2,8) = Kp;
D(3,9) = Kp;
D(4,10) = Kp;
D(5,10) = 0; % thrust
D(1,17) = Kd;
D(2,18) = Kd;
D(3,19) = Kd;
D(4,20) = Kd;
D(5,20) = 0;
sys2 = LinearSystem([],[],[],[],[],D);

t_1out_2in = AffineTransform(sys1.getOutputFrame(),sys2.getInputFrame(),eye(20),zeros(20,1));
t_2out_1in = AffineTransform(sys2.getOutputFrame(),sys1.getInputFrame(),eye(5),zeros(5,1));
sys1.getOutputFrame().addTransform(t_1out_2in);
sys2.getOutputFrame().addTransform(t_2out_1in);

p = feedback(sys1,sys2);

x0 = [0 0 3 0 0 0 -.75 -.75 -.75 0 10 0 0 0 0 0 0 0 0 0]';
tf = .5;
pts = 10;
u0 = repmat([-.75 -.75 -.75 0  0]', 1, pts);
utraj = PPTrajectory(foh(linspace(0,tf,pts),u0));
utraj = setOutputFrame(utraj, p.getInputFrame);
pp = cascade(utraj,p);
xtraj = pp.simulate([0 tf], x0);

v = sys1.constructVisualizer();
v.playback(xtraj, struct('slider', true));

Things to take into consideration when tuning your PD controller:

• P gain
• D gain
• Mechanical reduction in your URDF
• Inertia matrix of the actuated joints

[RussTedrake]

We actually have a pdcontrol function which implements this. :) Time stepping models also have an option to take position commands as input (adds additional constraints). I agree that we should have a tool for abstracting an arbitrarily stiff pd controller with a first order approximation, but then it is probably not strictly an rbm anymore... So we have to do it carefully.