If I understand this correctly, given the Jacobian Derivative $\dot{J}(q)$ derived using the method presented from the paper by (Rhee, J. Y., & Lee, B., 2017)
where the algorithm summarized computes:
$$ \overline{\underline{\bf{\text{Algorithm 1}}\textrm{ Jacobian and Jacobian Differentiation}}}\\ \begin{array}{lll} 1: & \omega_0^0 \leftarrow\left[0,0,0\right]^T & \\ 2: & z_0^0 \leftarrow\left[0,0,1\right]^T & \\ 3: & \pmb{J}_1 \leftarrow \left[\begin{matrix} z_0^0\times d_n^0 \\ z_0^0\end{matrix}\right] & \\ 4: & \bf{\text{for }} \textrm{i = 2 to n } \bf{\text{do}} & \triangleright \textrm{ Jacobian Computation loop} \\ 5: & \quad \pmb{J}_i = \left[\begin{matrix} z_{i-1}^0 \times d_{i-1,n}^0 \\ z_{i-1}^0\end{matrix}\right] & \\ 6: & \quad \omega_{i-1}^0 \leftarrow w_{i-2}^0 + z_{i-2}^0\dot{q}_{i-1} \\ 7: & \bf{\text{end for}} & \\ 8: & \beta \leftarrow J_{v,i}\dot{q}_i & \\ 9: & \bf{\text{for }} \textrm{i = n to 2 } \bf{\text{do}} & \triangleright \textrm{ Jacobian Differentiation loop}\\ 10: & \quad \dot{z}_{i-1}^0 \leftarrow \omega_{i-1}^0 \times z_{i-1}^0 & \\ 11: & \quad \alpha \leftarrow \left[0,0,0\right]^T & \\ 12: & \quad \bf{\text{for }} \textrm{j = 1 to i-1 } \bf{\text{do}} \\ 13: & \qquad \alpha \leftarrow \alpha + z_{j-1}^0 \times \left(d_n^0 - d_{i-1}^0\right)\dot{q}_j & \\ 14: & \quad \bf{\text{end for}} & \\ 15: & \quad \dot{J}_i = \left[\begin{matrix} \left(\dot{z}_{i-1}^0\right) \times (d_{n}^0- d_{i-1,n}^0) + z_{i-1}^0 \times\left(\alpha + \beta\right) \\ \dot{z}_{i-1}^0 \end{matrix}\right] & \\ 16: & \quad \beta \leftarrow \beta + J_{v,i-1}\dot{q}_{i-1} & \\ 17: & \bf{\text{end for}} & \\ 18: & \dot{J}_1 = \left[\begin{matrix} z_0^0 \times \beta \\ \left[0,0,0\right]^T \end{matrix}\right] & \\ \end{array} \\ $$
My question
I want to compute the roll, pitch, yaw accelerations for the second-order differential kinematics of a robot, stating:
$$\ddot{x} = J(q)\ddot{q} + \dot{J}\dot{q}$$
How do I proceed?
Do I approach it similarly to how the analytical jacobian is computed?
$$\Gamma = [r, p, y]^\intercal$$
$$A = \begin{bmatrix} 1 &0 &sin(p) \\ 0 &cos(r) &-cos(p) sin(r)\\ 0 &sin(r) &cos(p) cos(r) \end{bmatrix}$$
$$J_a(q) = \begin{bmatrix} I &0\\ 0 &A^{-1}(\Gamma) \end{bmatrix} J(q)$$
where instead of $J(q)$ I use $\dot{J}(q)$, and the same with $[r, p, y]^\intercal$, I use $[\dot{r}, \dot{p}, \dot{y}]^\intercal$ ?
I apologize if this is too elementary, but I am genuinely curious.
in ROS,where the angles retrieved are rpy
, again, those are angles of the end effector, not the joint angles, so again I'm still not seeing what the issue is there :( $\endgroup$