def __init__(self, router, router_real_time):
self.expected_number_of_actuators = 7 # example works for 7dof Gen3
# self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator
self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = 0
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
self.actuator_count += 1
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
def __init__(self, router, router_real_time):
# Maximum allowed waiting time during actions (in seconds)
self.ACTION_TIMEOUT_DURATION = 20
self.torque_amplification = 2.0 # Torque measure on last actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = self.base.GetActuatorCount().count
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
class EthernetBridgeConfigurationExample:
def __init__(self, router):
# Create required services
self.interconnect_config = InterconnectConfigClient(router)
self.device_manager = DeviceManagerClient(router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
def GetDeviceIdFromDevType(self, device_type, device_index=0):
devices = self.device_manager.ReadAllDevices()
current_index = 0
for device in devices.device_handle:
if device.device_type == device_type:
if current_index == device_index:
print("Found the Interconnect on device identifier {}".
format(device.device_identifier))
return device.device_identifier
current_index += 1
return None
def EnableEthernetBridge(self):
# Configure the Interconnect to enable the bridge
ethernet_configuration = InterconnectConfig_pb2.EthernetConfiguration()
ethernet_configuration.device = InterconnectConfig_pb2.ETHERNET_DEVICE_EXPANSION
ethernet_configuration.enabled = True
ethernet_configuration.speed = InterconnectConfig_pb2.ETHERNET_SPEED_100M
ethernet_configuration.duplex = InterconnectConfig_pb2.ETHERNET_DUPLEX_FULL
try:
self.interconnect_config.SetEthernetConfiguration(
ethernet_configuration, self.interconnect_device_id)
except Exception as e:
print("An unexpected error occured : {}".format(e))
class I2CBridge:
def __init__(self, router):
'''
Implements methods for establishing and operating I2C bridge through
the base
'''
self.router = router
# Create device manager client. Device manager is used get a list of devices present in the arm. In this example
# we use device manager to determine the device ID associated with the interconnect.
self.device_manager = DeviceManagerClient(self.router)
# Create interconnect configuration client. This client is used to perform I2C bus configuration and I2C bus actions.
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
"""
GetDeviceIdFromDevType(devType, devIndex)
Get device ID according to a given device type (Actuator or interconnect).
Inputs:
devType : Device type
Index argument correspond to the position of the device (i.e.: 0 being the first,1 the second, etc.)
"""
def GetDeviceIdFromDevType(self, device_type, device_index=0):
devices = self.device_manager.ReadAllDevices()
current_index = 0
for device in devices.device_handle:
if device.device_type == device_type:
if current_index == device_index:
print("Found the Interconnect on device identifier {}".
format(device.device_identifier))
return device.device_identifier
current_index += 1
return None
"""
WriteValue(device_address, data, timeout_ms)
Writes a data array to I2C bus to a given device.
inputs:
device_address: device's I2C address.
data: list containing data to write to device
timeout_ms: write operation timeout in milliseconds
"""
def WriteValue(self, device_address, data, timeout_ms):
i2c_write_parameter = InterconnectConfig_pb2.I2CWriteParameter()
i2c_write_parameter.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION
i2c_write_parameter.device_address = device_address
bytesData = bytes(data)
i2c_write_parameter.data.data = bytesData
i2c_write_parameter.data.size = len(bytesData)
i2c_write_parameter.timeout = timeout_ms
return self.interconnect_config.I2CWrite(
i2c_write_parameter, deviceId=self.interconnect_device_id)
"""
ReadValue(device_address, bytes_to_read, timeout_ms)
Reads a data array from I2C bus from a given device.
inputs:
device_address: device's I2C address.
bytes_to_read: number of bytes to read from device
timeout_ms: read operation timeout in milliseconds
"""
def ReadValue(self, device_address, bytes_to_read, timeout_ms):
# Create the I2C read request
i2c_read_request = InterconnectConfig_pb2.I2CReadParameter()
i2c_read_request.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION
i2c_read_request.device_address = device_address
i2c_read_request.size = bytes_to_read
i2c_read_request.timeout = timeout_ms
# Read the data and print it
read_result = self.interconnect_config.I2CRead(
i2c_read_request, deviceId=self.interconnect_device_id)
data = read_result.data
print("We were supposed to read {} bytes and we read {} bytes.".format(
bytes_to_read, read_result.size))
print("The data is : {0:b}".format(ord(data)))
"""
Configure(is_enabled, mode, addressing)
Configure expansion bus I2C bus on interconnect.
Inputs:
is_enabled: Enables i2cbus on interconnect's expansion bus if true, disable it otherwise.
mode: I2C mode in which the bus is set ( InterconnectConfig_pb2.I2C_MODE_STANDARD,
InterconnectConfig_pb2.I2C_MODE_FAST or InterconnectConfig_pb2.I2C_MODE_FAST_PLUS)
addressing: Addressing size used on I2C bus (I2C_DEVICE_ADDRESSING_7_BITS or
I2C_DEVICE_ADDRESSING_10_BITS).
"""
def Configure(self, is_enabled, mode, addressing):
# Create the configuration
I2CConfiguration = InterconnectConfig_pb2.I2CConfiguration()
I2CConfiguration.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION
I2CConfiguration.enabled = is_enabled
I2CConfiguration.mode = mode
I2CConfiguration.addressing = addressing
# Set the configuration
self.interconnect_config.SetI2CConfiguration(
I2CConfiguration, deviceId=self.interconnect_device_id)
session_info = Session_pb2.CreateSessionInfo()
session_info.username = '******'
session_info.password = '******'
session_info.session_inactivity_timeout = 60000 # (milliseconds)
session_info.connection_inactivity_timeout = 2000 # (milliseconds)
print("Session created")
session_manager = SessionManager(router)
session_manager.CreateSession(session_info)
# Create required services
base_client_service = BaseClient(router)
device_manager_service = DeviceManagerClient(router)
# Find the number of actuator in angular action and trajectory
sub_device_info = device_manager_service.ReadAllDevices()
act_count = 0
for dev in sub_device_info.device_handle:
if dev.device_type is Common_pb2.BIG_ACTUATOR or dev.device_type is Common_pb2.SMALL_ACTUATOR:
act_count += 1
# Move arm to ready position
print("\nMoving the arm to a safe position before executing example")
action_type = Base_pb2.RequestedActionType()
action_type.action_type = Base_pb2.REACH_JOINT_ANGLES
action_list = base_client_service.ReadAllActions(action_type)
action_handle = None
# print("Actions:")
# print( action_list.action_list)
for action in action_list.action_list:
class GpioBridge:
'''
Implements methods for establishing and operating GPIO bridge through
the base
'''
GpioEnum = (
InterconnectConfig_pb2.GPIO_IDENTIFIER_1,
InterconnectConfig_pb2.GPIO_IDENTIFIER_2,
InterconnectConfig_pb2.GPIO_IDENTIFIER_3,
InterconnectConfig_pb2.GPIO_IDENTIFIER_4
)
def __init__(self, router):
self.router = router
self.device_manager = DeviceManagerClient(self.router)
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print ("Could not find the Interconnect in the device list, exiting...")
sys.exit(0)
def GetDeviceIdFromDevType(self, device_type, device_index = 0):
devices = self.device_manager.ReadAllDevices()
current_index = 0
for device in devices.device_handle:
if device.device_type == device_type:
if current_index == device_index:
print ("Found the Interconnect on device identifier {}".format(device.device_identifier))
return device.device_identifier
current_index += 1
return None
def InitGpioInputsAndOutputs(self):
gpio_config = InterconnectConfig_pb2.GPIOConfiguration()
# Pins 1 and 2 as output
gpio_config.mode = InterconnectConfig_pb2.GPIO_MODE_OUTPUT_PUSH_PULL
gpio_config.pull = InterconnectConfig_pb2.GPIO_PULL_NONE
gpio_config.default_value = InterconnectConfig_pb2.GPIO_VALUE_LOW
gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_1
print ("Setting pin #1 as output...")
self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id)
time.sleep(1)
gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_2
print ("Setting pin #2 as output...")
self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id)
time.sleep(1)
# Pins 3 and 4 as input pullup
gpio_config.mode = InterconnectConfig_pb2.GPIO_MODE_INPUT_FLOATING
gpio_config.pull = InterconnectConfig_pb2.GPIO_PULL_UP
gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_3
print ("Setting pin #3 as input pullup...")
self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id)
time.sleep(1)
gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_4
print ("Setting pin #4 as input pullup...")
self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id)
time.sleep(1)
def SetOutputPinValue(self, identifier, value):
gpio_state = InterconnectConfig_pb2.GPIOState()
gpio_state.identifier = identifier
gpio_state.value = value
print ("GPIO pin {} will be put at value {}".format(InterconnectConfig_pb2.GPIOIdentifier.Name(identifier), InterconnectConfig_pb2.GPIOValue.Name(value)))
self.interconnect_config.SetGPIOState(gpio_state,deviceId=self.interconnect_device_id)
def ReadInputPinValue(self, identifier):
gpio_identification = InterconnectConfig_pb2.GPIOIdentification()
gpio_identification.identifier = identifier
state = self.interconnect_config.GetGPIOState(gpio_identification,deviceId=self.interconnect_device_id)
if (state.value == InterconnectConfig_pb2.GPIO_VALUE_HIGH):
return 1
elif (state.value == InterconnectConfig_pb2.GPIO_VALUE_LOW):
return 0
else:
print ("Error : the value read is unspecified")
return -1
def ExampleSetAndReadValues(self):
# We sleep a bit between the reads and the writes
# Technically the InterconnectConfig service runs at 25ms but we sleep 100ms to make sure we let enough time
sleep_time_sec = 0.1
# The Arduino reads pin 1 and sets pin 3 the same
# The Arduino reads pin 2 and sets pin 4 the same
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_HIGH)
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_LOW)
time.sleep(sleep_time_sec)
pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be HIGH
pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be LOW
print ("Value read for pin #3 is : {}".format(pin3_in))
print ("Value read for pin #4 is : {}".format(pin4_in))
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_LOW)
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_HIGH)
time.sleep(sleep_time_sec)
pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be LOW
pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be HIGH
print ("Value read for pin #3 is : {}".format(pin3_in))
print ("Value read for pin #4 is : {}".format(pin4_in))
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_HIGH)
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_HIGH)
time.sleep(sleep_time_sec)
pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be HIGH
pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be HIGH
print ("Value read for pin #3 is : {}".format(pin3_in))
print ("Value read for pin #4 is : {}".format(pin4_in))
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_LOW)
self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_LOW)
time.sleep(sleep_time_sec)
pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be LOW
pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be LOW
print ("Value read for pin #3 is : {}".format(pin3_in))
print ("Value read for pin #4 is : {}".format(pin4_in))
class UARTBridge:
def __init__(self, router, ip_address):
self.router = router
self.base_ip_address = ip_address
# Create services
self.base = BaseClient(self.router)
self.device_manager = DeviceManagerClient(self.router)
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
def GetDeviceIdFromDevType(self, device_type, device_index=0):
devices = self.device_manager.ReadAllDevices()
current_index = 0
for device in devices.device_handle:
if device.device_type == device_type:
if current_index == device_index:
print("Found the Interconnect on device identifier {}".
format(device.device_identifier))
return device.device_identifier
current_index += 1
return None
def Configure(self, port_id, enabled, speed, word_length, stop_bits,
parity):
'''
Enable and configure UART on interconnect. This will open a TCP port on the interconnect. This
port allows bridging TCP socket to UART.
'''
uart_config = Common_pb2.UARTConfiguration()
uart_config.port_id = port_id
uart_config.enabled = enabled # Setting enabled to true opens the TCP port dedicated to UART bridging. Setting this
# field to false disables designated uart and closes the TCP port.
uart_config.speed = speed
uart_config.word_length = word_length
uart_config.stop_bits = stop_bits
uart_config.parity = parity
self.interconnect_config.SetUARTConfiguration(
uart_config, deviceId=self.interconnect_device_id)
def EnableBridge(self, bridge_type, target=0, output=0):
# Create bridge configuration
bridge_config = Base_pb2.BridgeConfig()
bridge_config.device_identifier = self.interconnect_device_id
bridge_config.bridgetype = bridge_type
# If either target or ouput port has valid port value, add port config to bridge configuration
if target or output:
bridge_config.port_config.target_port = 0
bridge_config.port_config.out_port = 0
if target:
bridge_config.port_config.target_port = target
if output:
bridge_config.port_config.out_port = output
# Send the configuration and return the result
bridge_result = self.base.EnableBridge(bridge_config)
return bridge_result
def DisableBridge(self, bridge_id):
return self.base.DisableBridge(bridge_id)
def ExampleSendDataAndReadItBack(self):
# Enable port bridging on base.
bridge_result = self.EnableBridge(Base_pb2.BRIDGE_TYPE_UART)
bridge_id = bridge_result.bridge_id
if bridge_result.status != Base_pb2.BRIDGE_STATUS_OK:
print("Error creating bridge on base, exiting...")
return
# Get created bridge's configuration.
bridge_config = self.base.GetBridgeConfig(bridge_id)
base_port = bridge_config.port_config.out_port
interconnect_port = bridge_config.port_config.target_port
print(
"UARTBridge ID # %i created between Interconnect (dev# %i)'s port #%i and external port #%i"
% (bridge_id.bridge_id, self.interconnect_device_id,
interconnect_port, base_port))
# Open a socket to base's forwarded port.
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.connect((self.base_ip_address, base_port))
client_socket.setblocking(0)
print("UART bridge object initialized")
# Send data to be written on UART using TCP connection.
client_socket.send(
b"This data is being written on Interconnect's expansion port UART\n"
)
# Wait for data to be received from UART
print("Waiting 10 seconds for data from uart...")
sys.stdout.write("Received data : ")
sys.stdout.flush()
startTime = time.time()
while time.time() - startTime < 10:
readready, _, _ = select.select([client_socket], [], [], 1)
if readready:
data = client_socket.recv(1)
if len(data):
sys.stdout.write(data.decode("utf-8"))
sys.stdout.flush()
# Disconnect client socket.
client_socket.close()
# Disable bridge on base.
self.DisableBridge(bridge_id)