重力补偿原理
1.力传感器数据分析
将六维力传感器三个力分量与力矩分量的零点值分别记为 ( F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) (Fx0,Fy0,Fz0)， ( M x 0 , M y 0 , M z 0 ) (M_{x0},M_{y0},M_{z0}) (Mx0,My0,Mz0)，
传感器与末端工装重力为 G G G,质心在六维力传感器坐标系中的坐标为 ( x , y , z ) (x,y,z) (x,y,z)，
重力 G G G在三个坐标轴方向上的分力与作用力矩分别为 ( G x , G y , G z ) (G_x,G_y,G_z) (Gx,Gy,Gz)， ( M g x , M g y , M g z ) (M_{gx},M_{gy},M_{gz}) (Mgx,Mgy,Mgz)。
根据力与力矩的关系可以得到（正方向由右手定则确定）：
{ M g x = G z × y − G y × z M g y = G x × z − G z × x M g z = G y × x − G x × 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
将六维力传感器直接测量得到的三个方向的力分量与力矩分量分别记为 ( F x , F y , F z ) (F_x,F_y,F_z) (Fx,Fy,Fz)， ( M x , M y , M z ) (M_x,M_y,M_z) (Mx,My,Mz)，
假设标定的时候没有外部作用力在末端夹持器上，则力传感器所测得的力和力矩由负载重力及零点组成，则有
{ F x = G x + F x 0 F y = G y + F y 0 F z = G z + F z 0 \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
{ M x = M g x + M x 0 M y = M g y + M y 0 M z = M g z + M z 0 \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
联立得：
{ M x = ( F z − F z 0 ) × y − ( F y − F y 0 ) × z + M x 0 M y = ( F x − F x 0 ) × z − ( F z − F z 0 ) × x + M y 0 M z = ( F y − F y 0 ) × x − ( F x − F x 0 ) × y + M z 0 \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
{ M x = F z × y − F y × z + M x 0 + F y 0 × z − F z 0 × y M y = F x × z − F z × x + M y 0 + F z 0 × x − F x 0 × z M z = F y × x − F x × y + M z 0 + F x 0 × y − F y 0 × 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
其中的 F x 0 , F y 0 , F z 0 , M x 0 , M y 0 , M z 0 , x , y , z F_{x0},F_{y0},F_{z0},M_{x0},M_{y0},M_{z0},x,y,z Fx0,Fy0,Fz0,Mx0,My0,Mz0,x,y,z为定值常数（忽略力传感器数据波动，并且末端工装不进行更换）
2.最小二乘法求解参数
2.1力矩方程
令
{ k 1 = M x 0 + F y 0 × z − F z 0 × y k 2 = M y 0 + F z 0 × x − F x 0 × z k 3 = M z 0 + F x 0 × y − F y 0 × 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
则：
[ M x M y M z ] = [ 0 F z − F y 1 0 0 − F z 0 F x 0 1 0 F y − F x 0 0 0 1 ] [ x y z k 1 k 2 k 3 ] \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−FyFx0100010001
xyzk1k2k3
控制机械臂末端夹持器的位姿，取N个不同的姿态(N ≥ \geq ≥ 3，要求至少有三个姿态下的机械臂末端夹持器的指向向量不共面），得到N组六维力数据传感器数据，
M = F ⋅ A M=F \cdot A M=F⋅A ，其中 A = [ x , y , z , k 1 , k 2 , k 3 ] T A=[x,y,z,k_1,k_2,k_3]^\mathrm{T} A=[x,y,z,k1,k2,k3]T，则矩阵的最小二乘解为 A = ( F T F ) − 1 ⋅ F T ⋅ M A=(F^\mathrm{T}F)^{-1}\cdot F^{\mathrm{T}}\cdot M A=(FTF)−1⋅FT⋅M，
由多组数据可以解出负载质心在六维力传感器坐标系中的坐标 ( x , y , z ) (x,y,z) (x,y,z)以及常数 ( k 1 , k 2 , k 3 ) (k_1,k_2,k_3) (k1,k2,k3)的值。
2.2力方程
在机器人的安装固定过程中，由于安装平台会存在一定的斜度，使得机器人的基坐标系与世界坐标系出现一定的夹角，同样也会使得传感器的测量值与实际值存在一定偏差，因此需要进行倾角的计算。
记世界坐标系为 O {O} O，基坐标系为 B B B，法兰坐标系为 W W W，力传感器坐标系为 C {C} C，则由基坐标系向世界坐标系的姿态变换矩阵为：
B O R = [ 1 0 0 0 c o s U s i n U 0 s i n U c o s U ] [ c o s V 0 s i n V 0 1 0 − s i n V 0 c o s V ] \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=
1000cosUsinU0sinUcosU
cosV0−sinV010sinV0cosV
其中 U U U是绕世界坐标系 x x x轴的旋转角， V V V是绕基坐标系的 y y y轴的旋转角
由末端法兰到基坐标系的姿态变换矩阵为：
W B R = R z ( A ) R y ( B ) R x ( C ) ^B_WR=R_z(A) R_y(B) R_x(C) WBR=Rz(A)Ry(B)Rx(C)
A B C A B C ABC的值可以通过机器人示教器读出
因为力传感器安装到末端时，会存在 Z Z Z轴上的一定偏角 α \alpha α（可通过更换机器人参考坐标系手动将其重合，即偏角为0），由力传感器向法兰坐标系坐标系的姿态变换矩阵为：
C W R = [ c o s α − s i n α 0 s i n α c o s α 0 0 0 1 ] \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
则由世界坐标系到六维力传感器坐标系的姿态变换矩阵为
0 C R = W C R ⋅ B W R ⋅ O B R = C W R T ⋅ W B R T ⋅ B O R T {^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中的表示为 O G = [ 0 , 0 , − g ] T {^{O}G=[0,0,-g]^\mathrm{T}} OG=[0,0,−g]T,
通过坐标变换，将重力分解到力传感器坐标系 C {C} C得到
C G = O C R ⋅ O G = W C R ⋅ B W R ⋅ O B R ⋅ O G = C W R T ⋅ W B R T ⋅ B O R T ⋅ O G = C W R T ⋅ W B R T ⋅ [ c o s U s i n V ∗ g − s i n U ∗ g − c o s U c o s V ∗ 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
则：
[ F x , F y , F z ] T = [ G x , G y , G z ] T + [ F x 0 , F y 0 , F z 0 ] T = O C R ⋅ O G + [ F x 0 , F y 0 , F z 0 ] 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
[ F x F y F z ] = [ C W R T ⋅ W B R T I ] [ c o s U s i n V ∗ g − s i n U ∗ g − c o s U c o s V ∗ g F x 0 F y 0 F z 0 ] \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
令：
{ L x = c o s U s i n V ∗ g L y = − s i n U ∗ g L z = − c o s U c o s V ∗ 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
则可以表示为:
[ F x F y F z ] = [ C W R T ⋅ W B R T I ] [ L x L y L z F x 0 F y 0 F z 0 ] \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 ⋅ B f=R \cdot B f=R⋅B，其中 B = [ L x , L y , L z , F x 0 , F y 0 , F z 0 ] T B=[L_x,L_y,L_z,F_{x0},F_{y0},F_{z0}]^{\mathrm{T}} B=[Lx,Ly,Lz,Fx0,Fy0,Fz0]T，则矩阵的最小二乘解为 B = ( R T R ) − 1 ⋅ R T ⋅ f B=(R^\mathrm{T}R)^{-1} \cdot R^\mathrm{T}\cdot f B=(RTR)−1⋅RT⋅f，
由多组数据可以解出力传感器的零点值 ( F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) (Fx0,Fy0,Fz0)，安装倾角 U , V U,V U,V以及重力 g g g的大小。
g = L x 2 + L y 2 + L z 2 g=\sqrt{
{L_x}^2 + {L_y}^2 + {L_z}^2}\\ g=Lx2+Ly2+Lz2
U = a r c s i n ( − L y g ) U = arcsin(\frac{-L_y}{g}) U=arcsin(g−Ly)
V = a r c t a n ( − L x L z ) V = arctan(\frac{-L_x}{L_z}) V=arctan(Lz−Lx)
3.外部接触力计算
由此可以得出外部接触力为：
{ F e x = F x − F x 0 − G x F e y = F y − F y 0 − G y F e z = F z − F z 0 − G z \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
[ F e x F e y F e z ] = [ F x F y F z ] − [ O C R I ] [ 0 0 − g F x 0 F y 0 F z 0 ] \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
又：
{ M x 0 = k 1 − F y 0 × z + F z 0 × y M y 0 = k 2 − F z 0 × x + F x 0 × z M z 0 = k 3 − F x 0 × y + F y 0 × 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×x
C G = O C R ⋅ O G = [ G x , G y , G z ] T ^{C}G=^C_OR \cdot {^{O}G} = [G_x,G_y,G_z]^\mathrm{T} CG=OCR⋅OG=[Gx,Gy,Gz]T
{ M g x = G z × y − G y × z M g y = G x × z − G z × x M g z = G y × x − G x × 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
即:
[ M g x M g y M g z ] = [ 0 − z y z 0 − x − y x 0 ] [ G x G y G z ] = [ 0 − z y z 0 − x − y x 0 ] ⋅ O C R ⋅ [ 0 0 − 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−x0
GxGyGz
=
0z−y−z0xy−x0
⋅OCR⋅
00−g
得出外部接触力矩为：
{ M e x = M x − M x 0 − M g x M e y = M y − M y 0 − M g y M e z = M z − M Z 0 − M g z \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
重力补偿计算
'''
class GravityCompensation:
M = np.empty((0, 0))
F = np.empty((0, 0))
f = np.empty((0, 0))
R = np.empty((0, 0))
x = 0
y = 0
z = 0
k1 = 0
k2 = 0
k3 = 0
U = 0
V = 0
g = 0
F_x0 = 0
F_y0 = 0
F_z0 = 0
M_x0 = 0
M_y0 = 0
M_z0 = 0
F_ex = 0
F_ey = 0
F_ez = 0
M_ex = 0
M_ey = 0
M_ez = 0
def Update_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()
def Update_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]])
def Solve_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)
def Update_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()
def Update_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)))
def Solve_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)
def Solve_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)
def Solve_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.y
M_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.z
M_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.x
Torque_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)
def eulerAngles2rotationMat(self, theta):
theta = [i * math.pi / 180.0 for 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 R
def main():
#
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