open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Junhan Chang
2025-04-17 15:19:47 +08:00
parent a47a3f5c3a
commit c78ac482d8
262 changed files with 39871 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
unilabos/devices/gripper/__init__.py Normal file
View File

46
unilabos/devices/gripper/mock.py Normal file
View File

@@ -0,0 +1,46 @@
import time
class MockGripper:
def __init__(self):
self._position: float = 0.0
self._velocity: float = 2.0
self._torque: float = 0.0
self._status = "Idle"
@property
def position(self) -> float:
return self._position
@property
def velocity(self) -> float:
return self._velocity
@property
def torque(self) -> float:
return self._torque
@property
def status(self) -> str:
return self._status
def push_to(self, position: float, torque: float, velocity: float = 0.0):
self._status = "Running"
current_pos = self.position
if velocity == 0.0:
velocity = self.velocity
move_time = abs(position - current_pos) / velocity
for i in range(20):
self._position = current_pos + (position - current_pos) / 20 * (i+1)
self._torque = torque / (20 - i)
self._velocity = velocity
time.sleep(move_time / 20)
self._torque = torque
self._status = "Idle"
def edit_id(self, wf_name: str = "gripper_run", params: str = "{}", resource: dict = {"Gripper1": {}}):
v = list(resource.values())[0]
v["sample_id"] = "EDITED"
time.sleep(10)
return resource

408
unilabos/devices/gripper/rmaxis_v4.py Normal file
View File

