python中7自由度雅可比矩阵逆运动学

Question

我在计算dtheta时遇到问题。根据我的讲座，要计算dtheta，这是下面的代码

Guess initial jointangles θ
Solve the equation J∗dθ=dp=pf−pi Increment jointangles θf = θi+dθ

calculate the numerical value of J at each point, you can use the following lines: 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()

The method subs() is used to substitute the numerical values of θ for the symbolicones, and then the method evalf() is used to evaluate the overall value for each element of the expression.Lastly,

you need to solve the expression in a manner that you can find values of θthat “work” for reaching the new point. You can find such values using the solve()function.

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值。我需要7dtheta值，因为一个矩阵有7dthetadtheta = 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}