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 @@
-
+
+
-
+