Update links/attached bodies only after finishing update to the joint configuration

[mpomarlan]

Currently, the setf joint method in the bullet-reasoning package calls update-link. This is understandable, since a joint may be updated independently of others, but it does have an unfortunate consequence: if the robot is holding something, then updating the arm configuration with something like moving-tcp will result in the held object doing an unreasonable ballet around the robot, because every time one link is updated, so is the rest of the arm and the attached objects.

This is especially visually annoying when the robot holds a large object like a tray. (Functionally annoying in my branch also, as I'm implementing some proper support testing for carrying things on trays, and it doesn't like it at all when the tray flips over during a partial arm configuration update.)

[mpomarlan]

Proposed solution (being tested in my branch):

I defined a setf-joint-state-internal function which only makes the sanity checks (the named joint actually exists) and sets the joint value, WITHOUT updating child links or attached bodies.

The btr::set-robot-state-from-joints function now calls setf-joint-state-internal (instead of (setf joint-state)), and, when all joints have been set, calls (setf (pose robot) (pose robot)) to indirectly call update-link-poses, which results in the attached bodies and child links being properly positioned "in one go", without a weird dance of the held object.