181 lines
7.0 KiB
Python
181 lines
7.0 KiB
Python
# cython: language_level=3
|
||
import time
|
||
import numpy as np
|
||
import atexit
|
||
from scipy.spatial.transform import Rotation as R
|
||
import sys
|
||
from pathlib import Path
|
||
sys.path.append(str(Path(__file__).resolve().parent.parent))
|
||
from hardware.dobot_nova5 import dobot_nova5
|
||
from tools.yaml_operator import *
|
||
from hardware.force_sensor import XjcSensor
|
||
import os
|
||
import math
|
||
|
||
class Identifier:
|
||
"""
|
||
reference: https://blog.csdn.net/qq_34935373/article/details/106208345
|
||
"""
|
||
def __init__(self,arm:dobot_nova5,sensor:XjcSensor,config_path:str,):
|
||
self.arm = arm
|
||
self.sensor = sensor
|
||
self.config_path = config_path
|
||
# solve for xyz: M = F @ A, A=[x, y, z, k1, k2, k3]
|
||
self.M = None
|
||
self.F = None
|
||
|
||
# solve for F0F1F2: f = R @ B, B=[0, 0, -g, Fx0, Fy0, Fz0]
|
||
self.R = None
|
||
self.f = None
|
||
|
||
self.len_data = 0
|
||
|
||
def __add_data(self, wrench: np.ndarray, ee_pose: np.ndarray):
|
||
force = wrench[:3].copy()
|
||
torque = wrench[3:].copy()
|
||
# import pdb
|
||
# pdb.set_trace()
|
||
if self.M is None:
|
||
self.M = torque.reshape((3, 1))
|
||
f_skew = self.__skew_symmetric(force)
|
||
self.F = np.hstack((f_skew, np.eye(3)))
|
||
else:
|
||
self.M = np.vstack((self.M, torque.reshape((3, 1))))
|
||
f_skew = self.__skew_symmetric(force)
|
||
self.F = np.vstack((self.F, np.hstack((f_skew, np.eye(3)))))
|
||
|
||
if self.R is None:
|
||
self.R = np.hstack((ee_pose.T, np.eye(3)))
|
||
self.f = force.reshape((3, 1))
|
||
else:
|
||
self.R = np.vstack((self.R, np.hstack((ee_pose.T, np.eye(3)))))
|
||
self.f = np.vstack((self.f, force.reshape((3, 1))))
|
||
self.len_data += 1
|
||
|
||
def __solve(self):
|
||
A, residuals, rank, s = np.linalg.lstsq(self.F, self.M, rcond=None)
|
||
B, residuals, rank, s = np.linalg.lstsq(self.R, self.f, rcond=None)
|
||
|
||
x, y, z, k1, k2, k3 = A.flatten()
|
||
Gx, Gy, Gz, Fx0, Fy0, Fz0 = B.flatten()
|
||
|
||
mass_center = np.array([x, y, z])
|
||
F_0 = np.array([Fx0, Fy0, Fz0])
|
||
M_x0 = k1 - Fy0 * z + Fz0 * y
|
||
M_y0 = k2 - Fz0 * x + Fx0 * z
|
||
M_z0 = k3 - Fx0 * y + Fy0 * x
|
||
M_0 = np.array([M_x0, M_y0, M_z0])
|
||
gravity_base = np.array([Gx, Gy, Gz])
|
||
return mass_center, F_0, M_0, gravity_base
|
||
|
||
def __read_data_once(self,num):
|
||
sum = np.zeros(6)
|
||
i = 0
|
||
# 读取num次数据，取平均值
|
||
while i < num:
|
||
# data = self.sensor.read()
|
||
data = self.sensor.read_data_f32()
|
||
if data is not None:
|
||
sum += data
|
||
i += 1
|
||
time.sleep(0.2)
|
||
return sum/i
|
||
|
||
def __skew_symmetric(self, v: np.ndarray):
|
||
""" 从3维向量生成对应的3x3反对称矩阵 """
|
||
if len(v) != 3:
|
||
raise ValueError("输入数组的长度必须为3!")
|
||
return np.array([[0, v[2], -v[1]],
|
||
[-v[2], 0, v[0]],
|
||
[v[1], -v[0], 0]])
|
||
|
||
def set_position(self,x,y,z,roll,pitch,yaw):
|
||
'''
|
||
x y z : mm
|
||
r p y : deg
|
||
'''
|
||
pose_command = np.concatenate([[x,y,z], [roll,pitch,yaw]])
|
||
cur = self.arm.getAngel() # 接口调用: 获取机器人的关节位置
|
||
cur_str = "{" + ",".join(map(str, cur)) + "}"
|
||
target_J = self.arm.getInverseKin(poses_list=pose_command,useJointNear=1,JointNear=cur_str)
|
||
print(target_J)
|
||
self.arm.RunPoint_P_inJoint(target_J)
|
||
|
||
def identify_param_auto(self,ready_pose,cnt,sample_num = 5):
|
||
if cnt < 15:
|
||
raise ValueError("数据量太少，请重新输入，至少要有15组数据")
|
||
control_pose = np.array(ready_pose)
|
||
desired_cart_pose = control_pose.copy()
|
||
# move to the initial position
|
||
self.set_position(x=desired_cart_pose[0],y=desired_cart_pose[1],z=desired_cart_pose[2],
|
||
roll=desired_cart_pose[3],pitch=desired_cart_pose[4],yaw=desired_cart_pose[5])
|
||
# self.sensor.set_zero()
|
||
time.sleep(1)
|
||
delta = 60/cnt*6
|
||
quarter = cnt // 6
|
||
for i in range(cnt):
|
||
if i % 2 == 0:
|
||
control_pose[3]+= delta * np.where((i <= quarter) | (3* quarter< i <= 5*quarter) , 1, -1)
|
||
else:
|
||
control_pose[4]+= delta * np.where((i <= quarter) | (3* quarter< i <= 5*quarter), -1, 1)
|
||
desired_cart_pose = control_pose.copy()
|
||
print("desired_cart_pose:",desired_cart_pose)
|
||
self.set_position(x=desired_cart_pose[0],
|
||
y=desired_cart_pose[1],
|
||
z=desired_cart_pose[2],
|
||
roll=desired_cart_pose[3],
|
||
pitch=desired_cart_pose[4],
|
||
yaw=desired_cart_pose[5])
|
||
print("move done")
|
||
time.sleep(1)
|
||
wrench = self.__read_data_once(sample_num)
|
||
print(f"wrench:{wrench}")
|
||
print("read done")
|
||
cur_pose = self.arm.getPose()
|
||
b_R_e = R.from_euler('xyz', cur_pose[3:], degrees=False).as_matrix()
|
||
self.__add_data(wrench, b_R_e)
|
||
print(f"=======添加了第{i+1}组数据=======")
|
||
time.sleep(0.5)
|
||
mass_center, F_0, M_0, gravity_base = self.__solve()
|
||
# output all results
|
||
print(f"mass_center_position: {mass_center}")
|
||
print(f"force_zero: {F_0}")
|
||
print(f"torque_zero: {M_0}")
|
||
print(f"gravity_base: {gravity_base}")
|
||
dict_data = {
|
||
'mass_center_position': mass_center.tolist(),
|
||
'force_zero': F_0.tolist(),
|
||
'torque_zero': M_0.tolist(),
|
||
'gravity_base': gravity_base.tolist()
|
||
}
|
||
confirmation = input("是否确认更新数据？(y/n): ").strip().lower()
|
||
|
||
if confirmation == 'y' or confirmation == 'Y' or confirmation == 'yes' or confirmation == 'YES':
|
||
# 执行更新操作
|
||
if not os.path.exists(self.config_path):
|
||
# 如果文件不存在，则创建一个新的config.yaml文件
|
||
with open(self.config_path, 'w') as file:
|
||
pass
|
||
|
||
update_yaml(self.config_path, dict_data)
|
||
print("更新操作已完成。")
|
||
else:
|
||
print("更新操作已取消。")
|
||
|
||
|
||
|
||
if __name__ == '__main__':
|
||
arm = dobot_nova5()
|
||
arm.setEndEffector(i=9,tool_i="x,y,z,rx,ry,rz")
|
||
arm.chooseEndEffector(i=9)
|
||
arm.init()
|
||
sensor = XjcSensor()
|
||
sensor.connect()
|
||
sensor.disable_active_transmission()
|
||
sensor.disable_active_transmission()
|
||
sensor.disable_active_transmission()
|
||
atexit.register(sensor.disconnect)
|
||
identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/none_playload.yaml")
|
||
ready_pose = [-1,-1,-1,-1,-1,-1]
|
||
time.sleep(1)
|
||
identifier.identify_param_auto(ready_pose,45) |