repeated service calls cause strange behavior, publication of joint angle updates compromised?

edit

Hi.

Using Linux 14.04, ROS Indigo. I am implementing a force controller loop that also consists of an inner position control loop.

My set-up is as follows: - I have tried asyncspinner/multithreaded spinner and just spinner to subscribe to Baxter's Joint States to get joint angle positions, velocities, torques, and gravitational torques. Optionally I subscribe to the endpoint wrench (and may also re-publish a low-pass filtered endpoint wrench). I also publish new joint angle reference positions to /robot/limb/side/JointCommand.

The force controller computes the error either in the force or the moment of the endpoint and uses the Jacobian Transpose to compute a joint angle update. This update is added to the current angles and then fed into the position controller. The position controller runs until the update is achieved.

The force controller is activated through a service call that can be done manually or in code.

Up to this point everything works fine. However, I am looking for reactivity in the force controller. I am seeking a behavior, whereby if the endpoint is pushed the wrist should respond quickly and adjust to achieve the set-point.

To this, I can either put a while loop around the force_controller and ask it to continue to run as long as the error has not been minimized beyond a threshold. Or equivalently, I could make cyclical calls with the client service through a while loop.

However, in my experience as soon as I do this, the arm does not move to the reference joint angles computed by the position_controller and invariably (no matter what output, technique, or setup options) always moves towards a straight arm configuration bit-by-bit.

I should clarify, if I I issue service calls manually, the force controller moves the end-effector in the expected direction. So this manual method works. But as soon as I put it in a while loop whether I while the service client, or while the server side, I get the strange behavior. Sleep times and nothing changes.

I have debugged over and over to make sure that the correct joint angle references are published to joint command (which for the baxter robot are fed directly in the control boards and moves the robot) and I believe they are. In fact, when the while loop is removed around the force control, and I do this manually, the program behaves well, it just lacks reactivity.

Update: I have further tested by creating a wrapper node around a client instantiation and call. That is, I run a while loop around a function in which ros::init, a new ros node, a new service client object, and a new client call are all made, and then upon the function's exit they are killed. I too have tried changing sleep times around the call of the function in the while loop but same effect.

The client code can be seen here: https://github.com/birlrobotics/birl_...

The server code can be seen here: https://github.com ... (more)