search

ARC-OPT / wbc

C++ library for optimization-based control of redundant robots
https://arc-opt.github.io/Documentation/
BSD 3-Clause "New" or "Revised" License
35 stars 13 forks source link

Problem with subsequent calls of e.g., spaceJacobian() #32

Open dmronga opened 2 years ago

dmronga commented 2 years ago

Two subsequent calls of the same jacobian method, e.g. spaceJacobian may lead to problems. E.g. in the following code snippet

cout<<robot_model.spaceJacobian("kuka_lbr_l_link_1","kuka_lbr_l_tcp")*robot_model.spaceJacobian("kuka_lbr_l_link_3","kuka_lbr_l_tcp")<<endl;
base::MatrixXd jac_1 = robot_model.spaceJacobian("kuka_lbr_l_link_1","kuka_lbr_l_tcp");
base::MatrixXd jac_2 = robot_model.spaceJacobian("kuka_lbr_l_link_3","kuka_lbr_l_tcp");
cout<<jac_1*jac_2<<endl<<endl;
cout<<"............."<<endl;

Gives the following result:

           0            0  2.22547e-30 -6.56036e-13     0.318285  3.25555e-13     0.318285
           0            0 -3.67039e-18     0.152322  1.56571e-12   -0.0364877  1.80818e-12
           0            0 -1.33528e-29 -9.09349e-13     0.352262  3.92098e-13     0.352262
           0            0     0.261653     0.741985   -0.0626773    -0.741985 -0.000296794
           0            0  6.23216e-13 -5.04035e-12       1.4124  3.59833e-12       1.4124
           0            0    -0.236415      -1.0607    0.0566318     0.763907  0.000268167

           0            0  2.22547e-30 -6.56036e-13     0.318285  3.25555e-13     0.318285
           0            0 -3.67039e-18     0.152322  1.56571e-12   -0.0364877  1.80818e-12
           0            0 -1.33528e-29 -9.09349e-13     0.352262  3.92098e-13     0.352262
           0            0     0.261653     0.741985   -0.0626773    -0.741985  8.12363e-13
           0            0  6.23216e-13 -5.04035e-12       1.4124  3.59833e-12       1.4124
           0            0    -0.236415      -1.0607    0.0566318     0.763907 -8.10076e-12

.............

i.e., the bottom rightmost entries are not identical. Initially, the problem was that references on the same matrix were returned. But even after introducing maps that store each matrix internally, the problem persists.