Problem in foot contact plugin

[Infas12]

In foot_contact_plugin.cc:

                for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j){                 
                    // std::cout << i <<" "<< contacts.contact(i).position_size() <<" Force:"
                    //           << contacts.contact(i).wrench(j).body_1_wrench().force().x() << " "
                    //           << contacts.contact(i).wrench(j).body_1_wrench().force().y() << " "
                    //           << contacts.contact(i).wrench(j).body_1_wrench().force().z() << "\n";
                    Fx += contacts.contact(i).wrench(0).body_1_wrench().force().x(); // Notice: the force is in local coordinate, not in world or base coordnate.
                    Fy += contacts.contact(i).wrench(0).body_1_wrench().force().y();
                    Fz += contacts.contact(i).wrench(0).body_1_wrench().force().z();
                }

Seems like components of wrench(0) are being added to F for multiple times. Maybe this should be wrench(j)? I understand that the number of contacts should be exactly one, but still this is weird...

[xyq1223]

Hello,do you have better results after changing wrench(0) to wrench(j)?If you get better results, you're on the right track.