ksenia_mikhailova
/
modbus_test
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pick obj part

This commit is contained in:
Kseninia Mikhaylova 2024-12-12 16:58:39 +03:00
parent 82a8d9c480
commit 1ede0084f0
4 changed files with 96 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gui/robot.py
View File

@ -49,7 +49,6 @@ class ChangeRobot(QWidget):
def updateConnectButtonText(self):
radioState = True
logger.info(self.status())
if self.status() == "not_connected":
self.connectButton.setText("Соединить")
elif self.status() == "connected":

111
robot/client_socket.py
View File

@ -41,6 +41,7 @@ class SocketRobotArm:
command_data = None
motion_type = "reset"
motion_type = "normal"
def __init__(self, application_path, *args, **kwargs):
self.socket = None
@ -57,7 +58,9 @@ class SocketRobotArm:
self.prepare_data = PrepareRobotData()
self.socket_manager = SocketManager()
self.urdf_manager = UrdfManager(
robot_start_position=robot_start_position, application_path=application_path
robot_start_position,
application_path,
self.palletizing,
)
def __exit__(self, exc_type, exc_value, traceback):
@ -69,10 +72,8 @@ class SocketRobotArm:
def start_loop(self, urdf):
self.urdf_manager.start_loop(urdf)
def load_models(self):
self.urdf_manager.load_models()
self.body_id = self.urdf_manager.body_id
self.num_joints = p.getNumJoints(self.body_id)
def load_palletizing(self):
self.urdf_manager.load_palletizing()
@ -91,13 +92,17 @@ class SocketRobotArm:
def get_status(self):
return self.socket_manager.status
def motionFund(self, index, radians):
def motionFund(self, radians):
if self.motion_type == "reset":
for index in radians:
p.resetJointState(self.urdf_manager.body_id, index, radians)
time.sleep(0.3)
else:
p.setJointMotorControl2(
self.urdf_manager.body_id, index, p.POSITION_CONTROL, radians
p.setJointMotorControlArray(
self.urdf_manager.body_id,
[i for i in range(len(radians))],
p.POSITION_CONTROL,
radians,
)
time.sleep(1)
@ -116,12 +121,8 @@ class SocketRobotArm:
self._fetch_command_count()
time.sleep(0.5)
num_joints = p.getNumJoints(self.urdf_manager.body_id)
axisJointPoses = self.axis_coordinates
for i in range(num_joints - 1):
v = axisJointPoses[i] if i < len(axisJointPoses) else 0
v = np.radians(v)
self.motionFund(i, v)
self.motionFund(np.radians(axisJointPoses))
time.sleep(0.5)
@ -129,25 +130,33 @@ class SocketRobotArm:
coordinates = [float(p) * 0.001 for p in self.world_coordinates[:3]]
angles = [math.radians(p) for p in self.world_coordinates[3:6]]
worldJointPoses = self.convert_to_joint_base(coordinates, angles)
for i in range(num_joints):
for i in range(self.num_joints):
v = worldJointPoses[i] if i < len(worldJointPoses) else 0
self.motionFund(i, v)
self.motionFund(worldJointPoses)
time.sleep(0.5)
calcWorldPosition = p.getLinkState(
self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
self.urdf_manager.body_id,
self.num_joints - 1,
computeForwardKinematics=True,
)
calcWorldPosition = list(
[p * 1000 for p in calcWorldPosition[0]]
+ [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
+ [
np.degrees(p)
for p in p.getEulerFromQuaternion(calcWorldPosition[1])
]
)
time.sleep(0.5)
position_diff = np.linalg.norm(
self.world_coordinates[:3] - np.array(calcWorldPosition[:3])
)
print("Разница между расчетом по модели и мировыми координатами:", position_diff)
print(
"Разница между расчетом по модели и мировыми координатами:",
position_diff,
)
self.set_text(text=f"Команд в очереди {self.command_count}")
time.sleep(0.5)
@ -190,6 +199,44 @@ class SocketRobotArm:
def imitate(self):
threading.Thread(target=self.imitate_func, daemon=True).start()
def palletizing(self, position, box_id):
conv = position[:]
conv[1] += 0.4
position[1] += 0.02
for pos in [conv, position]:
jointPoses = self.convert_to_joint_base(
pos,
np.radians([0, 0, -90]),
)
self.motionFund(jointPoses)
time.sleep(2)
end_effector_state = p.getLinkState(self.body_id, self.num_joints - 1)
end_effector_pos = end_effector_state[0] # Позиция наконечника
end_effector_orn = end_effector_state[1] # Ориентация наконечника
# Позиция стенки (или коробки), к которой мы прикрепляем наконечник
object_pos, object_orn = p.getBasePositionAndOrientation(box_id)
# Определяем смещение, которое будет применено для фиксации позиции (без вращения)
parentFramePosition = [position[i] - end_effector_pos[i] for i in range(3)]
parentFrameOrientation = end_effector_orn # Ориентация робота (наконечника)
# Создаём констрейнт для прикрепления
constraint_id = p.createConstraint(
parentBodyUniqueId=self.body_id, # Идентификатор робота
parentLinkIndex=self.num_joints - 1, # Индекс последнего звена (наконечника)
childBodyUniqueId=box_id, # Идентификатор коробки
childLinkIndex=-1, # -1 означает базовую ссылку объекта
jointType=p.JOINT_FIXED, # Фиксированный констрейнт
jointAxis=[0, 0, 0], # Без вращательной оси
parentFramePosition=parentFramePosition, # Позиция относительно робота
childFramePosition=[1, 1, 1], # Позиция объекта (относительно себя)
parentFrameOrientation=parentFrameOrientation # Ориентация относительно робота
)
def set_text(self, text):
logger.info(text)
@ -267,11 +314,7 @@ class SocketRobotArm:
points = self.convert_point_to_free()
for point in points:
jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)]
num_joints = p.getNumJoints(self.urdf_manager.body_id)
for i in range(num_joints):
v = jointPoses[i] if i < len(jointPoses) else 0
self.motionFund(i, v)
self.motionFund(jointPoses)
def convert_point_to_free(self):
points = self.steps_from_file()
@ -301,32 +344,18 @@ class SocketRobotArm:
return res
def convert_to_joint_base(self, coordinates, angles):
body_id = 0
num_joints = p.getNumJoints(body_id)
lower_limits = []
upper_limits = []
for i in range(num_joints):
limit = p.getJointInfo(body_id, i)
for i in range(self.num_joints):
limit = p.getJointInfo(self.body_id, i)
lower_limits.append(limit[8])
upper_limits.append(limit[9])
joint_angles = p.calculateInverseKinematics(
body_id,
endEffectorLinkIndex=num_joints - 1,
self.body_id,
endEffectorLinkIndex=self.num_joints - 1,
targetPosition=[float(c) for c in coordinates],
targetOrientation=angles,
# lowerLimits=lower_limits,
# upperLimits=upper_limits,
# jointRanges=[4, 4, 4, 4, 4, 4],
# restPoses=[
# math.radians(0.1873),
# math.radians(59.059),
# math.radians(-99.4105),
# math.radians(-0.2214),
# math.radians(41.7877),
# math.radians(-0.0011),
# ],
targetOrientation=p.getQuaternionFromEuler(angles),
)
return joint_angles

