search

cogimon / rtt-gazebo-robot-sim

RTT-Gazebo base component for different robotic platforms
4 stars 4 forks source link

Torque Feedback bug #24

Open xwavex opened 7 years ago

xwavex commented 7 years ago

As @hsiuchin-lin correctly pointed out, it seems like we are ready the commanded torques as feedback currently. We should investigate this issue.

And probably revert the following commit: https://github.com/cogimon/rtt-gazebo-robot-sim/commit/b273c164a6ac9cea75c4fc83897cb0411146ea94

This would mean changing

full_feedback->joint_feedback.torques(i) = _model->GetJoint(_joint_names[i])->GetForce(0u);

back to 

gazebo::physics::JointWrench w = _model->GetJoint(_joint_names[i])->GetForceTorque(0u);
gazebo::math::Vector3 a = _model->GetJoint(_joint_names[i])->GetLocalAxis(0u);
full_feedback->joint_feedback.torques(i) = a.Dot(w.body1Torque);

Still, we might check if we need to use body1Torque, body2Torque, or the mean of both...

http://answers.gazebosim.org/question/15987/understanding-gazebophysicsjointgetforce-return-wrench/