Gravity compensation principle
1. Force sensor data analysis
Record the zero point values of the three force components and torque components of the six-dimensional force sensor as (F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0})(Fx 0,Fy 0,Fz 0)， ( M x 0 , M y 0 , M z 0 ) (M_{x0},M_{y0},M_{z0}) (Mx 0,My 0,Mz 0)，
The gravity of the sensor and end tooling is GGG , the coordinates of the center of mass in the six-dimensional force sensor coordinate system are(x, y, z) (x, y, z)(x,y,z)，
Gravity GGThe component forces and moments of G in the three coordinate axes are(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)。
According to the relationship between force and moment, it can be obtained (the positive direction is determined by the right-hand rule):
{ M gx = G z × y − G y × z M gy = G x × z − G z × x M gz = 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
The force components and torque components in the three directions directly measured by the six-dimensional force sensor are recorded as (F x, F y, F z) (F_x, F_y, F_z) respectively.(Fx,Fy,Fz)， ( M x , M y , M z ) (M_x,M_y,M_z) (Mx,My,Mz)，
Assuming that there is no external force on the end holder during calibration, the force and moment measured by the force sensor are composed of the load gravity and the zero point, then {
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+Fx 0Fy=Gy+Fy 0Fz=Gz+Fz 0
{ 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+Mx 0My=Mgy+My 0Mz=Mgz+Mz 0
联立得：
{ 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−Fz 0)×y−(Fy−Fy 0)×z+Mx 0My=(Fx−Fx 0)×z−(Fz−Fz 0)×x+My 0Mz=(Fy−Fy 0)×x−(Fx−Fx 0)×y+Mz 0
{ 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+Mx 0+Fy 0×z−Fz 0×yMy=Fx×z−Fz×x+My 0+Fz 0×x−Fx 0×zMz=Fy×x−Fx×y+Mz 0+Fx 0×y−Fy 0×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 Fx 0,Fy 0,Fz 0,Mx 0,My 0,Mz 0,x,y,z is a fixed value constant (ignoring the fluctuation of force sensor data, and the end tooling is not replaced)
2. Solving parameters using least squares method
2.1 Moment equation
令
{ 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=Mx 0+Fy 0×z−Fz 0×yk2=My 0+Fz 0×x−Fx 0×zk3=Mz 0+Fx 0×y−Fy 0×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
xyzk 1k2 _k 3
Control the posture of the gripper at the end of the robotic arm and take N different postures (N ≥ \geq≥ 3, it is required that the pointing vectors of the end gripper of the robot arm in at least three postures are not coplanar), and N sets of six-dimensional force data sensor data are obtained,
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 , then the least squares solution of the matrix isA = ( FTF ) − 1 ⋅ FT ⋅ MA=(F^\mathrm{T}F)^{-1}\cdot F^{\mathrm{T}}\cdot MA=(FTF)−1⋅FT⋅M，
From multiple sets of data, the coordinates (x, y, z) (x, y, z) of the load center of mass in the six-dimensional force sensor coordinate system can be solved(x,y,z ) and constants( k 1 , k 2 , k 3 ) (k_1,k_2,k_3)(k1,k2,k3) value.
2.2 Force equation
During the installation and fixation process of the robot, since the installation platform will have a certain slope, the base coordinate system of the robot and the world coordinate system will have a certain angle, which will also cause a certain deviation between the measured value of the sensor and the actual value. Therefore, Calculation of inclination angle is required.
Let the world coordinate system be O {O}O , the base coordinate system isBBB , the flange coordinate system isWWW , the force sensor coordinate system isC {C}C , then the attitude transformation matrix from the base coordinate system to the world coordinate system is:
BOR = [ 1 0 0 0 cos U sin U 0 sin U cos U ] [ cos V 0 sin V 0 1 0 − sin V 0 cos 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=
1000cos Us in U0s in the Ucos U
basket V0− s inV010s inV0basket V
Among them UUU is around the world coordinate systemxxRotation angle of x- axis, VVV is yyaround the base coordinate systemrotation angle of y- axis
The attitude transformation matrix from the end flange to the base coordinate system is:
WBR = 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 The value of A BC can be read through the robot teaching pendant
Because when the force sensor is installed at the end, there will be ZZA certain deflection angle α \alphaon the Z axisα (it can be manually coincident by changing the robot reference coordinate system, that is, the deflection angle is 0). The posture transformation matrix from the force sensor to the flange coordinate system is: CWR = [ cos α − sin α 0 sin α cos
α 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α _s in α0− s in αcosα _0001
Then the attitude transformation matrix from the world coordinate system to the six-dimensional force sensor coordinate system is
0 CR = 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
gravity in world coordinate systemO {O}The expression in O is OG = [ 0 , 0 , − g ] T {^{O}G=[0,0,-g]^\mathrm{T}}And G=[0,0,−g]T,
Through coordinate transformation, the gravity is decomposed into the force sensor coordinate system 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⋅And G=WCR⋅BWR⋅OBR⋅And G=CWRT⋅WBRT⋅BORT⋅And G=CWRT⋅WBRT⋅
cosUsinV∗g- s in U∗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+[Fx 0,Fy 0,Fz 0]T=OCR⋅And G+[Fx 0,Fy 0,Fz 0]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- s in U∗g−cosUcosV∗gFx 0Fy 0Fz 0
令：
{ 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=- s in U∗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]
LxLyLzFx 0Fy 0Fz 0
In the same way, N sets of six-dimensional force sensor data are obtained by taking N different postures.
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,Fx 0,Fy 0,Fz 0]T , then the least squares solution of the matrix isB = ( RTR ) − 1 ⋅ RT ⋅ f B=(R^\mathrm{T}R)^{-1} \cdot R^\mathrm{T}\cdot fB=(RTR)−1⋅RT⋅f，
The zero point value of the force sensor (F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) can be solved from multiple sets of data(Fx 0,Fy 0,Fz 0) , installation inclination angleU, VU,VU,V andgravityggg size.
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=a rc t an (Lz−Lx)
3. Calculation of external contact force
It can be concluded that the external contact force is:
{ F ex = F x − F x 0 − G x F ey = F y − F y 0 − G y F ez = 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−Fx 0−GxFey=Fy−Fy 0−GyFno=Fz−Fz 0−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}
FexFeyFno
=
FxFyFz
−[OCRI]
00−gFx 0Fy 0Fz 0
又：
{ 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. ⎩
⎨
⎧Mx 0=k1−Fy 0×z+Fz 0×yMy 0=k2−Fz 0×x+Fx 0×zMz 0=k3−Fx 0×y+Fy 0×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⋅And G=[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
The external contact moment is:
{ M ex = M x − M x 0 − M gx M ey = M y − M y 0 − M gy M ez = M z − MZ 0 − M gz \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−Mx 0−MgxMey=My−My 0−MgyMno=Mz−MZ 0−Mgz
[1] Gao Peiyang. Research on constant force grinding and polishing system of industrial robots based on force sensors [D]. Huazhong University of Science and Technology, 2019. [2] Zhang Lijian, Hu Ruiqin, Yi Wangmin. Force on the
end load of industrial robots based on six-dimensional force sensors Perception Research[J]. Journal of Automation, 2017, 43(03):439-447.DOI:10.16383/j.aas.2017.c150753.
[3]Dong Hao. Research on compliance control algorithm of shaft hole assembly industrial robot[D] .Henan University of Science and Technology, 2018.
code show as below:
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 Min Mizuki