remove world coord
This commit is contained in:
parent
d12288edb3
commit
82a8d9c480
|
|
@ -40,6 +40,8 @@ class SocketRobotArm:
|
|||
command_type = "base"
|
||||
command_data = None
|
||||
|
||||
motion_type = "reset"
|
||||
|
||||
def __init__(self, application_path, *args, **kwargs):
|
||||
self.socket = None
|
||||
self.host = None
|
||||
|
|
@ -71,7 +73,7 @@ class SocketRobotArm:
|
|||
def load_models(self):
|
||||
self.urdf_manager.load_models()
|
||||
self.body_id = self.urdf_manager.body_id
|
||||
|
||||
|
||||
def load_palletizing(self):
|
||||
self.urdf_manager.load_palletizing()
|
||||
|
||||
|
|
@ -89,12 +91,20 @@ class SocketRobotArm:
|
|||
def get_status(self):
|
||||
return self.socket_manager.status
|
||||
|
||||
def motionFund(self, index, radians):
|
||||
if self.motion_type == "reset":
|
||||
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
|
||||
)
|
||||
time.sleep(1)
|
||||
|
||||
def upd_model(self):
|
||||
threading.Thread(target=self.upd_model_func, daemon=True).start()
|
||||
|
||||
def upd_model_func(self):
|
||||
# logger.info("UPDATE MODEL FUNC")\
|
||||
|
||||
self.get_coordinates("axis")
|
||||
self.set_text(text=f"Координаты осей {self.axis_coordinates}")
|
||||
time.sleep(0.5)
|
||||
|
|
@ -106,55 +116,38 @@ class SocketRobotArm:
|
|||
self._fetch_command_count()
|
||||
time.sleep(0.5)
|
||||
|
||||
def motionFund(i, axis):
|
||||
motion_type = "reset"
|
||||
try:
|
||||
v = axis[i]
|
||||
if v is None:
|
||||
v = 0
|
||||
except IndexError:
|
||||
v = 0
|
||||
v = np.radians(v)
|
||||
if motion_type == "reset":
|
||||
p.resetJointState(self.urdf_manager.body_id, i, v)
|
||||
else:
|
||||
p.setJointMotorControl2(
|
||||
self.urdf_manager.body_id, i, p.POSITION_CONTROL, v
|
||||
)
|
||||
|
||||
num_joints = p.getNumJoints(self.urdf_manager.body_id)
|
||||
axisJointPoses = self.axis_coordinates
|
||||
for i in range(num_joints - 1):
|
||||
motionFund(i, axisJointPoses)
|
||||
v = axisJointPoses[i] if i < len(axisJointPoses) else 0
|
||||
v = np.radians(v)
|
||||
self.motionFund(i, v)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
if False:
|
||||
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):
|
||||
v = worldJointPoses[i] if i < len(worldJointPoses) else 0
|
||||
self.motionFund(i, v)
|
||||
|
||||
coordinates = self.world_coordinates[:3]
|
||||
angles = [
|
||||
self.world_coordinates[3],
|
||||
self.world_coordinates[4],
|
||||
self.world_coordinates[5],
|
||||
]
|
||||
worldJointPoses = self.convert_to_joint_base(
|
||||
[float(p) * 0.001 for p in coordinates], [math.radians(p) for p in angles]
|
||||
)
|
||||
for i in range(num_joints):
|
||||
motionFund(i, np.degrees(list(worldJointPoses)))
|
||||
time.sleep(2)
|
||||
time.sleep(0.5)
|
||||
|
||||
calcWorldPosition = p.getLinkState(
|
||||
self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
|
||||
)
|
||||
calcWorldPosition = list(
|
||||
[p * 1000 for p in calcWorldPosition[0]]
|
||||
+ [np.degrees(p) for p in p.getEulerFromQuaternion(calcWorldPosition[1])]
|
||||
)
|
||||
time.sleep(0.5)
|
||||
calcWorldPosition = p.getLinkState(
|
||||
self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
|
||||
)
|
||||
calcWorldPosition = list(
|
||||
[p * 1000 for p in calcWorldPosition[0]]
|
||||
+ [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 Difference:", position_diff)
|
||||
position_diff = np.linalg.norm(
|
||||
self.world_coordinates[:3] - np.array(calcWorldPosition[:3])
|
||||
)
|
||||
print("Разница между расчетом по модели и мировыми координатами:", position_diff)
|
||||
|
||||
self.set_text(text=f"Команд в очереди {self.command_count}")
|
||||
time.sleep(0.5)
|
||||
|
|
@ -275,17 +268,10 @@ class SocketRobotArm:
|
|||
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):
|
||||
try:
|
||||
val = jointPoses[i]
|
||||
except Exception as e:
|
||||
val = 0
|
||||
# p.setJointMotorControl2(
|
||||
# self.urdf_manager.body_id, i, p.POSITION_CONTROL, val
|
||||
# )
|
||||
p.resetJointState(self.urdf_manager.body_id, i, val)
|
||||
time.sleep(0.3)
|
||||
# p.setJointMotorControlArray(self.urdf_manager.body_id, p.POSITION_CONTROL, jointPoses)
|
||||
v = jointPoses[i] if i < len(jointPoses) else 0
|
||||
self.motionFund(i, v)
|
||||
|
||||
def convert_point_to_free(self):
|
||||
points = self.steps_from_file()
|
||||
|
|
@ -304,7 +290,8 @@ class SocketRobotArm:
|
|||
point["m5"],
|
||||
]
|
||||
jointPoses = self.convert_to_joint_base(
|
||||
[float(p) * 0.001 for p in points], np.radians([float(x) for x in angles])
|
||||
[float(p) * 0.001 for p in points],
|
||||
np.radians([float(x) for x in angles]),
|
||||
)
|
||||
res.append(
|
||||
self.prepare_data.make_step(
|
||||
|
|
|
|||
|
|
@ -24,13 +24,13 @@ class UrdfManager:
|
|||
|
||||
p.resetSimulation()
|
||||
self.load_models()
|
||||
p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
|
||||
|
||||
def run_pybullet(self, type="DIRECT"):
|
||||
self.physics_client = p.connect(getattr(p, type))
|
||||
logger.info(f"Connect to {self.physics_client} by {type}")
|
||||
|
||||
def load_models(self):
|
||||
p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client)
|
||||
p.loadURDF(
|
||||
os.path.join(self.application_path, "urdf", "plane.urdf"),
|
||||
physicsClientId=self.physics_client,
|
||||
|
|
|
|||
Loading…
Reference in New Issue