End-effector Force Control behaviour Issue

[jaygnu]

Description

We have a question about End-effector Force Control behaviour on our Kinova Jaco2 j2n6s300 arm & noetic-devel kinova-ros.

Our experiment set up is quite simple, we have a small box (180gms) gripped by the arm. We apply a constant end-effector force in the +z direction by continuously pumping kinova_msgs.msg.CartesianForce messages into the *_driver/in/cartesian_force topic in torque_control mode. Our expectation was that the box would be lifted up with a constant acceleration/linearly increasing velocity with position changes only in +z axis all the way till the arm periphery is reached as long as the applied force is larger than what is required to overcome weight of the box 1.8N.

However we found the behaviour different in the following ways and was wondering if you could provide some guidance. Attached pdfs a few graphs of expected (from a basic simulation) and actual trajectories with the arm

1. The position changes (from any initial state) reaches a steady state instead of increasing continuously till the force applied ends.
2. While the acceleration and velocity of the end-effector increases for higher values of forces (as expected), but they quickly come back down to 0 even though the force continues to be applied
3. Although we are applying force only in the +z direction, we can see motion and change in the end-effector force values (measured from *_driver/out/tool_wrench.force) in x,y & z direction
4. On applying cartesian_force [0, 0, +ft, 0, 0, 0], via cartesian_force topic we were expecting the current *_driver/out/tool_wrench.force values to be increased by approximately the same amount. But we could not find any discernible relation b/w those.

Just wondering if you could help us understand this behaviour or if it is a bug; perhaps it is something basic that we are overlooking ?

The code we use is very similar to the publishForceCmd method from

kinova-ros/kinova_demo/nodes/kinova_demo/robot_control_modules.py

Line 166 in 7b5cd2f

def publishForceCmd(force_cmds, duration_sec, prefix):

Expected Vs Actual Behaviour

Please refer the pdf

expected_vs_actual_behaviour.pdf

Thanks in advance

Version

ROS distribution : noetic-devel

Branch and commit you are using : https://github.com/Kinovarobotics/kinova-ros/tree/noetic-devel

[felixmaisonneuve]

Hi @jaygnu,

Have you tried applying a force against the gripper to see if the arm is "pushing" at the requested force?
If you send a command of 2.5N, the arm should move up if your payload generate a force less then that, the arm should stop when it faces a force equal to 2.5N (e.g. if you push down on the arm) and the arm should go back down you push harder. For those two last cases, you should read a 2.5N force in *_driver/out/tool_wrench.

For the first case, where the cartesian_force is bigger then the force applied on the arm (which is your case), the arm should accelerate in Z. So joint torques should increase, trying to reach their target force. However, this will stop once the arm reaches its velocity limit. Once the arm reaches its limit, the velocity of the arm should become constant and tool_wrench should drop to almost 0, because there is no more acceleration on the arm.

I am not a wizard on this, so I might be wrong, but I am pretty sure what you are seeing is normal behaviour.

Also, the Jaco2 arm is not very precise, so you might notice a small movement (couple cm) on x and y axis even if your command is only on the z axis

Best,
Felix

[jaygnu]

Hi @felixmaisonneuve,

Thanks very much for the reply. As you suggested, we performed some tests and it appears the arm is pushing close (though not exact) to the requested force value. Details in pdf attached. The payload value is 3.2N. If the requested force is greater than the payload, then the arm goes up and if it is less, it won't, as expected.

However, the main question is if the requested force is greater than the payload, why would the velocity (and acceleration) drop to zero ? e.g., at time step 40, for forces 4.0N & 4.9N in the graphs. In both cases the wrench does show that there is an applied force that is close to the requested force, but the velocity has dropped to zero (and the position is constant). It also appears from the graph that the velocity and acceleration limits are not reached, since a slightly larger force leads to larger velocity and acceleration, hence the question. As you suggested, we were expecting "the velocity of the arm should become constant" too. But instead it drops to zero and the arm is motionless in floating mode although there is an upward force as per the /tool_wrench

While digging a bit more on the reasons for behaviour, we also noticed that even when no payload is applied (and no force is requested), the end effector (/tool_wrench) still shows values around 1 to 3N (on all axis) usually and it varies based on the position/orientation of the arm. Shouldn't it be close to zero ? In addition, we noted that X-Y cartesian coordinates are not as per what is listed below, it seems to be interchanged. Would that have an impact ?

kinova-ros/README.md

Line 171 in a9e3c80

- +x axis is directing to the left when facing the base panel (where power switch and cable socket locate).

Thanks
test.pdf

[felixmaisonneuve]

If your arm reaches its force limit (4.0N and 4.9N), I expect it to stop moving, like your graphs show. This is something I would expect to happen when your payload is too heavy.
You say your tool_wrench is around 1N to 3N without any payload. This is probably because your torques sensors are offset (this happens normally after some time). You can reset your torque offset using Development Center in . Procedure is explained in the user guide, but you basicaly have to put your arm in a candle-like position, then click the "Apply to all" button for the section in Development Center.
In a candle-like position, all your joint torques should be close to 0N. This is probably not your case currently.

