重力补偿原理
1.力传感器数据分析
将六维力传感器三个力分量与力矩分量的零点值分别记为
(Fx0,Fy0,Fz0)(F_{x0},F_{y0},F_{z0})(Fx0,Fy0,Fz0)，(Mx0,My0,Mz0)(M_{x0},M_{y0},M_{z0})(Mx0,My0,Mz0)，
传感器与末端工装重力为
GGG,质心在六维力传感器坐标系中的坐标为(x,y,z)(x,y,z)(x,y,z)，
重力
GGG在三个坐标轴方向上的分力与作用力矩分别为(Gx,Gy,Gz)(G_x,G_y,G_z)(Gx,Gy,Gz)，(Mgx,Mgy,Mgz)(M_{gx},M_{gy},M_{gz})(Mgx,Mgy,Mgz)。
根据力与力矩的关系可以得到（正方向由右手定则确定）：
{Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y\left\{ \begin{array}{c} M_{gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_{gz}=G_y \times x-G_x \times y \end{array} \right.⎩⎨⎧Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y
将六维力传感器直接测量得到的三个方向的力分量与力矩分量分别记为
(Fx,Fy,Fz)(F_x,F_y,F_z)(Fx,Fy,Fz)，(Mx,My,Mz)(M_x,M_y,M_z)(Mx,My,Mz)，
假设标定的时候没有外部作用力在末端夹持器上，则力传感器所测得的力和力矩由负载重力及零点组成，则有
{Fx=Gx+Fx0Fy=Gy+Fy0Fz=Gz+Fz0\left\{ \begin{array}{c} F_x=G_x+F_{x0}\\ F_y=G_y+F_{y0}\\ F_z=G_z+F_{z0} \end{array} \right.⎩⎨⎧Fx=Gx+Fx0Fy=Gy+Fy0Fz=Gz+Fz0{Mx=Mgx+Mx0My=Mgy+My0Mz=Mgz+Mz0\left\{ \begin{array}{c} M_x=M_{gx}+M_{x0}\\ M_y=M_{gy}+M_{y0}\\ M_z=M_{gz}+M_{z0} \end{array} \right.⎩⎨⎧Mx=Mgx+Mx0My=Mgy+My0Mz=Mgz+Mz0
联立得：
{Mx=(Fz−Fz0)×y−(Fy−Fy0)×z+Mx0My=(Fx−Fx0)×z−(Fz−Fz0)×x+My0Mz=(Fy−Fy0)×x−(Fx−Fx0)×y+Mz0\left\{ \begin{array}{c} M_x=(F_z-F_{z0}) \times y-(F_y-F_{y0}) \times z+M_{x0}\\ M_y=(F_x-F_{x0}) \times z-(F_z-F_{z0}) \times x+M_{y0}\\ M_z=(F_y-F_{y0}) \times x-(F_x-F_{x0}) \times y+M_{z0} \end{array} \right.⎩⎨⎧Mx=(Fz−Fz0)×y−(Fy−Fy0)×z+Mx0My=(Fx−Fx0)×z−(Fz−Fz0)×x+My0Mz=(Fy−Fy0)×x−(Fx−Fx0)×y+Mz0{Mx=Fz×y−Fy×z+Mx0+Fy0×z−Fz0×yMy=Fx×z−Fz×x+My0+Fz0×x−Fx0×zMz=Fy×x−Fx×y+Mz0+Fx0×y−Fy0×x\left\{ \begin{array}{c} M_x=F_z \times y - F_y \times z + M_{x0} + F_{y0} \times z -F_{z0} \times y \\ M_y=F_x \times z - F_z \times x + M_{y0} + F_{z0} \times x -F_{x0} \times z \\ M_z=F_y \times x - F_x \times y + M_{z0} + F_{x0} \times y -F_{y0} \times x \end{array} \right.⎩⎨⎧Mx=Fz×y−Fy×z+Mx0+Fy0×z−Fz0×yMy=Fx×z−Fz×x+My0+Fz0×x−Fx0×zMz=Fy×x−Fx×y+Mz0+Fx0×y−Fy0×x
其中的
Fx0,Fy0,Fz0,Mx0,My0,Mz0,x,y,zF_{x0},F_{y0},F_{z0},M_{x0},M_{y0},M_{z0},x,y,zFx0,Fy0,Fz0,Mx0,My0,Mz0,x,y,z为定值常数（忽略力传感器数据波动，并且末端工装不进行更换）
2.最小二乘法求解参数
2.1力矩方程
令
{k1=Mx0+Fy0×z−Fz0×yk2=My0+Fz0×x−Fx0×zk3=Mz0+Fx0×y−Fy0×x\left\{ \begin{array}{c} k_1=M_{x0} + F_{y0} \times z -F_{z0} \times y\\ k_2=M_{y0} + F_{z0} \times x -F_{x0} \times z\\ k_3=M_{z0} + F_{x0} \times y -F_{y0} \times x \end{array} \right.⎩⎨⎧k1=Mx0+Fy0×z−Fz0×yk2=My0+Fz0×x−Fx0×zk3=Mz0+Fx0×y−Fy0×x
则：
[MxMyMz]=[0Fz−Fy100−Fz0Fx010Fy−Fx0001][xyzk1k2k3]\begin{equation} \left[ \begin{array}{c} M_x \\ M_y \\ M_z \end{array} \right]= \left[ \begin{array}{ccc} 0 & F_z & -F_y & 1 & 0 & 0 \\ -F_z & 0 & F_x & 0 & 1 & 0 \\ F_y & -F_x & 0 & 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{c} x \\ y \\ z \\ k1 \\ k2 \\ k3 \end{array} \right] \end{equation}MxMyMz=0−FzFyFz0−Fx−FyFx0100010001xyzk1k2k3
控制机械臂末端夹持器的位姿，取N个不同的姿态(N
≥\geq≥ 3，要求至少有三个姿态下的机械臂末端夹持器的指向向量不共面），得到N组六维力数据传感器数据，M=F⋅AM=F \cdot AM=F⋅A ，其中A=[x,y,z,k1,k2,k3]TA=[x,y,z,k_1,k_2,k_3]^\mathrm{T}A=[x,y,z,k1,k2,k3]T，则矩阵的最小二乘解为A=(FTF)−1⋅FT⋅MA=(F^\mathrm{T}F)^{-1}\cdot F^{\mathrm{T}}\cdot MA=(FTF)−1⋅FT⋅M，
由多组数据可以解出负载质心在六维力传感器坐标系中的坐标
(x,y,z)(x,y,z)(x,y,z)以及常数(k1,k2,k3)(k_1,k_2,k_3)(k1,k2,k3)的值。
2.2力方程
在机器人的安装固定过程中，由于安装平台会存在一定的斜度，使得机器人的基坐标系与世界坐标系出现一定的夹角，同样也会使得传感器的测量值与实际值存在一定偏差，因此需要进行倾角的计算。
记世界坐标系为
O{O}O，基坐标系为BBB，法兰坐标系为WWW，力传感器坐标系为C{C}C，则由基坐标系向世界坐标系的姿态变换矩阵为：BOR=[1000cosUsinU0sinUcosU][cosV0sinV010−sinV0cosV]\begin{equation} ^O_BR= \left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & cosU & sinU \\ 0 & sinU & cosU \end{array} \right] \left[ \begin{array}{ccc} cosV & 0 &sinV\\ 0 & 1 & 0\\ -sinV & 0 & cosV \\ \end{array} \right] \end{equation}BOR=1000cosUsinU0sinUcosUcosV0−sinV010sinV0cosV
其中
UUU是绕世界坐标系xxx轴的旋转角，VVV是绕基坐标系的yyy轴的旋转角
由末端法兰到基坐标系的姿态变换矩阵为：
WBR=Rz(A)Ry(B)Rx(C)^B_WR=R_z(A) R_y(B) R_x(C)WBR=Rz(A)Ry(B)Rx(C)ABCA B CABC的值可以通过机器人示教器读出
因为力传感器安装到末端时，会存在
ZZZ轴上的一定偏角α\alphaα（可通过更换机器人参考坐标系手动将其重合，即偏角为0），由力传感器向法兰坐标系坐标系的姿态变换矩阵为：CWR=[cosα−sinα0sinαcosα0001]\begin{equation} ^W_CR= \left[ \begin{array}{ccc} cos\alpha & -sin\alpha & 0 \\ sin\alpha & cos\alpha & 0 \\ 0 & 0 & 1 \end{array} \right] \end{equation}CWR=cosαsinα0−sinαcosα0001
则由世界坐标系到六维力传感器坐标系的姿态变换矩阵为
0CR=WCR⋅BWR⋅OBR=CWRT⋅WBRT⋅BORT{^C_0R} = {^C_WR} \cdot {^W_BR} \cdot {^B_OR} = {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot {^O_BR}^\mathrm{T}0CR=WCR⋅BWR⋅OBR=CWRT⋅WBRT⋅BORT
重力在世界坐标系
O{O}O中的表示为OG=[0,0,−g]T{^{O}G=[0,0,-g]^\mathrm{T}}OG=[0,0,−g]T,
通过坐标变换，将重力分解到力传感器坐标系
C{C}C得到CG=OCR⋅OG=WCR⋅BWR⋅OBR⋅OG=CWRT⋅WBRT⋅BORT⋅OG=CWRT⋅WBRT⋅[cosUsinV∗g−sinU∗g−cosUcosV∗g]^{C}G=^C_OR \cdot {^{O}G}= {^C_WR} \cdot {^W_BR} \cdot {^B_OR} \cdot {^{O}G} ={^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot {^O_BR}^\mathrm{T} \cdot {^{O}G} \\ ={^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot \left[ \begin{array}{c} cosUsinV * g \\ -sinU * g \\ -cosUcosV * g \end{array} \right]CG=OCR⋅OG=WCR⋅BWR⋅OBR⋅OG=CWRT⋅WBRT⋅BORT⋅OG=CWRT⋅WBRT⋅cosUsinV∗g−sinU∗g−cosUcosV∗g
则：
[Fx,Fy,Fz]T=[Gx,Gy,Gz]T+[Fx0,Fy0,Fz0]T=OCR⋅OG+[Fx0,Fy0,Fz0]T[F_x,F_y,F_z]^{\mathrm{T}}=[G_x,G_y,G_z]^{\mathrm{T}}+[F_{x0},F_{y0},F_{z0}]^{\mathrm{T}}=^C_OR \cdot {^{O}G}+[F_{x0},F_{y0},F_{z0}]^{\mathrm{T}}[Fx,Fy,Fz]T=[Gx,Gy,Gz]T+[Fx0,Fy0,Fz0]T=OCR⋅OG+[Fx0,Fy0,Fz0]T[FxFyFz]=[CWRT⋅WBRTI][cosUsinV∗g−sinU∗g−cosUcosV∗gFx0Fy0Fz0]\begin{equation} \left[ \begin{array}{c} F_x \\ F_y \\ F_z \end{array} \right]= \left[ \begin{array}{ccc} {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} & I \end{array} \right] \left[ \begin{array}{c} cosUsinV * g \\ -sinU * g \\ -cosUcosV * g \\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation}FxFyFz=[CWRT⋅WBRTI]cosUsinV∗g−sinU∗g−cosUcosV∗gFx0Fy0Fz0
令：
{Lx=cosUsinV∗gLy=−sinU∗gLz=−cosUcosV∗g\left\{ \begin{array}{c} L_x=cosUsinV * g\\ L_y=-sinU * g\\ L_z= -cosUcosV * g \end{array} \right.⎩⎨⎧Lx=cosUsinV∗gLy=−sinU∗gLz=−cosUcosV∗g
则可以表示为:
[FxFyFz]=[CWRT⋅WBRTI][LxLyLzFx0Fy0Fz0]\begin{equation} \left[ \begin{array}{c} F_x \\ F_y \\ F_z \end{array} \right]= \left[ \begin{array}{ccc} {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} & I \end{array} \right] \left[ \begin{array}{c} L_x \\ L_y \\ L_z\\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation}FxFyFz=[CWRT⋅WBRTI]LxLyLzFx0Fy0Fz0
同理，取N个不同的姿态得到N组六维力传感器数据，
f=R⋅Bf=R \cdot Bf=R⋅B，其中B=[Lx,Ly,Lz,Fx0,Fy0,Fz0]TB=[L_x,L_y,L_z,F_{x0},F_{y0},F_{z0}]^{\mathrm{T}}B=[Lx,Ly,Lz,Fx0,Fy0,Fz0]T，则矩阵的最小二乘解为B=(RTR)−1⋅RT⋅fB=(R^\mathrm{T}R)^{-1} \cdot R^\mathrm{T}\cdot fB=(RTR)−1⋅RT⋅f，
由多组数据可以解出力传感器的零点值
(Fx0,Fy0,Fz0)(F_{x0},F_{y0},F_{z0})(Fx0,Fy0,Fz0)，安装倾角U,VU,VU,V以及重力ggg的大小。g=Lx2+Ly2+Lz2g=\sqrt{{L_x}^2 + {L_y}^2 + {L_z}^2}\\g=Lx2+Ly2+Lz2U=arcsin(−Lyg)U = arcsin(\frac{-L_y}{g})U=arcsin(g−Ly)V=arctan(−LxLz)V = arctan(\frac{-L_x}{L_z})V=arctan(Lz−Lx)
3.外部接触力计算
由此可以得出外部接触力为：
{Fex=Fx−Fx0−GxFey=Fy−Fy0−GyFez=Fz−Fz0−Gz\left\{ \begin{array}{c} F_{ex}=F_x-F_{x0}-G_x\\ F_{ey}=F_y-F_{y0}-G_y\\ F_{ez}=F_z-F_{z0}-G_z \end{array} \right.⎩⎨⎧Fex=Fx−Fx0−GxFey=Fy−Fy0−GyFez=Fz−Fz0−Gz[FexFeyFez]=[FxFyFz]−[OCRI][00−gFx0Fy0Fz0]\begin{equation} \left[ \begin{array}{c} F_{ex} \\ F_{ey} \\ F_{ez} \end{array} \right]= \left[ \begin{array}{c} F_{x} \\ F_{y} \\ F_{z} \end{array} \right]- \left[ \begin{array}{ccc} ^C_OR & I \end{array} \right] \left[ \begin{array}{c} 0 \\ 0 \\ -g \\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation}FexFeyFez=FxFyFz−[OCRI]00−gFx0Fy0Fz0
又：
{Mx0=k1−Fy0×z+Fz0×yMy0=k2−Fz0×x+Fx0×zMz0=k3−Fx0×y+Fy0×x\left\{ \begin{array}{c} M_{x0}=k_1 - F_{y0} \times z +F_{z0} \times y \\ M_{y0}=k_2 - F_{z0} \times x +F_{x0} \times z\\ M_{z0}=k_3 - F_{x0} \times y +F_{y0} \times x \end{array} \right.⎩⎨⎧Mx0=k1−Fy0×z+Fz0×yMy0=k2−Fz0×x+Fx0×zMz0=k3−Fx0×y+Fy0×xCG=OCR⋅OG=[Gx,Gy,Gz]T^{C}G=^C_OR \cdot {^{O}G} = [G_x,G_y,G_z]^\mathrm{T}CG=OCR⋅OG=[Gx,Gy,Gz]T{Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y\left\{ \begin{array}{c} M_{gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_{gz}=G_y \times x-G_x \times y \end{array} \right.⎩⎨⎧Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y
即:
[MgxMgyMgz]=[0−zyz0−x−yx0][GxGyGz]=[0−zyz0−x−yx0]⋅OCR⋅[00−g]\begin{equation} \left[ \begin{array}{c} M_{gx} \\ M_{gy} \\ M_{gz} \end{array} \right]= \left[ \begin{array}{ccc} 0 & -z & y\\ z & 0 &-x \\ -y & x & 0 \end{array} \right] \left[ \begin{array}{c} G_x \\ G_y \\ G_z \end{array} \right]= \left[ \begin{array}{ccc} 0 & -z & y\\ z & 0 &-x \\ -y & x & 0 \end{array} \right] \cdot ^C_OR \cdot \left[ \begin{array}{ccc} 0 \\ 0 \\ -g \end{array} \right] \end{equation}MgxMgyMgz=0z−y−z0xy−x0GxGyGz=0z−y−z0xy−x0⋅OCR⋅00−g
得出外部接触力矩为：
{Mex=Mx−Mx0−MgxMey=My−My0−MgyMez=Mz−MZ0−Mgz\left\{ \begin{array}{c} M_{ex}=M_x-M_{x0}-M_{gx}\\ M_{ey}=M_y-M_{y0}-M_{gy}\\ M_{ez}=M_z-M_{Z0}-M_{gz} \end{array} \right.⎩⎨⎧Mex=Mx−Mx0−MgxMey=My−My0−MgyMez=Mz−MZ0−Mgz
[1]高培阳. 基于力传感器的工业机器人恒力磨抛系统研究[D].华中科技大学,2019.
[2]张立建,胡瑞钦,易旺民.基于六维力传感器的工业机器人末端负载受力感知研究[J].自动化学报,2017,43(03):439-447.DOI:10.16383/j.aas.2017.c150753.
[3]董浩. 轴孔装配工业机器人柔顺控制算法研究[D].河南理工大学,2018.
代码如下：
from numpy import*import numpy as np'''Made by 水木皆Ming重力补偿计算'''classGravityCompensation:M = np.empty((0,0))F = np.empty((0,0))f = np.empty((0,0))R = np.empty((0,0))x =0y =0z =0k1 =0k2 =0k3 =0U =0V =0g =0F_x0 =0F_y0 =0F_z0 =0M_x0 =0M_y0 =0M_z0 =0F_ex =0F_ey =0F_ez =0M_ex =0M_ey =0M_ez =0defUpdate_M(self, torque_data):M_x = torque_data[0]M_y = torque_data[1]M_z = torque_data[2]if(any(self.M)):M_1 = matrix([M_x, M_y, M_z]).transpose()self.M = vstack((self.M, M_1))else:self.M = matrix([M_x, M_y, M_z]).transpose()defUpdate_F(self, force_data):F_x = force_data[0]F_y = force_data[1]F_z = force_data[2]if(any(self.F)):F_1 = matrix([[0, F_z,-F_y,1,0,0],[-F_z,0, F_x,0,1,0],[F_y,-F_x,0,0,0,1]])self.F = vstack((self.F, F_1))else:self.F = matrix([[0, F_z,-F_y,1,0,0],[-F_z,0, F_x,0,1,0],[F_y,-F_x,0,0,0,1]])defSolve_A(self):A = dot(dot(linalg.inv(dot(self.F.transpose(), self.F)), self.F.transpose()), self.M)self.x = A[0,0]self.y = A[1,0]self.z = A[2,0]self.k1 = A[3,0]self.k2 = A[4,0]self.k3 = A[5,0]# print("A= \n" , A)print("x= ", self.x)print("y= ", self.y)print("z= ", self.z)print("k1= ", self.k1)print("k2= ", self.k2)print("k3= ", self.k3)defUpdate_f(self, force_data):F_x = force_data[0]F_y = force_data[1]F_z = force_data[2]if(any(self.f)):f_1 = matrix([F_x, F_y, F_z]).transpose()self.f = vstack((self.f, f_1))else:self.f = matrix([F_x, F_y, F_z]).transpose()defUpdate_R(self, euler_data):# 机械臂末端到基坐标的旋转矩阵R_array = self.eulerAngles2rotationMat(euler_data)alpha =(0)*180/ np.pi# 力传感器到末端的旋转矩阵R_alpha = np.array([[math.cos(alpha),-math.sin(alpha),0],[math.sin(alpha), math.cos(alpha),0],[0,0,1]])R_array = np.dot(R_alpha, R_array.transpose())if(any(self.R)):R_1 = hstack((R_array, np.eye(3)))self.R = vstack((self.R, R_1))else:self.R = hstack((R_array, np.eye(3)))defSolve_B(self):B = dot(dot(linalg.inv(dot(self.R.transpose(), self.R)), self.R.transpose()), self.f)self.g = math.sqrt(B[0]* B[0]+ B[1]* B[1]+ B[2]* B[2])self.U = math.asin(-B[1]/ self.g)self.V = math.atan(-B[0]/ B[2])self.F_x0 = B[3,0]self.F_y0 = B[4,0]self.F_z0 = B[5,0]# print("B= \n" , B)print("g= ", self.g /9.81)print("U= ", self.U *180/ math.pi)print("V= ", self.V *180/ math.pi)print("F_x0= ", self.F_x0)print("F_y0= ", self.F_y0)print("F_z0= ", self.F_z0)defSolve_Force(self, force_data, euler_data):Force_input = matrix([force_data[0], force_data[1], force_data[2]]).transpose()my_f = matrix([cos(self.U)*sin(self.V)*self.g,-sin(self.U)*self.g,-cos(self.U)*cos(self.V)*self.g, self.F_x0, self.F_y0, self.F_z0]).transpose()R_array = self.eulerAngles2rotationMat(euler_data)R_array = R_array.transpose()R_1 = hstack((R_array, np.eye(3)))Force_ex = Force_input - dot(R_1, my_f)print('接触力：\n', Force_ex)defSolve_Torque(self, torque_data, euler_data):Torque_input = matrix([torque_data[0], torque_data[1], torque_data[2]]).transpose()M_x0 = self.k1 - self.F_y0 * self.z + self.F_z0 * self.yM_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.zM_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.xTorque_zero = matrix([M_x0, M_y0, M_z0]).transpose()Gravity_param = matrix([[0,-self.z, self.y],[self.z,0,-self.x],[-self.y, self.x,0]])Gravity_input = matrix([cos(self.U)*sin(self.V)*self.g,-sin(self.U)*self.g,-cos(self.U)*cos(self.V)*self.g]).transpose()R_array = self.eulerAngles2rotationMat(euler_data)R_array = R_array.transpose()Torque_ex = Torque_input - Torque_zero - dot(dot(Gravity_param, R_array), Gravity_input)print('接触力矩：\n', Torque_ex)defeulerAngles2rotationMat(self, theta):theta =[i * math.pi /180.0for i in theta]# 角度转弧度R_x = np.array([[1,0,0],[0, math.cos(theta[0]),-math.sin(theta[0])],[0, math.sin(theta[0]), math.cos(theta[0])]])R_y = np.array([[math.cos(theta[1]),0, math.sin(theta[1])],[0,1,0],[-math.sin(theta[1]),0, math.cos(theta[1])]])R_z = np.array([[math.cos(theta[2]),-math.sin(theta[2]),0],[math.sin(theta[2]), math.cos(theta[2]),0],[0,0,1]])# 第一个角为绕X轴旋转，第二个角为绕Y轴旋转，第三个角为绕Z轴旋转R = np.dot(R_x, np.dot(R_y, R_z))return Rdefmain():#force_data =[-6.349214527290314e-05,0.0016341784503310919,-24.31537437438965]torque_data=[-0.25042885541915894,0.32582423090934753,2.255179606436286e-05]euler_data =[-80.50866918099089,77.83705434751874,-9.294185889510375+12]force_data1 =[-7.469202995300293,2.3709897994995117,-23.0179500579834]torque_data1=[-0.2169264256954193,0.3719269931316376,0.10870222747325897]euler_data1 =[-105.99038376663763,60.89987226261212,-10.733422007074305+12]force_data2 =[-14.45930004119873,0.995974063873291,-19.523677825927734]torque_data2=[-0.19262456893920898,0.3845194876194,0.1622740775346756]euler_data2 =[-114.24258417090118,43.78913507089547,-19.384088817327235+12]compensation = GravityCompensation()compensation.Update_F(force_data)compensation.Update_F(force_data1)compensation.Update_F(force_data2)compensation.Update_M(torque_data)compensation.Update_M(torque_data1)compensation.Update_M(torque_data2)compensation.Solve_A()compensation.Update_f(force_data)compensation.Update_f(force_data1)compensation.Update_f(force_data2)compensation.Update_R(euler_data)compensation.Update_R(euler_data1)compensation.Update_R(euler_data2)compensation.Solve_B()# compensation.Solve_Force(force_data, euler_data)# compensation.Solve_Force(force_data1, euler_data1)# compensation.Solve_Force(force_data2, euler_data2)## compensation.Solve_Torque(torque_data, euler_data)# compensation.Solve_Torque(torque_data1, euler_data1)# compensation.Solve_Torque(torque_data2, euler_data2)if __name__ =='__main__':main()
Made by 水木皆Ming
本文转载自: https://blog.csdn.net/zhang1079528541/article/details/129503851
版权归原作者 水木皆Ming 所有， 如有侵权，请联系我们删除。
版权归原作者 水木皆Ming 所有， 如有侵权，请联系我们删除。