From b94d7ab1e5542711953ff0b2d1bd3cc950c03118 Mon Sep 17 00:00:00 2001 From: Kseninia Mikhaylova Date: Fri, 13 Dec 2024 10:36:18 +0300 Subject: [PATCH] exp --- robot/client_socket.py | 36 ++++++++++++++++++++---------------- urdf/sample_without.urdf | 5 +++-- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/robot/client_socket.py b/robot/client_socket.py index 66935a1..4a2e643 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -201,6 +201,7 @@ class SocketRobotArm: threading.Thread(target=self.imitate_func, daemon=True).start() def palletizing(self, position, box_id): + pos = position[:] conv = position[:] conv[1] += 0.4 @@ -211,6 +212,7 @@ class SocketRobotArm: for pos in [conv, position]: jointPoses = self.convert_to_joint_base( pos, + # np.radians([0, 0, 0]), np.radians([0, 0, -90]), ) json_res.append( @@ -223,29 +225,31 @@ class SocketRobotArm: with open(f"data/palletizing.json", "w") as myfile: myfile.write(json.dumps(json_res)) - end_effector_state = p.getLinkState(self.body_id, self.num_joints - 1) + end_effector_state = p.getLinkState( + self.body_id, self.num_joints - 1, computeForwardKinematics=True + ) 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 # Ориентация робота (наконечника) + # Используем ориентацию объекта (коробки) как ориентацию родителя + parentFrameOrientation = object_orn + + # # Определяем смещение для фиксации позиции (без вращения) + parentFramePosition = [pos[i] - end_effector_pos[i] for i in range(3)] # Создаём констрейнт для прикрепления 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, # Ориентация относительно робота + parentBodyUniqueId=self.body_id, + parentLinkIndex=self.num_joints - 1, + childBodyUniqueId=box_id, + childLinkIndex=-1, + jointType=p.JOINT_FIXED, + jointAxis=[0, 0, 0], + parentFramePosition=[0,0,0], + childFramePosition=[0, 0, 0], + parentFrameOrientation=p.getQuaternionFromEuler(np.radians([0, 0, 0])), ) def set_text(self, text): @@ -326,7 +330,7 @@ class SocketRobotArm: for point in points: jointPoses = [np.radians(float(point[f"m{i}"])) for i in range(6)] self.motionFund(jointPoses) - + def get_palletizing_json(self): with open(f"data/palletizing.json", "r") as fp: data = json.load(fp) diff --git a/urdf/sample_without.urdf b/urdf/sample_without.urdf index d9e1113..c21f509 100644 --- a/urdf/sample_without.urdf +++ b/urdf/sample_without.urdf @@ -186,10 +186,11 @@ - + + - +