手动实现李群SE(3)到李代数se(3)之间的对指数运算关系

我们可以推导出以下公式来进行李群和李代数之间的对应转换计算。
已知ω⃗\vec\omegaω和v⃗\vec vv，求RRR和ppp，从而获得TTT，对应T=exp(E)T= exp(E)T=exp(E)，其中E=upgrade([ω⃗,v⃗])E=upgrade([\vec \omega, \vec v])E=upgrade([ω,v])：

已知TTT，可以从其中提取出RRR和ppp，进而可以计算出ω⃗\vec \omegaω和v⃗\vec vv，即E=log(T)E=log(T)E=log(T)，[ω⃗,v⃗]=degrade(E)[\vec \omega, \vec v]=degrade(E)[ω,v]=degrade(E)

代码如下：

import numpy as np
from scipy.linalg import expm,logm
from numpy import pi,sin,cos,tan,arccos,matmul
from numpy.linalg import norm
# from robotools import Euler_Rodrigues,SE3tose3np.set_printoptions(precision=3,suppress=True)
deg = pi/180def vec2sM(vec):return np.array([[0,-vec[2],vec[1]],[vec[2],0,-vec[0]],[-vec[1],vec[0],0]])def sM2vec(sM):return np.array([sM[2][1],sM[0][2],sM[1][0]])def upgradeStoE(Screw):'''规定：Screw=(S,S0),单位旋量screw=(s,s0)，Screw=theta*screw规定：omg为三维的单位向量，OMG = theta*omg,把运动旋量Screw∈se(3)，转换（升维）成T的矩阵对数E矩阵#E=Eu*theta，其中theta为转角，Eu为单位螺旋对应的矩阵对数'''theta = norm(Screw[:3])if abs(theta)<1e-5:theta = norm(Screw[-3:])screw   = Screw.reshape((6,1))/thetaomg,vel = screw[:3],screw[-3:]sMomg   = vec2sM(omg)bottom  = np.array([[0,0,0,0]])Eu = np.r_[np.c_[sMomg,vel],bottom]E = Eu.astype(float)*thetareturn Edef degradeEtoS(Ematrix):'''输入：李代数的4×4标准表示E矩阵输出：对应的李代数向量形式Screw'''OMG,VEL = sM2vec(Ematrix[:3,:3]).reshape(3,1),Ematrix[:3,3].reshape(3,1)theta = np.linalg.norm(OMG)if abs(theta)<1e-5:theta = norm(VEL)omg,vel = OMG/theta,VEL/thetascrew = np.vstack((omg,vel))Screw = (screw*theta).reshape(1,6) return Screwdef expEtoT(E):'''输入：李代数的4×4标准表示矩阵EStep1:通过反对称矩阵sMOMG提取出thetaStep2:通过theta获得单位标准表示矩阵EuStep3:使用Euler_Rodrigues公式计算Trans矩阵'''sMOMG = sM2vec(E[:3,:3]) #OMG改为sMOMGV = E[:3,3]  theta = norm(sMOMG)if abs(theta)<=1e-5:theta = norm(V)I = np.eye(3)bottom = np.array([[0,0,0,1]]).reshape((1,4))T = np.vstack((np.hstack((I,V.reshape(3,1))),bottom))return TEu = E/thetaI = np.eye(4)T = I + sin(theta)*Eu + (1-cos(theta))*matmul(Eu,Eu)return Tdef logTtoE(T):'''输入齐次转移矩阵T，得到对应李代数se(3)的4×4标准表示矩阵E思路：Step1:提取旋转矩阵R和位置坐标向量pStep2:通过旋转矩阵的迹计算转角theta和反对称矩阵:此处要注意theta=0时的特殊情况Step3:通过sM2vec函数求omg向量Step4:通过Ginv函数和theta与p计算vStep5：拼接矩阵(sMomg,v)*theta和(0,0,0,1)为E矩阵'''I = np.eye(3)R,p = T[:3,:3],T[:3,3]theta = arccos((np.trace(R)-1)/2)if abs(theta) < 1e-5:omg = np.array([0,0,0]) #这句感觉可以不写theta = norm(p)E = np.vstack((np.hstack((I,p/theta)),np.array([0,0,0,1])))return Eomg = sM2vec((R-R.transpose())/(2*sin(theta)))sMomg = vec2sM(omg)Ginv = 1/theta*I-1/2*sMomg+(1/theta-0.5/tan(theta/2))*np.matmul(sMomg,sMomg)v = matmul(Ginv,p.reshape((3,1)))E = np.vstack((np.hstack((sMomg,v))*theta,np.array([[0,0,0,0]])))return Es = np.array([0,0,0,3.37,-3.37,0])
theta = pi/6
Twist = s*theta
E = upgradeStoE(Twist)
T = expEtoT(E)
# T = expm(E)
# EM= logTtoE(T)
EM = logm(T)
Screw = degradeEtoS(EM)print(f"Twist={Twist}")
print(f"E={E}")
print(f"T={T}")
print(f"EM ={EM}")
print(f"Screw={Screw}")