I don't think your axis are inverted, the readme file is confusing (it might also be wrong, I will have to check), but even if your axis were inverted, I don't think it would have an impact.

[jaygnu]

Hi @felixmaisonneuve, Thanks once again for your patient answers.

Would you be able to elaborate a bit on your statement "If your arm reaches its force limit (4.0N and 4.9N), I expect it to stop moving. .... payload is too heavy.". The suspended weight is just 320gms.

Prior to our tests, we had tried re-calibration of the torque sensors using the ROS/README a few times. What we had observed is that in candle position, the torques can be made pretty close to zero, but on moving the arm to different positions, the values change to 1N to 3N even without any payload.
https://github.com/Kinovarobotics/kinova-ros/tree/noetic-devel#re-calibrate-torque-sensors

Though, we want to try using the Development Center you suggested for re-calibration, which might be helpful to check the axis as well. In the portal Resources/Software I can see Gen 2 SDK v1.5.1 and the Gen2 SDK user guide. Is that the right option for Kinova Jaco2 j2n6s300 arm ? Also it appears SDK latest integration is with Ubuntu 16.04. Does it work on later versions of ubuntu ?

Cheers

[felixmaisonneuve]

Can you compare _driver/out/tool_wrench.force with the cartesian_force you send to the arm?
If the force read in tool_wrench is close to what you sent, the arm won't move.

Keep in mind the Jaco2 arm is not precise in any way and it was not designed to do some advanced torque control movements. The torque readings and tracking (when the arm is in movement) might not be accurate. Because of that, it is normal to see tool_wrench.force and motion in all direction even if your command is only in +z. Altough it should not be moving forward, it should only be a relatively small offset on x and y axis.

You have to make sure the cartesian_force you send is big enough to compensate for that if you want the arm to move.

Also, when the arm is not in a candle position, it is normal to see torque values on joints, because they apply a torque to keep the arm in place. It is also possible to see a small force/wrench on the end-effector even no force is applied because of the sensors imprecision.

Development Center comes with the Gen 2 SDK v1.5.1. It can be used for any configuratio of Jaco arm and it is also working with later versions of Ubuntu as well (I have not tried it on Ubuntu22.04 yet). The installation procedure is the same, whatever you Ubuntu version.

Gen 2 SDK v1.5.1 also comes with the TorqueConsole. You can use this application to execute some features related to torque. You can use deveopmenet center to monitor quite a few things while your are playing with the arm.

Be carefull however, you cannot use ROS at the same time as DevCenter or Torque

[Leong1230]

Hi, I face the problem that the robot does not move using publishForceCmd(force_cmds, duration_sec, prefix) by setting the force_cmds to even 20N without playload. I have calibrated the torque sensors using gravity_compensated_mode.py (but the robot can not be manually moved through this code) and then moved the robot to the home position. The robot arm can not be moved using the admittance control (rosservice call /...._driver/in/start_force_control) either. It seems that force/torque control does not work on my robot.

I succeed in connecting the robot by setting the robotType=j2n6s300, so I think the robot is Jaco 2 instead of Jaco 1, right?
Could you please help with the problem?

[martinleroux]

Indeed, this means you have a Jaco2 robot.

Very old units may not have force control enabled. You can validate this by trying to run the Torque Console which is included with the Kinova SDK.

[Leong1230]

Hi Martin,

Thank you for your reply!

I have tried the Torque Console but nothing happened after I clicked 'Switch Torque Control'. The robot stays stiff. I can get "Good connection", does this indicate that the robot has force control enabled? If not, how could I check wether the robot supports this function? What should I supposed to see in the interface to indicate the robot has succeeded in turning into torque control mode? And in the terminal console, I got the same results as Mik in the terminal where I started the Torque Console here.

QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerActive_clicked()
QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerNotActive_clicked()
QMetaObject::connectSlotsByName: No matching signal for on_Home_pressed()
QMetaObject::connectSlotsByName: No matching signal for on_SetAngular_pressed()
QMetaObject::connectSlotsByName: No matching signal for on_SetCartesian_pressed()
QMetaObject::connectSlotsByName: No matching signal for on_SendActuatorGainDamping_pressed()
QMetaObject::connectSlotsByName: No matching signal for on_groupBox_3_toggled(bool)
QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerActive_pressed()
QMetaObject::connectSlotsByName: No matching signal for on_VibrationControllerNotActive_pressed()

Also, when I launched the demo in the Admittance module in the Development Center, the robot can not move either.

[martinleroux]

Hi Leong,

This may suggest that your robot is in assistive mode instead of service mode, so torque control may very well be disabled.

Please contact us at [email protected], we will share with you a procedure to enable it. To go faster through triage, in your e-mail you can mention discussing with me (Martin) on Github and add a link to this issue. I will get back to you as quickly as possible.

Cheers,
Martin