def receive_packet(packet):
device_id = packet.device_id
packet_id = packet.packet_id
data = bytearray(packet.data)
if packet_id == PacketID.POSITION:
position = BPLProtocol.decode_floats(data)[0]
print("Position Received: {} - {}".format(device_id, position))
def tx_transmit(self, packet):
device_id = packet.device_id
packet_id = packet.packet_id
data = bytearray(packet.data)
rospy.logdebug("Transmitting {}, {}, {}".format(
device_id, packet_id, data))
encoded_packet = BPLProtocol.encode_packet(device_id, packet_id, data)
self.sock.sendto(encoded_packet, (self.ip_address, self.port))
def tx_transmit(self, packet):
device_id = packet.device_id
packet_id = packet.packet_id
data = bytearray(packet.data)
rospy.logdebug("Transmitting {}, {}, {}".format(device_id, packet_id, data))
encoded_packet = BPLProtocol.encode_packet(device_id, packet_id, data)
try:
self.serial_port.write(encoded_packet)
except serial.SerialException as e:
rospy.logwarn("Unable to write to serial")
from bplprotocol import BPLProtocol, PacketID, PacketReader
import socket
MANIPULATOR_IP_ADDRESS = "192.168.2.4"
MANIPULATOR_PORT = 6789
if __name__ == '__main__':
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.settimeout(0)
# To TARE THe sensor send an ATI_FT Packet to Device 0x0D
tare_packet = \
BPLProtocol.encode_packet(0x0D, PacketID.ATI_FT_READING, BPLProtocol.encode_floats([0., 0., 0., 0., 0., 0.]))
sock.sendto(tare_packet, manipulator_address)
print("Tared the FT Sensor")
time.sleep(0.5)
# To Read the packers request from the ATI_FT Packet from device 0x0D
request_timeout = 0.1 # seconds
pr = PacketReader()
readings = 0
while True:
request_packet = BPLProtocol.encode_packet(
request_timeout = 0.5 # Seconds
serial_port = serial.Serial(serial_port_name,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0)
# Requesting Information
device_id = 0x01 # Jaws
print(f"Requesting Position from Device {device_id}")
# Request POSITION from the jaws
serial_port.write(
BPLProtocol.encode_packet(device_id, PacketID.REQUEST,
bytes([PacketID.POSITION])))
start_time = time.time()
position = None
while True:
time.sleep(0.0001)
try:
read_data = serial_port.read()
except BaseException:
read_data = b''
if read_data != b'':
packets = packet_reader.receive_bytes(read_data)
if packets:
for packet in packets:
read_device_id, read_packet_id, data_bytes = packet
DEVICE_IDS = [1, 2, 3, 4, 5, 6, 7]
FREQUENCY = 10
if __name__ == '__main__':
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.settimeout(0)
packet_to_send = b''
for device_id in DEVICE_IDS:
heartbeat_packets_set = BPLProtocol.encode_packet(device_id, PacketID.HEARTBEAT_SET, bytes(HEARTBEAT_PACKETS))
sock.sendto(heartbeat_packets_set, manipulator_address)
heartbeat_frequency_set = BPLProtocol.encode_packet(device_id, PacketID.HEARTBEAT_FREQUENCY, bytes([FREQUENCY]))
sock.sendto(heartbeat_frequency_set, manipulator_address)
print(f"Heartbeat set for Device 0x{device_id:X}")
# Read the corresponding packets from the TX2.
positions = ["-"] * len(DEVICE_IDS)
pr = PacketReader()
while True:
try:
recv_bytes, address = sock.recvfrom(4096)
if __name__ == '__main__':
device_id = 0x02 # Joint B
serial_port_name = "COM10"
serial_port = serial.Serial(serial_port_name,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0)
# Send to a position of 0.5 radians
serial_port.write(
BPLProtocol.encode_packet(device_id, PacketID.POSITION,
BPLProtocol.encode_floats([0.5])))
time.sleep(5)
# Send a velocity command of 0.3
serial_port.write(
BPLProtocol.encode_packet(device_id, PacketID.VELOCITY,
BPLProtocol.encode_floats([0.3])))
time.sleep(3)
# Send a velocity of 0
serial_port.write(
BPLProtocol.encode_packet(device_id, PacketID.VELOCITY,
BPLProtocol.encode_floats([0.0])))
time.sleep(3)
MANIPULATOR_PORT = 6789
DEVICE_IDS = [1, 2, 3, 4, 5, 6, 7]
REQUEST_FREQUENCY = 100
if __name__ == '__main__':
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.settimeout(0)
pr = PacketReader()
# Device_id 0XFF is broadcast that is received by all devices.
request_packet = BPLProtocol.encode_packet(0xFF, PacketID.REQUEST,
bytes([PacketID.TEMPERATURE]))
positions = ["-"] * len(DEVICE_IDS)
next_req_time = time.time() + (1 / REQUEST_FREQUENCY)
while True:
try:
recv_bytes, address = sock.recvfrom(4096)
except BaseException:
recv_bytes = b''
if recv_bytes:
packets = pr.receive_bytes(recv_bytes)
for device_id, packet_id, data in packets:
if packet_id == PacketID.POSITION:
from bplprotocol import BPLProtocol, PacketID
import time
import socket
if __name__ == '__main__':
MANIPULATOR_IP_ADDRESS = '192.168.2.4'
MANIPULATOR_PORT = 6789
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Desired position from end effector to base A -> G
desired_positions = [10.0, 0.5, 1.5707, 1.5707, 1.5707, 2.8, 3.14159]
packets = b''
for index, position in enumerate(desired_positions):
device_id = index + 1
packets += BPLProtocol.encode_packet(
device_id, PacketID.POSITION,
BPLProtocol.encode_floats([position]))
# Send joints to desired_positions
sock.sendto(packets, manipulator_address)
if __name__ == '__main__':
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.settimeout(0)
enabled = 1.0
v_min = 20.0 # V
v_max = 27.0 # V
thresh_time = 3.0 # seconds
# To Send the voltage threshold parameters to 708 (0x0D)
voltage_threshold_packet = \
BPLProtocol.encode_packet(0x0D, PacketID.VOLTAGE_THRESHOLD_PARAMETERS, BPLProtocol.encode_floats([enabled, v_min, v_max, thresh_time]))
sock.sendto(voltage_threshold_packet, manipulator_address)
time.sleep(0.5)
# Save the parameters
# Set the device to factory Mode
set_factory_mode = BPLProtocol.encode_packet(0x0D, PacketID.MODE,
bytes([8]))
sock.sendto(set_factory_mode, manipulator_address)
save_packet = BPLProtocol.encode_packet(0x0D, PacketID.SAVE, bytes([0]))
sock.sendto(save_packet, manipulator_address)
time.sleep(1.5)
request_timeout = 0.5 # Seconds
MANIPULATOR_IP_ADDRESS = '192.168.2.4'
MANIPULATOR_PORT = 6789
manipulator_address = (MANIPULATOR_IP_ADDRESS, MANIPULATOR_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Requesting Information
device_id = 0x01 # Jaws
print(f"Requesting Position from Device {device_id}")
# Request POSITION from the jaws
sock.sendto(BPLProtocol.encode_packet(device_id, PacketID.REQUEST, bytes([PacketID.POSITION])), manipulator_address)
start_time = time.time()
position = None
while True:
time.sleep(0.0001)
try:
read_data, remote_address = sock.recvfrom(4096)
except BaseException:
read_data = b''
if read_data != b'':
packets = packet_reader.receive_bytes(read_data)
if packets:
for packet in packets:
read_device_id, read_packet_id, data_bytes = packet
packet_reader = PacketReader()
serial_port_name = "COM10"
serial_port = serial.Serial(serial_port_name,
baudrate=115200,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=0)
request_packet = b''
# Request packets can be concatenated
for device_id in device_ids:
request_packet += BPLProtocol.encode_packet(device_id,
PacketID.REQUEST,
bytes(PacketID.POSITION))
while True:
# Send request packet
serial_port.write(request_packet)
position_responses = {}
# Read request packets
start_time = time.time()
while time.time() < start_time + 1 / frequency:
time.sleep(0.0001)
try:
read_data = serial_port.read()