运行结果如下：

手动实现李群SE(3)到李代数se(3)之间的对指数运算关系相关推荐

Python中李群SO(3)与李代数so(3)之间指数映射与实现源码
Python中李群SO(3)与李代数so(3)之间指数映射与实现源码调用scipy.linalg.expm() 对于李群SE(3).SO(3),和与其对应的李代数se(3),so(3).指数映射是十 ...
使用numpy实现李代数se(3)和SE(3)之间的指对数映射
SE(3)李群和se(3)李代数元素之间存在着指对数的映射关系(矩阵指数.对数运算关系),对于se(3)李代数,存在着向量形式(六维螺旋向量S)和4×4标准矩阵(E矩阵)两种表达,他们之间可以相互转换 ...
小米8 SE和小米9 SE 开源 Android 9 Pie 内核代码
开发四年只会写业务代码,分布式高并发都不会还做程序员? 近日,小米在 GitHub 开源了小米8 SE和小米9 SE的内核源码(https://github.com/MiCode/Xiaomi_K ...
java se和java_Java：改进了Java SE 6和Java SE 7的客户端和桌面部分！
java se和java Java 6和Java 7中的客户端改进了解有关Java SE 6和Java SE 7的客户端和桌面部分的改进,包括新的applet插件,Java Deployment T ...
Java：对Java SE 6和Java SE 7的客户端和桌面部分的改进！
Java 6和Java 7中的客户端改进了解有关Java SE 6和Java SE 7的客户端和桌面部分的改进,包括新的applet插件,Java Deployment Toolkit,成形和半透明 ...
java se 7u67_Java SE 7u72和Java SE 7u71有什么不同
展开全部 Java SE 7u72和32313133353236313431303231363533e59b9ee7ad9431333433643134Java SE 7u71没有什么不同,基本上只是 ...
android的se模式,Android中的SE 模块化LG G5 SE评测
因为苹果的启用,让"SE"这个名号被很多人所知,同时其也被拉到了"小屏"阵营.不过这款Android中的"SE"或许会改变很多人的认知,因为 ...
lg g5 android,【LGG5SE评测】Android中的SE 模块化LG G5 SE评测（全文）_LG G5 SE_手机评测-中关村在线...
1G5 SE和G5外观竟完全一样? [中关村在线]LGG5SE评测:因为苹果的启用,让"SE"这个名号被很多人所知,同时其也被拉到了"小屏"阵营.不过这款And ...
java se包括哪些_Java SE
Java SE 常见问题解答这一组 Java 平台标准版常见问题解答简要回答了有关该技术的常见问题.还包括到本网站中更详细信息的链接. 概述 Oracle Java SE Advanced Desk ...

手动实现李群SE(3)到李代数se(3)之间的对指数运算关系

手动实现李群SE(3)到李代数se(3)之间的对指数运算关系相关推荐

最新文章

热门文章