simulator works, but is a bit of a hack

kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+import sys
+import socket
+import numpy as np
+import time
+import xml.etree.ElementTree as ET
+
+import errno
+# import rospy
+# from std_msgs.msg import String
+
+def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
+ q = act_joint_pos
+ qd = setpoint_joint_pos
+ root = ET.Element('Rob', {'TYPE':'KUKA'})
+ ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+ 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+ ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+ 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+ ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]),
+ 'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])})
+ ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]),
+ 'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])})
+ ET.SubElement(root, 'Delay', {'D':str(timeout_count)})
+ ET.SubElement(root, 'IPOC').text=str(ipoc)
+ return ET.tostring(root)
+
+def parse_rsi_xml_sen(data):
+ root = ET.fromstring(data)
+ AK = root.find('AK').attrib
+ desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'],
+ AK['A4'], AK['A5'], AK['A6']]).astype(np.float64)
+ IPOC = root.find('IPOC').text
+ return desired_joint_correction, int(IPOC)
+
+
+# if __name__ == '__main__':
+def main():
+ node_name = 'kuka_rsi_simulation'
+# rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
+# rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
+
+ cycle_time = 0.004
+ act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
+ cmd_joint_pos = act_joint_pos.copy()
+ des_joint_correction_absolute = np.zeros(6)
+ timeout_count = 0
+ ipoc = 0
+
+ # import argparse
+ # parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
+ # parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)')
+ # parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)')
+ # parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. <Sen Type:"ImFree">')
+ # # Only parse known arguments
+ # args, _ = parser.parse_known_args()
+ # host = args.rsi_hw_iface_ip
+ # port = int(args.rsi_hw_iface_port)
+ # sen_type = args.sen
+
+ host = "127.0.0.1"
+ port = 49152
+ sen_type = 'ImFree'
+
+
+ # rospy.init_node(node_name)
+ # rospy.loginfo('{}: Started'.format(node_name))
+
+ try:
+ s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ # rospy.loginfo('{}, Successfully created socket'.format(node_name))
+ print('{}, Successfully created socket'.format(node_name))
+ s.settimeout(1)
+ except socket.error as e:
+ # rospy.logfatal('{}Could not create socket'.format(node_name))
+ print('{}Could not create socket'.format(node_name))
+ sys.exit()
+
+ def shutdown_hook():
+ # rospy.loginfo('{}: Shutting down'.format(node_name))
+ print('{}: Shutting down'.format(node_name))
+ s.close()
+
+ # rospy.on_shutdown(shutdown_hook)
+
+ # while not rospy.is_shutdown():
+ while True:
+ time.sleep(0.001) # this is a hack, make this a ros2 node
+ try:
+ msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
+ # rsi_act_pub.publish(str(msg))
+ s.sendto(msg, (host, port))
+ recv_msg, addr = s.recvfrom(1024)
+ # rsi_cmd_pub.publish(str(recv_msg))
+ des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
+ act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
+ ipoc += 1
+ time.sleep(cycle_time / 2)
+ except socket.timeout:
+ # rospy.logwarn('{}: Socket timed out'.format(node_name))
+ print('{}: Socket timed out'.format(node_name))
+ timeout_count += 1
+ except socket.error as e:
+ if e.errno != errno.EINTR:
+ raise
+
+
+if __name__ == '__main__':
+ main()

kuka_rsi_simulator/scripts/kuka_rsi_simulator

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import sys
 import socket
@@ -7,8 +7,8 @@ import time
 import xml.etree.ElementTree as ET
 
 import errno
-import rospy
-from std_msgs.msg import String
+# import rospy
+# from std_msgs.msg import String
 
 def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
  q = act_joint_pos
@@ -35,8 +35,8 @@ def parse_rsi_xml_sen(data):
  return desired_joint_correction, int(IPOC)
 
 node_name = 'kuka_rsi_simulation'
-rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
-rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
+# rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
+# rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
 
 cycle_time = 0.004
 act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
@@ -48,47 +48,53 @@ ipoc = 0
 if __name__ == '__main__':
  import argparse
  parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
- parser.add_argument('rsi_hw_iface_ip', help='The ip address of the RSI control interface')
- parser.add_argument('rsi_hw_iface_port', help='The port of the RSI control interface')
+ parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)')
+ parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)')
  parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. <Sen Type:"ImFree">')
  # Only parse known arguments
  args, _ = parser.parse_known_args()
  host = args.rsi_hw_iface_ip
  port = int(args.rsi_hw_iface_port)
  sen_type = args.sen
 
- rospy.init_node(node_name)
- rospy.loginfo('{}: Started'.format(node_name))
+ # rospy.init_node(node_name)
+ # rospy.loginfo('{}: Started'.format(node_name))
 
  try:
  s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
- rospy.loginfo('{}, Successfully created socket'.format(node_name))
+ # rospy.loginfo('{}, Successfully created socket'.format(node_name))
+ print('{}, Successfully created socket'.format(node_name))
  s.settimeout(1)
  except socket.error as e:
