Open hubernikus opened 1 year ago
Fair point. We will add this issue to our backlog and see if we can address it. Thanks!
I'm not sure if that's quite right. I will rewrite your steps with math formatting to make sure I understand them.
Starting with the definition $q{des} = w{des} * dt$; $q_{fb} = 0$, I agree this is the formulation in the VelocityImpedance controller. And, we have the impedance control law:
$$\tau = K(q{des} - q{fb}) + D (w{des} - w{fb})$$
So you substitute and expand (I think you were missing the feedback term w_{fb}
in your first comment):
$$\tau = K(w{des} dt - 0) + D w{des} - D *w_{fb}$$
$$\tau = (K dt + D) w{des} - D * w{fb}$$
Then, if you define a modified desired velocity:
$$\hat{w}{des} = (K dt + D) w{des} / D $$
you can rewrite the final control law as:
$$\tau = (K dt + D) w{des} - D * w{fb} = D * (\hat{w}{des} - w{fb})$$
I have two points. First, where do you see a computational saving? You still have to calculate $\hat{w}$. Or maybe you suggest that we calculate the matrix $K dt + D$ in advance and store it as some $\hat{D}$, so that we can have $\tau = \hat{D} w{des} - D * w{fb}$.
But that leads to the second point, which is that $q{des} = w{des} * dt$ represents an integration, and not a normal product. The angular velocity is integrated into angular displacement, and the K
matrix then scales the imaginary vector part of the quaternion. So I don't think you can just factor out dt
and have the same behavior.
Let me know if I misunderstood anything.
Yes, good point. Did not know nice formulas were possible.
The computational saving is minor, I agree. But in the end the step of integrating from $q{des} = w{des} dt$ and then taking the angular difference $\Delta q = q_{des} - q_0$ (since the desired velocity is set to a zero state), is the same as just multiplying the $\Deltaq = w{des} \hat k$ a factor $\hat k$ (which is 1 if you take the actual orientation vector, and an factor which depends on the rotation-angle otherwise).
I agree, that it is not uniquely a multiplication, but also requires a slight modification of the damping matrix $D$. However, I feel it is a bit misleading letting a user define a proportional $K$ matrix, which actually only acts on the desired velocity, but not the orientation.
The VelocityImpedance controller as used by the CartesianTwist is in reality a passive controller, this can be check by looking at the underlying math (des == desired; fb == feedback) q_des = w_des * dt ; q_fb = 0
tau_ctrl = K(q_des - q_fb) + D (w_des - w_fb) = (K dt + D) w_des - D using new dynamics which are given by: hat{w}_des = (K dt + D) / D w_des we can rewrite the control law as a simple passive one: tau_ctrl = D (hat{w}_des - w)
I think using the simplified control law, the controller can be simplified (in the number of operations), hence made simpler for a user to understand as well as lower computational cost. Additionally, reducing the redundant parameter K will allow easier tuning.