7 DOF Inverse kinematics with jacobian matrix in python

Question

I am having problem calculating dtheta. to calculating the dtheta this is a following code according to my lecture

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))

Here is my inverse kinematics code for 7dof in python

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')

Here is the problem I am facing

After solving the equation sol = solve(Jeval * dtheta - dp , dtheta) to get dtheta value. I need 7 dtheta value as a matrix has 7 dtheta dtheta = Matrix([dtheta1,dtheta2,dtheta3,dtheta4,dtheta5,dtheta6,dtheta7]) but I am getting the values for 3 of them, the rest are still in symbols like in following output

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

Looks like your system is redundant. Let's check out the degrees of freedom: you are trying to control 3 degrees of freedom (DOF), the coordinates of your arm tip, using a 7-DOF robot. Therefore, there are 7 - 3 = 4 redundant degrees of freedom that can be used to do something else. Here the solver chose to keep dtheta_{3,5,6} as free parameters (it could have chosen others). If you have preferred values for them, you could set them to e.g. dtheta_6=0.0, dtheta_3=0.0 and dtheta_5=0.0.

A simple example of such system is a human arm: when our task is to put a finger on a designated point, we are still free to move our elbow while keeping the finger there. The arm is redundant for the task, so that we could use it to carry out another supplementary task, e.g. "put the finger here and the elbow there". There is a detailed discussion of this question in the first two sections of this paper.