Put simply, I think my robot arm is not making an effort to get to the "desired_state".
Observed behavior: The "generalized_force" output of my InverseDynamicsController is not as expected, leading the robot arm to follow a seemingly random trajectory (visually similar to falling as though there is no gravity compensation).
Expected behavior: I am fixing the robot arm's "desired_state" and setting a zero "desired_acceleration", so I expect it to hold its initial position.
I have connected an InverseDynamicsController to a HardwareStation. Currently, I am simply fixing the InverseDynamicsController "desired_state" input port to the starting positions of the robot with zero velocities, and the "desired_acceleration" input port to 0. Therefore, I expect the robot will maintain its position.
I am confident that my diagram is correctly connected; the "generalized_force" output of the InverseDynamicsController is connected to the "actuation" port of my HardwareStation. I've also attached the diagram.
I've narrowed down the problem to being with InverseDynamics's CalcOutputForce() function. I've validated the HardwareStation's "estimated_state", PidController output, and Adder output, are all good. But the output of the InverseDynamics comes out weird.
The robot arm is attached to a mobile base that is joint locked (but the behavior is the same even if I weld the mobile base to the world). Otherwise, there are no other external forces on the arm, so I would expect InverseDynamics to just need to compensate for gravity.
Anyone have pointers on what could be the problem with InverseDynamics? I've given MultibodyTree<T>::CalcInverseDynamics() a read... but not with much success.
My code is also all open here.
I was able to get some code from the OP and we found the root cause of the problems: the actuators were added in a different order from the joints. This case was not handled properly yet by the InverseDynamics method.
I've opened a PR to resolve it.
(As a short-term workaround, it's simple enough to shuffle the order of the actuators in the urdf).
