advices on the implement of admittance control of each joint
Hi all, I want to implement the admittance control in joint space and test it in gazebo.
My robot has 5 revolute joints, using the controller position_controllers/JointGroupPositionController. I added 5 force/torque sensors to these joints using the gazebo plugin libgazebo_ros_ft_sensor.so, from which I can get the overall joint torque t_total every inerval. Using the function getTorques(q,qd,qdd,w,t) in moveit dynamic_solver to get the required torque t_dy for the movement. Then the external force acted on each joint is t_ext = t_total - t_dy. Using the admittance control equation M*xdd+D*xd+K*x=t_ext to solve the offset x of each joint. Add this x to desired q, then send the actual q to topic /arm_position_controller/commandto let the robot move.
I'm not sure whether the way to get t_ext is right or the ft_sensor gives not reliable torque, I can't get desired behavior of the robot.
@xibeisiber Would you mind letting us know about how you solved the problem? I have faced the same issue as well. One thing that I can say, is that if the force sensor is at the wrist and we have another link after that, we need to consider the effect of that link on the force sensor, which would have two parts:
Otherwise the force control method would consider these forces as external forces, which makes strange behaviour in the object.
unfortunately. I think the function
getTorques(q,qd,qdd,w,t)should already consider the gravity and acceleration from the links after the sensor. As far as I can remember, the sensor seems to give quite noisy results, therefore the reliable estimation of external force is not so straightforward..