Hi everyone,
I am right now starting to work with ROS and KUKA IIWA for teleoperation. In order to validate this application, I thought I would start with the keyboard, simply controlling each axis with a different key. I am using this API: GitHub - jonaitken/KUKA-IIWA-API: Ros Enabled API for easy integration with the KUKA IIWA
And I have this node running:
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
import tty
import sys
import termios
from client_lib import *
from typing import Dict, Tuple
my_client = kuka_iiwa_ros_client()
while (not my_client.isready): pass
print('Started Application!')
my_client.send_command('setTool basket')
my_client.send_command('setJointAcceleration 5.0')
my_client.send_command('setJointVelocity 10.0')
my_client.send_command('setJointJerk 1.0') #Rate of change of acceleration.
my_client.send_command('setCartVelocity 50')
def get_key():
return sys.stdin.read(1)[0]
keys_to_position: Dict[str, Tuple[float, ...]] = {
"1": (1, 0, 0, 0, 0, 0, 0),
"2": (0, 1, 0, 0, 0, 0, 0),
"3": (0, 0, 1, 0, 0, 0, 0),
"4": (0, 0, 0, 1, 0, 0, 0),
"5": (0, 0, 0, 0, 1, 0, 0),
"6": (0, 0, 0, 0, 0, 1, 0),
"7": (0, 0, 0, 0, 0, 0, 1),
"q": (-1, 0, 0, 0, 0, 0, 0),
"w": (0, -1, 0, 0, 0, 0, 0),
"e": (0, 0, -1, 0, 0, 0, 0),
"r": (0, 0, 0, -1, 0, 0, 0),
"t": (0, 0, 0, 0, -1, 0, 0),
"y": (0, 0, 0, 0, 0, -1, 0),
"u": (0, 0, 0, 0, 0, 0, -1)
}
recognised_keys ={"1","2","3","4","5","6", "7","q", "w", "e", "r", "t", "y", "u"}
class TeleopStateMachine:
def __init__(self) -> None:
self.states = {
"idle": self.idle_state,
"moving": self.moving_state,
"exit": self.exit
}
self.current_state = "idle"
#Função de transição
def main(self, event: str) -> None:
self.current_state = self.states[self.current_state](event)
def exit(self):
pass
def idle_state(self,event:str) -> str:
if event == "":
return "idle"
elif event == chr(27):
return "exit"
return "moving"
def moving_state(self,event: str) -> str:
if event in recognised_keys:
actual_joint_position: Tuple[float, ...] = tuple(my_client.JointPosition[0])
joint_position_increment: Tuple[float, ...] = keys_to_position[event]
final_joint_position: Tuple[float, ...] = tuple(sum(x) for x in zip(actual_joint_position, joint_position_increment))
command_str: str = "setPosition " + " ".join(map(str,final_joint_position))
my_client.send_command(command_str)
return "idle"
elif event == chr(27):
return "exit"
return "idle"
def main():
rate = rospy.Rate(20)
orig_settings = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin)
state_machine = TeleopStateMachine()
try:
if not rospy.is_shutdown():
while state_machine.current_state != "exit":
key = get_key()
state_machine.main(event = key)
rate.sleep()
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
if __name__ == "__main__":
main()So, when I press a key that is recognised, the respective axis will move 1 degree in the intended direction. However, when I test this on the robot, I realised that maybe it cannot move (accelerate and decelerate) at a fair rate. I notice a lot of lag and I understand that it is maybe due to the quantity of commands of position that are sent when I press down a key for some seconds. I also tried adjusting the joint acceleration and velocity but it really doesn't make much of a difference. Is there anything wrong with the logic of this program? Is there any other way I could validate this simple application?
Also, my objective later is to teleoperate the robot with a haptic device, so I ask, is there any way I should be programming this stage that could be helpful for later to incorporate on that application? Or is it very different from this? I assumed it would be the same logic.
Thank you so much for you help!