17
robot/conveyor.py
View File

@ -5,7 +5,9 @@ from logger import logger
class ConveyorBelt:
def __init__(self, size, position, box_size, box_spacing, box_speed):
def __init__(
self, size, position, box_size, box_spacing, box_speed, palletizinFunc
):
self.conveyor_size = size
self.conveyor_position = position
self.conveyor_id = self.create_conveyor(size, position)
@ -21,6 +23,7 @@ class ConveyorBelt:
self.thread = threading.Thread(target=self.run)
self.thread.start()
self.palletizingFunc = palletizinFunc
def create_conveyor(self, size, position):
collision_shape_id = p.createCollisionShape(
@ -88,7 +91,6 @@ class ConveyorBelt:
return box_id
def move_boxes(self):
conveyor_stopped = False
if not self.conveyor_stopped:
for box_id in self.boxes[:]:
pos, orn = p.getBasePositionAndOrientation(box_id)
@ -101,13 +103,13 @@ class ConveyorBelt:
# Удаляем коробку с конвейера
self.boxes.remove(box_id)
self.conveyor_stopped = True
self.call_robot()
right_wall_position = [pos[0], pos[1] + self.box_size[1] * 0.5, pos[2]]
self.call_robot(right_wall_position, box_id)
else:
# Двигаем коробку вдоль конвейера (по оси X)
new_pos = [pos[0] + self.box_speed, pos[1], pos[2]]
p.resetBasePositionAndOrientation(box_id, new_pos, orn)
# Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество
if len(self.boxes) < int(
self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)
@ -115,11 +117,12 @@ class ConveyorBelt:
new_box = self.create_box(self.box_position())
self.boxes.append(new_box)
def call_robot(self):
def call_robot(self, position, box_id):
# Здесь можно вызвать робота для выполнения операции, например:
logger.info("Конвейер остановлен, вызываем робота для работы с коробкой.")
time.sleep(5)
self.conveyor_stopped = False
self.palletizingFunc(position, box_id)
# time.sleep(5)
# self.conveyor_stopped = False
def run(self):
while not self.stop_thread:

13
robot/urdf_manager.py
View File

@ -15,9 +15,10 @@ class UrdfManager:
physics_client = None
body_id = None
def __init__(self, robot_start_position, application_path):
def __init__(self, robot_start_position, application_path, palletizing):
self.robot_start_position = robot_start_position
self.application_path = application_path
self.palletizing = palletizing
def start_loop(self, urdf):
self.urdf_filename = urdf
@ -46,18 +47,22 @@ class UrdfManager:
)
def load_palletizing(self):
pallet_position = [1.5, -1, 0.1]
# p.resetSimulation()
pallet_position = [1.5, -1.5, 0.1]
urdf_path_pallet = os.path.join(
self.application_path, "urdf", "europallet.urdf"
)
self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position)
self.conv_pos = [-1.5, -1.5, 0.5]
self.conv_size = [3.0, 0.5, 0.1]
ConveyorBelt(
[3.0, 0.5, 0.1],
[-1.5, -1, 0.5],
self.conv_size,
self.conv_pos,
[0.2, 0.2, 0.2],
0.2,
0.1,
self.palletizing
)
time.sleep(1)