search

ShuoYangRobotics / legged-kalman-filter

GNU General Public License v3.0
25 stars 3 forks source link

Error in kinematics #3

Open Hilton-Marques opened 2 months ago

Hilton-Marques commented 2 months ago

Hi, I am comparing the kinematics function provided in the code with the one provided by a1.urdf (attached). For example, using the following joint values: 

q << 0.03825680613517761 , 0.22205634117126466 , -0.51015815734863279 ,
            -0.064267051219940183 , 0.21205172538757325 , -0.4968580722808838, 
             0.037793239951133727 , 0.23021507263183594 , -0.50618834495544429, 
            -0.036705967783927915 , 0.22857995033264161 , -0.50719976425170898;

I got the following results for the foot contact using your source code 0.13425 0.138573 -0.201489 0.136302 -0.143812 -0.199491 -0.228419 0.138466 -0.201147 -0.228085 -0.13825 -0.201325 While pinocchio with the urdf provided me with 0.1933 0.1455 -0.3834 0.1946 -0.1555 -0.3813 -0.1716 0.1454 -0.3837 -0.1708 -0.1449 -0.3837 Did you notice the difference, am I missing something? Interestingly, however, when I got the values for the calf joints with pinocchio, they were quite close to those in your source code: 0.1365 0.1382 -0.1917 0.1384 -0.1432 -0.1897 -0.2261 0.1381 -0.1914 -0.2258 -0.1379 -0.1916 a1.zip

Hilton-Marques commented 2 months ago

After some hours of search, I found here that the parameter rho_opt, instead of zero, should be 0.20. This also suggests that the initial height of the robot, instead of 0.15 should be 0.35. I recommend the following fixes in the code: (1) first rho_opt << 0.20, 0.20, 0.20; (2) second Eigen::Vector3d(0,0,0.35)