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.

edit retag close merge delete

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..