search

robotology-legacy / mex-wholebodymodel

Matlab MEX interface to the iWholeBodyModel interface.
11 stars 9 forks source link

wbm_jacobian retrieves always the CoM jacobian when called with non-existent frames #79

Closed gabrielenava closed 7 years ago

gabrielenava commented 7 years ago

As in the title. Tested in WBM-class branch and updateRepo branch. This is the CoM jacobian (e.g., for Walkman):

wbm_jacobian('com')

ans =

  Columns 1 through 12

    1.0000   -0.0000   -0.0000    0.0000   -0.0801    0.0107    0.0638    0.0031   -0.0007   -0.0068    0.0011    0.0002
   -0.0000    1.0000   -0.0000    0.0801   -0.0000    0.0142   -0.0086    0.0495   -0.0120    0.0008    0.0095   -0.0027
   -0.0000   -0.0000    1.0000   -0.0107   -0.0142    0.0000   -0.0163   -0.0138    0.0033   -0.0053    0.0028   -0.0003
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0

  Columns 13 through 24

    0.0015   -0.0000   -0.0070    0.0001    0.0005    0.0016   -0.0000    0.0463   -0.0008   -0.0001    0.0164   -0.0006
   -0.0002    0.0000   -0.0026   -0.0069    0.0021    0.0013   -0.0000   -0.0009    0.0493   -0.0043    0.0000    0.0000
    0.0027   -0.0000   -0.0044    0.0072   -0.0016    0.0023   -0.0000    0.0068    0.0022   -0.0002   -0.0007   -0.0009
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0

  Columns 25 through 31

   -0.0000    0.0476   -0.0035    0.0006    0.0149   -0.0009   -0.0000
    0.0005   -0.0026   -0.0439    0.0090   -0.0033   -0.0001   -0.0005
    0.0000    0.0014    0.0158   -0.0032   -0.0059   -0.0006    0.0002
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0

and this is the POTATOES jacobian:

>> wbm_jacobian('potatoes')

ans =

  Columns 1 through 12

    1.0000   -0.0000   -0.0000    0.0000   -0.0801    0.0107    0.0638    0.0031   -0.0007   -0.0068    0.0011    0.0002
   -0.0000    1.0000   -0.0000    0.0801   -0.0000    0.0142   -0.0086    0.0495   -0.0120    0.0008    0.0095   -0.0027
   -0.0000   -0.0000    1.0000   -0.0107   -0.0142    0.0000   -0.0163   -0.0138    0.0033   -0.0053    0.0028   -0.0003
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0

  Columns 13 through 24

    0.0015   -0.0000   -0.0070    0.0001    0.0005    0.0016   -0.0000    0.0463   -0.0008   -0.0001    0.0164   -0.0006
   -0.0002    0.0000   -0.0026   -0.0069    0.0021    0.0013   -0.0000   -0.0009    0.0493   -0.0043    0.0000    0.0000
    0.0027   -0.0000   -0.0044    0.0072   -0.0016    0.0023   -0.0000    0.0068    0.0022   -0.0002   -0.0007   -0.0009
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0
         0         0         0         0         0         0         0         0         0         0         0         0

  Columns 25 through 31

   -0.0000    0.0476   -0.0035    0.0006    0.0149   -0.0009   -0.0000
    0.0005   -0.0026   -0.0439    0.0090   -0.0033   -0.0001   -0.0005
    0.0000    0.0014    0.0158   -0.0032   -0.0059   -0.0006    0.0002
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
         0         0         0         0         0         0         0
gabrielenava commented 7 years ago

Fixed. I noticed that the forward kinematics instead was working fine:

>> wbm_forwardKinematics('potatoes')
Error using mexWholeBodyModel
forwardKinematics call Link ID does not exist.

Error in wbm_forwardKinematics (line 25)
            vqT_lnk = mexWholeBodyModel('forward-kinematics', varargin{1});

so I copy-paste these code lines from modelforwardkinematics.cpp into modeljacobian.cpp:

  if (com.compare(refLnk) != 0) {
    if ( !robotModel->getFrameList().idToIndex(refLnk, refLnkID) ) {
      mexErrMsgIdAndTxt("MATLAB:mexatexit:invalidInputs", "forwardKinematics call Link ID does not exist.");
    }
  }

and this solved the issue:

>> wbm_jacobian('potatoes')
Error using mexWholeBodyModel
jacobian call Link ID does not exist.

Error in wbm_jacobian (line 22)
            wf_J_lnk = mexWholeBodyModel('jacobian', varargin{1});

>>

I will commit the changes in updateRepo branch and then close this issue.

Ganimed commented 7 years ago

Thanks for mentioning this issue! I will add these line of code to the remaining methods which using refLnk.

Ganimed commented 7 years ago

Bugfixed additionally the trasformationMatrix and the dJdq function.

Ganimed commented 7 years ago

@gabrielenava: Can you merge my newest changes into your repository? Thx.

gabrielenava commented 7 years ago

Done at https://github.com/robotology/mex-wholebodymodel/commit/bc92ab2157d7f42fd7672f68ceac180f7e8de5dd

traversaro commented 7 years ago

@gabrielenava Please do not close a bug until it is fixed in master (i.e. for all users), thanks.

gabrielenava commented 7 years ago

got it, I will close it after merging the repo then

Ganimed commented 7 years ago

:+1: