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

pallet pos

This commit is contained in:
Kseninia Mikhaylova 2024-12-17 15:26:25 +03:00
parent e4742efa86
commit 0b55d0a050
1 changed files with 7 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
robot/urdf_manager.py
View File

@ -40,6 +40,7 @@ class UrdfManager:
def load_models(self): def load_models(self):
p.loadURDF( p.loadURDF(
os.path.join(self.application_path, "urdf", "plane.urdf"), os.path.join(self.application_path, "urdf", "plane.urdf"),
[0,0,-1],
physicsClientId=self.physics_client, physicsClientId=self.physics_client,
) )
@ -60,8 +61,8 @@ class UrdfManager:
self.start_loop(self.urdf_filename) self.start_loop(self.urdf_filename)
k = 0.2 k = 0.2
self.pallet_target = [1.3 + k, 0.6, 0.1] self.pallet_target = [1.366773, 0.59591, -0.624086]
self.pallet_size = [0.3 + k, 1.2, 0.144] self.pallet_size = [0.8, 1.2, 0.144]
self.pallet_position = [ self.pallet_position = [
(self.pallet_target[i] - self.pallet_size[i] * 0.5) (self.pallet_target[i] - self.pallet_size[i] * 0.5)
for i in range(len(self.pallet_target)) for i in range(len(self.pallet_target))
@ -71,7 +72,7 @@ class UrdfManager:
self.box_size = [0.2, 0.2, 0.2] self.box_size = [0.2, 0.2, 0.2]
# положение дальнего угла # положение дальнего угла
self.conv_target = [1.1 + 0.5, -0.63, 0.4] self.conv_target = [1.045, -0.989, -0.094]
self.conv_size = [0.5, 3.0, 0.1] self.conv_size = [0.5, 3.0, 0.1]
self.conv_direction = self.conv_size.index(max(self.conv_size)) self.conv_direction = self.conv_size.index(max(self.conv_size))
self.conv_pos = [ self.conv_pos = [
@ -106,11 +107,7 @@ class UrdfManager:
# logger.info(p) # logger.info(p)
for i in range(len(self.box_size)): for i in range(len(self.box_size)):
p[i] = ( p[i] = self.pallet_target[i] - p[i] - self.box_size[i] * 0.5
self.pallet_target[i]
- p[i]
- self.box_size[i] * 0.5
)
dir_wall = (self.conv_direction + 1) % (3 - 1) dir_wall = (self.conv_direction + 1) % (3 - 1)
p[dir_wall] -= self.box_size[dir_wall] p[dir_wall] -= self.box_size[dir_wall]
p[self.conv_direction] -= self.box_size[self.conv_direction] * 0.5 p[self.conv_direction] -= self.box_size[self.conv_direction] * 0.5