Something wrong when return contact frame and contact force

Hi all,
Recently, I am working on grasping. There are 4 fingers in the hand, and the object is a cylinder. Figure 0 is along the +x axis, figure 1 is along the +y axis, figure 2 is along the -x axis, and figure 3 is along the -y axis. The scenario is shown by the following image.


I want to return the contact frames and contact forces between 4 fingers and the object.
Currently, I use contact->frame to return the contact frame, and use mj_contactForce() to return the 6D contact forces in the corresponding contact frames.
What I expect is: the contact frames follow some rules (such as: z direction for all frames point to the object or something). Also, for contact forces, when I look them from global coordinate, I expect them to be symmetric.

However, the result is quite different with my expectation.
First, the contact frames seems random (which is shown in below result). This might not be a big problem. The problem is contact forces. Based on returned contact frames, the contact forces are impossible to be symmetric in global coordinate. And all contact forces have big value along the local x axis.

Did I use the wrong function to find contact frame and contact force?

Thanks!


The results are shown below:

Code:

Contact frame for finger 0:
-0.99999654 0.00231421 0.00125196
0.00231421 0.99999732 -0.00000290
-0.00125196 0.00000000 -0.99999922

Contact force for finger 0:
4.55506608
-0.02022803
-1.67575286
0.00000000
0.00000000
0.00000000


Contact frame for finger 1:
0.00936841 -0.99995610 -0.00015060
0.00000141 -0.00015059 0.99999999
-0.99995612 -0.00936841 -0.00000000

Contact force for finger 1:
4.37781804
0.90673421
0.05835685
0.00000000
0.00000000
0.00000000


Contact frame for finger 2:
0.99992353 0.01230330 -0.00124658
-0.01230329 0.99992431 0.00001534
0.00124668 0.00000000 0.99999922

Contact force for finger 2:
4.57540128
-0.05646759
1.68250531
0.00000000
0.00000000
0.00000000


Contact frame for finger 3:
0.00602381 0.99998185 0.00013453
-0.00000081 -0.00013453 0.99999999
0.99998186 -0.00602381 0.00000000

Contact force for finger 3:
4.38785709
0.88563866
-0.03452370
0.00000000
0.00000000
0.00000000

The concrete code is shown in the following:

Code:

mjtNum* f0_contact_pos = mj_stackAlloc(d, 3);//contact position in unkown frame. check
    mjtNum* f1_contact_pos = mj_stackAlloc(d, 3);
    mjtNum* f2_contact_pos = mj_stackAlloc(d, 3);
    mjtNum* f3_contact_pos = mj_stackAlloc(d, 3);

    mjtNum* f0_contact_frame = mj_stackAlloc(d, 9); // Fan, test whether it's contact frame coordinates in world frame, check
    mjtNum* f1_contact_frame = mj_stackAlloc(d, 9);
    mjtNum* f2_contact_frame = mj_stackAlloc(d, 9);
    mjtNum* f3_contact_frame = mj_stackAlloc(d, 9);

    mjtNum* f0_contact_force = mj_stackAlloc(d, 6);//contact force in contact frame
    mjtNum* f1_contact_force = mj_stackAlloc(d, 6);
    mjtNum* f2_contact_force = mj_stackAlloc(d, 6);
    mjtNum* f3_contact_force = mj_stackAlloc(d, 6);

    for (int i = 0; i < d->ncon; i++)
    {
        mjContact* cur_contact = &((d->contact)[i]);
        if (!strcmp(mj_id2name(m, 1, cur_contact->geom1), "object") || !strcmp(mj_id2name(m, 1, cur_contact->geom2), "object"))
        {
            std::cout << '\n' << mj_id2name(m, 1, cur_contact->geom1) << "   contact with     " << mj_id2name(m, 1, cur_contact->geom2) << '\n';//normal
            if (!strcmp(mj_id2name(m, 1, cur_contact->geom1), "finger0_dist") || !strcmp(mj_id2name(m, 1, cur_contact->geom2), "finger0_dist")){
                //calculate position/force/normal of contact points.
                mju_copy(f0_contact_pos, cur_contact->pos, 3);
                mju_copy(f0_contact_frame, cur_contact->frame, 9);
                mj_contactForce(m, d, i, f0_contact_force);// extract 6D force:torque in contact frame//Fan, test id!
                //cout test:
                std::cout << "Contact Position for finger 0: " << '\n';
                mju_printMat(f0_contact_pos, 3, 1);
                std::cout << "Contact frame for finger 0:    " << '\n';
                mju_printMat(f0_contact_frame, 3, 3);
                std::cout << "Contact force for finger 0:    " << '\n';
                mju_printMat(f0_contact_force, 6, 1);
            }
               
            else if (!strcmp(mj_id2name(m, 1, cur_contact->geom1), "finger1_dist") || !strcmp(mj_id2name(m, 1, cur_contact->geom2), "finger1_dist")){
                mju_copy(f1_contact_pos, cur_contact->pos, 3);
                mju_copy(f1_contact_frame, cur_contact->frame, 9);
                mj_contactForce(m, d, i, f1_contact_force);// extract 6D force:torque in contact frame//Fan, test id!
                //cout test:
                std::cout << "Contact Position for finger 1: " << '\n';
                mju_printMat(f1_contact_pos, 3, 1);
                std::cout << "Contact frame for finger 1:    " << '\n';
                mju_printMat(f1_contact_frame, 3, 3);
                std::cout << "Contact force for finger 1:    " << '\n';
                mju_printMat(f1_contact_force, 6, 1);
            }
            else if (!strcmp(mj_id2name(m, 1, cur_contact->geom1), "finger2_dist") || !strcmp(mj_id2name(m, 1, cur_contact->geom2), "finger2_dist")){
                mju_copy(f2_contact_pos, cur_contact->pos, 3);
                mju_copy(f2_contact_frame, cur_contact->frame, 9);
                mj_contactForce(m, d, i, f2_contact_force);// extract 6D force:torque in contact frame//Fan, test id!
               
                //cout test:
                std::cout << "Contact Position for finger 2: " << '\n';
                mju_printMat(f2_contact_pos, 3, 1);
                std::cout << "Contact frame for finger 2:    " << '\n';
                mju_printMat(f2_contact_frame, 3, 3);
                std::cout << "Contact force for finger 2:    " << '\n';
                mju_printMat(f2_contact_force, 6, 1);
            }
            else if (!strcmp(mj_id2name(m, 1, cur_contact->geom1), "finger3_dist") || !strcmp(mj_id2name(m, 1, cur_contact->geom2), "finger3_dist")){
                mju_copy(f3_contact_pos, cur_contact->pos, 3);
                mju_copy(f3_contact_frame, cur_contact->frame, 9);
                mj_contactForce(m, d, i, f3_contact_force);// extract 6D force:torque in contact frame//Fan, test id!
               
                //cout test:
                std::cout << "Contact Position for finger 3: " << '\n';
                mju_printMat(f3_contact_pos, 3, 1);
                std::cout << "Contact frame for finger 3:    " << '\n';
                mju_printMat(f3_contact_frame, 3, 3);
                std::cout << "Contact force for finger 3:    " << '\n';
                mju_printMat(f3_contact_force, 6, 1);
            }
           
        }//if one geom is object

    }//for i = 1:ncon

 

Thank you so much!

The transpose thing can explain everything. Now the forces are symmetric and x-axis points to the object.

Sorry I didn't understand the comment behind frame[9] (i.e. normal is in [0-2]). But now I am clear.


Thanks again!
Best regards!

 

That would be very nice.

Thanks a lot!