Hi Haojun,
Peter's toolbox and the Robotics System Toolbox in MATLAB make slightly different assumptions how the inertia matrix is specified.
- In MATLAB's Robotics System Toolbox, the Inertia property of the rigidBody object represents the inertia tensor relative to the link frame. This it the convention that is used in Featherstone's Rigid Body Dynamics book.
- Peter's toolbox assumes that the I property is the inertia tensor relative to the center of mass (CoM) of the link, but with axes aligned with the link frame. This assumption is also used in URDF.
Given that, you can convert between the two representations with a similarity transform for the rotation and tensor generalization of the parallel axis theorem to account for the translation. The details of the conversion are described in this answer, but the short version is that in your method2_Transformation script, you can do the following to compute the Inertia property from the values you used with Peter's toolbox. Here is an example for the first body:
I_rb = L1.I - L1.m*skew(L1.r)^2;
body1.Inertia = [I_rb(1,1), I_rb(2,2), I_rb(3,3), I_rb(2,3), I_rb(1,3), I_rb(1,2)]; % order is [Ixx Iyy Izz Iyz Ixz Ixy]
Here, L1 is the Link object that is defined in method1_DH, L1.m is the link's mass, L1.r is the position of the link's CoM relative to its frame, and skew is the function in Peter's toolbox that converts the CoM position vector into a skew-symmetric matrix.
If you do the same calculation for all 4 bodies the torque outputs between the 2 different scripts will match.See the attached modified script.
The new edition of Peter's book actually calls out this discrepancy between the toolboxes and we standardize on assumption (1).
