model test
This commit is contained in:
parent
c2f0343f0c
commit
3022f1b6b7
|
|
@ -0,0 +1,290 @@
|
||||||
|
[
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 1396.513,
|
||||||
|
"calc": 1396.5033449596588,
|
||||||
|
"delta": 0.009655040341158383
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.758,
|
||||||
|
"calc": 1.7414073758299926,
|
||||||
|
"delta": 0.016592624170007397
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1322.749,
|
||||||
|
"calc": 1322.768036512948,
|
||||||
|
"delta": -0.01903651294787778
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": -0.002,
|
||||||
|
"calc": -0.00046227946559921414,
|
||||||
|
"delta": -0.001537720534400786
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 90.0,
|
||||||
|
"calc": 0.0012258600963992801,
|
||||||
|
"delta": 89.9987741399036
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": 0.0019896756606109637,
|
||||||
|
"delta": -0.0019896756606109637
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": -1.645,
|
||||||
|
"calc": 29.354975856798607,
|
||||||
|
"delta": -30.999975856798606
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1396.533,
|
||||||
|
"calc": 1383.8229716462674,
|
||||||
|
"delta": 12.710028353732469
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1322.7,
|
||||||
|
"calc": 1322.3343071885358,
|
||||||
|
"delta": 0.3656928114642142
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": -89.998,
|
||||||
|
"calc": -0.6236563522406797,
|
||||||
|
"delta": -89.37434364775933
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 90.0,
|
||||||
|
"calc": -0.07248312898598147,
|
||||||
|
"delta": 90.07248312898598
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": 89.37963743161471,
|
||||||
|
"delta": -89.37963743161471
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": -61.982,
|
||||||
|
"calc": -34.61074827998304,
|
||||||
|
"delta": -27.371251720016957
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.689,
|
||||||
|
"calc": 1.4271628423169411,
|
||||||
|
"delta": 0.26183715768305893
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1955.771,
|
||||||
|
"calc": 1953.7999944946446,
|
||||||
|
"delta": 1.9710055053553788
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": 14.123199109608166,
|
||||||
|
"delta": -14.123199109608166
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 25.0,
|
||||||
|
"calc": -62.538975654348604,
|
||||||
|
"delta": 87.5389756543486
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.003,
|
||||||
|
"calc": 0.311660007824218,
|
||||||
|
"delta": -0.308660007824218
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 72.252,
|
||||||
|
"calc": 93.33332435556116,
|
||||||
|
"delta": -21.081324355561165
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.696,
|
||||||
|
"calc": 1.7236742678899477,
|
||||||
|
"delta": -0.027674267889947757
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 2450.564,
|
||||||
|
"calc": 2432.3159722289647,
|
||||||
|
"delta": 18.24802777103514
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": 0.0,
|
||||||
|
"delta": 0.0
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": -90.0,
|
||||||
|
"delta": 90.0
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": -1.9006084288684064,
|
||||||
|
"delta": 1.9006084288684064
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 1396.542,
|
||||||
|
"calc": 1395.1210132964286,
|
||||||
|
"delta": 1.420986703571316
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.748,
|
||||||
|
"calc": 3.0729777781727323,
|
||||||
|
"delta": -1.3249777781727323
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1322.702,
|
||||||
|
"calc": 1330.894305872972,
|
||||||
|
"delta": -8.192305872971929
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": 90.004,
|
||||||
|
"calc": 89.5871816818438,
|
||||||
|
"delta": 0.41681831815620285
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.001,
|
||||||
|
"calc": -0.4263670446483205,
|
||||||
|
"delta": 0.4273670446483205
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 90.003,
|
||||||
|
"calc": -0.011498402419446057,
|
||||||
|
"delta": 90.01449840241945
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 1279.539,
|
||||||
|
"calc": 1298.6529839631573,
|
||||||
|
"delta": -19.113983963157352
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.716,
|
||||||
|
"calc": 1.1941396666698758,
|
||||||
|
"delta": 0.5218603333301242
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1439.725,
|
||||||
|
"calc": 1421.9584575235335,
|
||||||
|
"delta": 17.766542476466384
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": 0.004,
|
||||||
|
"calc": 44.59848837135974,
|
||||||
|
"delta": -44.594488371359745
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.005,
|
||||||
|
"calc": -89.56861104656603,
|
||||||
|
"delta": 89.57361104656603
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.001,
|
||||||
|
"calc": -44.59999872029401,
|
||||||
|
"delta": 44.60099872029401
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 1396.518,
|
||||||
|
"calc": 1396.5150298333174,
|
||||||
|
"delta": 0.0029701666826440487
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.704,
|
||||||
|
"calc": 1.7043452192066975,
|
||||||
|
"delta": -0.00034521920669750017
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1322.743,
|
||||||
|
"calc": 1323.2042092827587,
|
||||||
|
"delta": -0.46120928275877304
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": 90.002,
|
||||||
|
"calc": 89.35749570446917,
|
||||||
|
"delta": 0.6445042955308224
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": -0.006,
|
||||||
|
"calc": -0.394888295984799,
|
||||||
|
"delta": 0.388888295984799
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 90.0,
|
||||||
|
"calc": 0.0004372310675820825,
|
||||||
|
"delta": 89.99956276893242
|
||||||
|
}
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": 1396.513,
|
||||||
|
"calc": 1396.5034170137394,
|
||||||
|
"delta": 0.009582986260511461
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1.758,
|
||||||
|
"calc": 1.7415043671341195,
|
||||||
|
"delta": 0.016495632865880516
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 1322.749,
|
||||||
|
"calc": 1322.7684959219296,
|
||||||
|
"delta": -0.019495921929546967
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": -0.002,
|
||||||
|
"calc": 0.3046030193918365,
|
||||||
|
"delta": -0.3066030193918365
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 90.0,
|
||||||
|
"calc": 0.0009973795000818987,
|
||||||
|
"delta": 89.99900262049992
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"point": 0.0,
|
||||||
|
"calc": 0.001993574278186638,
|
||||||
|
"delta": -0.001993574278186638
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
]
|
||||||
|
|
@ -138,16 +138,9 @@ class UrdfManager:
|
||||||
urdf_path = os.path.join(
|
urdf_path = os.path.join(
|
||||||
self.application_path, "urdf", f"{self.urdf_filename}.urdf"
|
self.application_path, "urdf", f"{self.urdf_filename}.urdf"
|
||||||
)
|
)
|
||||||
# self.body_id = p.loadURDF(
|
|
||||||
# urdf_path,
|
|
||||||
# self.robot_start_position,
|
|
||||||
# useFixedBase=1,
|
|
||||||
# physicsClientId=self.physics_client,
|
|
||||||
# )
|
|
||||||
self.robot = pi.RobotBase(
|
self.robot = pi.RobotBase(
|
||||||
urdf_path, self.robot_start_position, p.getQuaternionFromEuler([0, 0, 0])
|
urdf_path, self.robot_start_position, p.getQuaternionFromEuler([0, 0, 0])
|
||||||
)
|
)
|
||||||
print(self.robot.__dir__())
|
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
|
|
||||||
def get_pybullet_image(self):
|
def get_pybullet_image(self):
|
||||||
|
|
@ -201,12 +194,7 @@ class SocketRobotArm:
|
||||||
self.q_app = None
|
self.q_app = None
|
||||||
self.imitate_point = None
|
self.imitate_point = None
|
||||||
|
|
||||||
robot_start_position = [
|
robot_start_position = [0,0,0]
|
||||||
-0.008700576263427698,
|
|
||||||
0.04579925857591629,
|
|
||||||
0.0036192361602783203,
|
|
||||||
]
|
|
||||||
robot_start_position = [0, 0, 0]
|
|
||||||
|
|
||||||
self.prepare_data = PrepareRobotData()
|
self.prepare_data = PrepareRobotData()
|
||||||
self.socket_manager = SocketManager()
|
self.socket_manager = SocketManager()
|
||||||
|
|
@ -236,6 +224,8 @@ class SocketRobotArm:
|
||||||
|
|
||||||
def connect(self, host, slave_id):
|
def connect(self, host, slave_id):
|
||||||
self.socket_manager.connect(host)
|
self.socket_manager.connect(host)
|
||||||
|
with open("delta.json", "w") as f:
|
||||||
|
pass
|
||||||
|
|
||||||
def get_status(self):
|
def get_status(self):
|
||||||
return self.socket_manager.status
|
return self.socket_manager.status
|
||||||
|
|
@ -244,55 +234,105 @@ class SocketRobotArm:
|
||||||
threading.Thread(target=self.upd_model_func, daemon=True).start()
|
threading.Thread(target=self.upd_model_func, daemon=True).start()
|
||||||
|
|
||||||
def upd_model_func(self):
|
def upd_model_func(self):
|
||||||
# logger.info("UPDATE MODEL FUNC")
|
# logger.info("UPDATE MODEL FUNC")\
|
||||||
|
mode = 'by_joint'
|
||||||
|
# mode = 'by_world'
|
||||||
|
|
||||||
self.get_axis()
|
self.get_axis()
|
||||||
self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
|
self.set_text(text=f"Координаты осей {self.start_axis_coordinates}")
|
||||||
self.urdf_manager.robot.set_joint_position(
|
|
||||||
{
|
|
||||||
f"joint_{i+1}": math.radians(c)
|
|
||||||
for i, c in enumerate(self.start_axis_coordinates)
|
|
||||||
}
|
|
||||||
)
|
|
||||||
time.sleep(1)
|
|
||||||
|
|
||||||
self.get_world()
|
self.get_world()
|
||||||
self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
|
self.set_text(text=f"Мировые координаты {self.start_world_coordinates}")
|
||||||
angles = self.start_world_coordinates
|
|
||||||
|
|
||||||
self.urdf_manager.robot.set_endeffector_pose(
|
if mode == 'by_joint':
|
||||||
[a * 0.001 for a in angles[0:3]], p.getQuaternionFromEuler(angles[3:])
|
self.urdf_manager.robot.set_joint_position(
|
||||||
)
|
{
|
||||||
|
f"joint_{i+1}": math.radians(c)
|
||||||
|
for i, c in enumerate(self.start_axis_coordinates)
|
||||||
|
}
|
||||||
|
)
|
||||||
|
|
||||||
time.sleep(1)
|
|
||||||
|
if mode == 'by_world':
|
||||||
|
coordinates = self.start_world_coordinates[:3]
|
||||||
|
angles = self.start_world_coordinates[3:]
|
||||||
|
# angles[1] -= 90
|
||||||
|
self.urdf_manager.robot.set_endeffector_pose(
|
||||||
|
[a * 0.001 for a in coordinates], p.getQuaternionFromEuler(angles)
|
||||||
|
)
|
||||||
|
|
||||||
|
time.sleep(5)
|
||||||
|
if mode == 'by_joint':
|
||||||
|
self.calc_world_by_joint()
|
||||||
|
if mode == 'by_world':
|
||||||
|
self.calc_joint_by_world()
|
||||||
|
|
||||||
self._get_command_count()
|
self._get_command_count()
|
||||||
self.set_text(text=f"Команд в очереди {self.remote_command_count}")
|
self.set_text(text=f"Команд в очереди {self.remote_command_count}")
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
def calc_world_by_joint(self):
|
def calc_world_by_joint(self):
|
||||||
num_joints = p.getNumJoints(self.urdf_manager.body_id)
|
[end_effector_position, end_effector_orientation] = (
|
||||||
end_effector_state = p.getLinkState(
|
self.urdf_manager.robot.get_endeffector_pose()
|
||||||
self.urdf_manager.body_id, num_joints - 1, computeForwardKinematics=True
|
|
||||||
)
|
)
|
||||||
|
euler_orientation = p.getEulerFromQuaternion(end_effector_orientation)
|
||||||
|
try:
|
||||||
|
with open("delta.json", "r") as f:
|
||||||
|
data = json.load(f)
|
||||||
|
except (FileNotFoundError, json.JSONDecodeError):
|
||||||
|
data = []
|
||||||
|
data.append(
|
||||||
|
{
|
||||||
|
"position": [
|
||||||
|
{
|
||||||
|
"point": self.start_world_coordinates[i],
|
||||||
|
"calc": a * 1000,
|
||||||
|
"delta": self.start_world_coordinates[i] - a * 1000,
|
||||||
|
}
|
||||||
|
for i, a in enumerate(end_effector_position)
|
||||||
|
],
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": self.start_world_coordinates[i + 3],
|
||||||
|
"calc": a,
|
||||||
|
"delta": self.start_world_coordinates[i + 3] - a,
|
||||||
|
}
|
||||||
|
for i, a in enumerate(np.degrees(euler_orientation))
|
||||||
|
],
|
||||||
|
}
|
||||||
|
)
|
||||||
|
with open("delta.json", "w") as f:
|
||||||
|
json.dump(data, f, indent=4)
|
||||||
|
logger.info('write log')
|
||||||
|
|
||||||
end_effector_position = end_effector_state[4]
|
def calc_joint_by_world(self):
|
||||||
end_effector_orientation = end_effector_state[5]
|
joint_state = (
|
||||||
euler_orientation = list(p.getEulerFromQuaternion(end_effector_orientation))
|
self.urdf_manager.robot.get_joint_state()
|
||||||
euler_orientation[1] += math.radians(90)
|
)
|
||||||
|
states= np.degrees([
|
||||||
|
joint_state[f'joint_{i}']['position'] for i in range(1,6)
|
||||||
|
])
|
||||||
|
try:
|
||||||
|
with open("delta.json", "r") as f:
|
||||||
|
data = json.load(f)
|
||||||
|
except (FileNotFoundError, json.JSONDecodeError):
|
||||||
|
data = []
|
||||||
|
data.append(
|
||||||
|
{
|
||||||
|
"orientation": [
|
||||||
|
{
|
||||||
|
"point": self.start_axis_coordinates[i],
|
||||||
|
"calc": a,
|
||||||
|
"delta": self.start_axis_coordinates[i] - a,
|
||||||
|
}
|
||||||
|
for i, a in enumerate(np.degrees(states))
|
||||||
|
],
|
||||||
|
}
|
||||||
|
)
|
||||||
|
with open("delta.json", "w") as f:
|
||||||
|
json.dump(data, f, indent=4)
|
||||||
|
logger.info('write log')
|
||||||
|
|
||||||
logger.info(
|
|
||||||
f"Вычисленное положение {[a * 1000 for a in end_effector_position]}"
|
|
||||||
)
|
|
||||||
logger.info(
|
|
||||||
f"""{[f'{a*1000} -- {self.start_world_coordinates[i]} -- {self.start_world_coordinates[i] - a*1000}'
|
|
||||||
for i,a in enumerate(end_effector_position)]}"""
|
|
||||||
)
|
|
||||||
logger.info(f"Вычисленная ориентация {np.degrees(euler_orientation)}")
|
|
||||||
logger.info(
|
|
||||||
f"""{[f'{a} -- {self.start_world_coordinates[i + 3]} -- {self.start_world_coordinates[i + 3] - a}'
|
|
||||||
for i,a in enumerate(np.degrees(euler_orientation))]}"""
|
|
||||||
)
|
|
||||||
|
|
||||||
def cycle_base(self):
|
def cycle_base(self):
|
||||||
self.upd_model()
|
self.upd_model()
|
||||||
|
|
@ -323,9 +363,6 @@ class SocketRobotArm:
|
||||||
step = 4
|
step = 4
|
||||||
empty = 1
|
empty = 1
|
||||||
for i in range(0, len(self.add_rcc_list), step):
|
for i in range(0, len(self.add_rcc_list), step):
|
||||||
# if not self.socket:
|
|
||||||
# return
|
|
||||||
|
|
||||||
self.command_data = f"Отправка данных {i}...{i+step-1}"
|
self.command_data = f"Отправка данных {i}...{i+step-1}"
|
||||||
|
|
||||||
self.send_data(make_addrcc_data(self.add_rcc_list[i : i + step], empty))
|
self.send_data(make_addrcc_data(self.add_rcc_list[i : i + step], empty))
|
||||||
|
|
@ -335,52 +372,9 @@ class SocketRobotArm:
|
||||||
def imitate(self):
|
def imitate(self):
|
||||||
threading.Thread(target=self.imitate_func, daemon=True).start()
|
threading.Thread(target=self.imitate_func, daemon=True).start()
|
||||||
|
|
||||||
def convert_to_joint(self, coordinates, angles=[]):
|
|
||||||
num_joints = p.getNumJoints(self.urdf_manager.body_id)
|
|
||||||
if len(angles) == 3:
|
|
||||||
angles[1] = float(angles[1]) - 90
|
|
||||||
target_orientation = p.getQuaternionFromEuler(
|
|
||||||
[math.radians(float(a)) for a in angles]
|
|
||||||
)
|
|
||||||
else:
|
|
||||||
target_orientation = None
|
|
||||||
|
|
||||||
joint_angles = p.calculateInverseKinematics(
|
|
||||||
self.urdf_manager.body_id,
|
|
||||||
endEffectorLinkIndex=num_joints - 1,
|
|
||||||
targetPosition=coordinates,
|
|
||||||
targetOrientation=target_orientation,
|
|
||||||
maxNumIterations=100,
|
|
||||||
)
|
|
||||||
# logger.info(f"convrt to joint {joint_angles}")
|
|
||||||
return joint_angles
|
|
||||||
|
|
||||||
def set_text(self, text):
|
def set_text(self, text):
|
||||||
logger.info(text)
|
logger.info(text)
|
||||||
|
|
||||||
def set_joint(self, coordinates):
|
|
||||||
# logger.info("set joints")
|
|
||||||
num_joints = p.getNumJoints(self.urdf_manager.body_id)
|
|
||||||
if coordinates is None:
|
|
||||||
return
|
|
||||||
# p.setJointMotorControlArray(
|
|
||||||
# bodyIndex=self.urdf_manager.body_id,
|
|
||||||
# controlMode=p.POSITION_CONTROL,
|
|
||||||
# jointIndices=range(0, num_joints),
|
|
||||||
# targetPositions=coordinates,
|
|
||||||
# positionGains=6 * [0.5],
|
|
||||||
# )
|
|
||||||
self.urdf_manager.robot.set_(
|
|
||||||
{
|
|
||||||
"q0": coordinates[0],
|
|
||||||
"q1": coordinates[1],
|
|
||||||
"q2": coordinates[2],
|
|
||||||
"q3": coordinates[3],
|
|
||||||
"q4": coordinates[4],
|
|
||||||
"q5": coordinates[5],
|
|
||||||
}
|
|
||||||
)
|
|
||||||
|
|
||||||
def send_data(self, data):
|
def send_data(self, data):
|
||||||
return self.socket_manager.send_data(data)
|
return self.socket_manager.send_data(data)
|
||||||
|
|
||||||
|
|
@ -468,26 +462,31 @@ class SocketRobotArm:
|
||||||
]
|
]
|
||||||
angles = [
|
angles = [
|
||||||
point["m3"],
|
point["m3"],
|
||||||
|
# float(point["m4"]) - 90,
|
||||||
point["m4"],
|
point["m4"],
|
||||||
point["m5"],
|
point["m5"],
|
||||||
]
|
]
|
||||||
# coor = [(float(point) * 0.001) for point in points]
|
|
||||||
# angles = self.convert_to_joint(
|
|
||||||
# coor,
|
|
||||||
# [
|
|
||||||
# point["m3"],
|
|
||||||
# point["m4"],
|
|
||||||
# point["m5"],
|
|
||||||
# ],
|
|
||||||
# )
|
|
||||||
# logger.info(f"point {point} {angles}")
|
|
||||||
# self.set_joint(angles)
|
|
||||||
self.urdf_manager.robot.set_endeffector_pose(
|
self.urdf_manager.robot.set_endeffector_pose(
|
||||||
[(float(point) * 0.001) for point in points],
|
[(float(point) * 0.001) for point in points],
|
||||||
p.getQuaternionFromEuler([float(point) for point in angles])
|
p.getQuaternionFromEuler([float(point) for point in angles]),
|
||||||
)
|
)
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
|
|
||||||
|
def convert_to_joint(self, coordinates, angles=[]):
|
||||||
|
if len(angles) == 3:
|
||||||
|
# angles[1] = float(angles[1]) - 90
|
||||||
|
target_orientation = p.getQuaternionFromEuler(
|
||||||
|
[math.radians(float(a)) for a in angles]
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
target_orientation = None
|
||||||
|
self.urdf_manager.robot.set_endeffector_pose(coordinates, target_orientation)
|
||||||
|
[end_effector_position, end_effector_orientation] = (
|
||||||
|
self.urdf_manager.robot.get_endeffector_pose()
|
||||||
|
)
|
||||||
|
|
||||||
|
return list(end_effector_position) + list(end_effector_orientation)
|
||||||
|
|
||||||
def convert_file_to_joint(self):
|
def convert_file_to_joint(self):
|
||||||
result = []
|
result = []
|
||||||
with open(f"data/{self.filename}.nc.result", "r") as fp:
|
with open(f"data/{self.filename}.nc.result", "r") as fp:
|
||||||
|
|
|
||||||
|
|
@ -12,9 +12,9 @@
|
||||||
"urdf": "sample"
|
"urdf": "sample"
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"name": "test (ip small)",
|
"name": "test",
|
||||||
"host": "192.168.70.55",
|
"host": "127.0.0.1",
|
||||||
"slave_id": 11,
|
"slave_id": 11,
|
||||||
"urdf": "borunte_macro"
|
"urdf": "sample"
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
|
|
@ -118,6 +118,7 @@
|
||||||
<origin rpy="0 0 0" xyz="0 0 0.0" />
|
<origin rpy="0 0 0" xyz="0 0 0.0" />
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh filename="../urdf_support/sample/7.stl"/>
|
<mesh filename="../urdf_support/sample/7.stl"/>
|
||||||
|
<!-- <box size="0.1 0.1 0.1"/> -->
|
||||||
</geometry>
|
</geometry>
|
||||||
<material name="black"/>
|
<material name="black"/>
|
||||||
</visual>
|
</visual>
|
||||||
|
|
@ -133,7 +134,7 @@
|
||||||
</joint>
|
</joint>
|
||||||
|
|
||||||
<joint name="joint_2" type="revolute">
|
<joint name="joint_2" type="revolute">
|
||||||
<limit lower="-2.444" upper="1.134" effort="0" velocity="0"/>
|
<limit lower="-2.444" upper="1.135" effort="0" velocity="0"/>
|
||||||
<parent link="link_1" />
|
<parent link="link_1" />
|
||||||
<child link="link_2" />
|
<child link="link_2" />
|
||||||
<axis xyz="0 -1 0" />
|
<axis xyz="0 -1 0" />
|
||||||
|
|
|
||||||
Binary file not shown.
|
|
@ -33,8 +33,37 @@ coordinates = [
|
||||||
"axis": [0, -0.002, -0.002, 0.003, -0.001, 0],
|
"axis": [0, -0.002, -0.002, 0.003, -0.001, 0],
|
||||||
},
|
},
|
||||||
]
|
]
|
||||||
coordinates_index = 2
|
c_angles = [
|
||||||
|
{
|
||||||
|
"world": [1396.513, 1.758, 1322.749, -0.002, 90, 0],
|
||||||
|
"axis": [0.002, -0.001, 0, 0, 0, 0],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [-1.645, 1396.533, 1322.700, -89.998, 90.000, 0],
|
||||||
|
"axis": [89.998, -0.002, -0.001, 0.001, -0.001, -0.001],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [-61.982, 1.689, 1955.771, 0, 25, 0.003],
|
||||||
|
"axis": [0.003, 65, -0.001, 0.001, 0, -0.001],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [72.252, 1.696, 2450.564, 0, 0, 0],
|
||||||
|
"axis": [0.002, -0.001, 89.999, 0.002, 0, -0.001],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [1396.542, 1.748, 1322.702, 90.004, 0.001, 90.003],
|
||||||
|
"axis": [0.002, -0.003, 0, 89.999, -0.001, 0],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [1279.539, 1.716, 1439.725, 0.004, 0.005, 0.001],
|
||||||
|
"axis": [0.001, -0.002, -0.001, 0.004, 89.998, 0],
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"world": [1396.518, 1.704, 1322.743, 90.002, -0.006, 90],
|
||||||
|
"axis": [0, -0.001, 0, 0.002, 0, 90.003],
|
||||||
|
},
|
||||||
|
]
|
||||||
|
coordinates_index = 0
|
||||||
|
|
||||||
def handle_client(conn, addr):
|
def handle_client(conn, addr):
|
||||||
print(f"Connected by {addr}")
|
print(f"Connected by {addr}")
|
||||||
|
|
@ -45,12 +74,22 @@ def handle_client(conn, addr):
|
||||||
break
|
break
|
||||||
req = json.loads(data)
|
req = json.loads(data)
|
||||||
res = {"queryData": ["ok"]}
|
res = {"queryData": ["ok"]}
|
||||||
|
if "axis-0" in req["queryAddr"]:
|
||||||
|
global coordinates_index
|
||||||
|
print(coordinates_index)
|
||||||
|
c = c_angles[coordinates_index]
|
||||||
|
limit = len(c_angles) - 1
|
||||||
|
# limit = 2
|
||||||
|
if coordinates_index < limit:
|
||||||
|
coordinates_index += 1
|
||||||
|
else:
|
||||||
|
coordinates_index = 0
|
||||||
|
|
||||||
if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
|
if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
|
||||||
res["queryData"] = coordinates[coordinates_index]["axis"]
|
res["queryData"] = c["axis"]
|
||||||
|
|
||||||
if "queryAddr" in req and "world-0" in req["queryAddr"]:
|
if "queryAddr" in req and "world-0" in req["queryAddr"]:
|
||||||
res["queryData"] = coordinates[coordinates_index]["world"]
|
res["queryData"] = c["world"]
|
||||||
|
|
||||||
if req["reqType"] == "command":
|
if req["reqType"] == "command":
|
||||||
res["cmdReply"] = ["ok"]
|
res["cmdReply"] = ["ok"]
|
||||||
|
|
@ -70,6 +109,8 @@ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||||
s.bind((HOST, PORT))
|
s.bind((HOST, PORT))
|
||||||
s.listen()
|
s.listen()
|
||||||
print(f"Server listening on {HOST}:{PORT}")
|
print(f"Server listening on {HOST}:{PORT}")
|
||||||
|
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
conn, addr = s.accept()
|
conn, addr = s.accept()
|
||||||
handle_client(conn, addr)
|
handle_client(conn, addr)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue