Publisher Node for Position Control

[Pranav186]

Hello !
I have been successful in establishing FRI using ROS. I am able to read joint_states and rostopic list gives me the topic /iiwa/TorqueController/command. I am trying to write a publisher node for this topic using python to be able to move the robot.
Following is the code-

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray

rospy.init_node('test_node')
pub=rospy.Publisher("/iiwa/TorqueController/command", Float64MultiArray, queue_size=10)

msg=Float64MultiArray()
msg.data = [0]*7

rate = rospy.Rate(10)
while not rospy.is_shutdown():
    pub.publish(msg)
    print("publishing")
    rate.sleep()

The robot does not move and the terminal output shows

publishing
publishing
publishing

I guess something is wrong with the message definition?

[costashatz]

/iiwa/TorqueController/command

In the title you say position control, but this topic is for torque control.

msg.data = [0]*7

You are sending zero torques and thus the robot should not move (KUKA automatically always adds a gravity compensation term).

To use the robot in position control you need to do the following:

• Alter this line to <arg name="controller" default="PositionController"/>
• When launching your sunrise app from the smartpad (see here for the steps to follow), make sure that you select PositionControl and a stiffness value that is bigger than zero (the bigger the stiffness, the more aggressive and more accurate the robot will be in following position commands)
• If you do the above things correctly, you should be able to see topic /iiwa/PositionController/command
• You can then send commands as you have in your python script (changing the topic name). Be careful to not send values too far away from the current joint configuration if you have big stiffness (>=200), as the robot will move very fast and there's possibility of breaking the robot.

Hope that this is helpful.

[Pranav186]

I followed the steps and the position control is working well ! Thanks !!

[costashatz]

Good to know!

[Pranav186]

Is there a way to publish angular velocity to the joints?

[costashatz]

Is there a way to publish angular velocity to the joints?

There's no velocity control through FRI for KUKA available (not in this repo, KUKA FRI does not have any interface for this).

[Pranav186]

I see. Thanks for the answer !

[Pranav186]

Hi @costashatz, as I wrote earlier, the position control is working well. However, I am having an issue with torque control.
I am able to see the topic /iiwa/TorqueController/command. Following is the code that I am using-

#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64MultiArray

rospy.init_node('test_node')
pub=rospy.Publisher("/iiwa/TorqueController/command", Float64MultiArray, queue_size=10)

msg=Float64MultiArray()
msg.data = [0,0,0,0,0,0,0.5]

rate = rospy.Rate(10)

while not rospy.is_shutdown():
    pub.publish(msg)
    print(msg)
    rate.sleep()

The last joint does not move and the terminal is printing msg as expected-

layout: 
  dim: []
  data_offset: 0
data: [0, 0, 0, 0, 0, 0, 0.5]

There are no errors in the roslaunch terminal
Does it have a minimum torque requirement or something?
I have set the stiffness to 20.

Also, it had worked earlier once and I think I did not make any changes to the code.

Let me know if you have an idea about this.
Thanks !!

[costashatz]

msg.data = [0,0,0,0,0,0,0.5]

0.5 Nm is a super super small torque. You need to give a bigger value to make the joint move. Try something like 5-10.

[costashatz]

I have set the stiffness to 20.

The stiffness in torque mode should be 0..

[JimmyDaSilva]

@Pranav186 Some guys from Disney made some experiments with torque control with the IIWA.
It seems there is a deadzone in the torque input. Meaning for the 1st axis at least, torques below 2Nm are not taken into account

Paper is here:
https://s3-us-west-1.amazonaws.com/disneyresearch/wp-content/uploads/20170613133705/Toward-Controlling-a-KUKA-LBR-IIWA-for-Interactive-Tracking-Paper.pdf

[Pranav186]

@costashatz , thanks for the answer. It worked with 1Nm and above.

@JimmyDaSilva , thanks for the link. The deadzone is clear in those graphs.

I will test the joints to determine the deadzones and work accordingly.

Thanks again for your answers !

[priyampnchl]

