From 658947e4b613e38bfd9e8ca8127e5cb9795f0603 Mon Sep 17 00:00:00 2001 From: John Wason Date: Fri, 12 Jul 2024 13:15:07 -0400 Subject: [PATCH] Add robot raconteur driver module, entrypoint, and tests --- config/abb_1200_5_90_rws_default_config.yml | 148 +++++ .../abb_6700_150_320_rws_default_config.yml | 154 +++++ setup.py | 14 +- .../robotraconteur/__init__.py | 0 .../robotraconteur/__main__.py | 3 + .../abb_robotraconteur_rws_driver.py | 551 ++++++++++++++++++ .../experimental.abb_robot.egm.robdef | 62 ++ .../experimental.abb_robot.rws.robdef | 179 ++++++ test/robotraconteur/test_abb_rws_driver.py | 308 ++++++++++ 9 files changed, 1418 insertions(+), 1 deletion(-) create mode 100644 config/abb_1200_5_90_rws_default_config.yml create mode 100644 config/abb_6700_150_320_rws_default_config.yml create mode 100644 src/abb_robot_client/robotraconteur/__init__.py create mode 100644 src/abb_robot_client/robotraconteur/__main__.py create mode 100644 src/abb_robot_client/robotraconteur/abb_robotraconteur_rws_driver.py create mode 100644 src/abb_robot_client/robotraconteur/experimental.abb_robot.egm.robdef create mode 100644 src/abb_robot_client/robotraconteur/experimental.abb_robot.rws.robdef create mode 100644 test/robotraconteur/test_abb_rws_driver.py diff --git a/config/abb_1200_5_90_rws_default_config.yml b/config/abb_1200_5_90_rws_default_config.yml new file mode 100644 index 0000000..94962e7 --- /dev/null +++ b/config/abb_1200_5_90_rws_default_config.yml @@ -0,0 +1,148 @@ + +device_info: + device: + name: abb_robot_rws + manufacturer: + name: ABB + uuid: ee260e78-d731-415a-84ca-4b02c487410c + model: + name: ABB_1200_5_90 + uuid: 9ad3c0ee-ec5e-4057-a297-d340e1484f01 + user_description: ABB Robot + serial_number: 123456789 + device_classes: + - class_identifier: + name: robot + uuid: 39b513e7-21b9-4b49-8654-7537473030eb + subclasses: + - serial + - serial_six_axis + implemented_types: + - experimental.robotics.motion_program.MotionProgramRobot +robot_type: serial +robot_capabilities: +- jog_command +- trajectory_command +- position_command +chains: +- kin_chain_identifier: robot_arm + H: + - x: 0.0 + y: 0.0 + z: 1.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + P: + - x: 0.0 + y: 0.0 + z: 0.3991 + - x: 0.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.448 + - x: 0.0 + y: 0.0 + z: 0.042 + - x: 0.451 + y: 0.0 + z: 0.0 + - x: 0.082 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.0 + flange_identifier: tool0 + flange_pose: + orientation: + w: 0.7071067811882787 + x: 0.0 + y: 0.7071067811848163 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + joint_numbers: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 +joint_info: +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_1 + joint_limits: + effort: 1000.0 + lower: -2.967 + upper: 2.967 + velocity: 5.027 + acceleration: 10 + joint_type: revolute +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_2 + joint_limits: + effort: 1000.0 + lower: -1.745 + upper: 2.269 + velocity: 4.189 + acceleration: 15 + joint_type: revolute +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_3 + joint_limits: + effort: 1000.0 + lower: -3.491 + upper: 1.222 + velocity: 5.236 + acceleration: 15 + joint_type: revolute +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_4 + joint_limits: + effort: 1000.0 + lower: -4.712 + upper: 4.712 + velocity: 6.981 + acceleration: 20 + joint_type: revolute +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_5 + joint_limits: + effort: 1000.0 + lower: -2.269 + upper: 2.269 + velocity: 7.069 + acceleration: 20 + joint_type: revolute +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_6 + joint_limits: + effort: 1000.0 + lower: -6.283 + upper: 6.283 + velocity: 10.472 + acceleration: 20 + joint_type: revolute diff --git a/config/abb_6700_150_320_rws_default_config.yml b/config/abb_6700_150_320_rws_default_config.yml new file mode 100644 index 0000000..bdf0c55 --- /dev/null +++ b/config/abb_6700_150_320_rws_default_config.yml @@ -0,0 +1,154 @@ + +device_info: + device: + name: abb_robot_rws + manufacturer: + name: ABB + uuid: ee260e78-d731-415a-84ca-4b02c487410c + model: + name: ABB_IRB6700_150_320 + uuid: 9d43d745-dbea-4a88-ac18-43a64bca67eb + user_description: ABB Robot + serial_number: 123456789 + device_classes: + - class_identifier: + name: robot + uuid: 39b513e7-21b9-4b49-8654-7537473030eb + subclasses: + - serial + - serial_six_axis + implemented_types: + - com.robotraconteur.robotics.robot.Robot +robot_type: serial +robot_capabilities: +- jog_command +- trajectory_command +- position_command +chains: +- H: + - x: 0.0 + y: 0.0 + z: 1.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 1.0 + z: 0.0 + - x: 1.0 + y: 0.0 + z: 0.0 + P: + - x: 0.0 + y: 0.0 + z: 0.78 + - x: 0.32 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 1.280 + - x: 0.0 + y: 0.0 + z: 0.2 + - x: 1.5925 + y: 0.0 + z: 0.0 + - x: 0.2 + y: 0.0 + z: 0.0 + - x: 0.0 + y: 0.0 + z: 0.0 + flange_identifier: tool0 + flange_pose: + orientation: + w: 0.707107 + x: 0.0 + y: 0.707107 + z: 0.0 + position: + x: 0.0 + y: 0.0 + z: 0.0 + joint_numbers: + - 0 + - 1 + - 2 + - 3 + - 4 + - 5 + kin_chain_identifier: robot_arm +joint_info: +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_1 + joint_limits: + effort: 0.0 + lower: -2.96706 + upper: 2.96706 + velocity: 1.745329 + acceleration: 10 + joint_type: revolute + passive: false +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_2 + joint_limits: + effort: 0.0 + lower: -1.134464 + upper: 1.48353 + velocity: 1.570796 + acceleration: 15 + joint_type: revolute + passive: false +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_3 + joint_limits: + effort: 0.0 + lower: -3.141593 + upper: 1.22173 + velocity: 1.570796 + acceleration: 15 + joint_type: revolute + passive: false +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_4 + joint_limits: + effort: 0.0 + lower: -5.235988 + upper: 5.235988 + velocity: 2.96706 + acceleration: 20 + joint_type: revolute + passive: false +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_5 + joint_limits: + effort: 0.0 + lower: -2.268928 + upper: 2.268928 + velocity: 2.094395 + acceleration: 20 + joint_type: revolute + passive: false +- default_effort_units: newton_meter + default_units: radian + joint_identifier: joint_6 + joint_limits: + effort: 0.0 + lower: -6.283185 + upper: 6.283185 + velocity: 3.316126 + acceleration: 20 + joint_type: revolute + passive: false diff --git a/setup.py b/setup.py index 0637fb6..70267b0 100644 --- a/setup.py +++ b/setup.py @@ -22,7 +22,19 @@ extras_require={ "aio": ["httpx", "websockets"], "testing": ["pytest", "pytest-asyncio"], + "robotraconteur": [ + "robotraconteur", + "robotraconteurcompanion", + "drekar-launch-process", + "robotraconteur-abstract-robot" + ], }, long_description=long_description, - long_description_content_type='text/markdown' + long_description_content_type='text/markdown', + package_data={"abb_robot_client.robotraconteur": ["*.robdef"]}, + entry_points={ + 'console_scripts': [ + 'abb-robot-client-robotraconteur=abb_robot_client.robotraconteur.abb_robotraconteur_rws_driver:main', + ], + }, ) \ No newline at end of file diff --git a/src/abb_robot_client/robotraconteur/__init__.py b/src/abb_robot_client/robotraconteur/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/abb_robot_client/robotraconteur/__main__.py b/src/abb_robot_client/robotraconteur/__main__.py new file mode 100644 index 0000000..fbe2e68 --- /dev/null +++ b/src/abb_robot_client/robotraconteur/__main__.py @@ -0,0 +1,3 @@ +from .abb_robotraconteur_rws_driver import main + +main() \ No newline at end of file diff --git a/src/abb_robot_client/robotraconteur/abb_robotraconteur_rws_driver.py b/src/abb_robot_client/robotraconteur/abb_robotraconteur_rws_driver.py new file mode 100644 index 0000000..ec9ae35 --- /dev/null +++ b/src/abb_robot_client/robotraconteur/abb_robotraconteur_rws_driver.py @@ -0,0 +1,551 @@ +import RobotRaconteur as RR +RRN = RR.RobotRaconteurNode.s +import RobotRaconteurCompanion as RRC +from abb_robot_client import egm, rws +import traceback +import threading +import time +import numpy as np +from robotraconteur_abstract_robot import AbstractRobot +from contextlib import suppress +import argparse +from RobotRaconteurCompanion.Util.RobDef import register_service_types_from_resources +from RobotRaconteurCompanion.Util.AttributesUtil import AttributesUtil +from RobotRaconteurCompanion.Util.InfoFileLoader import InfoFileLoader +import drekar_launch_process +import general_robotics_toolbox as rox + +class ABBRWSRobotImpl(AbstractRobot): + def __init__(self, robot_info, robot_url): + super().__init__(robot_info, 6) + + self._robot_url = robot_url + + self._base_set_controller_state = False + self._base_set_operational_mode = False + self.robot_info.robot_capabilities = 0 + + self._uses_homing = False + self._has_position_command = False + self._has_velocity_command = False + self._has_jog_command = False + + self._rws = None + self._egm = None + self._missed_egm = 0 + + self._abb_rws_const = self._node.GetConstants("experimental.abb_robot.rws") + self._task_cycle = self._abb_rws_const["TaskCycle"] + self._task_execution_state = self._abb_rws_const["TaskExecutionState"] + + self._rapid_exec_state = self._node.GetStructureType("experimental.abb_robot.rws.RAPIDExecutionState") + self._task_state_type = self._node.GetStructureType("experimental.abb_robot.rws.TaskState") + + self._event_log_entry_type = self._node.GetStructureType("experimental.abb_robot.rws.EventLogEntry") + + self._jointtarget_type = self._node.GetStructureType("experimental.abb_robot.rws.JointTarget") + self._robtarget_type = self._node.GetStructureType("experimental.abb_robot.rws.RobTarget") + + self._confdata_dtype = self._node.GetNamedArrayDType("experimental.abb_robot.rws.ConfData") + + self._egm_robot_type = self._node.GetStructureType("experimental.abb_robot.egm.EGMRobot") + self._egm_collision_info_type = self._node.GetStructureType("experimental.abb_robot.egm.EGMCollisionInfo") + # self._timespec2_dtype = self._node.GetNamedArrayDType("com.robotraconteur.datetime.TimeSpec2") + + def RRServiceObjectInit(self, context, service_path): + super().RRServiceObjectInit(context, service_path) + + self.egm_joint_command.InValueChanged += self._egm_joint_command_invalue_changed + self.egm_pose_command.InValueChanged += self._egm_pose_command_invalue_changed + self.egm_path_correction_command.InValueChanged += self._egm_correction_command_invalue_changed + + self._broadcast_downsampler.AddWireBroadcaster(self.egm_state) + + + def _start_robot(self): + self._egm = egm.EGM() + self._rws = rws.RWS(self._robot_url) + + super()._start_robot() + + def _stop_robot(self): + super()._stop_robot() + + def _close(self): + super()._close() + + def _send_robot_command(self, now, joint_pos_cmd, joint_vel_cmd): + pass + + def _verify_robot_state(self, now): + self._command_mode = self._robot_command_mode["halt"] + return True + + def _fill_robot_command(self, now): + return False, None, None + + def async_disable(self, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + async def _send_disable(self, handler): + raise NotImplemented() + + def async_enable(self, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + async def _send_enable(self, handler): + raise NotImplemented() + + def async_reset_errors(self, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def _send_reset_errors(self, handler): + raise NotImplemented() + + def async_jog_freespace(self, joint_position, max_velocity, wait, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def async_jog_joint(self, joint_velocity, timeout, wait, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def execute_trajectory(self, trajectory): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def jog_cartesian(self, velocity, timeout, wait): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def async_home(self, handler): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def tool_attached(self, chain, tool): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def tool_detached(self, chain, tool_name): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def payload_attached(self, chain, payload, pose): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def payload_detached(self, chain, payload_name): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + @AbstractRobot.command_mode.setter + def command_mode(self, value): + raise RR.InvalidOperationException("Invalid command for ABB RWS driver") + + def _run_timestep(self, now): + res = True + robot_state = None + while res: + res, robot_state1 = self._egm.receive_from_robot() + if res: + robot_state = robot_state1 + self._missed_egm = 0 + if robot_state is None: + self._missed_egm += 1 + + if robot_state is not None: + + self._send_egm_state(robot_state) + + egm_last_recv = self._stopwatch_ellapsed_s() + self._last_joint_state = egm_last_recv + self._last_endpoint_state = egm_last_recv + self._last_robot_state = egm_last_recv + self._enabled = robot_state.motors_on + self._ready = self._enabled #robot_state.rapid_running + + self._joint_position = np.deg2rad(robot_state.joint_angles) + self._endpoint_pose = self._node.ArrayToNamedArray(\ + np.concatenate((robot_state.cartesian[1],robot_state.cartesian[0]*1e-3)), self._pose_dtype) + + else: + if self._communication_failure: + self._joint_position = np.zeros((0,)) + self._endpoint_pose = np.zeros((0,),dtype=self._pose_dtype) + + if self._error: + self._ready = False + + super()._run_timestep(now) + + def _send_egm_state(self, robot_state): + + if not self._wires_ready: + return + + s = self._egm_robot_type() + robot_message = robot_state.robot_message + if robot_message.HasField("header"): + s.seqno = robot_message.header.seqno + s.tm = robot_message.header.tm + if robot_state.joint_angles is not None: + s.joint_position = robot_state.joint_angles + if robot_state.cartesian is not None: + s.cartesian_position = self._node.ArrayToNamedArray(\ + np.concatenate((robot_state.cartesian[1],robot_state.cartesian[0]*1e-3)), self._pose_dtype) + if robot_state.external_axes is not None: + s.external_joint_position = robot_state.external_axes + if robot_state.joint_angles_planned is not None: + s.joint_position_command = robot_state.joint_angles_planned + if robot_state.cartesian_planned is not None: + s.cartesian_position_command = self._node.ArrayToNamedArray(\ + np.concatenate((robot_state.cartesian_planned[1],robot_state.cartesian_planned[0]*1e-3)), self._pose_dtype) + if robot_state.external_axes_planned is not None: + s.external_joint_position_command = robot_state.external_axes_planned + s.motor_state = robot_state.motors_on + s.rapid_ctrl_exec_state = robot_state.rapid_running + if robot_message.HasField("utilizationRate"): + s.utilization_rate = robot_message.utilizationRate + if robot_state.move_index is not None: + s.move_index = robot_state.move_index + if robot_state.rapid_from_robot is not None: + s.rapid_from_robot = robot_state.rapid_from_robot + if robot_message.HasField("measuredForce"): + s.measured_force = list(robot_message.measuredForce.force) + + self.egm_state.OutValue = s + + + def getf_execution_state(self): + state = self._rws.get_execution_state() + ret = self._rapid_exec_state() + es = state.ctrlexecstate + if es == "running": + ret.ctrlexecstate = self._task_execution_state["running"] + elif es == "stopped": + ret.ctrlexecstate = self._task_execution_state["stopped"] + else: + ret.ctrlexecstate = self._task_execution_state["unknown"] + + c = state.cycle + if c == "forever": + ret.cycle = self._task_cycle["forever"] + elif c == "asis": + ret.cycle = self._task_cycle["asis"] + elif c == "once": + ret.cycle = self._task_cycle["once"] + elif c == "oncedone": + ret.cycle = self._task_cycle["oncedone"] + else: + ret.cycle = self._task_cycle["unknown"] + return ret + + def getf_controller_state(self): + state = self._rws.get_controller_state() + if state == "init": + return self._robot_controller_state["init"] + elif state == "motoron": + return self._robot_controller_state["motor_on"] + elif state == "motoroff": + return self._robot_controller_state["motor_off"] + elif state == "guardstop": + return self._robot_controller_state["guard_stop"] + elif state == "emergencystop": + return self._robot_controller_state["emergency_stop"] + elif state == "emergystopreset": + return self._robot_controller_state["emergency_stop_reset"] + else: + return self._robot_controller_state["undefined"] + + def getf_operation_mode(self): + mode = self._rws.get_operation_mode() + if mode == "INIT": + return self._robot_operational_mode["init"] + elif mode == "MANR": + return self._robot_operational_mode["manual_reduced_speed"] + elif mode == "MANF": + return self._robot_operational_mode["manual_full_speed"] + elif mode == "AUTO": + return self._robot_operational_mode["auto"] + else: + return self._robot_operational_mode["undefined"] + + + @property + def operational_mode(self): + return self.getf_operation_mode() + + @property + def controller_state(self): + return self.getf_controller_state() + + def start(self, cycle, tasks): + rws_cycle = "asis" + if cycle == self._task_cycle["forever"]: + rws_cycle = "forever" + elif cycle == self._task_cycle["asis"]: + rws_cycle = "asis" + elif cycle == self._task_cycle["once"]: + rws_cycle = "once" + elif cycle == self._task_cycle["oncedone"]: + rws_cycle = "oncedone" + + self._rws.start(rws_cycle, tasks) + + def stop(self): + self._rws.stop() + + def resetpp(self): + self._rws.resetpp() + + def activate_task(self, task): + self._rws.activate_task(task) + + def deactivate_task(self, task): + self._rws.deactivate_task(task) + + def getf_tasks(self): + rws_tasks = self._rws.get_tasks() + ret = [] + for t in rws_tasks.values(): + ret1 = self._task_state_type() + ret1.name = t.name + ret1.type = t.type_ + ret1.taskstate = t.taskstate + ret1.excstate = self._task_execution_state["unknown"] + if t.taskstate == "running": + ret1.taskstate = self._task_execution_state["running"] + elif t.taskstate == "stopped": + ret1.taskstate = self._task_execution_state["stopped"] + ret1.active = t.active + ret1.motiontask = t.motiontask + ret.append(ret1) + return ret + + def getf_digital_io(self, signal_name): + return self._rws.get_digital_io(signal_name) + + def setf_digital_io(self, signal_name, value): + self._rws.set_digital_io(signal_name, value) + + def getf_digital_io2(self, signal_name, network, unit): + return self._rws.get_digital_io(signal_name, network, unit) + + def setf_digital_io2(self, signal_name, network, unit, value): + self._rws.set_digital_io(signal_name, value, network, unit) + + def getf_analog_io(self, signal_name): + return self._rws.get_analog_io(signal_name) + + def setf_analog_io(self, signal_name, value): + self._rws.set_analog_io(signal_name, value) + + def getf_analog_io2(self, signal_name, network, unit): + return self._rws.get_analog_io(signal_name, network, unit) + + def setf_analog_io2(self, signal_name, network, unit, value): + self._rws.set_analog_io(signal_name, value, network, unit) + + def getf_rapid_variables(self, task): + raise RR.NotImplementedException("Not implemented") + + def getf_rapid_variable(self, var, task): + return self._rws.get_rapid_variable(var, task) + + def setf_rapid_variable(self, var, task, value): + self._rws.set_rapid_variable(var, value, task) + + def getf_ramdisk_path(self): + return self._rws.get_ramdisk_path() + + def read_file(self, filename): + return self._rws.read_file(filename) + + def upload_file(self, filename, data): + self._rws.upload_file(filename, data.tobytes()) + + def delete_file(self, filename): + self._rws.delete_file(filename) + + def list_files(self, path): + return self._rws.list_files(path) + + def read_event_log(self): + ret = [] + evts = self._rws.read_event_log() + for e in evts: + evt = self._event_log_entry_type() + evt.seqnum = e.seqnum + evt.msgtype = e.msgtype + # evt.tstamp = e.tstamp + evt.title = e.title + evt.desc = e.desc + evt.conseqs = e.conseqs + evt.causes = e.causes + evt.actions = e.actions + ret.append(evt) + return ret + + def _rws_jt_to_rr(self, jt): + ret = self._jointtarget_type() + ret.robax = jt.robax + ret.extax = jt.extax + return ret + + def _rr_jt_to_rws(self, jt): + ret = rws.JointTarget(jt.robax, jt.extax) + return ret + + def getf_jointtarget(self, mechunit): + jt = self._rws.get_jointtarget(mechunit) + return self._rws_jt_to_rr(jt) + + def getf_robtarget(self, mechunit): + rt = self._rws.get_robtarget(mechunit) + ret = self._robtarget_type() + ret.pose = self._geometry_util.rox_transform_to_pose(rox.Transform(rox.q2R(rt.rot), rt.trans)) + ret.robconf = self._node.ArrayToNamedArray(rt.robconf, self._confdata_dtype) + ret.extax = rt.extax + + return ret + + def getf_robtarget2(self, mechunit, tool, wobj, coordinate): + rt = self._rws.get_robtarget(mechunit, tool, wobj, coordinate) + ret = self._robtarget_type() + ret.pose = self._geometry_util.rox_transform_to_pose(rox.Transform(rox.q2R(rt.rot), rt.trans)) + ret.robconf = self._node.ArrayToNamedArray(rt.robconf, self._confdata_dtype) + ret.extax = rt.extax + + return ret + + @property + def speed_ratio(self): + return self._rws.get_speedratio()/100.0 + + @speed_ratio.setter + def speed_ratio(self, value): + self._rws.set_speedratio(int(value*100)) + + def getf_rapid_variable_jointtarget(self, var, task): + jt = self._rws.get_rapid_variable_jointtarget(var, task) + return self._rws_jt_to_rr(jt) + + def setf_rapid_variable_jointtarget(self, var, task, value): + self._rws.set_rapid_variable_jointtarget(var, self._rr_jt_to_rws(value), task) + + def getf_rapid_variable_jointtarget_array(self, var, task): + jt = self._rws.get_rapid_variable_jointtarget_array(var, task) + ret = [] + for j in jt: + ret.append(self._rws_jt_to_rr(j)) + return ret + + def setf_rapid_variable_jointtarget_array(self, var, task, value): + jt = [] + for j in value: + jt.append(self._rr_jt_to_rws(j)) + self._rws.set_rapid_variable_jointtarget_array(var, jt, task) + + def getf_rapid_variable_num(self, var, task): + return self._rws.get_rapid_variable_num(var, task) + + def setf_rapid_variable_num(self, var, task, value): + self._rws.set_rapid_variable_num(var, value, task) + + def getf_rapid_variable_num_array(self, var, task): + return self._rws.get_rapid_variable_num_array(var, task) + + def setf_rapid_variable_num_array(self, var, task, value): + self._rws.set_rapid_variable_num_array(var, value, task) + + def request_rmmp(self, timeout): + self._rws.request_rmmp(timeout) + + def poll_rmmp(self): + self._rws.poll_rmmp() + + def _egm_joint_command_invalue_changed(self, value, ts, ep): + try: + joint_command = value.joints + speed_command = value.joints_speed + if len(speed_command) == 0: + speed_command = None + external_joints = value.external_joints + if len(external_joints) == 0: + external_joints = None + external_joints_speed = value.external_joints_speed + if len(external_joints_speed) == 0: + external_joints_speed = None + rapid_to_robot = value.rapid_to_robot + if len(rapid_to_robot) == 0: + rapid_to_robot = None + self._egm.send_to_robot(joint_command, speed_command, external_joints, external_joints_speed, rapid_to_robot) + except: + traceback.print_exc() + + def _egm_pose_command_invalue_changed(self, value, ts, ep): + # print(f"egm_pose_command: {value.cartesian}") + try: + + cart_command = self._node.NamedArrayToArray(value.cartesian)[0] + speed_command = value.cartesian_speed + if len(speed_command) == 0: + speed_command = None + else: + speed_command1 = self._node.NamedArrayToArray(speed_command) + speed_command_lin = speed_command1[0][3:6]*1e3 + speed_command_rot = speed_command1[0][0:3] + speed_command = np.concatenate((speed_command_lin, speed_command_rot)) + external_joints = value.external_joints + if len(external_joints) == 0: + external_joints = None + external_joints_speed = value.external_joints_speed + if len(external_joints_speed) == 0: + external_joints_speed = None + rapid_to_robot = value.rapid_to_robot + if len(rapid_to_robot) == 0: + rapid_to_robot = None + self._egm.send_to_robot_cart(np.array(cart_command[4:7])*1e3, cart_command[0:4], + speed_command, external_joints, external_joints_speed, rapid_to_robot) + except: + traceback.print_exc() + + def _egm_correction_command_invalue_changed(self, value, ts, ep): + # print(f"egm_correction_command: {value.pos}, {value.age}") + try: + p = self._node.NamedArrayToArray(value.pos)[0]*1e3 + self._egm.send_to_robot_path_corr(p, value.age) + except: + traceback.print_exc() + +def main(): + + parser = argparse.ArgumentParser(description="ABB RWS/EGM robot driver service for Robot Raconteur") + parser.add_argument("--robot-info-file", type=argparse.FileType('r'),default=None,required=True,help="Robot info file (required)") + parser.add_argument("--robot-url", type=str,default="http://127.0.0.1:80",help="Robot Web Services URL") + parser.add_argument("--robot-name", type=str,default=None,help="Optional device name override") + + args, _ = parser.parse_known_args() + + RRC.RegisterStdRobDefServiceTypes(RRN) + register_service_types_from_resources(RRN, __package__, ["experimental.abb_robot.rws", "experimental.abb_robot.egm"]) + + with args.robot_info_file: + robot_info_text = args.robot_info_file.read() + + info_loader = InfoFileLoader(RRN) + robot_info, robot_ident_fd = info_loader.LoadInfoFileFromString(robot_info_text, "com.robotraconteur.robotics.robot.RobotInfo", "device") + + attributes_util = AttributesUtil(RRN) + robot_attributes = attributes_util.GetDefaultServiceAttributesFromDeviceInfo(robot_info.device_info) + + robot = ABBRWSRobotImpl(robot_info, args.robot_url) + try: + + robot._start_robot() + time.sleep(0.5) + with RR.ServerNodeSetup("experimental.abb_rws_robot.robot",59926): + + service_ctx = RRN.RegisterService("robot","experimental.abb_robot.rws.ABBRWSRobot",robot) + service_ctx.SetServiceAttributes(robot_attributes) + + print("Press Ctrl-C to exit") + drekar_launch_process.wait_exit() + robot._close() + except: + with suppress(Exception): + robot._close() + raise + + \ No newline at end of file diff --git a/src/abb_robot_client/robotraconteur/experimental.abb_robot.egm.robdef b/src/abb_robot_client/robotraconteur/experimental.abb_robot.egm.robdef new file mode 100644 index 0000000..edb1018 --- /dev/null +++ b/src/abb_robot_client/robotraconteur/experimental.abb_robot.egm.robdef @@ -0,0 +1,62 @@ +service experimental.abb_robot.egm + +import com.robotraconteur.datetime +import com.robotraconteur.geometry + +using com.robotraconteur.datetime.TimeSpec2 +using com.robotraconteur.geometry.Pose +using com.robotraconteur.geometry.Point +using com.robotraconteur.geometry.SpatialVelocity + +struct EGMRobot + field uint32 seqno + field uint32 tm + field TimeSpec2 feedback_time + field double[] joint_position + field Pose cartesian_position + field double[] external_joint_position + field double[] joint_position_command + field Pose cartesian_position_command + field double[] external_joint_position_command + field bool motor_state + field bool mci_convergence_met + field bool rapid_ctrl_exec_state + field double[] measured_force + field double utilization_rate + field uint32 move_index + field EGMCollisionInfo collision_info + field double[] rapid_from_robot +end + +struct EGMCollisionInfo + field bool collision_triggered + field double[] coll_det_quota +end + +# TODO: duplicated in experimental.abb_robot.motion_program +struct EGMJointTarget + field uint64 seqno + field uint64 state_seqno + field double[] joints + field double[] external_joints + field double[] joints_speed + field double[] external_joints_speed + field double[] rapid_to_robot +end + +struct EGMPoseTarget + field uint64 seqno + field uint64 state_seqno + field Pose cartesian + field double[] external_joints + field SpatialVelocity[1-] cartesian_speed + field double[] external_joints_speed + field double[] rapid_to_robot +end + +struct EGMPathCorrection + field uint64 seqno + field uint64 state_seqno + field Point pos + field uint32 age +end \ No newline at end of file diff --git a/src/abb_robot_client/robotraconteur/experimental.abb_robot.rws.robdef b/src/abb_robot_client/robotraconteur/experimental.abb_robot.rws.robdef new file mode 100644 index 0000000..10ab424 --- /dev/null +++ b/src/abb_robot_client/robotraconteur/experimental.abb_robot.rws.robdef @@ -0,0 +1,179 @@ +service experimental.abb_robot.rws + +import com.robotraconteur.device +import com.robotraconteur.robotics.robot +import com.robotraconteur.datetime +import com.robotraconteur.geometry +import com.robotraconteur.device.isoch +import com.robotraconteur.device.clock +import experimental.abb_robot.egm + +using com.robotraconteur.device.Device +using com.robotraconteur.device.DeviceInfo +using com.robotraconteur.robotics.robot.RobotInfo +using com.robotraconteur.robotics.robot.RobotOperationalMode +using com.robotraconteur.robotics.robot.RobotControllerState +using com.robotraconteur.robotics.robot.RobotState +using com.robotraconteur.robotics.robot.AdvancedRobotState +using com.robotraconteur.robotics.robot.RobotStateSensorData +using com.robotraconteur.datetime.DateTimeLocal +using com.robotraconteur.geometry.Pose +using com.robotraconteur.geometry.Point +using com.robotraconteur.device.isoch.IsochDevice +using com.robotraconteur.device.isoch.IsochInfo +using com.robotraconteur.device.clock.DeviceClock +using com.robotraconteur.device.clock.DeviceTime +using experimental.abb_robot.egm.EGMJointTarget +using experimental.abb_robot.egm.EGMPoseTarget +using experimental.abb_robot.egm.EGMPathCorrection +using experimental.abb_robot.egm.EGMRobot + + +enum TaskCycle + unknown = 0, + asis = 1, + forever, + once, + oncedone +end + +enum TaskExecutionState + unknown = 0, + running = 1, + stopped = 2 +end + +namedarray ConfData + field double cf1 + field double cf4 + field double cf6 + field double cfx +end + +struct RAPIDExecutionState + field TaskExecutionState ctrlexecstate + field TaskCycle cycle +end + +struct EventLogEntry + field int32 seqnum + field int32 msgtype + field DateTimeLocal tstamp + field string{list} args + field string title + field string desc + field string conseqs + field string causes + field string actions +end + +struct TaskState + field string name + field string type + field string taskstate + field TaskExecutionState excstate + field bool active + field bool motiontask +end + +struct IpcMessage + field string data + field string userdef + field string msgtype + field string cmd + field string queue_name +end + +struct JointTarget + field double[6] robax + field double[6] extax +end + +struct RobTarget + field Pose pose + field ConfData robconf + field double[6] extax +end + +object ABBRWSRobot + implements Device + implements DeviceClock + implements IsochDevice + + property DeviceInfo device_info [readonly,nolock] + property RobotInfo robot_info [readonly,nolock] + property RobotOperationalMode operational_mode [readonly, nolock] + property RobotControllerState controller_state [readonly, nolock] + + wire RobotState robot_state [readonly,nolock] + wire AdvancedRobotState advanced_robot_state [readonly,nolock] + pipe RobotStateSensorData robot_state_sensor_data [readonly,nolock] + property IsochInfo isoch_info [readonly,nolock] + property uint32 isoch_downsample [perclient] + wire DeviceTime device_clock_now [readonly,nolock] + + + function void start(TaskCycle cycle, string{list} tasks) + function void stop() + function void resetpp() + function void activate_task(string task) + function void deactivate_task(string task) + function TaskState{list} getf_tasks() + + function RAPIDExecutionState getf_execution_state() + function RobotControllerState getf_controller_state() + function RobotOperationalMode getf_operation_mode() + + function int32 getf_digital_io(string signal_name) + function void setf_digital_io(string signal_name, int32 value) + function int32 getf_digital_io2(string signal_name, string network, string unit) + function void setf_digital_io2(string signal_name, string network, string unit, int32 value) + + function double getf_analog_io(string signal_name) + function void setf_analog_io(string signal_name, double value) + function double getf_analog_io2(string signal_name, string network, string unit) + function void setf_analog_io2(string signal_name, string network, string unit, double value) + + function string{list} getf_rapid_variables(string task) + function string getf_rapid_variable(string var, string task) + function void setf_rapid_variable(string var, string task, string value) + + function string getf_ramdisk_path() + function uint8[] read_file(string filename) + function void upload_file(string filename, uint8[] data) + function void delete_file(string filename) + function string{list} list_files(string path) + + function EventLogEntry{list} read_event_log() + + function JointTarget getf_jointtarget(string mechunit) + + function RobTarget getf_robtarget(string mechunit) + + function RobTarget getf_robtarget2(string mechunit, string tool, string wobj, string coordinate) + + function JointTarget getf_rapid_variable_jointtarget(string var, string task) + function void setf_rapid_variable_jointtarget(string var, string task, JointTarget value) + function JointTarget{list} getf_rapid_variable_jointtarget_array(string var, string task) + function void setf_rapid_jointtarget_array(string var, string task, JointTarget{list} value) + + function double getf_rapid_variable_num(string var, string task) + function void setf_rapid_variable_num(string var, string task, double value) + function double[] getf_rapid_variable_num_array(string var, string task) + function void setf_rapid_variable_num_array(string var, string task, double[] value) + + property double speed_ratio + + wire EGMRobot egm_state [readonly] + wire EGMJointTarget egm_joint_command [writeonly] + wire EGMPoseTarget egm_pose_command [writeonly] + wire EGMPathCorrection egm_path_correction_command [writeonly] + + function varvalue getf_param(string param_name) + function void setf_param(string param_name, varvalue value) + event param_changed(string param_name) + + function void request_rmmp(double timeout) + function void poll_rmmp() +end + diff --git a/test/robotraconteur/test_abb_rws_driver.py b/test/robotraconteur/test_abb_rws_driver.py new file mode 100644 index 0000000..119e10e --- /dev/null +++ b/test/robotraconteur/test_abb_rws_driver.py @@ -0,0 +1,308 @@ +import RobotRaconteur as RR +import time +from contextlib import suppress +import numpy as np + +def _new_rr_client_node(): + n = RR.RobotRaconteurNode() + n.Init() + + t = RR.TcpTransport(n) + n.RegisterTransport(t) + + return n + +class _client: + def __init__(self, node, client): + self.node = node + self.client = client + self.rws_const = node.GetConstants("experimental.abb_robot.rws", client) + self.task_cycle = self.rws_const["TaskCycle"] + self.task_exec_state = self.rws_const["TaskExecutionState"] + self.egm_joint_command = node.GetStructureType("experimental.abb_robot.egm.EGMJointTarget", client) + self.egm_pose_command = node.GetStructureType("experimental.abb_robot.egm.EGMPoseTarget", client) + self.egm_path_corr_command = node.GetStructureType("experimental.abb_robot.egm.EGMPathCorrection", client) + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + self.node.Shutdown() + +def _connect_client(): + n = _new_rr_client_node() + + c = n.ConnectService("rr+tcp://localhost:59926?service=robot") + + return _client(n, c) + +def test_rws_metadata(): + + with _connect_client() as c_obj: + c = c_obj.client + + device_info = c.device_info + assert device_info is not None + assert device_info.device.name is not None + robot_info = c.robot_info + assert robot_info is not None + assert len(robot_info.chains) > 0 + n_joints = len(robot_info.joint_info) + assert n_joints >= 6 + assert len(robot_info.chains[0].H) == n_joints + assert len(robot_info.chains[0].P) == n_joints + 1 + +def test_rws_controller_state(): + with _connect_client() as c_obj: + c = c_obj.client + + print(c.getf_execution_state()) + print(c.getf_controller_state()) + print(c.getf_operation_mode()) + + _ = c.operational_mode + _ = c.controller_state + +def test_rws_tasks(): + with _connect_client() as c_obj: + c = c_obj.client + + with suppress(Exception): + c.stop() + time.sleep(0.5) + c.setf_analog_io("mode", 0) + c.resetpp() + c.start(c_obj.task_cycle["forever"], ["T_ROB1"]) + time.sleep(0.1) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["running"] + assert c.getf_execution_state().cycle == c_obj.task_cycle["forever"] + time.sleep(1) + c.stop() + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["stopped"] + + task_state = c.getf_tasks() + + c.deactivate_task("T_ROB1") + c.activate_task("T_ROB1") + +def test_rws_egm_feedback(): + with _connect_client() as c_obj: + c = c_obj.client + + robot_info = c.robot_info + n_joints = len(robot_info.joint_info) + + # Make sure EGM is running + c.resetpp() + c.start(c_obj.task_cycle["forever"], ["T_ROB1"]) + time.sleep(1) + c.stop() + + robot_state, _ = c.robot_state.PeekInValue() + assert robot_state is not None + assert len(robot_state.joint_position) == n_joints + adv_robot_state, _ = c.advanced_robot_state.PeekInValue() + assert adv_robot_state is not None + assert len(adv_robot_state.joint_position) == n_joints + robot_state_sns_ep = c.robot_state_sensor_data.Connect(-1) + robot_state_sns = robot_state_sns_ep.ReceivePacketWait(1) + robot_state_sns_ep.Close() + assert robot_state_sns is not None + assert robot_state_sns.data_header is not None + assert len(robot_state_sns.robot_state.joint_position) == n_joints + + egm_state, _ = c.egm_state.PeekInValue() + assert egm_state is not None + +def test_rws_isoch(): + with _connect_client() as c_obj: + c = c_obj.client + isoch_info = c.isoch_info + print(isoch_info) + assert isoch_info is not None + isoch_downsample = c.isoch_downsample + assert isoch_downsample == 0 + c.isoch_downsample = 1 + assert c.isoch_downsample == 1 + c.isoch_downsample = 0 + assert c.isoch_downsample == 0 + + device_now, _ = c.device_clock_now.PeekInValue() + print(device_now) + +def test_rws_signal(): + with _connect_client() as c_obj: + c = c_obj.client + c.setf_digital_io("test_digital_io1", 1) + assert c.getf_digital_io("test_digital_io1") == 1 + c.setf_digital_io("test_digital_io1", 0) + assert c.getf_digital_io("test_digital_io1") == 0 + + c.setf_digital_io2("test_digital_io2", "DeviceNet", "d651", 1) + assert c.getf_digital_io2("test_digital_io2", "DeviceNet", "d651") == 1 + c.setf_digital_io2("test_digital_io2", "DeviceNet", "d651", 0) + assert c.getf_digital_io2("test_digital_io2", "DeviceNet", "d651") == 0 + + c.setf_analog_io("test_analog_io1", 14.285) + assert abs(c.getf_analog_io("test_analog_io1") - 14.285) < 0.001 + c.setf_analog_io("test_analog_io1", 0) + assert abs(c.getf_analog_io("test_analog_io1") - 0) < 0.001 + + c.setf_analog_io2("test_analog_io2", "DeviceNet", "d651", 8.562) + assert abs(c.getf_analog_io2("test_analog_io2", "DeviceNet", "d651") - 8.562) < 0.001 + c.setf_analog_io2("test_analog_io2", "DeviceNet", "d651", 0) + assert abs(c.getf_analog_io2("test_analog_io2", "DeviceNet", "d651") - 0) < 0.001 + +def test_rws_rapid_vars(): + with _connect_client() as c_obj: + c = c_obj.client + # TODO: getf_rapid_variables() is not implemented + # rapid_vars = c.getf_rapid_variables("T_ROB1") + # assert "test_var_str" in rapid_vars + c.setf_rapid_variable("test_var_str", "T_ROB1", "\"Hello World!\"") + assert c.getf_rapid_variable("test_var_str", "T_ROB1") == "\"Hello World!\"" + c.setf_rapid_variable("test_var_num", "T_ROB1/Module1", str(123.456)) + assert abs(float(c.getf_rapid_variable("test_var_num", "T_ROB1/Module1")) - 123.456) < 0.001 + +def test_rws_files(): + with _connect_client() as c_obj: + c = c_obj.client + + ramdisk_path = c.getf_ramdisk_path() + data = "Hello World from file!".encode("utf-8") + c.upload_file(ramdisk_path + "/test_file.txt", data) + file_list = c.list_files(ramdisk_path) + assert "test_file.txt" in file_list + assert c.read_file(ramdisk_path + "/test_file.txt").tobytes() == data + c.delete_file(ramdisk_path + "/test_file.txt") + file_list = c.list_files(ramdisk_path) + assert "test_file.txt" not in file_list + +def test_evtlog(): + with _connect_client() as c_obj: + c = c_obj.client + + evts = c.read_event_log() + + print(evts) + +def test_current_targets(): + with _connect_client() as c_obj: + c = c_obj.client + + c.getf_jointtarget("ROB_1") + c.getf_robtarget("ROB_1") + c.getf_robtarget2("ROB_1", "tool0", "wobj0", "Base") + +def test_speed_ratio(): + with _connect_client() as c_obj: + c = c_obj.client + + _ = c.speed_ratio + c.speed_ratio= 0.5 + assert c.speed_ratio == 0.5 + c.speed_ratio = 1.0 + assert c.speed_ratio == 1.0 + +def test_egm_position_command(): + with _connect_client() as c_obj: + c = c_obj.client + + with suppress(Exception): + c.stop() + time.sleep(0.5) + c.setf_analog_io("mode", 1) + c.resetpp() + c.start(c_obj.task_cycle["once"], ["T_ROB1"]) + time.sleep(2) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["running"] + cmd = c_obj.egm_joint_command() + cmd.joints = [21, -10, -20, 30, 40, 50] + cmd.external_joints = [90,100] + cmd.joints_speed = [10,10,10,10,10,10] + cmd.external_joints_speed = [10,10] + cmd.rapid_to_robot = [88,99] + c.egm_joint_command.PokeOutValue(cmd) + time.sleep(3) + cmd2 = c_obj.egm_joint_command() + cmd2.joints = cmd.joints + cmd2.joints_speed = [0,0,0,0,0,0] + c.egm_joint_command.PokeOutValue(cmd2) + time.sleep(2) + egm_state, _ = c.egm_state.PeekInValue() + np.testing.assert_allclose(egm_state.joint_position, cmd.joints, atol=0.1) + c.setf_digital_io("stop_egm", 1) + time.sleep(3) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["stopped"] + +def test_egm_pose_command(): + with _connect_client() as c_obj: + c = c_obj.client + + with suppress(Exception): + c.stop() + time.sleep(0.5) + c.setf_analog_io("mode", 2) + c.resetpp() + c.start(c_obj.task_cycle["once"], ["T_ROB1"]) + time.sleep(2) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["running"] + cmd = c_obj.egm_pose_command() + cmd.cartesian[0]["position"]["x"] = 0.535 + cmd.cartesian[0]["position"]["y"] = 0.181 + cmd.cartesian[0]["position"]["z"] = 0.625 + cmd.cartesian[0]["orientation"]["w"] = 0.704416 + cmd.cartesian[0]["orientation"]["x"] = 0.1227878 + cmd.cartesian[0]["orientation"]["y"] = 0.6963642 + cmd.cartesian[0]["orientation"]["z"] = 0.0616284 + cmd.external_joints = [90,100] + cmd.cartesian_speed.resize((1,)) + cmd.cartesian_speed[0]["linear"]["x"] = 0.1 + cmd.cartesian_speed[0]["linear"]["y"] = 0.11 + cmd.cartesian_speed[0]["linear"]["z"] = 0.12 + cmd.cartesian_speed[0]["angular"]["x"] = 0.13 + cmd.cartesian_speed[0]["angular"]["y"] = 0.14 + cmd.cartesian_speed[0]["angular"]["z"] = 0.15 + cmd.external_joints_speed = [10,10] + cmd.rapid_to_robot = [82,93] + c.egm_pose_command.PokeOutValue(cmd) + time.sleep(3) + cmd2 = c_obj.egm_pose_command() + cmd2.cartesian = cmd.cartesian + c.egm_pose_command.PokeOutValue(cmd2) + time.sleep(2) + egm_state, _ = c.egm_state.PeekInValue() + np.testing.assert_allclose(c_obj.node.NamedArrayToArray(egm_state.cartesian_position), \ + c_obj.node.NamedArrayToArray(cmd.cartesian), atol=0.1) + c.setf_digital_io("stop_egm", 1) + time.sleep(3) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["stopped"] + +def test_egm_path_corr(): + with _connect_client() as c_obj: + c = c_obj.client + + with suppress(Exception): + c.stop() + time.sleep(0.5) + c.setf_analog_io("mode", 3) + c.resetpp() + c.start(c_obj.task_cycle["once"], ["T_ROB1"]) + time.sleep(2) + assert c.getf_execution_state().ctrlexecstate == c_obj.task_exec_state["running"] + cmd1 = c_obj.egm_path_corr_command() + cmd1.pos[0]["x"] = 0.1 + cmd1.pos[0]["y"] = 0.2 + cmd1.age = 1 + cmd2 = c_obj.egm_path_corr_command() + cmd2.pos[0]["x"] = -0.1 + cmd2.pos[0]["y"] = -0.2 + cmd2.age = 1 + for i in range(10): + c.egm_path_correction_command.PokeOutValue(cmd1) + time.sleep(0.5) + c.egm_path_correction_command.PokeOutValue(cmd2) + time.sleep(0.5) + time.sleep(0.1) + c.stop() +