@@ -0,0 +1,408 @@
import time
from asyncio import Event
from enum import Enum, auto
from dataclasses import dataclass
from typing import Dict, Tuple
from pymodbus.client import ModbusSerialClient as ModbusClient
from pymodbus.client import ModbusTcpClient as ModbusTcpClient
class CommandType(Enum):
COMMAND_NONE = 0
COMMAND_GO_HOME = 1
COMMAND_DELAY = 2
COMMAND_MOVE_ABSOLUTE = 3
COMMAND_PUSH = 4
COMMAND_MOVE_RELATIVE = 5
COMMAND_PRECISE_PUSH = 6
class ParamType(Enum):
BOOLEAN = 0
INT32 = 1
FLOAT = 2
ENUM = 3
class ParamEdit(Enum):
NORMAL = 0
READONLY = 1
@dataclass
class Param:
type: ParamType
editability: ParamEdit
address: int
# 用于存储参数的字典类型
ParamsDict = Dict[str, Param]
# Constants and other required elements can be defined as needed
IO_GAP_TIME = 0.01
EXECUTE_COMMAND_INDEX = 15 # Example index
COMMAND_REACH_SIGNAL = "reach_15"
# Define other constants or configurations as needed
def REVERSE(x):
return ((x << 16) & 0xFFFF0000) | ((x >> 16) & 0x0000FFFF)
def int32_to_uint16_list(int32_list):
uint16_list = []
for num in int32_list:
lower_half = num & 0xFFFF
upper_half = (num >> 16) & 0xFFFF
uint16_list.extend([upper_half, lower_half])
return uint16_list
def uint16_list_to_int32_list(uint16_list):
if len(uint16_list) % 2 != 0:
raise ValueError("Input list must have even number of uint16 elements.")
int32_list = []
for i in range(0, len(uint16_list), 2):
# Combine two uint16 values into one int32 value
high = uint16_list[i + 1]
low = uint16_list[i]
# Assuming the uint16_list is in big-endian order
int32_value = (high << 16) | low
int32_list.append(int(int32_value))
return int32_list
class RMAxis:
modbus_device = {}
def __init__(self, port, is_modbus_rtu, baudrate: int = 115200, address: str = "", slave_id: int = 1):
self.device = port
self.is_modbus_rtu = is_modbus_rtu
if is_modbus_rtu:
self.client = ModbusClient(port=port, baudrate=baudrate, parity='N', stopbits=1, bytesize=8, timeout=3)
else:
self.client = ModbusTcpClient(address, port)
if not self.client.connect():
raise Exception(f"Modbus Connection failed")
self.slave_id = slave_id
self._error_event = Event()
self.device_params = {} # Assuming some initialization for parameters
self.command_edited = {}
self.init_parameters(self.device_params)
def init_parameters(self, params):
params["current_command_position"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 4902)
params["current_command_velocity"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 4904)
params["current_command_acceleration"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 4906)
params["current_command_deacceleration"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 4908)
params["current_position"] = Param(ParamType.FLOAT, ParamEdit.READONLY, 0)
params["current_velocity"] = Param(ParamType.FLOAT, ParamEdit.READONLY, 2)
params["control_torque"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 4)
params["error"] = Param(ParamType.INT32, ParamEdit.READONLY, 6)
params["current_force_sensor"] = Param(ParamType.FLOAT, ParamEdit.READONLY, 18)
params["io_in_go_home"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1401)
params["io_in_error_reset"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1402)
params["io_in_start"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1403)
params["io_in_servo"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1404)
params["io_in_stop"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1405)
params["io_in_force_reset"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1424)
params["io_in_save_parameters"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1440)
params["io_in_load_parameters"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1441)
params["io_in_save_positions"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1442)
params["io_in_load_positions"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1443)
params["io_out_gone_home"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1501)
params["command_address"] = Param(ParamType.INT32, ParamEdit.NORMAL, 5000)
params["selected_command_index"] = Param(ParamType.INT32, ParamEdit.NORMAL, 4001)
params["io_out_reach_15"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1521)
params["io_out_moving"] = Param(ParamType.BOOLEAN, ParamEdit.NORMAL, 1505)
params["limit_pos"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 74)
params["limit_neg"] = Param(ParamType.FLOAT, ParamEdit.NORMAL, 72)
params["hardware_direct_output"] = Param(ParamType.INT32, ParamEdit.NORMAL, 158)
params["hardware_direct_input"] = Param(ParamType.INT32, ParamEdit.NORMAL, 160)
def get_version(self):
version_major = self.client.read_input_registers(8, 1, unit=self.slave_id).registers[0]
version_minor = self.client.read_input_registers(10, 1, unit=self.slave_id).registers[0]
version_build = self.client.read_input_registers(12, 1, unit=self.slave_id).registers[0]
version_type = self.client.read_input_registers(14, 1, unit=self.slave_id).registers[0]
return (version_major, version_minor, version_build, version_type)
def set_input_signal(self, signal, level):
param_name = f"io_in_{signal}"
self.set_parameter(param_name, level)
def get_output_signal(self, signal):
param_name = f"io_out_{signal}"
return self.get_device_parameter(param_name)
def config_motion(self, velocity, acceleration, deacceleration):
self.set_parameter("current_command_velocity", velocity)
self.set_parameter("current_command_acceleration", acceleration)
self.set_parameter("current_command_deacceleration", deacceleration)
def move_to(self, position):
self.set_parameter("current_command_position", position)
def go_home(self):
self.set_input_signal("go_home", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("go_home", True)
def move_absolute(self, position, velocity, acceleration, deacceleration, band):
command = {
'type': CommandType.COMMAND_MOVE_ABSOLUTE.value,
'position': position,
'velocity': velocity,
'acceleration': acceleration,
'deacceleration': deacceleration,
'band': band,
'push_force': 0,
'push_distance': 0,
'delay': 0,
'next_command_index': -1
}
self.execute_command(command)
def move_relative(self, position, velocity, acceleration, deacceleration, band):
command = {
'type': CommandType.COMMAND_MOVE_RELATIVE.value,
'position': position,
'velocity': velocity,
'acceleration': acceleration,
'deacceleration': deacceleration,
'band': band,
'push_force': 0,
'push_distance': 0,
'delay': 0,
'next_command_index': -1
}
self.execute_command(command)
def push(self, force, distance, velocity):
command = {
'type': CommandType.COMMAND_PUSH.value,
'position': 0,
'velocity': velocity,
'acceleration': 0,
'deacceleration': 0,
'band': 0,
'push_force': force,
'push_distance': distance,
'delay': 0,
'next_command_index': -1
}
self.execute_command(command)
def precise_push(self, force, distance, velocity, force_band, force_check_time):
command = {
'type': CommandType.COMMAND_PRECISE_PUSH.value,
'position': 0,
'velocity': velocity,
'acceleration': 0,
'deacceleration': 0,
'band': force_band,
'push_force': force,
'push_distance': distance,
'delay': force_check_time,
'next_command_index': -1
}
self.execute_command(command)
def is_moving(self):
return self.get_output_signal("moving")
def is_reached(self):
return self.get_output_signal(COMMAND_REACH_SIGNAL)
def is_push_empty(self):
return not self.is_moving()
def set_command(self, index, command):
print("Setting command", command)
self.command_edited[index] = True
command_buffer = [
command['type'],
int(command['position'] * 1000),
int(command['velocity'] * 1000),
int(command['acceleration'] * 1000),
int(command['deacceleration'] * 1000),
int(command['band'] * 1000),
int(command['push_force'] * 1000),
int(command['push_distance'] * 1000),
int(command['delay']),
int(command['next_command_index'])
]
buffer = int32_to_uint16_list(command_buffer)
response = self.client.write_registers(self.device_params["command_address"].address + index * 20, buffer, self.slave_id)
def get_command(self, index):
response = self.client.read_holding_registers(self.device_params["command_address"].address + index * 20, 20, self.slave_id)
print(response)
buffer = response.registers
command_buffer = uint16_list_to_int32_list(buffer)
command = {
'type': command_buffer[0],
'position': command_buffer[1] / 1000.0,
'velocity': command_buffer[2] / 1000.0,
'acceleration': command_buffer[3] / 1000.0,
'deacceleration': command_buffer[4] / 1000.0,
'band': command_buffer[5] / 1000.0,
'push_force': command_buffer[6] / 1000.0,
'push_distance': command_buffer[7] / 1000.0,
'delay': command_buffer[8],
'next_command_index': command_buffer[9]
}
return command
def execute_command(self, command):
self.set_command(EXECUTE_COMMAND_INDEX, command)
self.save_commands()
self.trig_command(EXECUTE_COMMAND_INDEX)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
def trig_command(self, index):
print("Triggering command", index)
self.set_parameter("selected_command_index", index)
self.set_input_signal("start", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("start", True)
def load_commands(self):
self.set_input_signal("load_positions", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("load_positions", True)
def save_commands(self):
for index, edited in self.command_edited.items():
if edited:
self.set_parameter("selected_command_index", index)
self.set_input_signal("save_positions", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("save_positions", True)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.command_edited[index] = False
@property
def position(self) -> float:
return self.get_device_parameter("current_position")
def get_position(self) -> float:
return self.get_device_parameter("current_position")
@property
def velocity(self) -> float:
return self.get_device_parameter("current_velocity")
@property
def torque(self) -> float:
return self.get_device_parameter("control_torque")
@property
def force_sensor(self) -> float:
return self.get_device_parameter("current_force_sensor")
def error_code(self):
return self.get_device_parameter("error")
def get_device_parameter(self, name):
# Assuming self.device_params is a dictionary with address and type
param = self.device_params.get(name)
if not param:
self._error_event.set()
raise Exception(f"parameter {name} does not exist")
address = param.address
if param.editability == ParamEdit.READONLY:
if param.type == ParamType.BOOLEAN:
return self.client.read_input_discretes(address, 1).bits[0]
elif param.type == ParamType.ENUM:
return self.client.read_input_registers(address, 1).registers[0]
elif param.type == ParamType.INT32:
return self.client.read_input_registers(address, 2).registers[0] # Handle as needed
elif param.type == ParamType.FLOAT:
return self.client.read_input_registers(address, 2).registers[0] # Handle as needed
else:
self._error_event.set()
raise Exception(f"parameter {name} has unknown data type {param.type}")
else:
if param.type == ParamType.BOOLEAN:
return self.client.read_holding_registers(address, 1).registers[0]
elif param.type == ParamType.ENUM:
return self.client.read_holding_registers(address, 1).registers[0]
elif param.type == ParamType.INT32:
return self.client.read_holding_registers(address, 2).registers[0] # Handle as needed
elif param.type == ParamType.FLOAT:
return self.client.read_holding_registers(address, 2).registers[0] # Handle as needed
else:
self._error_event.set()
raise Exception(f"parameter {name} has unknown data type {param['type']}")
def set_parameter(self, name, value):
param = self.device_params.get(name)
if not param:
self._error_event.set()
raise Exception(f"parameter {name} does not exist")
address = param.address
if param.editability == ParamEdit.READONLY:
raise Exception(f"parameter {name} is read only")
else:
if param.type == ParamType.BOOLEAN:
self.client.write_coil(address, bool(value))
elif param.type == ParamType.ENUM:
self.client.write_register(address, value)
elif param.type == ParamType.INT32:
self.client.write_register(address, int(value))
elif param.type == ParamType.FLOAT:
self.client.write_register(address, float(value))
else:
self._error_event.set()
raise Exception(f"parameter {name} has unknown data type {param['type']}")
def load_parameters(self):
self.set_input_signal("load_parameters", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("load_parameters", True)
def save_parameters(self):
self.set_input_signal("save_parameters", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("save_parameters", True)
def reset_error(self):
self.set_input_signal("error_reset", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("error_reset", True)
def set_servo_on_off(self, on_off):
self.set_input_signal("servo", on_off)
def stop(self):
self.set_input_signal("stop", False)
time.sleep(IO_GAP_TIME) # Assuming IO_GAP_TIME is 0.1 seconds
self.set_input_signal("stop", True)
def soft_reset(self):
self.client.write_register(186, 0x22205682)
async def wait_error(self):
await self._error_event.wait()
def close(self):
self.client.close()
del self.client
if __name__ == "__main__":
# gripper = RMAxis.create_rmaxis_modbus_rtu("COM7", 115200, 1)
gripper = RMAxis.create_rmaxis_modbus_rtu('/dev/tty.usbserial-B002YGXY', 115200, 0)
gripper.go_home()
# gripper.move_to(20)
# print("Moving abs...")
# gripper.move_absolute(20, 5, 100, 100, 0.1)
# print(gripper.get_command(EXECUTE_COMMAND_INDEX))
# gripper.go_home()
# print("Pushing...")
# gripper.push(0.7, 10, 20)