├── src └── rpi_abb_irc5 │ ├── ros │ ├── __init__.py │ └── rapid_commander.py │ ├── __init__.py │ ├── egm.proto │ ├── rpi_abb_irc5.py │ └── egm_pb2.py ├── srv ├── RapidStop.srv ├── RapidSetVariable.srv ├── RapidGetDigitalIO.srv ├── RapidGetVariable.srv ├── RapidReadEventLog.srv ├── RapidSetDigitalIO.srv ├── RapidStart.srv └── RapidGetStatus.srv ├── launch ├── abb_irc5_egm_driver.launch ├── abb_irc5_rapid_joint_states_publisher.launch └── abb_irc5_rapid_driver.launch ├── msg └── RapidEventLogMessage.msg ├── README.md ├── setup.py ├── scripts ├── test_rpi_abb_irc5_rapid ├── test_rpi_abb_irc5_egm ├── abb_irc5_rapid_joint_states_publisher_ros ├── abb_irc5_egm_driver_ros └── abb_irc5_rapid_driver_ros ├── .gitignore ├── LICENSE ├── package.xml ├── CMakeLists.txt └── rapid └── rpi_abb_irc5_rapid.mod /src/rpi_abb_irc5/ros/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /srv/RapidStop.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | -------------------------------------------------------------------------------- /srv/RapidSetVariable.srv: -------------------------------------------------------------------------------- 1 | string name 2 | string value 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /srv/RapidGetDigitalIO.srv: -------------------------------------------------------------------------------- 1 | string signal 2 | --- 3 | bool success 4 | int32 lvalue 5 | -------------------------------------------------------------------------------- /srv/RapidGetVariable.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | bool success 4 | string value 5 | 6 | -------------------------------------------------------------------------------- /srv/RapidReadEventLog.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | RapidEventLogMessage[] messages 4 | -------------------------------------------------------------------------------- /srv/RapidSetDigitalIO.srv: -------------------------------------------------------------------------------- 1 | string signal 2 | int32 lvalue 3 | --- 4 | bool success 5 | 6 | -------------------------------------------------------------------------------- /src/rpi_abb_irc5/__init__.py: -------------------------------------------------------------------------------- 1 | from __future__ import absolute_import 2 | 3 | from .rpi_abb_irc5 import * 4 | -------------------------------------------------------------------------------- /launch/abb_irc5_egm_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /srv/RapidStart.srv: -------------------------------------------------------------------------------- 1 | string CYCLE_ASIS="asis" 2 | string CYCLE_ONCE="once" 3 | string CYCLE_ONCE_DONE="oncedone" 4 | string CYCLE_FOREVER="forever" 5 | bool reset_pp 6 | string cycle 7 | --- 8 | bool success 9 | -------------------------------------------------------------------------------- /msg/RapidEventLogMessage.msg: -------------------------------------------------------------------------------- 1 | string MSG_TYPE_INFO=1 2 | string MSG_TYPE_WARNING=2 3 | string MSG_TYPE_ERROR=3 4 | int32 msgtype 5 | int32 code 6 | time tstamp 7 | string[] args 8 | string title 9 | string desc 10 | string conseqs 11 | string causes 12 | string actions 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DEPRECATED 2 | 3 | Please use the newer https://github.com/rpiRobotics/abb_robot_client package 4 | 5 | # rpi_abb_irc5 6 | ROS package providing a Python API to interact with an ABB IRC5 robot controller using the built in WebServices and Externally Guided Motion (EGM) features. 7 | -------------------------------------------------------------------------------- /launch/abb_irc5_rapid_joint_states_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['rpi_abb_irc5'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /scripts/test_rpi_abb_irc5_rapid: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rpi_abb_irc5 4 | import sys 5 | 6 | def main(): 7 | 8 | if (len(sys.argv) >= 2): 9 | rapid=rpi_abb_irc5.RAPID(sys.argv[1]) 10 | else: 11 | rapid=rpi_abb_irc5.RAPID() 12 | print rapid.get_execution_state() 13 | rapid.resetpp() 14 | rapid.start() 15 | print rapid.get_execution_state() 16 | 17 | if __name__ == '__main__': 18 | main() 19 | 20 | -------------------------------------------------------------------------------- /launch/abb_irc5_rapid_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /scripts/test_rpi_abb_irc5_egm: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rpi_abb_irc5 4 | import time 5 | import math 6 | import numpy as np 7 | 8 | def main(): 9 | try: 10 | egm=rpi_abb_irc5.EGM() 11 | t1=time.time() 12 | while True: 13 | res, state=egm.receive_from_robot(.01) 14 | angle=np.deg2rad(60*((time.time()-t1)/10)) 15 | if res: 16 | print "ID: " + str(state.robot_message.header.seqno) + " Joints: " + str(np.rad2deg(state.joint_angles)) 17 | joint_angles=[angle]*6 18 | egm.send_to_robot(state.joint_angles) 19 | except KeyboardInterrupt: 20 | pass 21 | 22 | if __name__ == '__main__': 23 | main() 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | 17 | # Generated by dynamic reconfigure 18 | *.cfgc 19 | /cfg/cpp/ 20 | /cfg/*.py 21 | 22 | # Ignore generated docs 23 | *.dox 24 | *.wikidoc 25 | 26 | # eclipse stuff 27 | .project 28 | .cproject 29 | 30 | # qcreator stuff 31 | CMakeLists.txt.user 32 | 33 | srv/_*.py 34 | *.pcd 35 | *.pyc 36 | qtcreator-* 37 | *.user 38 | 39 | /planning/cfg 40 | /planning/docs 41 | /planning/src 42 | 43 | *~ 44 | 45 | # Emacs 46 | .#* 47 | 48 | # Catkin custom files 49 | CATKIN_IGNORE 50 | -------------------------------------------------------------------------------- /srv/RapidGetStatus.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string CYCLE_ASIS="asis" 3 | string CYCLE_ONCE="once" 4 | string CYCLE_ONCE_DONE="oncedone" 5 | string CYCLE_FOREVER="forever" 6 | string OPMODE_INIT="INIT" #State init 7 | string OPMODE_AUTO_CH="AUTO_CH" #State change request for automatic mode 8 | string OPMODE_MANF_CH="MANF_CH" #State change request for manual mode & full speed 9 | string OPMODE_MANR="MANR" #State manual mode & reduced speed 10 | string OPMODE_MANF="MANF" #State manual mode & full speed 11 | string OPMODE_AUTO="AUTO" #State automatic mode 12 | string OPMODE_UNDEFINED="UNDEFINED" #Undefined 13 | string CTRLSTATE_INIT="init" 14 | string CTRLSTATE_MOTORON="motoron" 15 | string CTRLSTATE_MOTOROFF="motoroff" 16 | string CTRLSTATE_GUARDSTOP="guardstop" 17 | string CTRLSTATE_EMERGENCYSTOP="emergencystop" 18 | string CTRLSTATE_EMERGENCYSTOPRESET="emergencystopreset" 19 | string CTRLSTATE_SYSFAIL="sysfail" 20 | bool success 21 | bool running 22 | string cycle 23 | string opmode 24 | string ctrlstate 25 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, rpiRobotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /scripts/abb_irc5_rapid_joint_states_publisher_ros: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2017, Rensselaer Polytechnic Institute, Wason Technology LLC 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Rensselaer Polytechnic Institute, or Wason 15 | # Technology LLC, nor the names of its contributors may be used to 16 | # endorse or promote products derived from this software without 17 | # specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import rospy 32 | from rpi_abb_irc5 import RAPID 33 | from sensor_msgs.msg import JointState 34 | 35 | def main(): 36 | 37 | rospy.init_node('abb_irc5_rapid_joint_states_publisher') 38 | 39 | robot_host=rospy.get_param('~abb_irc5_uri') 40 | 41 | rapid=RAPID(robot_host) 42 | 43 | joint_state_pub=rospy.Publisher('joint_states',JointState, queue_size=100) 44 | 45 | rate=rospy.Rate(10) 46 | while not rospy.is_shutdown(): 47 | try: 48 | a=rapid.get_jointtarget() 49 | j=JointState() 50 | j.header.stamp=rospy.Time.now() 51 | j.name=['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'] 52 | j.position=a.robax 53 | joint_state_pub.publish(j) 54 | except: 55 | #TODO: report error? 56 | pass 57 | 58 | rate.sleep() 59 | 60 | if __name__ == '__main__': 61 | main() 62 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rpi_abb_irc5 4 | 0.1.0 5 | This package provides a Python API to interact with an ABB IRC5 robot controller using the built in WebServices and Externally Guided Motion (EGM) features. 6 | 7 | 8 | 9 | 10 | John Wason 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | http://github.com/rpiRobotics/rpi_abb_irc5 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | python-beautifulsoup 54 | python-protobuf 55 | python-requests 56 | python-websocket 57 | rospy 58 | python-beautifulsoup 59 | python-protobuf 60 | python-requests 61 | python-websocket 62 | rospy 63 | python-beautifulsoup 64 | python-protobuf 65 | python-requests 66 | python-ws4py 67 | python-numpy 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /scripts/abb_irc5_egm_driver_ros: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2017, Rensselaer Polytechnic Institute, Wason Technology LLC 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Rensselaer Polytechnic Institute, or Wason 15 | # Technology LLC, nor the names of its contributors may be used to 16 | # endorse or promote products derived from this software without 17 | # specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import rospy 32 | import rpi_abb_irc5 33 | from std_msgs.msg import Float64 34 | from sensor_msgs.msg import JointState 35 | import threading 36 | import copy 37 | 38 | joint_setpoint = None 39 | joint_setpoint_lock = threading.Lock() 40 | 41 | def joint_command_cb(i, msg): 42 | with joint_setpoint_lock: 43 | joint_setpoint[i] = msg.data 44 | 45 | def main(): 46 | 47 | global joint_setpoint 48 | 49 | rospy.init_node('abb_irc5_egm') 50 | 51 | egm_port = int(rospy.get_param('~egm_port', 6510)) 52 | joint_names = rospy.get_param('controller_joint_names') 53 | if not isinstance(joint_names, list): 54 | raise Exception("Invalid joint name list") 55 | 56 | joint_setpoint = [None] * len(joint_names) 57 | 58 | egm = rpi_abb_irc5.EGM(port = egm_port) 59 | 60 | joint_states_pub = rospy.Publisher("joint_states", JointState, queue_size = 10) 61 | 62 | joint_command_subs = [None] * len(joint_names) 63 | for i in xrange(len(joint_command_subs)): 64 | joint_command_subs[i] = rospy.Subscriber(joint_names[i] + "_position_controller/command", Float64, 65 | lambda msg, i=i: joint_command_cb(i,msg)) 66 | 67 | while not rospy.is_shutdown(): 68 | res, state=egm.receive_from_robot(0.1) 69 | if res: 70 | if (len(state.joint_angles) != len(joint_names)): 71 | raise Exception("controller_joint_names list length mismatch") 72 | 73 | joint_states = JointState() 74 | joint_states.header.stamp = rospy.Time.now() 75 | joint_states.name = joint_names 76 | joint_states.position = state.joint_angles 77 | joint_states_pub.publish(joint_states) 78 | 79 | for i in xrange(len(joint_setpoint)): 80 | if joint_setpoint[i] is None: 81 | joint_setpoint[i] = state.joint_angles[i] 82 | 83 | with joint_setpoint_lock: 84 | joint_angles=copy.copy(joint_setpoint) 85 | egm.send_to_robot(joint_angles) 86 | 87 | if __name__ == '__main__': 88 | main() 89 | 90 | -------------------------------------------------------------------------------- /src/rpi_abb_irc5/egm.proto: -------------------------------------------------------------------------------- 1 | // Definition of ABB sensor interface V1.0 2 | // 3 | // messages of type EgmRobot are sent out from the robot controller 4 | // messages of type EgmSensor are sent to the robot controller 5 | // 6 | package abb.egm; 7 | 8 | message EgmHeader 9 | { 10 | optional uint32 seqno = 1; // sequence number (to be able to find lost messages) 11 | optional uint32 tm = 2; // time stamp in ms 12 | 13 | enum MessageType { 14 | MSGTYPE_UNDEFINED = 0; 15 | MSGTYPE_COMMAND = 1; // for future use 16 | MSGTYPE_DATA = 2; // sent by robot controller 17 | MSGTYPE_CORRECTION = 3; // sent by sensor 18 | MSGTYPE_PATH_CORRECTION = 4; // sent by sensor 19 | } 20 | 21 | optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED]; 22 | } 23 | 24 | message EgmCartesian // Cartesian position in mm, interpreted relative to the sensor frame defined by EGMActPose 25 | { 26 | required double x = 1; 27 | required double y = 2; 28 | required double z = 3; 29 | } 30 | 31 | // If you have pose input, i.e. not joint input, you can choose to send orientation data as quaternion or as Euler angles. 32 | // If both are sent, Euler angles have higher priority. 33 | 34 | message EgmQuaternion // Quaternion orientation interpreted relative to the sensor frame defined by EGMActPose 35 | { 36 | required double u0 = 1; 37 | required double u1 = 2; 38 | required double u2 = 3; 39 | required double u3 = 4; 40 | } 41 | 42 | message EgmEuler // Euler angle orientation in degrees, interpreted relative to the sensor frame defined by EGMActPose 43 | { 44 | required double x = 1; 45 | required double y = 2; 46 | required double z = 3; 47 | } 48 | 49 | message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) interpreted relative to the sensor frame defined by EGMActPose 50 | { 51 | optional EgmCartesian pos = 1; 52 | optional EgmQuaternion orient = 2; 53 | optional EgmEuler euler = 3; 54 | } 55 | 56 | message EgmCartesianSpeed // Array of 6 speed reference values in mm/s or degrees/s 57 | { 58 | repeated double value = 1; 59 | } 60 | 61 | message EgmJoints // Array of 6 joint values for TCP robot in degrees 62 | { 63 | repeated double joints = 1; 64 | } 65 | 66 | message EgmExternalJoints // Array of 6 joint values for additional axis in degrees 67 | { 68 | repeated double joints = 1; 69 | } 70 | 71 | message EgmPlanned // Planned position for robot (joints or cartesian) and additional axis (array of 6 values) 72 | { 73 | optional EgmJoints joints = 1; 74 | optional EgmPose cartesian = 2; 75 | optional EgmJoints externalJoints = 3; 76 | } 77 | 78 | message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values) 79 | { 80 | optional EgmJoints joints = 1; 81 | optional EgmCartesianSpeed cartesians = 2; 82 | optional EgmJoints externalJoints = 3; 83 | } 84 | 85 | 86 | message EgmPathCorr // Cartesian path correction and measurment age 87 | { 88 | required EgmCartesian pos = 1; // Sensor measurement (x, y, z) 89 | required uint32 age = 2; // Sensor measurement age in ms 90 | } 91 | 92 | 93 | message EgmFeedBack // Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values) 94 | { 95 | optional EgmJoints joints = 1; 96 | optional EgmPose cartesian = 2; 97 | optional EgmJoints externalJoints = 3; 98 | } 99 | 100 | message EgmMotorState // Motor state 101 | { 102 | enum MotorStateType { 103 | MOTORS_UNDEFINED = 0; 104 | MOTORS_ON = 1; 105 | MOTORS_OFF = 2; 106 | } 107 | 108 | required MotorStateType state = 1; 109 | } 110 | 111 | message EgmMCIState // EGM state 112 | { 113 | enum MCIStateType { 114 | MCI_UNDEFINED = 0; 115 | MCI_ERROR = 1; 116 | MCI_STOPPED = 2; 117 | MCI_RUNNING = 3; 118 | } 119 | 120 | required MCIStateType state = 1 [default = MCI_UNDEFINED]; 121 | } 122 | 123 | message EgmRapidCtrlExecState // RAPID execution state 124 | { 125 | enum RapidCtrlExecStateType { 126 | RAPID_UNDEFINED = 0; 127 | RAPID_STOPPED = 1; 128 | RAPID_RUNNING = 2; 129 | }; 130 | 131 | required RapidCtrlExecStateType state = 1 [default = RAPID_UNDEFINED]; 132 | } 133 | 134 | message EgmTestSignals // Test signals 135 | { 136 | repeated double signals = 1; 137 | } 138 | 139 | 140 | // Robot controller outbound message 141 | message EgmRobot 142 | { 143 | optional EgmHeader header = 1; 144 | optional EgmFeedBack feedBack = 2; 145 | optional EgmPlanned planned = 3; 146 | 147 | optional EgmMotorState motorState = 4; 148 | optional EgmMCIState mciState = 5; 149 | optional bool mciConvergenceMet = 6; 150 | optional EgmTestSignals testSignals = 7; 151 | optional EgmRapidCtrlExecState rapidExecState = 8; 152 | } 153 | 154 | 155 | // Robot controller inbound message, sent from sensor 156 | message EgmSensor 157 | { 158 | optional EgmHeader header = 1; 159 | optional EgmPlanned planned = 2; 160 | optional EgmSpeedRef speedRef = 3; 161 | } 162 | 163 | // Robot controller inbound message, sent from sensor 164 | message EgmSensorPathCorr 165 | { 166 | optional EgmHeader header = 1; 167 | optional EgmPathCorr pathCorr = 2; 168 | } 169 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rpi_abb_irc5) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | message_generation 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | add_message_files( 51 | FILES 52 | RapidEventLogMessage.msg 53 | ) 54 | 55 | ## Generate services in the 'srv' folder 56 | add_service_files( 57 | FILES 58 | RapidStart.srv 59 | RapidStop.srv 60 | RapidGetStatus.srv 61 | RapidGetDigitalIO.srv 62 | RapidSetDigitalIO.srv 63 | RapidReadEventLog.srv 64 | RapidGetVariable.srv 65 | RapidSetVariable.srv 66 | ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | generate_messages( 77 | DEPENDENCIES 78 | std_msgs # Or other packages containing msgs 79 | ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | # generate_dynamic_reconfigure_options( 97 | # cfg/DynReconf1.cfg 98 | # cfg/DynReconf2.cfg 99 | # ) 100 | 101 | ################################### 102 | ## catkin specific configuration ## 103 | ################################### 104 | ## The catkin_package macro generates cmake config files for your package 105 | ## Declare things to be passed to dependent projects 106 | ## INCLUDE_DIRS: uncomment this if your package contains header files 107 | ## LIBRARIES: libraries you create in this project that dependent projects also need 108 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 109 | ## DEPENDS: system dependencies of this project that dependent projects also need 110 | catkin_package( 111 | # INCLUDE_DIRS include 112 | # LIBRARIES rpi_abb_irc5 113 | # CATKIN_DEPENDS python-beautifulsoup python-protobuf python-requests python-websocket 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | # include 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/rpi_abb_irc5.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/rpi_abb_irc5_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | # target_link_libraries(${PROJECT_NAME}_node 155 | # ${catkin_LIBRARIES} 156 | # ) 157 | 158 | ############# 159 | ## Install ## 160 | ############# 161 | 162 | # all install targets should use catkin DESTINATION variables 163 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 164 | 165 | ## Mark executable scripts (Python etc.) for installation 166 | ## in contrast to setup.py, you can choose the destination 167 | # install(PROGRAMS 168 | # scripts/my_python_script 169 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark executables and/or libraries for installation 173 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rpi_abb_irc5.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | 206 | -------------------------------------------------------------------------------- /src/rpi_abb_irc5/ros/rapid_commander.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2017, Rensselaer Polytechnic Institute, Wason Technology LLC 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Rensselaer Polytechnic Institute, or Wason 13 | # Technology LLC, nor the names of its contributors may be used to 14 | # endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | import rospy 30 | from collections import namedtuple 31 | 32 | from ..srv import \ 33 | RapidStart, RapidStartRequest, RapidStartResponse, \ 34 | RapidStop, RapidStopRequest, RapidStopResponse, \ 35 | RapidGetStatus, RapidGetStatusRequest, RapidGetStatusResponse, \ 36 | RapidGetDigitalIO, RapidGetDigitalIORequest, RapidGetDigitalIOResponse, \ 37 | RapidSetDigitalIO, RapidSetDigitalIORequest, RapidSetDigitalIOResponse, \ 38 | RapidReadEventLog, RapidReadEventLogRequest, RapidReadEventLogResponse 39 | 40 | from ..msg import RapidEventLogMessage 41 | 42 | class RAPIDCommander(object): 43 | 44 | CYCLE_ASIS=RapidGetStatusResponse.CYCLE_ASIS 45 | CYCLE_ONCE=RapidGetStatusResponse.CYCLE_ONCE 46 | CYCLE_ONCE_DONE=RapidGetStatusResponse.CYCLE_ONCE_DONE 47 | CYCLE_FOREVER=RapidGetStatusResponse.CYCLE_FOREVER 48 | OPMODE_INIT=RapidGetStatusResponse.OPMODE_INIT #State init 49 | OPMODE_AUTO_CH=RapidGetStatusResponse.OPMODE_AUTO_CH #State change request for automatic mode 50 | OPMODE_MANF_CH=RapidGetStatusResponse.OPMODE_MANF_CH #State change request for manual mode & full speed 51 | OPMODE_MANR=RapidGetStatusResponse.OPMODE_MANR #State manual mode & reduced speed 52 | OPMODE_MANF=RapidGetStatusResponse.OPMODE_MANF #State manual mode & full speed 53 | OPMODE_AUTO=RapidGetStatusResponse.OPMODE_AUTO #State automatic mode 54 | OPMODE_UNDEFINED=RapidGetStatusResponse.OPMODE_UNDEFINED #State undefined 55 | CTRLSTATE_INIT=RapidGetStatusResponse.CTRLSTATE_INIT 56 | CTRLSTATE_MOTORON=RapidGetStatusResponse.CTRLSTATE_MOTORON 57 | CTRLSTATE_MOTOROFF=RapidGetStatusResponse.CTRLSTATE_MOTOROFF 58 | CTRLSTATE_GUARDSTOP=RapidGetStatusResponse.CTRLSTATE_GUARDSTOP 59 | CTRLSTATE_EMERGENCYSTOP=RapidGetStatusResponse.CTRLSTATE_EMERGENCYSTOP 60 | CTRLSTATE_EMERGENCYSTOPRESET=RapidGetStatusResponse.CTRLSTATE_EMERGENCYSTOPRESET 61 | CTRLSTATE_SYSFAIL=RapidGetStatusResponse.CTRLSTATE_SYSFAIL 62 | 63 | LOG_MSG_TYPE_INFO = RapidEventLogMessage.MSG_TYPE_INFO 64 | LOG_MSG_TYPE_WARNING = RapidEventLogMessage.MSG_TYPE_WARNING 65 | LOG_MSG_TYPE_ERROR = RapidEventLogMessage.MSG_TYPE_ERROR 66 | 67 | 68 | 69 | def __init__(self, ns='rapid'): 70 | self.ns=ns 71 | 72 | self._start_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'start'), \ 73 | RapidStart) 74 | 75 | self._stop_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'stop'), \ 76 | RapidStop) 77 | 78 | self._get_digital_io_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'get_digital_io'), \ 79 | RapidGetDigitalIO) 80 | self._set_digital_io_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'set_digital_io'), \ 81 | RapidSetDigitalIO) 82 | 83 | self._get_status_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'status'), \ 84 | RapidGetStatus) 85 | 86 | self._read_event_log_srv=rospy.ServiceProxy(rospy.names.ns_join(ns,'read_event_log'), \ 87 | RapidReadEventLog) 88 | 89 | def start(self, reset_pp=True, cycle="asis"): 90 | 91 | req=RapidStartRequest() 92 | req.reset_pp=reset_pp 93 | req.cycle=cycle 94 | 95 | self._start_srv.wait_for_service(1.0) 96 | res=self._start_srv(req) 97 | 98 | if not res.success: 99 | raise Exception("RAPID Start Failed") 100 | 101 | def stop(self): 102 | 103 | req=RapidStopRequest() 104 | 105 | self._stop_srv.wait_for_service(1.0) 106 | res=self._stop_srv(req) 107 | 108 | if not res.success: 109 | raise Exception("RAPID Stop Failed") 110 | 111 | def get_digital_io(self, signal): 112 | req=RapidGetDigitalIORequest() 113 | req.signal=signal 114 | 115 | self._get_digital_io_srv.wait_for_service(1.0) 116 | res=self._get_digital_io_srv(req) 117 | 118 | if not res.success: 119 | raise Exception("RAPID Get Digital IO Failed") 120 | 121 | return res.lvalue 122 | 123 | def set_digital_io(self, signal, lvalue): 124 | req=RapidSetDigitalIORequest() 125 | req.signal=signal 126 | req.lvalue=lvalue 127 | 128 | self._set_digital_io_srv.wait_for_service(1.0) 129 | res=self._set_digital_io_srv(req) 130 | 131 | if not res.success: 132 | raise Exception("RAPID Set Digital IO Failed") 133 | 134 | def get_status(self): 135 | req=RapidGetStatusRequest() 136 | 137 | self._get_status_srv.wait_for_service(1.0) 138 | res=self._get_status_srv(req) 139 | 140 | if not res.success: 141 | raise Exception("RAPID Get Status Failed") 142 | 143 | return RAPIDNodeStatus(res.running, res.cycle, res.opmode, res.ctrlstate) 144 | 145 | def read_event_log(self): 146 | req=RapidReadEventLogRequest() 147 | 148 | self._read_event_log_srv.wait_for_service(1.0) 149 | res=self._read_event_log_srv(req) 150 | 151 | if not res.success: 152 | raise Exception("RAPID Read Event Log Failed") 153 | 154 | ret=[] 155 | for e in res.messages: 156 | ret.append(RAPIDEventLogEntry(e.msgtype, e.code, e.tstamp, e.args, e.title, e.desc, e.conseqs, \ 157 | e.causes, e.actions)) 158 | 159 | return ret 160 | 161 | RAPIDNodeStatus=namedtuple('RAPIDNodeStatus', ['running','cycle','opmode','ctrlstate'], verbose=False) 162 | RAPIDEventLogEntry=namedtuple('RAPIDNodeEventLogEntry', ['msgtype', 'code', 'tstamp', 'args', 'title', \ 163 | 'desc', 'conseqs', 'causes', 'actions']) 164 | 165 | 166 | -------------------------------------------------------------------------------- /rapid/rpi_abb_irc5_rapid.mod: -------------------------------------------------------------------------------- 1 | MODULE rpi_abb_irc5_rapid 2 | 3 | PERS jointtarget JointTrajectory_0{10}; 4 | PERS jointtarget JointTrajectory_1{10}; 5 | PERS jointtarget JointTrajectory_2{10}; 6 | PERS jointtarget JointTrajectory_3{10}; 7 | PERS jointtarget JointTrajectory_4{10}; 8 | PERS jointtarget JointTrajectory_5{10}; 9 | PERS jointtarget JointTrajectory_6{10}; 10 | PERS jointtarget JointTrajectory_7{10}; 11 | PERS jointtarget JointTrajectory_8{10}; 12 | PERS jointtarget JointTrajectory_9{10}; 13 | PERS num JointTrajectoryCount:=0; 14 | PERS num JointTrajectoryTime{100}; 15 | PERS num CurrentJointTrajectoryCount:=0; 16 | 17 | VAR intnum joint_trajectory_count_intnum:=-1; 18 | VAR intnum joint_trajectory_time_intnum:=-1; 19 | VAR intnum joint_trajectory_0_intnum:=-1; 20 | VAR intnum joint_trajectory_1_intnum:=-1; 21 | VAR intnum joint_trajectory_2_intnum:=-1; 22 | VAR intnum joint_trajectory_3_intnum:=-1; 23 | VAR intnum joint_trajectory_4_intnum:=-1; 24 | VAR intnum joint_trajectory_5_intnum:=-1; 25 | VAR intnum joint_trajectory_6_intnum:=-1; 26 | VAR intnum joint_trajectory_7_intnum:=-1; 27 | VAR intnum joint_trajectory_8_intnum:=-1; 28 | VAR intnum joint_trajectory_9_intnum:=-1; 29 | 30 | VAR egmident egmID1; 31 | VAR egmstate egmSt1; 32 | 33 | CONST egm_minmax egm_minmax_joint1:=[-0.5,0.5]; 34 | 35 | PROC main() 36 | VAR num j; 37 | VAR num k; 38 | VAR jointtarget target; 39 | VAR zonedata zone; 40 | VAR rmqslot rmq; 41 | VAR jointtarget joints; 42 | 43 | CurrentJointTrajectoryCount:=0; 44 | ConnectInterrupts; 45 | 46 | 47 | ! "Move" a tiny amount to start EGM 48 | joints:= CJointT(); 49 | !joints.robax.rax_6 := joints.robax.rax_6 + .0001; 50 | MoveAbsj joints, v100, fine, tool0; 51 | 52 | StartEGM; 53 | 54 | IF JointTrajectoryCount = -1000 THEN 55 | 56 | CurrentJointTrajectoryCount:=-1000; 57 | 58 | EGMActJoint egmID1 \Tool:=tool0 \WObj:=wobj0, \J1:=egm_minmax_joint1 \J2:=egm_minmax_joint1 \J3:=egm_minmax_joint1 59 | \J4:=egm_minmax_joint1 \J5:=egm_minmax_joint1 \J6:=egm_minmax_joint1 \LpFilter:=100 \Samplerate:=4 \MaxPosDeviation:=1000 \MaxSpeedDeviation:=1000; 60 | 61 | EGMRunJoint egmID1, EGM_STOP_HOLD \J1 \J2 \J3 \J4 \J5 \J6 \CondTime:=2000000 \RampInTime:=0.05 \PosCorrGain:=1; 62 | 63 | CurrentJointTrajectoryCount:=0; 64 | 65 | ExitCycle; 66 | ENDIF 67 | 68 | IF JointTrajectoryCount <= 0 THEN 69 | 70 | WaitUntil FALSE; 71 | 72 | ExitCycle; 73 | ENDIF 74 | 75 | FOR i FROM 0 TO JointTrajectoryCount - 1 DO 76 | CurrentJointTrajectoryCount:=i+1; 77 | j:=i DIV 10; 78 | k:=(i MOD 10) + 1; 79 | TEST j 80 | CASE 0: 81 | target:=JointTrajectory_0{k}; 82 | CASE 1: 83 | target:=JointTrajectory_1{k}; 84 | CASE 2: 85 | target:=JointTrajectory_2{k}; 86 | CASE 3: 87 | target:=JointTrajectory_3{k}; 88 | CASE 4: 89 | target:=JointTrajectory_4{k}; 90 | CASE 5: 91 | target:=JointTrajectory_5{k}; 92 | CASE 6: 93 | target:=JointTrajectory_6{k}; 94 | CASE 7: 95 | target:=JointTrajectory_7{k}; 96 | CASE 8: 97 | target:=JointTrajectory_8{k}; 98 | CASE 9: 99 | target:=JointTrajectory_9{k}; 100 | DEFAULT: 101 | JointTrajectoryCount:=0; 102 | ENDTEST 103 | 104 | IF i < JointTrajectoryCount-2 THEN 105 | zone:= z100; 106 | ELSE 107 | zone:= fine; 108 | ENDIF 109 | 110 | MoveAbsJ target, v1000, \T:=JointTrajectoryTime{i+1}, zone, tool0; 111 | 112 | ENDFOR 113 | 114 | CurrentJointTrajectoryCount:=0; 115 | JointTrajectoryCount:=0; 116 | 117 | 118 | ERROR 119 | IF ERRNO = ERR_UDPUC_COMM THEN 120 | TPWrite "EGM UDP Command Timeout, Restarting!"; 121 | WaitTime 1; 122 | ExitCycle; 123 | ELSE 124 | RAISE; 125 | ENDIF 126 | ENDPROC 127 | 128 | LOCAL PROC ConnectInterrupts() 129 | 130 | IDelete joint_trajectory_count_intnum; 131 | IDelete joint_trajectory_time_intnum; 132 | IDelete joint_trajectory_0_intnum; 133 | IDelete joint_trajectory_1_intnum; 134 | IDelete joint_trajectory_2_intnum; 135 | IDelete joint_trajectory_3_intnum; 136 | IDelete joint_trajectory_4_intnum; 137 | IDelete joint_trajectory_5_intnum; 138 | IDelete joint_trajectory_6_intnum; 139 | IDelete joint_trajectory_7_intnum; 140 | IDelete joint_trajectory_8_intnum; 141 | IDelete joint_trajectory_9_intnum; 142 | 143 | CONNECT joint_trajectory_count_intnum WITH JointTrajectoryCountChanged; 144 | CONNECT joint_trajectory_time_intnum WITH JointTrajectoryChanged; 145 | CONNECT joint_trajectory_0_intnum WITH JointTrajectoryChanged; 146 | CONNECT joint_trajectory_1_intnum WITH JointTrajectoryChanged; 147 | CONNECT joint_trajectory_2_intnum WITH JointTrajectoryChanged; 148 | CONNECT joint_trajectory_3_intnum WITH JointTrajectoryChanged; 149 | CONNECT joint_trajectory_4_intnum WITH JointTrajectoryChanged; 150 | CONNECT joint_trajectory_5_intnum WITH JointTrajectoryChanged; 151 | CONNECT joint_trajectory_6_intnum WITH JointTrajectoryChanged; 152 | CONNECT joint_trajectory_7_intnum WITH JointTrajectoryChanged; 153 | CONNECT joint_trajectory_8_intnum WITH JointTrajectoryChanged; 154 | CONNECT joint_trajectory_9_intnum WITH JointTrajectoryChanged; 155 | 156 | IPers JointTrajectoryCount, joint_trajectory_count_intnum; 157 | IPers JointTrajectoryTime, joint_trajectory_time_intnum; 158 | IPers JointTrajectory_0, joint_trajectory_0_intnum; 159 | IPers JointTrajectory_1, joint_trajectory_1_intnum; 160 | IPers JointTrajectory_2, joint_trajectory_2_intnum; 161 | IPers JointTrajectory_3, joint_trajectory_3_intnum; 162 | IPers JointTrajectory_4, joint_trajectory_4_intnum; 163 | IPers JointTrajectory_5, joint_trajectory_5_intnum; 164 | IPers JointTrajectory_6, joint_trajectory_6_intnum; 165 | IPers JointTrajectory_7, joint_trajectory_7_intnum; 166 | IPers JointTrajectory_8, joint_trajectory_8_intnum; 167 | IPers JointTrajectory_9, joint_trajectory_9_intnum; 168 | 169 | ENDPROC 170 | 171 | PROC ResetProgram() 172 | CurrentJointTrajectoryCount:=0; 173 | JointTrajectoryCount:=0; 174 | ExitCycle; 175 | ENDPROC 176 | 177 | PROC StartEGM() 178 | 179 | !This call to EGMReset seems to be problematic. 180 | !It is shown in all documentation. Is it really necessary? 181 | !EGMReset egmID1; 182 | 183 | EGMGetId egmID1; 184 | egmSt1 := EGMGetState(egmID1); 185 | 186 | IF egmSt1 <= EGM_STATE_CONNECTED THEN 187 | EGMSetupUC ROB_1, egmID1, "conf1", "UCdevice:" \Joint \CommTimeout:=100; 188 | ENDIF 189 | 190 | EGMStreamStart egmID1; 191 | ENDPROC 192 | 193 | TRAP JointTrajectoryCountChanged 194 | CurrentJointTrajectoryCount:=0; 195 | ExitCycle; 196 | ENDTRAP 197 | 198 | TRAP JointTrajectoryChanged 199 | IF JointTrajectoryCount > 0 THEN 200 | JointTrajectoryCount:=0; 201 | ExitCycle; 202 | ENDIF 203 | ENDTRAP 204 | 205 | ENDMODULE 206 | 207 | 208 | 209 | -------------------------------------------------------------------------------- /scripts/abb_irc5_rapid_driver_ros: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2017, Rensselaer Polytechnic Institute, Wason Technology LLC 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Rensselaer Polytechnic Institute, or Wason 15 | # Technology LLC, nor the names of its contributors may be used to 16 | # endorse or promote products derived from this software without 17 | # specific prior written permission. 18 | # 19 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | 31 | import rospy 32 | from rpi_abb_irc5 import RAPID, JointTarget 33 | from rpi_abb_irc5.srv import \ 34 | RapidStart, RapidStartRequest, RapidStartResponse, \ 35 | RapidStop, RapidStopRequest, RapidStopResponse, \ 36 | RapidGetStatus, RapidGetStatusRequest, RapidGetStatusResponse, \ 37 | RapidGetDigitalIO, RapidGetDigitalIORequest, RapidGetDigitalIOResponse, \ 38 | RapidSetDigitalIO, RapidSetDigitalIORequest, RapidSetDigitalIOResponse, \ 39 | RapidReadEventLog, RapidReadEventLogRequest, RapidReadEventLogResponse, \ 40 | RapidGetVariable, RapidGetVariableRequest, RapidGetVariableResponse, \ 41 | RapidSetVariable, RapidSetVariableRequest, RapidSetVariableResponse 42 | 43 | from control_msgs.msg import FollowJointTrajectoryAction, \ 44 | FollowJointTrajectoryFeedback, FollowJointTrajectoryResult 45 | 46 | from rpi_abb_irc5.msg import RapidEventLogMessage 47 | from datetime import datetime 48 | from distutils.util import strtobool 49 | from actionlib import action_server 50 | import numpy as np 51 | 52 | import traceback 53 | 54 | class RapidNode(object): 55 | 56 | 57 | def __init__(self, robot_host): 58 | 59 | self.rapid=RAPID(robot_host) 60 | 61 | rospy.Service('rapid/start', RapidStart, self.rapid_start) 62 | rospy.Service('rapid/stop', RapidStop, self.rapid_stop) 63 | rospy.Service('rapid/status', RapidGetStatus, self.rapid_get_status) 64 | rospy.Service('rapid/get_digital_io', RapidGetDigitalIO, self.rapid_get_digital_io) 65 | rospy.Service('rapid/set_digital_io', RapidSetDigitalIO, self.rapid_set_digital_io) 66 | rospy.Service('rapid/read_event_log', RapidReadEventLog, self.rapid_read_event_log) 67 | rospy.Service('rapid/get_rapid_variable', RapidGetVariable, self.rapid_get_variable) 68 | rospy.Service('rapid/set_rapid_variable', RapidSetVariable, self.rapid_set_variable) 69 | 70 | 71 | def rapid_start(self, req): 72 | r=RapidStartResponse() 73 | try: 74 | if req.reset_pp: 75 | self.rapid.resetpp() 76 | cycle='asis' 77 | if len(req.cycle) > 0: 78 | cycle=req.cycle 79 | self.rapid.start(cycle) 80 | r.success=True 81 | return r 82 | except: 83 | traceback.print_exc() 84 | r.success=False 85 | return r 86 | 87 | def rapid_stop(self, req): 88 | r=RapidStopResponse() 89 | try: 90 | self.rapid.stop() 91 | r.success=True 92 | return r 93 | except: 94 | traceback.print_exc() 95 | r.success=False 96 | return r 97 | 98 | def rapid_get_status(self, req): 99 | r=RapidGetStatusResponse() 100 | try: 101 | s=self.rapid.get_execution_state() 102 | r.running=s.ctrlexecstate=='running' 103 | r.cycle=s.cycle 104 | r.opmode=self.rapid.get_operation_mode() 105 | r.ctrlstate=self.rapid.get_controller_state() 106 | r.success=True 107 | return r 108 | except: 109 | traceback.print_exc() 110 | r.success=False 111 | return r 112 | def rapid_get_digital_io(self, req): 113 | r=RapidGetDigitalIOResponse() 114 | try: 115 | r.lvalue=self.rapid.get_digital_io(req.signal) 116 | r.success=True 117 | return r 118 | except: 119 | traceback.print_exc() 120 | r.success=False 121 | return r 122 | 123 | def rapid_set_digital_io(self, req): 124 | r=RapidSetDigitalIOResponse() 125 | try: 126 | self.rapid.set_digital_io(req.signal, req.lvalue) 127 | r.success=True 128 | return r 129 | except: 130 | traceback.print_exc() 131 | r.success=False 132 | return r 133 | 134 | 135 | def rapid_read_event_log(self, req): 136 | r=RapidReadEventLogResponse() 137 | try: 138 | rapid_msgs=self.rapid.read_event_log() 139 | msgs2=[] 140 | for m in rapid_msgs: 141 | m2=RapidEventLogMessage() 142 | m2.msgtype=m.msgtype 143 | m2.code=m.code 144 | m2.tstamp = rospy.Time.from_sec((m.tstamp - datetime.utcfromtimestamp(0)).total_seconds()) 145 | m2.args=m.args 146 | m2.title=m.title 147 | m2.desc=m.desc 148 | m2.conseqs=m.conseqs 149 | m2.causes=m.causes 150 | m2.actions=m.actions 151 | msgs2.append(m2) 152 | r.messages=msgs2 153 | r.success=True 154 | return r 155 | except: 156 | traceback.print_exc() 157 | r.success=False 158 | return r 159 | 160 | 161 | def rapid_get_variable(self, req): 162 | r=RapidGetVariableResponse() 163 | try: 164 | r.value=self.rapid.get_rapid_variable(req.name) 165 | r.success=True 166 | return r 167 | except: 168 | traceback.print_exc() 169 | r.success=False 170 | return r 171 | 172 | def rapid_set_variable(self, req): 173 | r=RapidSetVariableResponse() 174 | try: 175 | self.rapid.set_rapid_variable(req.name, req.value) 176 | r.success=True 177 | return r 178 | except: 179 | traceback.print_exc() 180 | r.success=False 181 | return r 182 | 183 | class RapidTrajectoryServer(object): 184 | 185 | def __init__(self, robot_host): 186 | 187 | 188 | self._rapid=RAPID(robot_host) 189 | 190 | self._action = action_server.ActionServer("rapid/joint_trajectory_action", FollowJointTrajectoryAction, self.goal_cb, self.cancel_cb, auto_start=False) 191 | self._action.start() 192 | 193 | self._current_goal=None 194 | self._timer=rospy.Timer(rospy.Duration(0.1), self.timer_cb) 195 | 196 | 197 | def timer_cb(self, evt): 198 | with self._action.lock: 199 | if self._current_goal is None: 200 | return 201 | 202 | active_count=self._rapid.get_rapid_variable_num("JointTrajectoryCount") 203 | if (active_count == 0): 204 | gh=self._current_goal 205 | self._current_goal=None 206 | g = gh.get_goal() 207 | j=self._rapid.get_jointtarget().robax 208 | if np.max(np.abs(np.subtract(g.trajectory.points[-1].positions,j))) < np.deg2rad(0.1): 209 | rospy.loginfo("Trajectory completed") 210 | gh.set_succeeded() 211 | else: 212 | rospy.loginfo("Trajectory aborted") 213 | gh.set_aborted() 214 | 215 | 216 | def goal_cb(self, gh): 217 | 218 | if self._current_goal is not None: 219 | self._current_goal.set_canceled() 220 | 221 | self._current_goal=gh 222 | 223 | g = gh.get_goal() 224 | 225 | if len(g.trajectory.points) > 100: 226 | gh.set_rejected() 227 | return 228 | 229 | last_t = 0 230 | 231 | rapid_time=[0.0]*100 232 | rapid_jointtarget=[JointTarget(np.zeros(6,),np.zeros(6,))]*100 233 | rapid_count=len(g.trajectory.points) 234 | 235 | i=0 236 | 237 | for p in g.trajectory.points: 238 | 239 | time_from_start = p.time_from_start.to_sec() 240 | 241 | if (time_from_start < last_t): 242 | gh.set_rejected() 243 | rospy.logerr("Invalid duration_from_start in trajectory") 244 | return 245 | 246 | rapid_time[i]=time_from_start-last_t 247 | rapid_jointtarget[i]=JointTarget(p.positions,np.zeros((6,))) 248 | 249 | i+=1 250 | 251 | last_t = time_from_start 252 | 253 | 254 | gh.set_accepted() 255 | 256 | rospy.loginfo("Trajectory received") 257 | 258 | self._rapid.set_rapid_variable_num("JointTrajectoryCount",0) 259 | self._rapid.set_rapid_variable_num_array('JointTrajectoryTime', rapid_time) 260 | for i in xrange(10): 261 | if (rapid_count < i*10): 262 | break 263 | self._rapid.set_rapid_variable_jointtarget_array("JointTrajectory_%d" % i , rapid_jointtarget[i*10:(i*10+10)]) 264 | self._rapid.set_rapid_variable_num("JointTrajectoryCount",rapid_count) 265 | 266 | 267 | def cancel_cb(self, gh): 268 | if self._current_goal is None: 269 | return 270 | if (gh == self._current_goal): 271 | self._rapid.set_rapid_variable_num("JointTrajectoryCount",0) 272 | self._current_goal=None 273 | gh.set_canceled() 274 | 275 | pass 276 | 277 | 278 | if __name__ == '__main__': 279 | 280 | rospy.init_node('abb_irc5_rapid') 281 | 282 | robot_host=rospy.get_param('~abb_irc5_uri') 283 | 284 | r=RapidNode(robot_host) 285 | r2=RapidTrajectoryServer(robot_host) 286 | 287 | rospy.spin() 288 | 289 | 290 | #Stop RAPID on node shutdown 291 | try: 292 | print rospy.get_param('~abb_irc5_rapid_auto_stop', False) 293 | if bool(strtobool(str(rospy.get_param('~abb_irc5_rapid_auto_stop', False)))): 294 | r.rapid_stop() 295 | except: 296 | traceback.print_exc() 297 | pass 298 | -------------------------------------------------------------------------------- /src/rpi_abb_irc5/rpi_abb_irc5.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2017, Rensselaer Polytechnic Institute, Wason Technology LLC 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # * Redistributions in binary form must reproduce the above copyright 10 | # notice, this list of conditions and the following disclaimer in the 11 | # documentation and/or other materials provided with the distribution. 12 | # * Neither the name of the Rensselaer Polytechnic Institute, or Wason 13 | # Technology LLC, nor the names of its contributors may be used to 14 | # endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | 29 | from __future__ import absolute_import 30 | 31 | import socket 32 | import select 33 | from . import egm_pb2 34 | import requests 35 | from BeautifulSoup import BeautifulSoup 36 | import traceback 37 | from collections import namedtuple 38 | import numpy as np 39 | from datetime import datetime 40 | import errno 41 | import re 42 | from ws4py.client.threadedclient import WebSocketClient 43 | import threading 44 | import time 45 | import random 46 | 47 | class EGM(object): 48 | 49 | def __init__(self, port=6510): 50 | 51 | self.socket=socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 52 | self.socket.bind(('',port)) 53 | self.send_sequence_number=0 54 | self.egm_addr=None 55 | self.count=0 56 | 57 | def receive_from_robot(self, timeout=0): 58 | 59 | s=self.socket 60 | s_list=[s] 61 | try: 62 | res=select.select(s_list, [], s_list, timeout) 63 | except select.error as err: 64 | if err.args[0] == errno.EINTR: 65 | return False, None 66 | else: 67 | raise 68 | 69 | if len(res[0]) == 0 and len(res[2])==0: 70 | return False, None 71 | try: 72 | (buf, addr)=s.recvfrom(65536) 73 | except: 74 | self.egm_addr=None 75 | return False, None 76 | 77 | self.egm_addr=addr 78 | 79 | robot_message=egm_pb2.EgmRobot() 80 | robot_message.ParseFromString(buf) 81 | 82 | joint_angles=None 83 | rapid_running=False 84 | motors_on=False 85 | 86 | if robot_message.HasField('feedBack'): 87 | joints=robot_message.feedBack.joints.joints 88 | joint_angles=np.array(list(joints)) 89 | if robot_message.HasField('rapidExecState'): 90 | rapid_running = robot_message.rapidExecState.state == robot_message.rapidExecState.RAPID_RUNNING 91 | if robot_message.HasField('motorState'): 92 | motors_on = robot_message.motorState.state == robot_message.motorState.MOTORS_ON 93 | 94 | return True, EGMRobotState(joint_angles, rapid_running, motors_on, robot_message) 95 | 96 | def send_to_robot(self, joint_angles): 97 | 98 | if not self.egm_addr: 99 | return False 100 | 101 | self.send_sequence_number+=1 102 | 103 | sensorMessage=egm_pb2.EgmSensor() 104 | 105 | header=sensorMessage.header 106 | header.mtype=egm_pb2.EgmHeader.MessageType.Value('MSGTYPE_CORRECTION') 107 | header.seqno=self.send_sequence_number 108 | self.send_sequence_number+=1 109 | 110 | planned=sensorMessage.planned 111 | 112 | if joint_angles is not None: 113 | joint_angles2 = list(np.rad2deg(joint_angles)) 114 | planned.joints.joints.extend(joint_angles2) 115 | 116 | buf2=sensorMessage.SerializeToString() 117 | 118 | try: 119 | self.socket.sendto(buf2, self.egm_addr) 120 | except: 121 | return False 122 | 123 | return True 124 | 125 | EGMRobotState=namedtuple('EGMRobotState', ['joint_angles', 'rapid_running', 'motors_on', 'robot_message'], verbose=False) 126 | JointTarget=namedtuple('JointTarget', ['robax', 'extax']) 127 | RobTarget=namedtuple('RobTarget', ['trans','rot','robconf','extax']) 128 | 129 | class RAPID(object): 130 | 131 | def __init__(self, base_url='http://127.0.0.1:80', username='Default User', password='robotics'): 132 | self.base_url=base_url 133 | self.auth=requests.auth.HTTPDigestAuth(username, password) 134 | self._session=requests.Session() 135 | self._rmmp_session=None 136 | self._rmmp_session_t=None 137 | 138 | def _do_get(self, relative_url): 139 | url="/".join([self.base_url, relative_url]) 140 | res=self._session.get(url, auth=self.auth) 141 | try: 142 | return self._process_response(res) 143 | finally: 144 | res.close() 145 | 146 | 147 | def _do_post(self, relative_url, payload=None): 148 | url="/".join([self.base_url, relative_url]) 149 | res=self._session.post(url, data=payload, auth=self.auth) 150 | try: 151 | return self._process_response(res) 152 | finally: 153 | res.close() 154 | 155 | def _process_response(self, response): 156 | soup=BeautifulSoup(response.text) 157 | 158 | if (response.status_code == 500): 159 | raise Exception("Robot returning 500 Internal Server Error") 160 | 161 | if (response.status_code == 200 or response.status_code == 201 \ 162 | or response.status_code==202 or response.status_code==204): 163 | 164 | return soup.body 165 | 166 | if soup.body is None: 167 | raise Exception("Robot returning HTTP error " + str(response.status_code)) 168 | 169 | error_code=int(soup.find('span', attrs={'class':'code'}).text) 170 | error_message1=soup.find('span', attrs={'class': 'msg'}) 171 | if (error_message1 is not None): 172 | error_message=error_message1.text 173 | else: 174 | error_message="Received error from ABB robot: " + str(error_code) 175 | 176 | raise ABBException(error_message, error_code) 177 | 178 | def start(self, cycle='asis'): 179 | payload={"regain": "continue", "execmode": "continue" , "cycle": cycle, "condition": "none", "stopatbp": "disabled", "alltaskbytsp": "false"} 180 | res=self._do_post("rw/rapid/execution?action=start", payload) 181 | 182 | def stop(self): 183 | payload={"stopmode": "stop"} 184 | res=self._do_post("rw/rapid/execution?action=stop", payload) 185 | 186 | def resetpp(self): 187 | res=self._do_post("rw/rapid/execution?action=resetpp") 188 | 189 | def get_execution_state(self): 190 | soup = self._do_get("rw/rapid/execution") 191 | ctrlexecstate=soup.find('span', attrs={'class': 'ctrlexecstate'}).text 192 | cycle=soup.find('span', attrs={'class': 'cycle'}).text 193 | return RAPIDExecutionState(ctrlexecstate, cycle) 194 | 195 | def get_controller_state(self): 196 | soup = self._do_get("rw/panel/ctrlstate") 197 | return soup.find('span', attrs={'class': 'ctrlstate'}).text 198 | 199 | def get_operation_mode(self): 200 | soup = self._do_get("rw/panel/opmode") 201 | return soup.find('span', attrs={'class': 'opmode'}).text 202 | 203 | def get_digital_io(self, signal, network='Local', unit='DRV_1'): 204 | soup = self._do_get("rw/iosystem/signals/" + network + "/" + unit + "/" + signal) 205 | state = soup.find('span', attrs={'class': 'lvalue'}).text 206 | return int(state) 207 | 208 | def set_digital_io(self, signal, value, network='Local', unit='DRV_1'): 209 | lvalue = '1' if bool(value) else '0' 210 | payload={'lvalue': lvalue} 211 | res=self._do_post("rw/iosystem/signals/" + network + "/" + unit + "/" + signal + "?action=set", payload) 212 | 213 | def get_rapid_variable(self, var): 214 | soup = self._do_get("rw/rapid/symbol/data/RAPID/T_ROB1/" + var) 215 | state = soup.find('span', attrs={'class': 'value'}).text 216 | return state 217 | 218 | def set_rapid_variable(self, var, value): 219 | payload={'value': value} 220 | res=self._do_post("rw/rapid/symbol/data/RAPID/T_ROB1/" + var + "?action=set", payload) 221 | 222 | def read_event_log(self, elog=0): 223 | o=[] 224 | soup = self._do_get("rw/elog/" + str(elog) + "/?lang=en") 225 | state=soup.find('div', attrs={'class': 'state'}) 226 | ul=state.find('ul') 227 | 228 | for li in ul.findAll('li'): 229 | def find_val(v): 230 | return li.find('span', attrs={'class': v}).text 231 | msg_type=int(find_val('msgtype')) 232 | code=int(find_val('code')) 233 | tstamp=datetime.strptime(find_val('tstamp'), '%Y-%m-%d T %H:%M:%S') 234 | title=find_val('title') 235 | desc=find_val('desc') 236 | conseqs=find_val('conseqs') 237 | causes=find_val('causes') 238 | actions=find_val('actions') 239 | args=[] 240 | nargs=int(find_val('argc')) 241 | for i in xrange(nargs): 242 | arg=find_val('arg%d' % (i+1)) 243 | args.append(arg) 244 | 245 | o.append(RAPIDEventLogEntry(msg_type,code,tstamp,args,title,desc,conseqs,causes,actions)) 246 | return o 247 | 248 | def get_jointtarget(self, mechunit="ROB_1"): 249 | soup=self._do_get("rw/motionsystem/mechunits/" + mechunit + "/jointtarget") 250 | state=str(soup.find('li', attrs={'class': 'ms-jointtarget'})) 251 | robjoint=np.zeros((6,)) 252 | i=0 253 | for match in re.finditer('class=\"rax_(\\d)">(-?\\d+(?:\\.\\d+)?)',state): 254 | j=int(match.groups()[0]) 255 | assert i+1==j 256 | a=float(match.groups()[1]) 257 | robjoint[j-1]=np.deg2rad(a) 258 | i+=1 259 | 260 | #robjoint=np.array([np.deg2rad(float(state.find('span', attrs={'class': 'rax_' + str(i+1)}).text)) for i in xrange(6)]) 261 | #extjoint=np.array([np.deg2rad(float(state.find('span', attrs={'class': 'eax_' + chr(i)}).text)) for i in xrange(ord('a'),ord('g'))]) 262 | extjoint=None 263 | return JointTarget(robjoint,extjoint) 264 | 265 | def get_robtarget(self, mechunit='ROB_1', tool='tool0', wobj='wobj0', coordinate='Base'): 266 | soup=self._do_get("rw/motionsystem/mechunits/" + mechunit + "/robtarget?tool=%s&wobj=%s&coordinate=%s" % (tool, wobj, coordinate)) 267 | state=soup.find('li', attrs={'class': 'ms-robtargets'}) 268 | trans=np.array([(float(state.find('span', attrs={'class': i}).text)/1000.0) for i in 'xyz']) 269 | rot=np.array([(float(state.find('span', attrs={'class': 'q' + str(i+1)}).text)) for i in xrange(4)]) 270 | robconf=np.array([(float(state.find('span', attrs={'class': i}).text)) for i in ['cf1','cf4','cf6','cfx']]) 271 | extax=np.array([np.deg2rad(float(state.find('span', attrs={'class': 'eax_' + chr(i)}).text)) for i in xrange(ord('a'),ord('g'))]) 272 | return RobTarget(trans,rot,robconf,extax) 273 | 274 | def _rws_value_to_jointtarget(self, val): 275 | v1=re.match('^\\[\\[([^\\]]+)\\],\\[([^\\]]+)\\]',val) 276 | robax = np.deg2rad(np.fromstring(v1.groups()[0],sep=',')) 277 | extax = np.deg2rad(np.fromstring(v1.groups()[1],sep=',')) 278 | return JointTarget(robax,extax) 279 | 280 | def _jointtarget_to_rws_value(self, val): 281 | assert np.shape(val[0]) == (6,) 282 | assert np.shape(val[1]) == (6,) 283 | robax=','.join([format(x, '.4f') for x in np.rad2deg(val[0])]) 284 | extax=','.join([format(x, '.4f') for x in np.rad2deg(val[1])]) 285 | rws_value="[[" + robax + "],[" + extax + "]]" 286 | return rws_value 287 | 288 | def get_rapid_variable_jointtarget(self, var): 289 | v = self.get_rapid_variable(var) 290 | return self._rws_value_to_jointtarget(v) 291 | 292 | def set_rapid_variable_jointtarget(self,var,value): 293 | rws_value=self._jointtarget_to_rws_value(value) 294 | self.set_rapid_variable(var, rws_value) 295 | 296 | def _rws_value_to_jointtarget_array(self,val): 297 | m1=re.match('^\\[(.*)\\]$',val) 298 | if len(m1.groups()[0])==0: 299 | return [] 300 | arr=[] 301 | val1=m1.groups()[0] 302 | while len(val1) > 0: 303 | m2=re.match('^(\\[\\[[^\\]]+\\],\\[[^\\]]+\\]\\]),?(.*)$',val1) 304 | val1 = m2.groups()[1] 305 | arr.append(self._rws_value_to_jointtarget(m2.groups()[0])) 306 | 307 | return arr 308 | 309 | def _jointtarget_array_to_rws_value(self, val): 310 | return "[" + ','.join([self._jointtarget_to_rws_value(v) for v in val]) + "]" 311 | 312 | def get_rapid_variable_jointtarget_array(self, var): 313 | v = self.get_rapid_variable(var) 314 | return self._rws_value_to_jointtarget_array(v) 315 | 316 | def set_rapid_variable_jointtarget_array(self,var,value): 317 | rws_value=self._jointtarget_array_to_rws_value(value) 318 | self.set_rapid_variable(var, rws_value) 319 | 320 | def get_rapid_variable_num(self, var): 321 | return float(self.get_rapid_variable(var)) 322 | 323 | def set_rapid_variable_num(self, var, val): 324 | self.set_rapid_variable(var, str(val)) 325 | 326 | def get_rapid_variable_num_array(self, var): 327 | val1=self.get_rapid_variable(var) 328 | m=re.match("^\\[([^\\]]*)\\]$", val1) 329 | val2=m.groups()[0].strip() 330 | return np.fromstring(val2,sep=',') 331 | 332 | def set_rapid_variable_num_array(self, var, val): 333 | self.set_rapid_variable(var, "[" + ','.join([str(s) for s in val]) + "]") 334 | 335 | 336 | def read_ipc_message(self, queue_name, timeout=0): 337 | 338 | o=[] 339 | 340 | timeout_str="" 341 | if timeout > 0: 342 | timeout_str="&timeout=" + str(timeout) 343 | 344 | soup=self._do_get("rw/dipc/" + queue_name + "/?action=dipc-read" + timeout_str) 345 | 346 | state=soup.find('div', attrs={'class': 'state'}) 347 | ul=state.find('ul') 348 | 349 | for li in ul.findAll('li'): 350 | def find_val(v): 351 | return li.find('span', attrs={'class': v}).text 352 | msgtype=find_val('dipc-msgtype') 353 | cmd=int(find_val('dipc-cmd')) 354 | userdef=int(find_val('dipc-userdef')) 355 | data=find_val('dipc-data') 356 | 357 | o.append(RAPIDIpcMessage(data,userdef,msgtype,cmd)) 358 | 359 | #o.append(RAPIDEventLogEntry(msg_type,code,tstamp,args,title,desc,conseqs,causes,actions)) 360 | return o 361 | 362 | def send_ipc_message(self, target_queue, data, queue_name="rpi_abb_irc5", cmd=111, userdef=1, msgtype=1 ): 363 | payload={"dipc-src-queue-name": queue_name, "dipc-cmd": str(cmd), "dipc-userdef": str(userdef), \ 364 | "dipc-msgtype": str(msgtype), "dipc-data": data} 365 | res=self._do_post("rw/dipc/" + target_queue + "?action=dipc-send", payload) 366 | 367 | def get_ipc_queue(self, queue_name): 368 | res=self._do_get("rw/dipc/" + queue_name + "?action=dipc-read") 369 | return res 370 | 371 | def try_create_ipc_queue(self, queue_name, queue_size=4440, max_msg_size=444): 372 | try: 373 | payload={"dipc-queue-name": queue_name, "dipc-queue-size": str(queue_size), "dipc-max-msg-size": str(max_msg_size)} 374 | self._do_post("rw/dipc?action=dipc-create", payload) 375 | return True 376 | except ABBException as e: 377 | if e.code==-1073445879: 378 | return False 379 | raise 380 | 381 | def request_rmmp(self, timeout=5): 382 | t1=time.time() 383 | self._do_post('users/rmmp', {'privilege': 'modify'}) 384 | while time.time() - t1 < timeout: 385 | 386 | soup=self._do_get('users/rmmp/poll') 387 | status=soup.find('span', {'class': 'status'}).text 388 | if status=="GRANTED": 389 | self.poll_rmmp() 390 | return 391 | elif status!="PENDING": 392 | raise Exception("User did not grant remote access") 393 | time.sleep(0.25) 394 | raise Exception("User did not grant remote access") 395 | 396 | def poll_rmmp(self): 397 | 398 | # A "persistent session" can only make 400 calls before 399 | # being disconnected. Once this connection is lost, 400 | # the grant will be revoked. To work around this, 401 | # create parallel sessions with copied session cookies 402 | # to maintain the connection. 403 | 404 | url="/".join([self.base_url, 'users/rmmp/poll']) 405 | 406 | old_rmmp_session=None 407 | if self._rmmp_session is None: 408 | self._do_get(url) 409 | self._rmmp_session=requests.Session() 410 | self._rmmp_session_t=time.time() 411 | 412 | for c in self._session.cookies: 413 | self._rmmp_session.cookies.set_cookie(c) 414 | else: 415 | if time.time() - self._rmmp_session_t > 30: 416 | old_rmmp_session=self._rmmp_session 417 | rmmp_session=requests.Session() 418 | 419 | for c in self._session.cookies: 420 | rmmp_session.cookies.set_cookie(c) 421 | 422 | rmmp_session=self._rmmp_session 423 | 424 | res=rmmp_session.get(url, auth=self.auth) 425 | soup=self._process_response(res) 426 | 427 | if old_rmmp_session is not None: 428 | self._rmmp_session=rmmp_session 429 | self._rmmp_session_t=time.time() 430 | try: 431 | old_rmmp_session.close() 432 | except: 433 | pass 434 | 435 | return soup.find('span', {'class': 'status'}).text == "GRANTED" 436 | 437 | def subscribe_controller_state(self, callback, closed_callback=None): 438 | payload = {'resources':['1'], 439 | '1':'/rw/panel/ctrlstate', 440 | '1-p':'1'} 441 | 442 | return self._subscribe(payload, RAPIDControllerStateSubscription, callback, closed_callback) 443 | 444 | def subscribe_operation_mode(self, callback, closed_callback=None): 445 | payload = {'resources':['1'], 446 | '1':'/rw/panel/opmode', 447 | '1-p':'1'} 448 | 449 | return self._subscribe(payload, RAPIDOpmodeSubscription, callback, closed_callback) 450 | 451 | def subscribe_execution_state(self, callback, closed_callback=None): 452 | payload = {'resources':['1'], 453 | '1':'/rw/rapid/execution;ctrlexecstate', 454 | '1-p':'1'} 455 | 456 | return self._subscribe(payload, RAPIDExecutionStateSubscription, callback, closed_callback) 457 | 458 | def subscribe_rapid_pers_variable(self, var, callback, closed_callback=None): 459 | payload = {'resources':['1'], 460 | '1':'/rw/rapid/symbol/data/RAPID/T_ROB1/' + var + ';value', 461 | '1-p':'1'} 462 | 463 | return self._subscribe(payload, RAPIDPersVarSubscription, callback, closed_callback) 464 | 465 | def subscribe_ipc_queue(self, queue_name, callback, closed_callback=None): 466 | payload = {'resources':['1'], 467 | '1':'/rw/dipc/' + queue_name, 468 | '1-p':'1'} 469 | 470 | return self._subscribe(payload, RAPIDIpcQueueSubscription, callback, closed_callback) 471 | 472 | def subscribe_event_log(self, callback, closed_callback=None): 473 | payload = {'resources':['1'], 474 | '1':'/rw/elog/0', 475 | '1-p':'1'} 476 | 477 | return self._subscribe(payload, RAPIDElogSubscription, callback, closed_callback) 478 | 479 | def subscribe_digital_io(self, signal, network='Local', unit='DRV_1', callback=None, closed_callback=None): 480 | payload = {'resources':['1'], 481 | '1': '/rw/iosystem/signals/' + network + '/' + unit + '/' + signal + ';state', 482 | '1-p':'1'} 483 | 484 | return self._subscribe(payload, RAPIDSignalSubscription, callback, closed_callback) 485 | 486 | 487 | 488 | def _subscribe(self, payload, ws_type, callback, closed_callback): 489 | 490 | session=requests.Session() 491 | 492 | url="/".join([self.base_url, "subscription"]) 493 | res1=session.post(url, data=payload, auth=self.auth) 494 | try: 495 | res=self._process_response(res1) 496 | finally: 497 | res1.close() 498 | 499 | ws_url=res.find("a", {"rel": "self"})['href'] 500 | cookie = 'ABBCX={0}'.format(session.cookies['ABBCX']) 501 | header=[('Cookie',cookie), ('Authorization', self.auth.build_digest_header("GET", ws_url))] 502 | ws=ws_type(ws_url, ['robapi2_subscription'], header, callback, closed_callback, session) 503 | ws.connect() 504 | return ws 505 | 506 | RAPIDExecutionState=namedtuple('RAPIDExecutionState', ['ctrlexecstate', 'cycle'], verbose=False) 507 | RAPIDEventLogEntry=namedtuple('RAPIDEventLogEntry', ['msgtype', 'code', 'tstamp', 'args', 'title', 'desc', 'conseqs', 'causes', 'actions']) 508 | RAPIDIpcMessage=namedtuple('RAPIDIpcMessage',['data','userdef','msgtype','cmd']) 509 | RAPIDSignal=namedtuple('RAPIDSignal',['name','lvalue']) 510 | 511 | 512 | class ABBException(Exception): 513 | def __init__(self, message, code): 514 | super(ABBException, self).__init__(message) 515 | self.code=code 516 | 517 | class RAPIDSubscriptionClient(WebSocketClient): 518 | 519 | def __init__(self, ws_url, protocols, headers, callback, closed_callback,session): 520 | super(RAPIDSubscriptionClient,self).__init__(ws_url, protocols=protocols, headers=headers) 521 | self._callback=callback 522 | self._closed_callback=closed_callback 523 | self._session=session 524 | 525 | def opened(self): 526 | pass 527 | 528 | def closed(self, code, reason=None): 529 | if self._closed_callback is not None: 530 | self._closed_callback() 531 | 532 | def received_message(self, event_xml): 533 | if event_xml.is_text: 534 | soup=BeautifulSoup(event_xml.data) 535 | data=self.extract_data(soup) 536 | self._callback(data) 537 | else: 538 | print "Received Illegal Event " + str(event_xml) 539 | 540 | def extract_data(self, soup): 541 | return None 542 | 543 | class RAPIDControllerStateSubscription(RAPIDSubscriptionClient): 544 | def extract_data(self, soup): 545 | return soup.find("span", attrs={"class": "ctrlstate"}).text 546 | 547 | class RAPIDOpmodeSubscription(RAPIDSubscriptionClient): 548 | def extract_data(self, soup): 549 | return soup.find("span", attrs={"class": "opmode"}).text 550 | 551 | class RAPIDExecutionStateSubscription(RAPIDSubscriptionClient): 552 | def extract_data(self, soup): 553 | ctrlexecstate=soup.find('span', attrs={'class': 'ctrlexecstate'}).text 554 | return ctrlexecstate 555 | 556 | class RAPIDPersVarSubscription(RAPIDSubscriptionClient): 557 | def extract_data(self, soup): 558 | state=soup.find('div', attrs={'class': 'state'}) 559 | ul=state.find('ul') 560 | 561 | o=[] 562 | 563 | for li in ul.findAll('li'): 564 | url=li.find('a')['href'] 565 | m=re.match('^/rw/rapid/symbol/data/RAPID/T_ROB1/(.+);value$', url) 566 | o.append(m.groups()[0]) 567 | return o 568 | 569 | class RAPIDIpcQueueSubscription(RAPIDSubscriptionClient): 570 | def extract_data(self, soup): 571 | state=soup.find('div', attrs={'class': 'state'}) 572 | ul=state.find('ul') 573 | 574 | o=[] 575 | 576 | for li in ul.findAll('li'): 577 | data=li.find('span', attrs={'class': 'dipc-data'}).text 578 | 579 | o.append(data) 580 | return o 581 | 582 | class RAPIDElogSubscription(RAPIDSubscriptionClient): 583 | def extract_data(self, soup): 584 | state=soup.find('div', attrs={'class': 'state'}) 585 | ul=state.find('ul') 586 | 587 | o=[] 588 | 589 | for li in ul.findAll('li'): 590 | data=int(li.find('span', attrs={'class': 'seqnum'}).text) 591 | 592 | o.append(data) 593 | return o 594 | 595 | class RAPIDSignalSubscription(RAPIDSubscriptionClient): 596 | def extract_data(self, soup): 597 | state=soup.find('div', attrs={'class': 'state'}) 598 | ul=state.find('ul') 599 | 600 | o=[] 601 | for li in ul.findAll('li'): 602 | name=li["title"] 603 | lvalue=float(li.find('span', attrs={'class': 'lvalue'}).text) 604 | 605 | o.append(RAPIDSignal(name,lvalue)) 606 | return o 607 | -------------------------------------------------------------------------------- /src/rpi_abb_irc5/egm_pb2.py: -------------------------------------------------------------------------------- 1 | # Generated by the protocol buffer compiler. DO NOT EDIT! 2 | # source: egm.proto 3 | 4 | import sys 5 | _b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) 6 | from google.protobuf import descriptor as _descriptor 7 | from google.protobuf import message as _message 8 | from google.protobuf import reflection as _reflection 9 | from google.protobuf import symbol_database as _symbol_database 10 | from google.protobuf import descriptor_pb2 11 | # @@protoc_insertion_point(imports) 12 | 13 | _sym_db = _symbol_database.Default() 14 | 15 | 16 | 17 | 18 | DESCRIPTOR = _descriptor.FileDescriptor( 19 | name='egm.proto', 20 | package='abb.egm', 21 | serialized_pb=_b('\n\tegm.proto\x12\x07\x61\x62\x62.egm\"\xeb\x01\n\tEgmHeader\x12\r\n\x05seqno\x18\x01 \x01(\r\x12\n\n\x02tm\x18\x02 \x01(\r\x12@\n\x05mtype\x18\x03 \x01(\x0e\x32\x1e.abb.egm.EgmHeader.MessageType:\x11MSGTYPE_UNDEFINED\"\x80\x01\n\x0bMessageType\x12\x15\n\x11MSGTYPE_UNDEFINED\x10\x00\x12\x13\n\x0fMSGTYPE_COMMAND\x10\x01\x12\x10\n\x0cMSGTYPE_DATA\x10\x02\x12\x16\n\x12MSGTYPE_CORRECTION\x10\x03\x12\x1b\n\x17MSGTYPE_PATH_CORRECTION\x10\x04\"/\n\x0c\x45gmCartesian\x12\t\n\x01x\x18\x01 \x02(\x01\x12\t\n\x01y\x18\x02 \x02(\x01\x12\t\n\x01z\x18\x03 \x02(\x01\"?\n\rEgmQuaternion\x12\n\n\x02u0\x18\x01 \x02(\x01\x12\n\n\x02u1\x18\x02 \x02(\x01\x12\n\n\x02u2\x18\x03 \x02(\x01\x12\n\n\x02u3\x18\x04 \x02(\x01\"+\n\x08\x45gmEuler\x12\t\n\x01x\x18\x01 \x02(\x01\x12\t\n\x01y\x18\x02 \x02(\x01\x12\t\n\x01z\x18\x03 \x02(\x01\"w\n\x07\x45gmPose\x12\"\n\x03pos\x18\x01 \x01(\x0b\x32\x15.abb.egm.EgmCartesian\x12&\n\x06orient\x18\x02 \x01(\x0b\x32\x16.abb.egm.EgmQuaternion\x12 \n\x05\x65uler\x18\x03 \x01(\x0b\x32\x11.abb.egm.EgmEuler\"\"\n\x11\x45gmCartesianSpeed\x12\r\n\x05value\x18\x01 \x03(\x01\"\x1b\n\tEgmJoints\x12\x0e\n\x06joints\x18\x01 \x03(\x01\"#\n\x11\x45gmExternalJoints\x12\x0e\n\x06joints\x18\x01 \x03(\x01\"\x81\x01\n\nEgmPlanned\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12#\n\tcartesian\x18\x02 \x01(\x0b\x32\x10.abb.egm.EgmPose\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\"\x8d\x01\n\x0b\x45gmSpeedRef\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12.\n\ncartesians\x18\x02 \x01(\x0b\x32\x1a.abb.egm.EgmCartesianSpeed\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\">\n\x0b\x45gmPathCorr\x12\"\n\x03pos\x18\x01 \x02(\x0b\x32\x15.abb.egm.EgmCartesian\x12\x0b\n\x03\x61ge\x18\x02 \x02(\r\"\x82\x01\n\x0b\x45gmFeedBack\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12#\n\tcartesian\x18\x02 \x01(\x0b\x32\x10.abb.egm.EgmPose\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\"\x8c\x01\n\rEgmMotorState\x12\x34\n\x05state\x18\x01 \x02(\x0e\x32%.abb.egm.EgmMotorState.MotorStateType\"E\n\x0eMotorStateType\x12\x14\n\x10MOTORS_UNDEFINED\x10\x00\x12\r\n\tMOTORS_ON\x10\x01\x12\x0e\n\nMOTORS_OFF\x10\x02\"\xa2\x01\n\x0b\x45gmMCIState\x12?\n\x05state\x18\x01 \x02(\x0e\x32!.abb.egm.EgmMCIState.MCIStateType:\rMCI_UNDEFINED\"R\n\x0cMCIStateType\x12\x11\n\rMCI_UNDEFINED\x10\x00\x12\r\n\tMCI_ERROR\x10\x01\x12\x0f\n\x0bMCI_STOPPED\x10\x02\x12\x0f\n\x0bMCI_RUNNING\x10\x03\"\xc3\x01\n\x15\x45gmRapidCtrlExecState\x12U\n\x05state\x18\x01 \x02(\x0e\x32\x35.abb.egm.EgmRapidCtrlExecState.RapidCtrlExecStateType:\x0fRAPID_UNDEFINED\"S\n\x16RapidCtrlExecStateType\x12\x13\n\x0fRAPID_UNDEFINED\x10\x00\x12\x11\n\rRAPID_STOPPED\x10\x01\x12\x11\n\rRAPID_RUNNING\x10\x02\"!\n\x0e\x45gmTestSignals\x12\x0f\n\x07signals\x18\x01 \x03(\x01\"\xd1\x02\n\x08\x45gmRobot\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12&\n\x08\x66\x65\x65\x64\x42\x61\x63k\x18\x02 \x01(\x0b\x32\x14.abb.egm.EgmFeedBack\x12$\n\x07planned\x18\x03 \x01(\x0b\x32\x13.abb.egm.EgmPlanned\x12*\n\nmotorState\x18\x04 \x01(\x0b\x32\x16.abb.egm.EgmMotorState\x12&\n\x08mciState\x18\x05 \x01(\x0b\x32\x14.abb.egm.EgmMCIState\x12\x19\n\x11mciConvergenceMet\x18\x06 \x01(\x08\x12,\n\x0btestSignals\x18\x07 \x01(\x0b\x32\x17.abb.egm.EgmTestSignals\x12\x36\n\x0erapidExecState\x18\x08 \x01(\x0b\x32\x1e.abb.egm.EgmRapidCtrlExecState\"}\n\tEgmSensor\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12$\n\x07planned\x18\x02 \x01(\x0b\x32\x13.abb.egm.EgmPlanned\x12&\n\x08speedRef\x18\x03 \x01(\x0b\x32\x14.abb.egm.EgmSpeedRef\"_\n\x11\x45gmSensorPathCorr\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12&\n\x08pathCorr\x18\x02 \x01(\x0b\x32\x14.abb.egm.EgmPathCorr') 22 | ) 23 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 24 | 25 | 26 | 27 | _EGMHEADER_MESSAGETYPE = _descriptor.EnumDescriptor( 28 | name='MessageType', 29 | full_name='abb.egm.EgmHeader.MessageType', 30 | filename=None, 31 | file=DESCRIPTOR, 32 | values=[ 33 | _descriptor.EnumValueDescriptor( 34 | name='MSGTYPE_UNDEFINED', index=0, number=0, 35 | options=None, 36 | type=None), 37 | _descriptor.EnumValueDescriptor( 38 | name='MSGTYPE_COMMAND', index=1, number=1, 39 | options=None, 40 | type=None), 41 | _descriptor.EnumValueDescriptor( 42 | name='MSGTYPE_DATA', index=2, number=2, 43 | options=None, 44 | type=None), 45 | _descriptor.EnumValueDescriptor( 46 | name='MSGTYPE_CORRECTION', index=3, number=3, 47 | options=None, 48 | type=None), 49 | _descriptor.EnumValueDescriptor( 50 | name='MSGTYPE_PATH_CORRECTION', index=4, number=4, 51 | options=None, 52 | type=None), 53 | ], 54 | containing_type=None, 55 | options=None, 56 | serialized_start=130, 57 | serialized_end=258, 58 | ) 59 | _sym_db.RegisterEnumDescriptor(_EGMHEADER_MESSAGETYPE) 60 | 61 | _EGMMOTORSTATE_MOTORSTATETYPE = _descriptor.EnumDescriptor( 62 | name='MotorStateType', 63 | full_name='abb.egm.EgmMotorState.MotorStateType', 64 | filename=None, 65 | file=DESCRIPTOR, 66 | values=[ 67 | _descriptor.EnumValueDescriptor( 68 | name='MOTORS_UNDEFINED', index=0, number=0, 69 | options=None, 70 | type=None), 71 | _descriptor.EnumValueDescriptor( 72 | name='MOTORS_ON', index=1, number=1, 73 | options=None, 74 | type=None), 75 | _descriptor.EnumValueDescriptor( 76 | name='MOTORS_OFF', index=2, number=2, 77 | options=None, 78 | type=None), 79 | ], 80 | containing_type=None, 81 | options=None, 82 | serialized_start=1187, 83 | serialized_end=1256, 84 | ) 85 | _sym_db.RegisterEnumDescriptor(_EGMMOTORSTATE_MOTORSTATETYPE) 86 | 87 | _EGMMCISTATE_MCISTATETYPE = _descriptor.EnumDescriptor( 88 | name='MCIStateType', 89 | full_name='abb.egm.EgmMCIState.MCIStateType', 90 | filename=None, 91 | file=DESCRIPTOR, 92 | values=[ 93 | _descriptor.EnumValueDescriptor( 94 | name='MCI_UNDEFINED', index=0, number=0, 95 | options=None, 96 | type=None), 97 | _descriptor.EnumValueDescriptor( 98 | name='MCI_ERROR', index=1, number=1, 99 | options=None, 100 | type=None), 101 | _descriptor.EnumValueDescriptor( 102 | name='MCI_STOPPED', index=2, number=2, 103 | options=None, 104 | type=None), 105 | _descriptor.EnumValueDescriptor( 106 | name='MCI_RUNNING', index=3, number=3, 107 | options=None, 108 | type=None), 109 | ], 110 | containing_type=None, 111 | options=None, 112 | serialized_start=1339, 113 | serialized_end=1421, 114 | ) 115 | _sym_db.RegisterEnumDescriptor(_EGMMCISTATE_MCISTATETYPE) 116 | 117 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE = _descriptor.EnumDescriptor( 118 | name='RapidCtrlExecStateType', 119 | full_name='abb.egm.EgmRapidCtrlExecState.RapidCtrlExecStateType', 120 | filename=None, 121 | file=DESCRIPTOR, 122 | values=[ 123 | _descriptor.EnumValueDescriptor( 124 | name='RAPID_UNDEFINED', index=0, number=0, 125 | options=None, 126 | type=None), 127 | _descriptor.EnumValueDescriptor( 128 | name='RAPID_STOPPED', index=1, number=1, 129 | options=None, 130 | type=None), 131 | _descriptor.EnumValueDescriptor( 132 | name='RAPID_RUNNING', index=2, number=2, 133 | options=None, 134 | type=None), 135 | ], 136 | containing_type=None, 137 | options=None, 138 | serialized_start=1536, 139 | serialized_end=1619, 140 | ) 141 | _sym_db.RegisterEnumDescriptor(_EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE) 142 | 143 | 144 | _EGMHEADER = _descriptor.Descriptor( 145 | name='EgmHeader', 146 | full_name='abb.egm.EgmHeader', 147 | filename=None, 148 | file=DESCRIPTOR, 149 | containing_type=None, 150 | fields=[ 151 | _descriptor.FieldDescriptor( 152 | name='seqno', full_name='abb.egm.EgmHeader.seqno', index=0, 153 | number=1, type=13, cpp_type=3, label=1, 154 | has_default_value=False, default_value=0, 155 | message_type=None, enum_type=None, containing_type=None, 156 | is_extension=False, extension_scope=None, 157 | options=None), 158 | _descriptor.FieldDescriptor( 159 | name='tm', full_name='abb.egm.EgmHeader.tm', index=1, 160 | number=2, type=13, cpp_type=3, label=1, 161 | has_default_value=False, default_value=0, 162 | message_type=None, enum_type=None, containing_type=None, 163 | is_extension=False, extension_scope=None, 164 | options=None), 165 | _descriptor.FieldDescriptor( 166 | name='mtype', full_name='abb.egm.EgmHeader.mtype', index=2, 167 | number=3, type=14, cpp_type=8, label=1, 168 | has_default_value=True, default_value=0, 169 | message_type=None, enum_type=None, containing_type=None, 170 | is_extension=False, extension_scope=None, 171 | options=None), 172 | ], 173 | extensions=[ 174 | ], 175 | nested_types=[], 176 | enum_types=[ 177 | _EGMHEADER_MESSAGETYPE, 178 | ], 179 | options=None, 180 | is_extendable=False, 181 | extension_ranges=[], 182 | oneofs=[ 183 | ], 184 | serialized_start=23, 185 | serialized_end=258, 186 | ) 187 | 188 | 189 | _EGMCARTESIAN = _descriptor.Descriptor( 190 | name='EgmCartesian', 191 | full_name='abb.egm.EgmCartesian', 192 | filename=None, 193 | file=DESCRIPTOR, 194 | containing_type=None, 195 | fields=[ 196 | _descriptor.FieldDescriptor( 197 | name='x', full_name='abb.egm.EgmCartesian.x', index=0, 198 | number=1, type=1, cpp_type=5, label=2, 199 | has_default_value=False, default_value=0, 200 | message_type=None, enum_type=None, containing_type=None, 201 | is_extension=False, extension_scope=None, 202 | options=None), 203 | _descriptor.FieldDescriptor( 204 | name='y', full_name='abb.egm.EgmCartesian.y', index=1, 205 | number=2, type=1, cpp_type=5, label=2, 206 | has_default_value=False, default_value=0, 207 | message_type=None, enum_type=None, containing_type=None, 208 | is_extension=False, extension_scope=None, 209 | options=None), 210 | _descriptor.FieldDescriptor( 211 | name='z', full_name='abb.egm.EgmCartesian.z', index=2, 212 | number=3, type=1, cpp_type=5, label=2, 213 | has_default_value=False, default_value=0, 214 | message_type=None, enum_type=None, containing_type=None, 215 | is_extension=False, extension_scope=None, 216 | options=None), 217 | ], 218 | extensions=[ 219 | ], 220 | nested_types=[], 221 | enum_types=[ 222 | ], 223 | options=None, 224 | is_extendable=False, 225 | extension_ranges=[], 226 | oneofs=[ 227 | ], 228 | serialized_start=260, 229 | serialized_end=307, 230 | ) 231 | 232 | 233 | _EGMQUATERNION = _descriptor.Descriptor( 234 | name='EgmQuaternion', 235 | full_name='abb.egm.EgmQuaternion', 236 | filename=None, 237 | file=DESCRIPTOR, 238 | containing_type=None, 239 | fields=[ 240 | _descriptor.FieldDescriptor( 241 | name='u0', full_name='abb.egm.EgmQuaternion.u0', index=0, 242 | number=1, type=1, cpp_type=5, label=2, 243 | has_default_value=False, default_value=0, 244 | message_type=None, enum_type=None, containing_type=None, 245 | is_extension=False, extension_scope=None, 246 | options=None), 247 | _descriptor.FieldDescriptor( 248 | name='u1', full_name='abb.egm.EgmQuaternion.u1', index=1, 249 | number=2, type=1, cpp_type=5, label=2, 250 | has_default_value=False, default_value=0, 251 | message_type=None, enum_type=None, containing_type=None, 252 | is_extension=False, extension_scope=None, 253 | options=None), 254 | _descriptor.FieldDescriptor( 255 | name='u2', full_name='abb.egm.EgmQuaternion.u2', index=2, 256 | number=3, type=1, cpp_type=5, label=2, 257 | has_default_value=False, default_value=0, 258 | message_type=None, enum_type=None, containing_type=None, 259 | is_extension=False, extension_scope=None, 260 | options=None), 261 | _descriptor.FieldDescriptor( 262 | name='u3', full_name='abb.egm.EgmQuaternion.u3', index=3, 263 | number=4, type=1, cpp_type=5, label=2, 264 | has_default_value=False, default_value=0, 265 | message_type=None, enum_type=None, containing_type=None, 266 | is_extension=False, extension_scope=None, 267 | options=None), 268 | ], 269 | extensions=[ 270 | ], 271 | nested_types=[], 272 | enum_types=[ 273 | ], 274 | options=None, 275 | is_extendable=False, 276 | extension_ranges=[], 277 | oneofs=[ 278 | ], 279 | serialized_start=309, 280 | serialized_end=372, 281 | ) 282 | 283 | 284 | _EGMEULER = _descriptor.Descriptor( 285 | name='EgmEuler', 286 | full_name='abb.egm.EgmEuler', 287 | filename=None, 288 | file=DESCRIPTOR, 289 | containing_type=None, 290 | fields=[ 291 | _descriptor.FieldDescriptor( 292 | name='x', full_name='abb.egm.EgmEuler.x', index=0, 293 | number=1, type=1, cpp_type=5, label=2, 294 | has_default_value=False, default_value=0, 295 | message_type=None, enum_type=None, containing_type=None, 296 | is_extension=False, extension_scope=None, 297 | options=None), 298 | _descriptor.FieldDescriptor( 299 | name='y', full_name='abb.egm.EgmEuler.y', index=1, 300 | number=2, type=1, cpp_type=5, label=2, 301 | has_default_value=False, default_value=0, 302 | message_type=None, enum_type=None, containing_type=None, 303 | is_extension=False, extension_scope=None, 304 | options=None), 305 | _descriptor.FieldDescriptor( 306 | name='z', full_name='abb.egm.EgmEuler.z', index=2, 307 | number=3, type=1, cpp_type=5, label=2, 308 | has_default_value=False, default_value=0, 309 | message_type=None, enum_type=None, containing_type=None, 310 | is_extension=False, extension_scope=None, 311 | options=None), 312 | ], 313 | extensions=[ 314 | ], 315 | nested_types=[], 316 | enum_types=[ 317 | ], 318 | options=None, 319 | is_extendable=False, 320 | extension_ranges=[], 321 | oneofs=[ 322 | ], 323 | serialized_start=374, 324 | serialized_end=417, 325 | ) 326 | 327 | 328 | _EGMPOSE = _descriptor.Descriptor( 329 | name='EgmPose', 330 | full_name='abb.egm.EgmPose', 331 | filename=None, 332 | file=DESCRIPTOR, 333 | containing_type=None, 334 | fields=[ 335 | _descriptor.FieldDescriptor( 336 | name='pos', full_name='abb.egm.EgmPose.pos', index=0, 337 | number=1, type=11, cpp_type=10, label=1, 338 | has_default_value=False, default_value=None, 339 | message_type=None, enum_type=None, containing_type=None, 340 | is_extension=False, extension_scope=None, 341 | options=None), 342 | _descriptor.FieldDescriptor( 343 | name='orient', full_name='abb.egm.EgmPose.orient', index=1, 344 | number=2, type=11, cpp_type=10, label=1, 345 | has_default_value=False, default_value=None, 346 | message_type=None, enum_type=None, containing_type=None, 347 | is_extension=False, extension_scope=None, 348 | options=None), 349 | _descriptor.FieldDescriptor( 350 | name='euler', full_name='abb.egm.EgmPose.euler', index=2, 351 | number=3, type=11, cpp_type=10, label=1, 352 | has_default_value=False, default_value=None, 353 | message_type=None, enum_type=None, containing_type=None, 354 | is_extension=False, extension_scope=None, 355 | options=None), 356 | ], 357 | extensions=[ 358 | ], 359 | nested_types=[], 360 | enum_types=[ 361 | ], 362 | options=None, 363 | is_extendable=False, 364 | extension_ranges=[], 365 | oneofs=[ 366 | ], 367 | serialized_start=419, 368 | serialized_end=538, 369 | ) 370 | 371 | 372 | _EGMCARTESIANSPEED = _descriptor.Descriptor( 373 | name='EgmCartesianSpeed', 374 | full_name='abb.egm.EgmCartesianSpeed', 375 | filename=None, 376 | file=DESCRIPTOR, 377 | containing_type=None, 378 | fields=[ 379 | _descriptor.FieldDescriptor( 380 | name='value', full_name='abb.egm.EgmCartesianSpeed.value', index=0, 381 | number=1, type=1, cpp_type=5, label=3, 382 | has_default_value=False, default_value=[], 383 | message_type=None, enum_type=None, containing_type=None, 384 | is_extension=False, extension_scope=None, 385 | options=None), 386 | ], 387 | extensions=[ 388 | ], 389 | nested_types=[], 390 | enum_types=[ 391 | ], 392 | options=None, 393 | is_extendable=False, 394 | extension_ranges=[], 395 | oneofs=[ 396 | ], 397 | serialized_start=540, 398 | serialized_end=574, 399 | ) 400 | 401 | 402 | _EGMJOINTS = _descriptor.Descriptor( 403 | name='EgmJoints', 404 | full_name='abb.egm.EgmJoints', 405 | filename=None, 406 | file=DESCRIPTOR, 407 | containing_type=None, 408 | fields=[ 409 | _descriptor.FieldDescriptor( 410 | name='joints', full_name='abb.egm.EgmJoints.joints', index=0, 411 | number=1, type=1, cpp_type=5, label=3, 412 | has_default_value=False, default_value=[], 413 | message_type=None, enum_type=None, containing_type=None, 414 | is_extension=False, extension_scope=None, 415 | options=None), 416 | ], 417 | extensions=[ 418 | ], 419 | nested_types=[], 420 | enum_types=[ 421 | ], 422 | options=None, 423 | is_extendable=False, 424 | extension_ranges=[], 425 | oneofs=[ 426 | ], 427 | serialized_start=576, 428 | serialized_end=603, 429 | ) 430 | 431 | 432 | _EGMEXTERNALJOINTS = _descriptor.Descriptor( 433 | name='EgmExternalJoints', 434 | full_name='abb.egm.EgmExternalJoints', 435 | filename=None, 436 | file=DESCRIPTOR, 437 | containing_type=None, 438 | fields=[ 439 | _descriptor.FieldDescriptor( 440 | name='joints', full_name='abb.egm.EgmExternalJoints.joints', index=0, 441 | number=1, type=1, cpp_type=5, label=3, 442 | has_default_value=False, default_value=[], 443 | message_type=None, enum_type=None, containing_type=None, 444 | is_extension=False, extension_scope=None, 445 | options=None), 446 | ], 447 | extensions=[ 448 | ], 449 | nested_types=[], 450 | enum_types=[ 451 | ], 452 | options=None, 453 | is_extendable=False, 454 | extension_ranges=[], 455 | oneofs=[ 456 | ], 457 | serialized_start=605, 458 | serialized_end=640, 459 | ) 460 | 461 | 462 | _EGMPLANNED = _descriptor.Descriptor( 463 | name='EgmPlanned', 464 | full_name='abb.egm.EgmPlanned', 465 | filename=None, 466 | file=DESCRIPTOR, 467 | containing_type=None, 468 | fields=[ 469 | _descriptor.FieldDescriptor( 470 | name='joints', full_name='abb.egm.EgmPlanned.joints', index=0, 471 | number=1, type=11, cpp_type=10, label=1, 472 | has_default_value=False, default_value=None, 473 | message_type=None, enum_type=None, containing_type=None, 474 | is_extension=False, extension_scope=None, 475 | options=None), 476 | _descriptor.FieldDescriptor( 477 | name='cartesian', full_name='abb.egm.EgmPlanned.cartesian', index=1, 478 | number=2, type=11, cpp_type=10, label=1, 479 | has_default_value=False, default_value=None, 480 | message_type=None, enum_type=None, containing_type=None, 481 | is_extension=False, extension_scope=None, 482 | options=None), 483 | _descriptor.FieldDescriptor( 484 | name='externalJoints', full_name='abb.egm.EgmPlanned.externalJoints', index=2, 485 | number=3, type=11, cpp_type=10, label=1, 486 | has_default_value=False, default_value=None, 487 | message_type=None, enum_type=None, containing_type=None, 488 | is_extension=False, extension_scope=None, 489 | options=None), 490 | ], 491 | extensions=[ 492 | ], 493 | nested_types=[], 494 | enum_types=[ 495 | ], 496 | options=None, 497 | is_extendable=False, 498 | extension_ranges=[], 499 | oneofs=[ 500 | ], 501 | serialized_start=643, 502 | serialized_end=772, 503 | ) 504 | 505 | 506 | _EGMSPEEDREF = _descriptor.Descriptor( 507 | name='EgmSpeedRef', 508 | full_name='abb.egm.EgmSpeedRef', 509 | filename=None, 510 | file=DESCRIPTOR, 511 | containing_type=None, 512 | fields=[ 513 | _descriptor.FieldDescriptor( 514 | name='joints', full_name='abb.egm.EgmSpeedRef.joints', index=0, 515 | number=1, type=11, cpp_type=10, label=1, 516 | has_default_value=False, default_value=None, 517 | message_type=None, enum_type=None, containing_type=None, 518 | is_extension=False, extension_scope=None, 519 | options=None), 520 | _descriptor.FieldDescriptor( 521 | name='cartesians', full_name='abb.egm.EgmSpeedRef.cartesians', index=1, 522 | number=2, type=11, cpp_type=10, label=1, 523 | has_default_value=False, default_value=None, 524 | message_type=None, enum_type=None, containing_type=None, 525 | is_extension=False, extension_scope=None, 526 | options=None), 527 | _descriptor.FieldDescriptor( 528 | name='externalJoints', full_name='abb.egm.EgmSpeedRef.externalJoints', index=2, 529 | number=3, type=11, cpp_type=10, label=1, 530 | has_default_value=False, default_value=None, 531 | message_type=None, enum_type=None, containing_type=None, 532 | is_extension=False, extension_scope=None, 533 | options=None), 534 | ], 535 | extensions=[ 536 | ], 537 | nested_types=[], 538 | enum_types=[ 539 | ], 540 | options=None, 541 | is_extendable=False, 542 | extension_ranges=[], 543 | oneofs=[ 544 | ], 545 | serialized_start=775, 546 | serialized_end=916, 547 | ) 548 | 549 | 550 | _EGMPATHCORR = _descriptor.Descriptor( 551 | name='EgmPathCorr', 552 | full_name='abb.egm.EgmPathCorr', 553 | filename=None, 554 | file=DESCRIPTOR, 555 | containing_type=None, 556 | fields=[ 557 | _descriptor.FieldDescriptor( 558 | name='pos', full_name='abb.egm.EgmPathCorr.pos', index=0, 559 | number=1, type=11, cpp_type=10, label=2, 560 | has_default_value=False, default_value=None, 561 | message_type=None, enum_type=None, containing_type=None, 562 | is_extension=False, extension_scope=None, 563 | options=None), 564 | _descriptor.FieldDescriptor( 565 | name='age', full_name='abb.egm.EgmPathCorr.age', index=1, 566 | number=2, type=13, cpp_type=3, label=2, 567 | has_default_value=False, default_value=0, 568 | message_type=None, enum_type=None, containing_type=None, 569 | is_extension=False, extension_scope=None, 570 | options=None), 571 | ], 572 | extensions=[ 573 | ], 574 | nested_types=[], 575 | enum_types=[ 576 | ], 577 | options=None, 578 | is_extendable=False, 579 | extension_ranges=[], 580 | oneofs=[ 581 | ], 582 | serialized_start=918, 583 | serialized_end=980, 584 | ) 585 | 586 | 587 | _EGMFEEDBACK = _descriptor.Descriptor( 588 | name='EgmFeedBack', 589 | full_name='abb.egm.EgmFeedBack', 590 | filename=None, 591 | file=DESCRIPTOR, 592 | containing_type=None, 593 | fields=[ 594 | _descriptor.FieldDescriptor( 595 | name='joints', full_name='abb.egm.EgmFeedBack.joints', index=0, 596 | number=1, type=11, cpp_type=10, label=1, 597 | has_default_value=False, default_value=None, 598 | message_type=None, enum_type=None, containing_type=None, 599 | is_extension=False, extension_scope=None, 600 | options=None), 601 | _descriptor.FieldDescriptor( 602 | name='cartesian', full_name='abb.egm.EgmFeedBack.cartesian', index=1, 603 | number=2, type=11, cpp_type=10, label=1, 604 | has_default_value=False, default_value=None, 605 | message_type=None, enum_type=None, containing_type=None, 606 | is_extension=False, extension_scope=None, 607 | options=None), 608 | _descriptor.FieldDescriptor( 609 | name='externalJoints', full_name='abb.egm.EgmFeedBack.externalJoints', index=2, 610 | number=3, type=11, cpp_type=10, label=1, 611 | has_default_value=False, default_value=None, 612 | message_type=None, enum_type=None, containing_type=None, 613 | is_extension=False, extension_scope=None, 614 | options=None), 615 | ], 616 | extensions=[ 617 | ], 618 | nested_types=[], 619 | enum_types=[ 620 | ], 621 | options=None, 622 | is_extendable=False, 623 | extension_ranges=[], 624 | oneofs=[ 625 | ], 626 | serialized_start=983, 627 | serialized_end=1113, 628 | ) 629 | 630 | 631 | _EGMMOTORSTATE = _descriptor.Descriptor( 632 | name='EgmMotorState', 633 | full_name='abb.egm.EgmMotorState', 634 | filename=None, 635 | file=DESCRIPTOR, 636 | containing_type=None, 637 | fields=[ 638 | _descriptor.FieldDescriptor( 639 | name='state', full_name='abb.egm.EgmMotorState.state', index=0, 640 | number=1, type=14, cpp_type=8, label=2, 641 | has_default_value=False, default_value=0, 642 | message_type=None, enum_type=None, containing_type=None, 643 | is_extension=False, extension_scope=None, 644 | options=None), 645 | ], 646 | extensions=[ 647 | ], 648 | nested_types=[], 649 | enum_types=[ 650 | _EGMMOTORSTATE_MOTORSTATETYPE, 651 | ], 652 | options=None, 653 | is_extendable=False, 654 | extension_ranges=[], 655 | oneofs=[ 656 | ], 657 | serialized_start=1116, 658 | serialized_end=1256, 659 | ) 660 | 661 | 662 | _EGMMCISTATE = _descriptor.Descriptor( 663 | name='EgmMCIState', 664 | full_name='abb.egm.EgmMCIState', 665 | filename=None, 666 | file=DESCRIPTOR, 667 | containing_type=None, 668 | fields=[ 669 | _descriptor.FieldDescriptor( 670 | name='state', full_name='abb.egm.EgmMCIState.state', index=0, 671 | number=1, type=14, cpp_type=8, label=2, 672 | has_default_value=True, default_value=0, 673 | message_type=None, enum_type=None, containing_type=None, 674 | is_extension=False, extension_scope=None, 675 | options=None), 676 | ], 677 | extensions=[ 678 | ], 679 | nested_types=[], 680 | enum_types=[ 681 | _EGMMCISTATE_MCISTATETYPE, 682 | ], 683 | options=None, 684 | is_extendable=False, 685 | extension_ranges=[], 686 | oneofs=[ 687 | ], 688 | serialized_start=1259, 689 | serialized_end=1421, 690 | ) 691 | 692 | 693 | _EGMRAPIDCTRLEXECSTATE = _descriptor.Descriptor( 694 | name='EgmRapidCtrlExecState', 695 | full_name='abb.egm.EgmRapidCtrlExecState', 696 | filename=None, 697 | file=DESCRIPTOR, 698 | containing_type=None, 699 | fields=[ 700 | _descriptor.FieldDescriptor( 701 | name='state', full_name='abb.egm.EgmRapidCtrlExecState.state', index=0, 702 | number=1, type=14, cpp_type=8, label=2, 703 | has_default_value=True, default_value=0, 704 | message_type=None, enum_type=None, containing_type=None, 705 | is_extension=False, extension_scope=None, 706 | options=None), 707 | ], 708 | extensions=[ 709 | ], 710 | nested_types=[], 711 | enum_types=[ 712 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE, 713 | ], 714 | options=None, 715 | is_extendable=False, 716 | extension_ranges=[], 717 | oneofs=[ 718 | ], 719 | serialized_start=1424, 720 | serialized_end=1619, 721 | ) 722 | 723 | 724 | _EGMTESTSIGNALS = _descriptor.Descriptor( 725 | name='EgmTestSignals', 726 | full_name='abb.egm.EgmTestSignals', 727 | filename=None, 728 | file=DESCRIPTOR, 729 | containing_type=None, 730 | fields=[ 731 | _descriptor.FieldDescriptor( 732 | name='signals', full_name='abb.egm.EgmTestSignals.signals', index=0, 733 | number=1, type=1, cpp_type=5, label=3, 734 | has_default_value=False, default_value=[], 735 | message_type=None, enum_type=None, containing_type=None, 736 | is_extension=False, extension_scope=None, 737 | options=None), 738 | ], 739 | extensions=[ 740 | ], 741 | nested_types=[], 742 | enum_types=[ 743 | ], 744 | options=None, 745 | is_extendable=False, 746 | extension_ranges=[], 747 | oneofs=[ 748 | ], 749 | serialized_start=1621, 750 | serialized_end=1654, 751 | ) 752 | 753 | 754 | _EGMROBOT = _descriptor.Descriptor( 755 | name='EgmRobot', 756 | full_name='abb.egm.EgmRobot', 757 | filename=None, 758 | file=DESCRIPTOR, 759 | containing_type=None, 760 | fields=[ 761 | _descriptor.FieldDescriptor( 762 | name='header', full_name='abb.egm.EgmRobot.header', index=0, 763 | number=1, type=11, cpp_type=10, label=1, 764 | has_default_value=False, default_value=None, 765 | message_type=None, enum_type=None, containing_type=None, 766 | is_extension=False, extension_scope=None, 767 | options=None), 768 | _descriptor.FieldDescriptor( 769 | name='feedBack', full_name='abb.egm.EgmRobot.feedBack', index=1, 770 | number=2, type=11, cpp_type=10, label=1, 771 | has_default_value=False, default_value=None, 772 | message_type=None, enum_type=None, containing_type=None, 773 | is_extension=False, extension_scope=None, 774 | options=None), 775 | _descriptor.FieldDescriptor( 776 | name='planned', full_name='abb.egm.EgmRobot.planned', index=2, 777 | number=3, type=11, cpp_type=10, label=1, 778 | has_default_value=False, default_value=None, 779 | message_type=None, enum_type=None, containing_type=None, 780 | is_extension=False, extension_scope=None, 781 | options=None), 782 | _descriptor.FieldDescriptor( 783 | name='motorState', full_name='abb.egm.EgmRobot.motorState', index=3, 784 | number=4, type=11, cpp_type=10, label=1, 785 | has_default_value=False, default_value=None, 786 | message_type=None, enum_type=None, containing_type=None, 787 | is_extension=False, extension_scope=None, 788 | options=None), 789 | _descriptor.FieldDescriptor( 790 | name='mciState', full_name='abb.egm.EgmRobot.mciState', index=4, 791 | number=5, type=11, cpp_type=10, label=1, 792 | has_default_value=False, default_value=None, 793 | message_type=None, enum_type=None, containing_type=None, 794 | is_extension=False, extension_scope=None, 795 | options=None), 796 | _descriptor.FieldDescriptor( 797 | name='mciConvergenceMet', full_name='abb.egm.EgmRobot.mciConvergenceMet', index=5, 798 | number=6, type=8, cpp_type=7, label=1, 799 | has_default_value=False, default_value=False, 800 | message_type=None, enum_type=None, containing_type=None, 801 | is_extension=False, extension_scope=None, 802 | options=None), 803 | _descriptor.FieldDescriptor( 804 | name='testSignals', full_name='abb.egm.EgmRobot.testSignals', index=6, 805 | number=7, type=11, cpp_type=10, label=1, 806 | has_default_value=False, default_value=None, 807 | message_type=None, enum_type=None, containing_type=None, 808 | is_extension=False, extension_scope=None, 809 | options=None), 810 | _descriptor.FieldDescriptor( 811 | name='rapidExecState', full_name='abb.egm.EgmRobot.rapidExecState', index=7, 812 | number=8, type=11, cpp_type=10, label=1, 813 | has_default_value=False, default_value=None, 814 | message_type=None, enum_type=None, containing_type=None, 815 | is_extension=False, extension_scope=None, 816 | options=None), 817 | ], 818 | extensions=[ 819 | ], 820 | nested_types=[], 821 | enum_types=[ 822 | ], 823 | options=None, 824 | is_extendable=False, 825 | extension_ranges=[], 826 | oneofs=[ 827 | ], 828 | serialized_start=1657, 829 | serialized_end=1994, 830 | ) 831 | 832 | 833 | _EGMSENSOR = _descriptor.Descriptor( 834 | name='EgmSensor', 835 | full_name='abb.egm.EgmSensor', 836 | filename=None, 837 | file=DESCRIPTOR, 838 | containing_type=None, 839 | fields=[ 840 | _descriptor.FieldDescriptor( 841 | name='header', full_name='abb.egm.EgmSensor.header', index=0, 842 | number=1, type=11, cpp_type=10, label=1, 843 | has_default_value=False, default_value=None, 844 | message_type=None, enum_type=None, containing_type=None, 845 | is_extension=False, extension_scope=None, 846 | options=None), 847 | _descriptor.FieldDescriptor( 848 | name='planned', full_name='abb.egm.EgmSensor.planned', index=1, 849 | number=2, type=11, cpp_type=10, label=1, 850 | has_default_value=False, default_value=None, 851 | message_type=None, enum_type=None, containing_type=None, 852 | is_extension=False, extension_scope=None, 853 | options=None), 854 | _descriptor.FieldDescriptor( 855 | name='speedRef', full_name='abb.egm.EgmSensor.speedRef', index=2, 856 | number=3, type=11, cpp_type=10, label=1, 857 | has_default_value=False, default_value=None, 858 | message_type=None, enum_type=None, containing_type=None, 859 | is_extension=False, extension_scope=None, 860 | options=None), 861 | ], 862 | extensions=[ 863 | ], 864 | nested_types=[], 865 | enum_types=[ 866 | ], 867 | options=None, 868 | is_extendable=False, 869 | extension_ranges=[], 870 | oneofs=[ 871 | ], 872 | serialized_start=1996, 873 | serialized_end=2121, 874 | ) 875 | 876 | 877 | _EGMSENSORPATHCORR = _descriptor.Descriptor( 878 | name='EgmSensorPathCorr', 879 | full_name='abb.egm.EgmSensorPathCorr', 880 | filename=None, 881 | file=DESCRIPTOR, 882 | containing_type=None, 883 | fields=[ 884 | _descriptor.FieldDescriptor( 885 | name='header', full_name='abb.egm.EgmSensorPathCorr.header', index=0, 886 | number=1, type=11, cpp_type=10, label=1, 887 | has_default_value=False, default_value=None, 888 | message_type=None, enum_type=None, containing_type=None, 889 | is_extension=False, extension_scope=None, 890 | options=None), 891 | _descriptor.FieldDescriptor( 892 | name='pathCorr', full_name='abb.egm.EgmSensorPathCorr.pathCorr', index=1, 893 | number=2, type=11, cpp_type=10, label=1, 894 | has_default_value=False, default_value=None, 895 | message_type=None, enum_type=None, containing_type=None, 896 | is_extension=False, extension_scope=None, 897 | options=None), 898 | ], 899 | extensions=[ 900 | ], 901 | nested_types=[], 902 | enum_types=[ 903 | ], 904 | options=None, 905 | is_extendable=False, 906 | extension_ranges=[], 907 | oneofs=[ 908 | ], 909 | serialized_start=2123, 910 | serialized_end=2218, 911 | ) 912 | 913 | _EGMHEADER.fields_by_name['mtype'].enum_type = _EGMHEADER_MESSAGETYPE 914 | _EGMHEADER_MESSAGETYPE.containing_type = _EGMHEADER 915 | _EGMPOSE.fields_by_name['pos'].message_type = _EGMCARTESIAN 916 | _EGMPOSE.fields_by_name['orient'].message_type = _EGMQUATERNION 917 | _EGMPOSE.fields_by_name['euler'].message_type = _EGMEULER 918 | _EGMPLANNED.fields_by_name['joints'].message_type = _EGMJOINTS 919 | _EGMPLANNED.fields_by_name['cartesian'].message_type = _EGMPOSE 920 | _EGMPLANNED.fields_by_name['externalJoints'].message_type = _EGMJOINTS 921 | _EGMSPEEDREF.fields_by_name['joints'].message_type = _EGMJOINTS 922 | _EGMSPEEDREF.fields_by_name['cartesians'].message_type = _EGMCARTESIANSPEED 923 | _EGMSPEEDREF.fields_by_name['externalJoints'].message_type = _EGMJOINTS 924 | _EGMPATHCORR.fields_by_name['pos'].message_type = _EGMCARTESIAN 925 | _EGMFEEDBACK.fields_by_name['joints'].message_type = _EGMJOINTS 926 | _EGMFEEDBACK.fields_by_name['cartesian'].message_type = _EGMPOSE 927 | _EGMFEEDBACK.fields_by_name['externalJoints'].message_type = _EGMJOINTS 928 | _EGMMOTORSTATE.fields_by_name['state'].enum_type = _EGMMOTORSTATE_MOTORSTATETYPE 929 | _EGMMOTORSTATE_MOTORSTATETYPE.containing_type = _EGMMOTORSTATE 930 | _EGMMCISTATE.fields_by_name['state'].enum_type = _EGMMCISTATE_MCISTATETYPE 931 | _EGMMCISTATE_MCISTATETYPE.containing_type = _EGMMCISTATE 932 | _EGMRAPIDCTRLEXECSTATE.fields_by_name['state'].enum_type = _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE 933 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE.containing_type = _EGMRAPIDCTRLEXECSTATE 934 | _EGMROBOT.fields_by_name['header'].message_type = _EGMHEADER 935 | _EGMROBOT.fields_by_name['feedBack'].message_type = _EGMFEEDBACK 936 | _EGMROBOT.fields_by_name['planned'].message_type = _EGMPLANNED 937 | _EGMROBOT.fields_by_name['motorState'].message_type = _EGMMOTORSTATE 938 | _EGMROBOT.fields_by_name['mciState'].message_type = _EGMMCISTATE 939 | _EGMROBOT.fields_by_name['testSignals'].message_type = _EGMTESTSIGNALS 940 | _EGMROBOT.fields_by_name['rapidExecState'].message_type = _EGMRAPIDCTRLEXECSTATE 941 | _EGMSENSOR.fields_by_name['header'].message_type = _EGMHEADER 942 | _EGMSENSOR.fields_by_name['planned'].message_type = _EGMPLANNED 943 | _EGMSENSOR.fields_by_name['speedRef'].message_type = _EGMSPEEDREF 944 | _EGMSENSORPATHCORR.fields_by_name['header'].message_type = _EGMHEADER 945 | _EGMSENSORPATHCORR.fields_by_name['pathCorr'].message_type = _EGMPATHCORR 946 | DESCRIPTOR.message_types_by_name['EgmHeader'] = _EGMHEADER 947 | DESCRIPTOR.message_types_by_name['EgmCartesian'] = _EGMCARTESIAN 948 | DESCRIPTOR.message_types_by_name['EgmQuaternion'] = _EGMQUATERNION 949 | DESCRIPTOR.message_types_by_name['EgmEuler'] = _EGMEULER 950 | DESCRIPTOR.message_types_by_name['EgmPose'] = _EGMPOSE 951 | DESCRIPTOR.message_types_by_name['EgmCartesianSpeed'] = _EGMCARTESIANSPEED 952 | DESCRIPTOR.message_types_by_name['EgmJoints'] = _EGMJOINTS 953 | DESCRIPTOR.message_types_by_name['EgmExternalJoints'] = _EGMEXTERNALJOINTS 954 | DESCRIPTOR.message_types_by_name['EgmPlanned'] = _EGMPLANNED 955 | DESCRIPTOR.message_types_by_name['EgmSpeedRef'] = _EGMSPEEDREF 956 | DESCRIPTOR.message_types_by_name['EgmPathCorr'] = _EGMPATHCORR 957 | DESCRIPTOR.message_types_by_name['EgmFeedBack'] = _EGMFEEDBACK 958 | DESCRIPTOR.message_types_by_name['EgmMotorState'] = _EGMMOTORSTATE 959 | DESCRIPTOR.message_types_by_name['EgmMCIState'] = _EGMMCISTATE 960 | DESCRIPTOR.message_types_by_name['EgmRapidCtrlExecState'] = _EGMRAPIDCTRLEXECSTATE 961 | DESCRIPTOR.message_types_by_name['EgmTestSignals'] = _EGMTESTSIGNALS 962 | DESCRIPTOR.message_types_by_name['EgmRobot'] = _EGMROBOT 963 | DESCRIPTOR.message_types_by_name['EgmSensor'] = _EGMSENSOR 964 | DESCRIPTOR.message_types_by_name['EgmSensorPathCorr'] = _EGMSENSORPATHCORR 965 | 966 | EgmHeader = _reflection.GeneratedProtocolMessageType('EgmHeader', (_message.Message,), dict( 967 | DESCRIPTOR = _EGMHEADER, 968 | __module__ = 'egm_pb2' 969 | # @@protoc_insertion_point(class_scope:abb.egm.EgmHeader) 970 | )) 971 | _sym_db.RegisterMessage(EgmHeader) 972 | 973 | EgmCartesian = _reflection.GeneratedProtocolMessageType('EgmCartesian', (_message.Message,), dict( 974 | DESCRIPTOR = _EGMCARTESIAN, 975 | __module__ = 'egm_pb2' 976 | # @@protoc_insertion_point(class_scope:abb.egm.EgmCartesian) 977 | )) 978 | _sym_db.RegisterMessage(EgmCartesian) 979 | 980 | EgmQuaternion = _reflection.GeneratedProtocolMessageType('EgmQuaternion', (_message.Message,), dict( 981 | DESCRIPTOR = _EGMQUATERNION, 982 | __module__ = 'egm_pb2' 983 | # @@protoc_insertion_point(class_scope:abb.egm.EgmQuaternion) 984 | )) 985 | _sym_db.RegisterMessage(EgmQuaternion) 986 | 987 | EgmEuler = _reflection.GeneratedProtocolMessageType('EgmEuler', (_message.Message,), dict( 988 | DESCRIPTOR = _EGMEULER, 989 | __module__ = 'egm_pb2' 990 | # @@protoc_insertion_point(class_scope:abb.egm.EgmEuler) 991 | )) 992 | _sym_db.RegisterMessage(EgmEuler) 993 | 994 | EgmPose = _reflection.GeneratedProtocolMessageType('EgmPose', (_message.Message,), dict( 995 | DESCRIPTOR = _EGMPOSE, 996 | __module__ = 'egm_pb2' 997 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPose) 998 | )) 999 | _sym_db.RegisterMessage(EgmPose) 1000 | 1001 | EgmCartesianSpeed = _reflection.GeneratedProtocolMessageType('EgmCartesianSpeed', (_message.Message,), dict( 1002 | DESCRIPTOR = _EGMCARTESIANSPEED, 1003 | __module__ = 'egm_pb2' 1004 | # @@protoc_insertion_point(class_scope:abb.egm.EgmCartesianSpeed) 1005 | )) 1006 | _sym_db.RegisterMessage(EgmCartesianSpeed) 1007 | 1008 | EgmJoints = _reflection.GeneratedProtocolMessageType('EgmJoints', (_message.Message,), dict( 1009 | DESCRIPTOR = _EGMJOINTS, 1010 | __module__ = 'egm_pb2' 1011 | # @@protoc_insertion_point(class_scope:abb.egm.EgmJoints) 1012 | )) 1013 | _sym_db.RegisterMessage(EgmJoints) 1014 | 1015 | EgmExternalJoints = _reflection.GeneratedProtocolMessageType('EgmExternalJoints', (_message.Message,), dict( 1016 | DESCRIPTOR = _EGMEXTERNALJOINTS, 1017 | __module__ = 'egm_pb2' 1018 | # @@protoc_insertion_point(class_scope:abb.egm.EgmExternalJoints) 1019 | )) 1020 | _sym_db.RegisterMessage(EgmExternalJoints) 1021 | 1022 | EgmPlanned = _reflection.GeneratedProtocolMessageType('EgmPlanned', (_message.Message,), dict( 1023 | DESCRIPTOR = _EGMPLANNED, 1024 | __module__ = 'egm_pb2' 1025 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPlanned) 1026 | )) 1027 | _sym_db.RegisterMessage(EgmPlanned) 1028 | 1029 | EgmSpeedRef = _reflection.GeneratedProtocolMessageType('EgmSpeedRef', (_message.Message,), dict( 1030 | DESCRIPTOR = _EGMSPEEDREF, 1031 | __module__ = 'egm_pb2' 1032 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSpeedRef) 1033 | )) 1034 | _sym_db.RegisterMessage(EgmSpeedRef) 1035 | 1036 | EgmPathCorr = _reflection.GeneratedProtocolMessageType('EgmPathCorr', (_message.Message,), dict( 1037 | DESCRIPTOR = _EGMPATHCORR, 1038 | __module__ = 'egm_pb2' 1039 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPathCorr) 1040 | )) 1041 | _sym_db.RegisterMessage(EgmPathCorr) 1042 | 1043 | EgmFeedBack = _reflection.GeneratedProtocolMessageType('EgmFeedBack', (_message.Message,), dict( 1044 | DESCRIPTOR = _EGMFEEDBACK, 1045 | __module__ = 'egm_pb2' 1046 | # @@protoc_insertion_point(class_scope:abb.egm.EgmFeedBack) 1047 | )) 1048 | _sym_db.RegisterMessage(EgmFeedBack) 1049 | 1050 | EgmMotorState = _reflection.GeneratedProtocolMessageType('EgmMotorState', (_message.Message,), dict( 1051 | DESCRIPTOR = _EGMMOTORSTATE, 1052 | __module__ = 'egm_pb2' 1053 | # @@protoc_insertion_point(class_scope:abb.egm.EgmMotorState) 1054 | )) 1055 | _sym_db.RegisterMessage(EgmMotorState) 1056 | 1057 | EgmMCIState = _reflection.GeneratedProtocolMessageType('EgmMCIState', (_message.Message,), dict( 1058 | DESCRIPTOR = _EGMMCISTATE, 1059 | __module__ = 'egm_pb2' 1060 | # @@protoc_insertion_point(class_scope:abb.egm.EgmMCIState) 1061 | )) 1062 | _sym_db.RegisterMessage(EgmMCIState) 1063 | 1064 | EgmRapidCtrlExecState = _reflection.GeneratedProtocolMessageType('EgmRapidCtrlExecState', (_message.Message,), dict( 1065 | DESCRIPTOR = _EGMRAPIDCTRLEXECSTATE, 1066 | __module__ = 'egm_pb2' 1067 | # @@protoc_insertion_point(class_scope:abb.egm.EgmRapidCtrlExecState) 1068 | )) 1069 | _sym_db.RegisterMessage(EgmRapidCtrlExecState) 1070 | 1071 | EgmTestSignals = _reflection.GeneratedProtocolMessageType('EgmTestSignals', (_message.Message,), dict( 1072 | DESCRIPTOR = _EGMTESTSIGNALS, 1073 | __module__ = 'egm_pb2' 1074 | # @@protoc_insertion_point(class_scope:abb.egm.EgmTestSignals) 1075 | )) 1076 | _sym_db.RegisterMessage(EgmTestSignals) 1077 | 1078 | EgmRobot = _reflection.GeneratedProtocolMessageType('EgmRobot', (_message.Message,), dict( 1079 | DESCRIPTOR = _EGMROBOT, 1080 | __module__ = 'egm_pb2' 1081 | # @@protoc_insertion_point(class_scope:abb.egm.EgmRobot) 1082 | )) 1083 | _sym_db.RegisterMessage(EgmRobot) 1084 | 1085 | EgmSensor = _reflection.GeneratedProtocolMessageType('EgmSensor', (_message.Message,), dict( 1086 | DESCRIPTOR = _EGMSENSOR, 1087 | __module__ = 'egm_pb2' 1088 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSensor) 1089 | )) 1090 | _sym_db.RegisterMessage(EgmSensor) 1091 | 1092 | EgmSensorPathCorr = _reflection.GeneratedProtocolMessageType('EgmSensorPathCorr', (_message.Message,), dict( 1093 | DESCRIPTOR = _EGMSENSORPATHCORR, 1094 | __module__ = 'egm_pb2' 1095 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSensorPathCorr) 1096 | )) 1097 | _sym_db.RegisterMessage(EgmSensorPathCorr) 1098 | 1099 | 1100 | # @@protoc_insertion_point(module_scope) 1101 | --------------------------------------------------------------------------------