Regarding the position & torque control, I was wondering if the package allows for testing a command node (similar to the one posted by @Pranav186 earlier in the discussion) for Torque and Position command (by loading the correct controller at the time of launch using the "roslaunch iiwa_gazebo iiwa_gazebo.launch controller:="<controller_name>" command) using the Gazebo sim before testing on the real robot.

If you would rather I open a new issue, please let me know. Thanks!

[matthias-mayr]

for Torque and Position command

torque and position at the same time or either torque or position?
The latter is definitely possible.

[priyampnchl]

Yes, I meant either one, not both, at the same time.

When I run the Gazebo simulation with Torque controller, it initially starts drifting very slowly from the home (all joints at 0 degrees) and after one point starts flailing around randomly. It feels as if the gravity compensation is not quite right and is causing the robot to not be able to hold the position. (All this is with zero commanded torque, and after reading a few other discussions it is my understanding that the applied torque is applied as an overlay, on top of the gravity compensation and joint friction compensation. Is this the same with the gazebo model too?)

[matthias-mayr]

Do you have anything attached to the robot? That would not be accounted for in that case.

applied torque is applied as an overlay, on top of the gravity compensation and joint friction compensation

That is pretty much correct. In simulation there is no friction compensation. With the real robot, the KUKA cabinet applies the gravity compensation. In simulation it is this code:
https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_gazebo/src/gravity_compensation_hw_sim.cpp

I never tried to not send any task torques in simulation, but as long as nothing is attached, in theory it should hold still. Although it might be subject to small deviations and due to the lack of friction they could sum up.

[priyampnchl]

Do you have anything attached to the robot? That would not be accounted for in that case.

I do not have anything attached to the flange. Nor is anything on the simulated robot. I am wondering how accurate the mass & inertia values are in the URDF.

With the real robot, the KUKA cabinet applies the gravity compensation

In that case, when I initiate the node with zero commanded torques, or rather, no active node publishing to the command topic (I am assuming not having anything publishing to the command topic should have the same behaviour as publishing zero torques), the robot should hold its position, right? If this has been tested and verified to work 100%, I can test it on the robot.

The behaviour of our robot when running the torque command is somewhat similar to the one observed in this video posted on another issue except it is much faster and the Kuka cabinet brakes the robot roughly within one second of me tapping the stiffness value after selecting torque mode (presumably because it is hitting some velocity or acceleration limit). I should note that the bringup launch file loads an iiwa14 by default which I did not change and maybe that is what caused this behaviour, is this correct?

I also noticed some issue threads mentioning a calibration that needs to be done, possibly to calibrate the internal gravity compensation of the KUKA when something is attached to the end effector. Before reading that here, I was not aware of such functionality existing. It would be helpful if you could share some resource about how I can perform that calibration (as a just-in-case measure, even though I am sure we have never had anything attached to the flange since getting the robots and hence there is no reason for the robots to be un-calibrated in terms of the gravity compensation, and it should be exactly how it came from the factory, but even so, I would like to rule out this variable since we have recovered and restored the projects on the KUKA a few times. I am not sure if this can affect the gravity compensation since I think that and the system model is stored on the KUKA cabinet permanently).

Although it might be subject to small deviations and due to the lack of friction they could sum up.

It slowly builds up over the first 15 seconds (I need to zoom way into the tip of the robot in simulation and use the Z-axis line as a visual reference), and then it all goes haywire. I will share a screen-capture video when I am at the lab next.

[matthias-mayr]

I am wondering how accurate the mass & inertia values are in the URDF.

Since the simulation spawns a robot with the values that are stated in the URDF, this question does not really arise in a pure simulation setup.

The behaviour of our robot when running the torque command is somewhat similar to the one observed in #107 (comment) except it is much faster and the Kuka cabinet brakes the robot roughly within one second of me tapping the stiffness value after selecting torque mode (presumably because it is hitting some velocity or acceleration limit).

In this case my question would be if the robot setup is configured correctly, so the orientation and the tool setup if a tool is used.

An easy check that can be done is to check the external torques on the Smartpad. They should be close to 0nm in all configurations. Any external torque would make the robot move.

FYI, in the video they did not run full torque control, but the joint impedance controller with some stiffness to make the robot stand still, but have it somewhat compliant for kinesthetic teaching.

I should note that the bringup launch file loads an iiwa14 by default which I did not change and maybe that is what caused this behaviour, is this correct?

In general it makes sense to exclude such sources or errors, but in this specific case I would not know of any effect this should have on your behavior.

I think that and the system model is stored on the KUKA cabinet permanently

I am not aware of the iiwa's having some arm-specific calibration. It would just run a general model for iiwa7 or iiwa14 arms. UR does have model-specific ones, but e.g. Franka Emika does not do it either and we expect some deviations here. This is the reason the people in Stanford run their own calibration on top.

I will share a screen-capture video when I am at the lab next.

That'd be great.

[niederha]

@priyampnchl Here are some additional info on the oscillating behavior you're observing with your robot.
I think it is indeed the same issue that is discussed in this issue.

It is linked to this commit which essentially it deprecated the use of stiffness and damping for the torqueController. Anything other than 0 stiffness and 0 torque will result in this. To confirm that you should notice that if you choose a stiffness value low enough, you should be able to reproduce the exact behavior of the video and that 0 stiffness and 0 damping should still work as expected.

If you wish to use stiffness and damping, for now the solution is to revert this commit, or just change 0.1 to 0.0 at line 314.
Note that this commit was a trick to improve joint striction compensation (if I recall correctly) so you will lose in precision by disabling it but will be enabled to use stiffness and damping in torque mode.

If you want the best of both word you have to start your controller with 0 stiffness and damping, compute it separately on your computer and send it as part of the torque command.

I actually have to commit some kind of switch for this parameter, just never got around to it.

[hidoggiezzz]

msg.data = [0,0,0,0,0,0,0.5]

0.5 Nm is a super super small torque. You need to give a bigger value to make the joint move. Try something like 5-10.

We had the same issue with torque control, the robot still didn't move after we set the seventh joint torque to 10, here's the code we used

#include<iostream>

#include"ros/ros.h"
#include<vector>
#include<list>
#include "std_msgs/Float64MultiArray.h"
#include"sensor_msgs/JointStateConst.h"
class TC
{
	public:
	TC()
	{
		sub = nh.subscribe("iiwa/joint_states",10,&TC::callback,this);
        pub = nh.advertise<std_msgs::Float64MultiArray>("iiwa/TorqueController/command",10);
	}
	
	void callback(const sensor_msgs::JointStateConstPtr& p)
	{
		std_msgs::Float64MultiArray msg;
		
		torque = {0,0,0,0,0,0,10};
		for(int i=1;i<7;i++)
		{
			msg.data.push_back(torque[i]);
			torque[i]=0;
		}
		
		pub.publish(msg);
	}

	public:
	ros::NodeHandle nh;
    ros::Subscriber sub;
    ros::Publisher pub;
	std::array<double,7> torque;

};
int main(int argc, char  *argv[])
{
    ros::init(argc,argv,"control");
    TC c;
	while(ros::ok())
    {
		ros::spinOnce();
    }

    return 0;
}

When we use position control, the same code can make the robot move normally.Let me know if you have an idea about this.
Thanks !