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_baxter/blob/master/controllers/force_controller/src/force_control_client.cpp
The server code can be seen here: https://github.com/birlrobotics/birl_baxter/blob/master/controllers/force_controller/src/controller.cpp
This is as close I think as it gets to calling the service manually, but still the same behavior. I think there is definitely something strange in the way that the service behaves.
I wonder if anyone else has come up with this kind of problem or if you have any insights into it.
Update 2: I tried a publisher-subscribe method and I got the same behavior. This led me to believe that the problem did not lie with the communication type but with other timing.
After further examination in my code I found a logic error between the way the force and position control loops communicate with each other!! Thanks
Post score: 0
Comment by gvdhoorn on 2015-11-17:
From your description it sounds like this could be Baxter related. Have you sent a message to the Baxter research google group?
Comment by Juan on 2015-11-17:
Yes. I wonder if this is a service related question.
How does the service communication work when multiple client calls are made? There is no queue not seems in case of network latency.
Or any problems publishing from within a serviced call through a multithreaded spinner?