python中基于jacobian矩阵的7自由度逆运动学

Question

我有计算dtheta的问题。为了计算dtheta，根据我的演讲，下面是一个代码

猜测初始连接θ 求解方程J∗dθ=dp=pf−pi Increment jointanglesθf =θi+dθ，在每一点上计算J的数值，可以使用以下几行: Jsub = J.subs({theta1:theta_i，theta2:theta_i1，theta3:theta_i2，theta4:theta_i3，theta5:theta_i4，theta6:theta_i5})，Jeval = Jsub.evalf()，使用subs()方法代替θ的数值来代替符号，然后用evalf()方法计算expression.Lastly的每个元素的整体值，您需要以这样一种方式来解决这个表达式:您可以找到θ的值，这些值可以“工作”到新的点。您可以使用such ()函数找到这样的值。解(dtheta1，dtheta2，dtheta3，dtheta4，dtheta5，dtheta6)

这是我在python中7 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}

Answer 1

看来你的系统是多余的。让我们看看自由度：你试图控制3自由度(自由度)，你的手臂尖端的坐标，使用7自由度机器人。因此，有7-3=4个多余的自由度，可以用来做其他事情。在这里，求解器选择将dtheta_{3,5,6}保持为空闲参数(它可以选择其他参数)。如果您有它们的首选值，可以将它们设置为例如dtheta_6=0.0、dtheta_3=0.0和dtheta_5=0.0。

这种系统的一个简单的例子是人类的手臂:当我们的任务是将手指放在指定的点上时，我们仍然可以自由地移动手肘，同时将手指放在那里。手臂对于这项任务来说是多余的，所以我们可以用它来执行另一项补充任务，例如“把手指放在这里，肘部在那里”。本论文的前两部分详细讨论了这个问题。