【MR】几何雅可比vs解析雅可比 + python加载Indy7的URDF逆动力学仿真（MR+Pybullet）对比...
Python加载urdf解析为MR参数，Pybullet加载urdf，逆动力学仿真
知识点：
几何雅可比，就是关节空间速度映射到末端的空间速度
解析雅可比，就是不同姿态表示下所求的导数，比如欧拉角表示法，用来表示末端姿态的变化，通过末端欧拉角速度向角速度的转换，需要通过一个雅可比矩阵来完成
解析雅可比与几何雅可比关系的推导：
解析雅可比计算推导
Python加载URDF解析为MR参数：(含解析雅可比计算）
from urdfpy import URDF
import numpy as np
import modern_robotics as mr
def TransformToXYZ(T):
return np.array(T[0:3,3])
def getJoint(robot,link_name):
ret_link=[]
for link in robot.links:
if link.name == link_name:
ret_link=link
break;
return ret_link
def w_p_to_Slist(w,p,ROBOT_DOF):
Slist = []
for i in range(0,ROBOT_DOF):
w_ = w[i];
p_ = p[i];
v_ = -np.cross(w_,p_)
Slist.append([w_[0],w_[1],w_[2],v_[0],v_[1],v_[2]])
return np.transpose(Slist)
#几何雅可比转解析雅可比-后增加
def analyticaljacobian(M,Blist, thetalist):
Jb = mr.JacobianBody(Blist, thetalist)
Tsb = mr.FKinBody(M,Blist, thetalist)
Ajacobian = np.vstack(
(np.hstack((np.identity(3), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), np.linalg.inv(Tsb[0:3,0:3]))))
).dot(Jb)
return Ajacobian
#原版本
def AnalyticJacobianBody(M,Blist, thetalist):
"""Computes the Analytic Jacobian for an open chain robot
:param Blist: The joint screw axes in the end-effector frame when the
manipulator is at the home position, in the format of a
matrix with axes as the columns
:param thetalist: A list of joint coordinates
:return: The body Jacobian corresponding to the inputs (6xn real
numbers)
Example Input:
Blist = np.array([[0, 0, 1, 0, 0.2, 0.2],
[1, 0, 0, 2, 0, 3],
[0, 1, 0, 0, 2, 1],
[1, 0, 0, 0.2, 0.3, 0.4]]).T
thetalist = np.array([0.2, 1.1, 0.1, 1.2])
Output:
np.array([[-0.04528405, 0.99500417, 0, 1]
[ 0.74359313, 0.09304865, 0.36235775, 0]
[-0.66709716, 0.03617541, -0.93203909, 0]
[ 2.32586047, 1.66809, 0.56410831, 0.2]
[-1.44321167, 2.94561275, 1.43306521, 0.3]
[-2.06639565, 1.82881722, -1.58868628, 0.4]])
"""
Jb = mr.JacobianBody(Blist, thetalist)
Tsb = mr.FKinBody(M,Blist, thetalist)
Rsb= Tsb[0:3,0:3]
#r = mr.so3ToVec(Rsb) #SO3 这里有问题 应为 ？mr.so3ToVec(MatrixLog3(Rsb))
#norm_r = np.sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2])
#omega_r = mr.VecToso3(r) #转轴矢量的so3
#A = np.eye(3) - (1-np.cos(norm_r))/(norm_r*norm_r) * omega_r+ (norm_r - np.sin(norm_r)) / (norm_r**3) * omega_r @ omega_r
A_ = np.eye(6)
A_[0:3,0:3] = Rsb
A_[3:6,3:6] = Rsb
Ja = A_ @ Jb
return Ja
def loadURDF(urdf_name):
print(urdf_name)
robot = URDF.load(urdf_name)
LinkNum = len(robot.links)
JointNum = len(robot.actuated_joints)
p_ = []
w_= []
M_list = [np.eye(4)]
Glist =[]
com_p_=[]
for joint in robot.actuated_joints:
child_link = getJoint(robot,joint.child)
p_.append(TransformToXYZ(robot.link_fk()[child_link]))
G = np.eye(6)
G[0:3,0:3] = child_link.inertial.inertia
G[3:6,3:6] = child_link.inertial.mass*np.eye(3)
Glist.append(G)
child_M = robot.link_fk()[child_link]
child_M_R, child_M_p =mr.TransToRp(child_M)
child_w = np.array(child_M_R @ np.array(joint.axis).T)
w_.append( child_w )
child_M[0:3,0:3] = np.eye(3)
CoM_M = child_M@ child_link.inertial.origin
M_list.append(CoM_M)
eef_link = getJoint(robot,robot.end_links[0].name)#获取末端连杆的关节
M = robot.link_fk()[eef_link]
M_list.append(M)
Slist = w_p_to_Slist(w_,p_,JointNum)
Blist = mr.Adjoint(mr.TransInv(M))@ Slist
Mlist = []
for i in range(1,len(M_list)):
M_i_1 = M_list[i-1]
M_i = M_list[i]
Mlist.append(mr.TransInv(M_i_1) @ M_i)
Mlist = np.array(Mlist)
Glist = np.array(Glist)
return {"M":M,"Slist" : Slist,"Blist": Blist,"Mlist":Mlist,"Glist":Glist,"actuated_joints_num":len(robot.actuated_joints)}indy7_sim.p（Pybullet与MR逆动力学仿真对比）
import pybullet as p
import time
import pybullet_data
import numpy as np
np.set_printoptions(precision=6, suppress=True)
import math
from modern_robotics import *
import modern_robotics as mr
import numpy as np
from mr_urdf_loader import loadURDF
from mr_urdf_loader import *
def pos_orn_to_T(pos,orn):
T = np.eye(4)
T[0:3,3] = np.array(pos).T
T[0:3,0:3] = np.reshape(p.getMatrixFromQuaternion(orn),(3,3))
return T
#几何雅可比转解析雅可比 与
def analyticaljacobian(M,Blist, thetalist):
Jb = mr.JacobianBody(Blist, thetalist)
Tsb = mr.FKinBody(M,Blist, thetalist)
Ajacobian = np.vstack(
(np.hstack((np.identity(3), np.zeros((3, 3)))),
np.hstack((np.zeros((3, 3)), np.linalg.inv(Tsb[0:3,0:3]))))
).dot(Jb)
return Ajacobian
urdf_name = "V:/learn/MR/download/mr_urdf_loader-main/example/indy7/indy7/indy7.urdf"
## Modern Robotics setup
MR=loadURDF(urdf_name)
M = MR["M"]
Slist = MR["Slist"]
Mlist = MR["Mlist"]
Glist = MR["Glist"]
Blist = MR["Blist"]
actuated_joints_num = MR["actuated_joints_num"]
## pybullet setup
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
robotID = p.loadURDF(urdf_name, [0, 0, 0],[0, 0, 0, 1],useFixedBase=1)
numJoints = p.getNumJoints(robotID)
p.resetBasePositionAndOrientation(robotID, [0, 0, 0], [0, 0, 0, 1])
for i in range(0,numJoints):
p.setJointMotorControl2(robotID, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
#p.resetJointState(robotID,2,1) 无明显变化
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 1/240.0;
p.setTimeStep(timeStep)
q = [0,0,0,0,0,0] #q = [0,0,0,0,0,0]
q_dot = [0,0,0,0,0,0]#[0,0,0,0,0,0]
q_ddot = [0,1,0,0,0,0]
g = np.array([0,0,-9.8])
Ftip = [0,0,0,0,0,0]
print(Glist[0][0:3,0:3])
print(Glist[1][0:3,0:3])
print(Glist[2][0:3,0:3])
print(Glist[3][0:3,0:3])
''' print(Glist[4][0:3,0:3])
print(Glist[5][0:3,0:3]) '''
print(p.getDynamicsInfo(robotID, 1)[2]) #主惯性矩
print(p.getDynamicsInfo(robotID, 2)[2])
print(p.getDynamicsInfo(robotID, 3)[2])
print(p.getDynamicsInfo(robotID, 4)[2])
while p.isConnected():
# get Joint Staes
jointStates = np.array(p.getJointStates(robotID,[1,2,3,4,5,6]),dtype=object)
# get Link Staes
linkState = np.array(p.getLinkState(robotID,7,1,1),dtype=object)
q = np.array(jointStates[:,0])
q_dot = np.array(jointStates[:,1])
# Pybullet Forward Kinematics
pb_Tsb = pos_orn_to_T(linkState[0],linkState[1])
# Modern Robotics Forward Kinematics
mr_Tsb = mr.FKinSpace(M,Slist,q)
mr_Tsb = mr.FKinBody(M,Blist,q)
# Pybullet Jacobian
#pb_J = p.calculateJacobian(robotID,4,[0,0,0],[q[0],q[1],q[2]],[q_dot[0],q_dot[1],q_dot[2]],[0,0,0])
# Modern Robotics Jacobian
mr_Jb= JacobianBody(Blist, q)
mr_Js= JacobianSpace(Slist, q)
#mr_ja mr_ja2 结果一样
mr_Ja = AnalyticJacobianBody(M,Blist, q) # pb_j = mr_Ja
mr_Ja2=analyticaljacobian(M,Blist, q)
#print("=============mr_Ja=============")
#print(mr_Ja)
#print("=============mr_Ja2=============")
#print(mr_Ja2)
#pybullet InverseDynamics
print(q)
pb_ID= np.array(p.calculateInverseDynamics(robotID,[q[0],q[1],q[2],q[3],q[4],q[5] ],[q_dot[0],q_dot[1],q_dot[2],q_dot[3],q_dot[4],q_dot[5] ] ,[0,0,0,0,0,0] ))
#modern_robotics InverseDynamics
mr_ID =mr.InverseDynamics(q, q_dot, q_ddot, g, Ftip, Mlist, Glist, Slist)
print("=============pb_Tsb=============")
print(pb_Tsb)
print("=============mr_Tsb=============")
print(mr_Tsb)
print("=============pb_ID=============")
print(pb_ID)
print("=============mr_ID=============")
print(mr_ID)
#set torques
for i in range(0,actuated_joints_num):
p.setJointMotorControl2(robotID, i+1, p.TORQUE_CONTROL,force=mr_ID[i])
#p.setJointMotorControl2(robotID, i+1, p.TORQUE_CONTROL,force=pb_ID[i])
p.stepSimulation()
time.sleep(timeStep)indy7_urdf_loader.py测试加载urdf解析为MR参数程序
import numpy as np
np.set_printoptions(precision=6, suppress=True)
from mr_urdf_loader import loadURDF
from modern_robotics import *
urdf_name = "V:/learn/MR/download/mr_urdf_loader-main/example/indy7/indy7/indy7.urdf"
MR=loadURDF(urdf_name)
M = np.array(MR["M"])
Slist = np.array(MR["Slist"])
Mlist = np.array(MR["Mlist"])
Glist = np.array(MR["Glist"])
Blist = np.array(MR["Blist"])
actuated_joints_num = MR["actuated_joints_num"]
thetalist = np.array([0,0,np.pi/2.0])
dthetalist = np.array([0,0,0.1])
ddthetalist = np.array([0,0,0])
g = np.array([0,0,-9.8])
Ftip = [0,0,0,0,0,0]
#print(M)
print(Mlist)
print(Glist)
print("FKinSpace\n", FKinSpace(M,Slist,thetalist))
print("FKinBody\n", FKinBody(M,Blist,thetalist))
print("JacobianSpace\n", JacobianSpace(Slist,thetalist))
print("JacobianBody\n", JacobianBody(Blist,thetalist))
print("InverseDynamics\n" ,InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, Glist, Slist))测试加载URDF解析为MR参数程序的输出：
PS V:\learn\MR\download\mr_urdf_loader-main> v:; cd 'v:\learn\MR\download\mr_urdf_loader-main'; & 'C:\Users\cxy\AppData\Local\Programs\Python\Python39\python.exe' 'c:\Users\cxy\.vscode\extensions\ms-python.python-2022.16.1\pythonFiles\lib\python\debugpy\adapter/../..\debugpy\launcher' '63759' '--' 'v:\learn\MR\download\mr_urdf_loader-main\example\indy7\indy7_urdf_loader.py'
V:/learn/MR/download/mr_urdf_loader-main/example/indy7/indy7/indy7.urdf
[[[ 1. 0. 0. 0. ]
[ 0. 1. 0. 0. ]
[ 0. 0. 1. 0.0775]
[ 0. 0. 0. 1. ]]
[[ 1. 0. 0. 0. ]
[ 0. 1. 0. -0.109 ]
[ 0. 0. 1. 0.222 ]
[ 0. 0. 0. 1. ]]
[[ 1. 0. 0. -0. ]
[ 0. 1. 0. 0.0305]
[ 0. 0. 1. 0.45 ]
[ 0. 0. 0. 1. ]]
[[ 1. 0. 0. -0. ]
[ 0. 1. 0. 0.075 ]
[ 0. 0. 1. 0.267 ]
[ 0. 0. 0. 1. ]]
[[ 1. 0. 0. 0. ]
[ 0. 1. 0. -0.114 ]
[ 0. 0. 1. 0.083 ]
[ 0. 0. 0. 1. ]]
[[ 1. 0. 0. -0. ]
[ 0. 1. 0. -0.069 ]
[ 0. 0. 1. 0.168 ]
[ 0. 0. 0. 1. ]]
[[ 1. -0. 0. 0. ]
[ 0. 1. -0. -0. ]
[ 0. 0. 1. 0.06 ]
[ 0. 0. 0. 1. ]]]
[[[ 0.154186 -0.000002 0.000017 0. 0. 0. ]
[-0.000002 0.12937 -0.048543 0. 0. 0. ]
[ 0.000017 -0.048543 0.059644 0. 0. 0. ]
[ 0. 0. 0. 11.80301 0. 0. ]
[ 0. 0. 0. 0. 11.80301 0. ]
[ 0. 0. 0. 0. 0. 11.80301 ]]
[[ 0.29357 -0. 0.000014 0. 0. 0. ]
[-0. 0.280941 0.03728 0. 0. 0. ]
[ 0.000014 0.03728 0.036206 0. 0. 0. ]
[ 0. 0. 0. 7.992921 0. 0. ]
[ 0. 0. 0. 0. 7.992921 0. ]
[ 0. 0. 0. 0. 0. 7.992921]]
[[ 0.034246 0.000001 0.000007 0. 0. 0. ]
[ 0.000001 0.03406 0.00186 0. 0. 0. ]
[ 0.000007 0.00186 0.004505 0. 0. 0. ]
[ 0. 0. 0. 2.991341 0. 0. ]
[ 0. 0. 0. 0. 2.991341 0. ]
[ 0. 0. 0. 0. 0. 2.991341]]
[[ 0.006704 0.000004 0.000002 0. 0. 0. ]
[ 0.000004 0.002792 -0.00128 0. 0. 0. ]
[ 0.000002 -0.00128 0.006193 0. 0. 0. ]
[ 0. 0. 0. 2.12317 0. 0. ]
[ 0. 0. 0. 0. 2.12317 0. ]
[ 0. 0. 0. 0. 0. 2.12317 ]]
[[ 0.009949 0. 0.000003 0. 0. 0. ]
[ 0. 0.009782 -0.000935 0. 0. 0. ]
[ 0.000003 -0.000935 0.002715 0. 0. 0. ]
[ 0. 0. 0. 2.288651 0. 0. ]
[ 0. 0. 0. 0. 2.288651 0. ]
[ 0. 0. 0. 0. 0. 2.288651]]
[[ 0.000435 0. -0. 0. 0. 0. ]
[ 0. 0.000445 0.000001 0. 0. 0. ]
[-0. 0.000001 0.000596 0. 0. 0. ]
[ 0. 0. 0. 0.400839 0. 0. ]
[ 0. 0. 0. 0. 0.400839 0. ]
[ 0. 0. 0. 0. 0. 0.400839]]]
FKinSpace
[[ 0. -0. -1. -0.578 ]
[ 0. 1. -0. -0.1865]
[ 1. -0. 0. 0.7495]
[ 0. 0. 0. 1. ]]
FKinBody
[[ 0. -0. -1. -0.578 ]
[ 0. 1. -0. -0.1865]
[ 1. -0. 0. 0.7495]
[ 0. 0. 0. 1. ]]
JacobianSpace
[[ 0. 0. 0. 0. 0. 0. ]
[ 0. -1. -1. -0. -1. -0. ]
[ 1. 0. 0. 1. -0. 1. ]
[-0. 0.2995 0.7495 -0.0035 1.0995 -0.1865]
[-0. 0. 0. 0. 0. 0. ]
[-0. 0. 0. 0. 0. 0. ]]
JacobianBody
[[ 1. -0. -0. 0. -0. 0. ]
[-0. -1. -1. 0. -1. 0. ]
[ 0. 0. 0. 1. 0. 1. ]
[-0. -0.578 -0.578 0.183 -0.228 0. ]
[-0.578 0. 0. -0. -0. -0. ]
[-0.1865 0.45 -0. 0. -0. -0. ]]
InverseDynamics
[ 0.000019 -0. 0. ]