我在计算 dtheta 时遇到问题。根据我的讲座，计算 dtheta 这是以下代码
猜测初始关节角 θ
求解方程 J∗dθ=dp=pf−pi 增量关节角 θf = θi+dθ
计算 J 在每个点的数值，可以使用以下行： Jsub = J.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3 ], theta5:theta_i[4], theta6:theta_i[5]}) Jeval = Jsub.evalf()
方法 subs() 用于将 θ 的数值替换为符号，然后方法 evalf() 用于评估表达式中每个元素的整体值。最后，
您需要以一种可以找到“起作用”以达到新点的 θ 值的方式求解表达式。您可以使用 solve() 函数找到这些值。
解决（Jeval * dtheta-p_delta，（dtheta1，dtheta2，dtheta3，dtheta4，dtheta5，dtheta6））
这是我在 python 中 7dof 的逆运动学代码
import sim
import time
import cv2
import numpy as np
import numpy
import matplotlib.pyplot
from mpl_toolkits.mplot3d import Axes3D
from sympy import Matrix, Symbol, symbols, solveset,solve, simplify, diff, det
from sympy import S, erf, log, sqrt, pi, sin, cos, tan
from sympy import init_printing
def T(x, y, z):
T_xyz = Matrix([[1, 0, 0, x],
[0, 1, 0, y],
[0, 0, 1, z],
[0, 0, 0, 1]])
return T_xyz
def Rx(roll):
R_x = Matrix([[1, 0, 0, 0],
[0, cos(roll), -sin(roll), 0],
[0, sin(roll), cos(roll), 0],
[0, 0, 0, 1]])
return R_x
def Ry(pitch):
R_y = Matrix([[ cos(pitch), 0, sin(pitch), 0],
[ 0, 1, 0, 0],
[-sin(pitch), 0, cos(pitch), 0],
[ 0, 0, 0, 1]])
return R_y
def Rz(yaw):
R_z = Matrix([[cos(yaw),-sin(yaw), 0, 0],
[sin(yaw), cos(yaw), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
return R_z
#Program Variables
joint1Angle = 0.0
joint2Angle = 0.0
joint3Angle = 0.0
joint4Angle = 0.0
joint5Angle = 0.0
joint6Angle = 0.0
joint7Angle = 0.0
gripper_finger_pos = 0.0
#Start Program and just in case, close all opened connections
print('Program started')
sim.simxFinish(-1)
#Connect to simulator running on localhost
#V-REP runs on port 19997, a script opens the API on port 19999
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
thistime = time.time()
#Connect to the simulation
if clientID != -1:
print('Connected to remote API server')
#Start main control loop
print('Starting control loop')
p0 = 0.064
p1 = 0.13986
p2 = 0.16057
p3 = 0.19469
p4 = 0.090084
p5 = 0.233536
p6 = 0.0606
#theta1 = joint1Angle-pi/2
#theta2 = -joint2Angle
#theta3 = joint3Angle
#theta4 = -joint4Angle + pi/2
#theta5 = joint5Angle
#theta6 = -joint6Angle-pi/2
#theta7 = joint7Angle
#joint angles as SymPy symbols
theta1,theta2,theta3,theta4,theta5,theta6,theta7 = symbols('theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 theta_7')
theta = Matrix([theta1,theta2,theta3,theta4,theta5,theta6,theta7])
#joint angles as SymPy symbols
dtheta1,dtheta2,dtheta3,dtheta4,dtheta5,dtheta6,dtheta7 = symbols('dtheta_1 dtheta_2 dtheta_3 dtheta_4 dtheta_5 dtheta_6 dtheta_7')
dtheta = Matrix([dtheta1,dtheta2,dtheta3,dtheta4,dtheta5,dtheta6,dtheta7])
# Not necessary but gives nice-looking latex output
init_printing()
gripper_offset = 0.17000
# Define transforms to each joint
T1 = Ry(-pi/2) * T(p0, 0, 0) * Rx(theta1)
T2 = T1 * T(p1, 0, 0) * Rz(theta2)
T3 = T2 * T(p2, 0, 0) * Rx(theta3)
T4 = T3 * T(p3, 0, 0) * Rz(theta4)
T5 = T4 * T(p4, 0, 0) * Rx(theta5)
T6 = T5 * T(p5, 0, 0) * Rz(theta6)
T7 = T6 * T(p6, 0, 0) * Rx(theta7) * T(gripper_offset, 0, 0)
# Find joint positions in space
p0 = Matrix([0,0,0,1])
p1 = T1 * p0
p2 = T2 * p0
p3 = T3 * p0
p4 = T4 * p0
p5 = T5 * p0
p6 = T6 * p0
p7 = T7 * p0
#print('p7=',p7)
#define p_i for initial
p_i = Matrix([p7[0], p7[1], p7[2]]) # coordinates of arm tip
#creating jacobian matrix
J = p_i.jacobian(theta)
#print('\n\nJ=', J)
dp = Matrix([0.0,0.0,20.0])
p_f = p_i + dp
theta_i = Matrix([-pi/2,0,0,pi/2,0,-pi/2,0])
#print(theta_i)
theta_f = Matrix([0,0,0,0,0,0,0])
while (sim.simxGetConnectionId(clientID) != -1):
#To calculate the numerical value of J at each point
Jsub = J.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
#o evaluate the overall value for each element of the expression.
Jeval = Jsub.evalf()
print('\n\n\nJeval=',Jeval)
#to solve and find values of θ that “work” for reaching the new point.
sol = solve(Jeval * dtheta - dp , dtheta)
theta_f = theta_i + dtheta
print('\nsol=',sol)
print(dtheta)
#print('theta_i=',theta_i)
p0sub = p0.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p1sub = p1.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p2sub = p2.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p3sub = p3.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p4sub = p4.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p5sub = p5.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p6sub = p6.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
p7sub = p7.subs({theta1:theta_i[0], theta2:theta_i[1], theta3:theta_i[2], theta4:theta_i[3], theta5:theta_i[4], theta6:theta_i[5],theta7:theta_i[6]})
theta_i = theta_f
p_i = p_f
print('\n\np7sub=',p7sub)
#End simulation
sim.simxFinish(clientID)
else:
print('Could not connect to remote API server')
#Close all simulation elements
sim.simxFinish(clientID)
cv2.destroyAllWindows()
print('Simulation ended')
这是我面临的问题
解方程sol = solve(Jeval * dtheta - dp , dtheta)后得到 dtheta 值。我需要 7 个 dtheta 值，因为矩阵有 7 个 dtheta dtheta = Matrix([dtheta1,dtheta2,dtheta3,dtheta4,dtheta5,dtheta6,dtheta7])，但我得到了其中 3 个的值，其余的仍然是符号，如以下输出
sol= {dtheta_2: 0.0546786247729014 - 0.649102066092439*dtheta_6, dtheta_4: 0.649102066092439*dtheta_6 - 0.0608587125301476, dtheta_1: -dtheta_3 + 0.712564118410481*dtheta_5}