- rospy.logfatal('{}Could not create socket'.format(node_name))
+ # rospy.logfatal('{}Could not create socket'.format(node_name))
+ print('{}Could not create socket'.format(node_name))
  sys.exit()
 
  def shutdown_hook():
- rospy.loginfo('{}: Shutting down'.format(node_name))
+ # rospy.loginfo('{}: Shutting down'.format(node_name))
+ print('{}: Shutting down'.format(node_name))
  s.close()
 
- rospy.on_shutdown(shutdown_hook)
+ # rospy.on_shutdown(shutdown_hook)
 
- while not rospy.is_shutdown():
+ # while not rospy.is_shutdown():
+ while True:
+ time.sleep(0.001) # this is a hack, make this a ros2 node
  try:
  msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
- rsi_act_pub.publish(msg)
+ # rsi_act_pub.publish(str(msg))
  s.sendto(msg, (host, port))
  recv_msg, addr = s.recvfrom(1024)
- rsi_cmd_pub.publish(recv_msg)
+ # rsi_cmd_pub.publish(str(recv_msg))
  des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
  act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
  ipoc += 1
  time.sleep(cycle_time / 2)
- except socket.timeout, msg:
- rospy.logwarn('{}: Socket timed out'.format(node_name))
+ except socket.timeout:
+ # rospy.logwarn('{}: Socket timed out'.format(node_name))
+ print('{}: Socket timed out'.format(node_name))
  timeout_count += 1
- except socket.error, e:
+ except socket.error as e:
  if e.errno != errno.EINTR:
  raise
 

kuka_rsi_simulator/scripts/kuka_rsi_simulator.orig

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+
+import sys
+import socket
+import numpy as np
+import time
+import xml.etree.ElementTree as ET
+
+import errno
+import rospy
+from std_msgs.msg import String
+
+def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
+ q = act_joint_pos
+ qd = setpoint_joint_pos
+ root = ET.Element('Rob', {'TYPE':'KUKA'})
+ ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+ 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+ ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+ 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+ ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]),
+ 'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])})
+ ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]),
+ 'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])})
+ ET.SubElement(root, 'Delay', {'D':str(timeout_count)})
+ ET.SubElement(root, 'IPOC').text=str(ipoc)
+ return ET.tostring(root)
+
+def parse_rsi_xml_sen(data):
+ root = ET.fromstring(data)
+ AK = root.find('AK').attrib
+ desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'],
+ AK['A4'], AK['A5'], AK['A6']]).astype(np.float64)
+ IPOC = root.find('IPOC').text
+ return desired_joint_correction, int(IPOC)
+
+node_name = 'kuka_rsi_simulation'
+rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
+rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
+
+cycle_time = 0.004
+act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
+cmd_joint_pos = act_joint_pos.copy()
+des_joint_correction_absolute = np.zeros(6)
+timeout_count = 0
+ipoc = 0
+
+if __name__ == '__main__':
+ import argparse
+ parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
+ parser.add_argument('rsi_hw_iface_ip', help='The ip address of the RSI control interface')
+ parser.add_argument('rsi_hw_iface_port', help='The port of the RSI control interface')
+ parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. <Sen Type:"ImFree">')
+ # Only parse known arguments
+ args, _ = parser.parse_known_args()
+ host = args.rsi_hw_iface_ip
+ port = int(args.rsi_hw_iface_port)
+ sen_type = args.sen
+
+ rospy.init_node(node_name)
+ rospy.loginfo('{}: Started'.format(node_name))
+
+ try:
+ s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ rospy.loginfo('{}, Successfully created socket'.format(node_name))
+ s.settimeout(1)
+ except socket.error as e:
+ rospy.logfatal('{}Could not create socket'.format(node_name))
+ sys.exit()
+
+ def shutdown_hook():
+ rospy.loginfo('{}: Shutting down'.format(node_name))
+ s.close()
+
+ rospy.on_shutdown(shutdown_hook)
+
+ while not rospy.is_shutdown():
+ try:
+ msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
+ rsi_act_pub.publish(msg)
+ s.sendto(msg, (host, port))
+ recv_msg, addr = s.recvfrom(1024)
+ rsi_cmd_pub.publish(recv_msg)
+ des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
+ act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
+ ipoc += 1
+ time.sleep(cycle_time / 2)
+ except socket.timeout, msg:
+ rospy.logwarn('{}: Socket timed out'.format(node_name))
+ timeout_count += 1
+ except socket.error, e:
+ if e.errno != errno.EINTR:
+ raise
+

kuka_rsi_simulator/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/kuka_rsi_simulator
+[install]
+install-scripts=$base/lib/kuka_rsi_simulator

kuka_rsi_simulator/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'kuka_rsi_simulator'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='dmitri',
+ maintainer_email='dmitri@dmitri.com',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main'
+ ],
+ },
+)