I am trying to implement an impedance control, but for now only in free-space (to test if it works).
How it works:
- Find Xm (impedance model position)
- 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
- The calculated wrench is feed into
force_modewhich 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