Universal Robots Forum

Control based on force_mode

I am trying to implement an impedance control, but for now only in free-space (to test if it works).

How it works:

  1. Find Xm (impedance model position)
  2. Based on Xm calculate the wrench which should cause the robot to move in a way which causes it to be close to the Xm
  3. The calculated wrench is feed into force_mode which suppose to cause it to reach the desired goal.

The issue I am having is that even though the Xm does reach the final goal (as can be seen on the graph) meaning I do obtain the required impedance model position. The wrench which is a function of xm doesn’t cause the robot to move in a similar manner to that of xm.
This can be seen in the above graph: Xm (impedance model), Xr(actual TCP position) , X0 (goal)

Question: Many control laws can be written in a form that creates a wrench and cause the robot to reach the desired goal position. I am just unsure where the issue might be, and why when I feed the wrench into force_mode it doesn’t cause the robot to move to the desired location. I think it is an issue with the robot and therefore any hints would help a lot