1 Star 0 Fork 0

陈狗翔 / UR5-control-with-RG2

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
serialize.py 6.42 KB
Copy Edit Web IDE Raw Blame History
chen authored 2019-10-28 03:30 . nothing special
# Copyright (c) 2016, Universal Robots A/S,
# All rights reserved.
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of the Universal Robots A/S nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
import struct
class ControlHeader(object):
__slots__ = ['command', 'size',]
@staticmethod
def unpack(buf):
rmd = ControlHeader()
(rmd.size, rmd.command) = struct.unpack_from('>HB', buf)
return rmd
class ControlVersion(object):
__slots__ = ['major', 'minor', 'bugfix', 'build']
@staticmethod
def unpack(buf):
rmd = ControlVersion()
(rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from('>IIII', buf)
return rmd
class ReturnValue(object):
__slots__ = ['success']
@staticmethod
def unpack(buf):
rmd = ReturnValue()
rmd.success = bool(struct.unpack_from('>B', buf)[0])
return rmd
class Message(object):
__slots__ = ['level', 'message', 'source']
EXCEPTION_MESSAGE = 0
ERROR_MESSAGE = 1
WARNING_MESSAGE = 2
INFO_MESSAGE = 3
@staticmethod
def unpack(buf):
rmd = Message()
offset = 0
msg_length = struct.unpack_from(">B", buf, offset)[0]
offset = offset + 1
rmd.message = buf[offset:offset+msg_length]
offset = offset + msg_length
src_length = struct.unpack_from(">B", buf, offset)[0]
offset = offset + 1
rmd.source = buf[offset:offset+src_length]
offset = offset + src_length
rmd.level = struct.unpack_from(">B", buf, offset)[0]
return rmd
def get_item_size(data_type):
if data_type.startswith('VECTOR6'):
return 6
elif data_type.startswith('VECTOR3'):
return 3
return 1
def unpack_field(data, offset, data_type):
size = get_item_size(data_type)
if(data_type == 'VECTOR6D' or
data_type == 'VECTOR3D'):
return [float(data[offset+i]) for i in range(size)]
elif(data_type == 'VECTOR6UINT32'):
return [int(data[offset+i]) for i in range(size)]
elif(data_type == 'DOUBLE'):
return float(data[offset])
elif(data_type == 'UINT32' or
data_type == 'UINT64'):
return int(data[offset])
elif(data_type == 'VECTOR6INT32'):
return [int(data[offset+i]) for i in range(size)]
elif(data_type == 'INT32' or
data_type == 'UINT8'):
return int(data[offset])
elif(data_type == 'BOOL'):
return bool(data[offset])
raise ValueError('unpack_field: unknown data type: ' + data_type)
class DataObject(object):
recipe_id = None
def pack(self, names, types):
if len(names) != len(types):
raise ValueError('List sizes are not identical.')
l = []
if(self.recipe_id is not None):
l.append(self.recipe_id)
for i in range(len(names)):
if self.__dict__[names[i]] is None:
raise ValueError('Uninitialized parameter: ' + names[i])
if types[i].startswith('VECTOR'):
l.extend(self.__dict__[names[i]])
else:
l.append(self.__dict__[names[i]])
return l
@staticmethod
def unpack(data, names, types):
if len(names) != len(types):
raise ValueError('List sizes are not identical.')
obj = DataObject()
offset = 0
obj.recipe_id = data[0]
for i in range(len(names)):
obj.__dict__[names[i]] = unpack_field(data[1:], offset, types[i])
offset += get_item_size(types[i])
return obj
@staticmethod
def create_empty(names, recipe_id):
obj = DataObject()
for i in range(len(names)):
obj.__dict__[names[i]] = None
obj.recipe_id = recipe_id
return obj
class DataConfig(object):
__slots__ = ['id', 'names', 'types', 'fmt']
@staticmethod
def unpack_recipe(buf):
rmd = DataConfig();
rmd.id = struct.unpack_from('>B', buf)[0]
rmd.types = buf.decode('utf-8')[1:].split(',')
rmd.fmt = '>B'
for i in rmd.types:
if i=='INT32':
rmd.fmt += 'i'
elif i=='UINT32':
rmd.fmt += 'I'
elif i=='VECTOR6D':
rmd.fmt += 'd'*6
elif i=='VECTOR3D':
rmd.fmt += 'd'*3
elif i=='VECTOR6INT32':
rmd.fmt += 'i'*6
elif i=='VECTOR6UINT32':
rmd.fmt += 'I'*6
elif i=='DOUBLE':
rmd.fmt += 'd'
elif i=='UINT64':
rmd.fmt += 'Q'
elif i=='UINT8':
rmd.fmt += 'B'
elif i =='BOOL':
rmd.fmt += '?'
elif i=='IN_USE':
raise ValueError('An input parameter is already in use.')
else:
raise ValueError('Unknown data type: ' + i)
return rmd
def pack(self, state):
l = state.pack(self.names, self.types)
return struct.pack(self.fmt, *l)
def unpack(self, data):
li = struct.unpack_from(self.fmt, data)
return DataObject.unpack(li, self.names, self.types)

Comment ( 0 )

Sign in for post a comment

Python
1
https://gitee.com/ChenGouXiang/UR5-control-with-RG2.git
git@gitee.com:ChenGouXiang/UR5-control-with-RG2.git
ChenGouXiang
UR5-control-with-RG2
UR5-control-with-RG2
master

Search