Measuring force of tip using Jacobian function in KDL
Hi, I understand from here that you can use the Jacobian function from the KDL library to calculate a the torque to input for a desired force at the tip. i.e. Torque = Jac(j,i)*Force
However, does anyone know a way to do the opposite, to measure the actual force exerted at the end tip by reading the respective measured joint torques. i.e. Measured Force = [some function?]*measured torque at joints.
I tried using the pr2_mechanism_model::JointState to get the measured effort of each joint and use back the Jacobian Transform to get the measured force,
for (unsigned int i = 0 ; i < 6 ; i++)
{
F_measured_(i) = 0;
for (unsigned int j = 0 ; j < kdl_chain_.getNrOfJoints() ; j++)
F_measured_ += J_(i,j) * tau_measured_(j);
}
However, when I can't compile it as the type for the JointState's measured force(double) is different from KDL::Wrench.
In member function ‘virtual void my_controller_ns::MyCartControllerClass::update()’:
/home/c/fuerte_workspace/sandbox/my_controller_pkg/src/my_cart_controller_file.cpp:112:47: error: no match for ‘operator+=’ in ‘((my_controller_ns::MyCartControllerClass*)this)->my_controller_ns::MyCartControllerClass::F_measured_ += (((my_controller_ns::MyCartControllerClass*)this)->my_controller_ns::MyCartControllerClass::J_.KDL::Jacobian::operator()(i, j) * ((my_controller_ns::MyCartControllerClass*)this)->my_controller_ns::MyCartControllerClass::tau_measured_.KDL::JntArray::operator()(j, 0u))’
/home/c/fuerte_workspace/sandbox/my_controller_pkg/src/my_cart_controller_file.cpp:112:47: note: candidate is:
/opt/ros/fuerte/stacks/orocos_kinematics_dynamics/orocos_kdl/install_dir/include/kdl/frames.inl:209:9: note: KDL::Wrench& KDL::Wrench::operator+=(const KDL::Wrench&)
/opt/ros/fuerte/stacks/orocos_kinematics_dynamics/orocos_kdl/install_dir/include/kdl/frames.inl:209:9: note: no known conversion for argument 1 from ‘double’ to ‘const KDL::Wrench&’
Can anyone advise me on how do I go about measuring the force of the tip of pr2's arm using the Jacobian function in KDL? Thank you in advance.
EDIT: Thanks Peter, I appreciate any help as I am not very familiar with coding or robotics. Anyway, I realised I made a very silly mistake. The code should be
for (unsigned int i = 0 ; i < 6 ; i++)
{
F_measured_(i) = 0;
for (unsigned int j = 0 ; j < kdl_chain_.getNrOfJoints() ; j++)
F_measured_(i) += J_(i,j) * tau_measured_(j);
}
I forgot the I wrote "F_measured_" instead of "F_measured_(i)" earlier. It can compile now but not sure if it actually works. Will let you know my results afterwards.
Hello!! Could you please help me in calculating force of tip when you know joint torques?? Seems like you are trying to implement the same, What type is F_measured_(i) ,J_(i,j) ,tau_measured_(j) ?? I defined tau_measured_(j) as KDL::JntArray , J_(i,j) ad KDL::jacobian. What should I define F_measured_(i) as??