0
$\begingroup$

I asked a question similar to this earlier, but I believe I have a new problem. I've been working on figuring out the inverse kinematics given an x,y,z coordinate. I've adopted the Jacobian method, taking the derivative of the forward kinematics equations with respect to their angles and input it into the Jacobian. I then take the inverse of it and multiply it by a step towards the goal distance. For more details, look at http://www.seas.upenn.edu/~meam520/notes02/IntroRobotKinematics5.pdf page 21 onwards.

For a better picture, below is something: 3 DOF Arm

Below is the code for my MATLAB script, which runs flawlessly and gives a solution in under 2 seconds:

ycurrent = 0; %Not using this  
xcurrent = 0; %Starting position (x)   
zcurrent = 0; %Starting position (y)    
xGoal = .5; %Goal x/z values of (1, 1)   
zGoal = .5;    
theta1 = 0.1; %Angle of first DOF    
theta2 = 0.1; %Angle of second DOF  
theta3 = 0.1; %Angle of third DOF
xchange = xcurrent - xGoal %Current distance from goal
zchange = zcurrent - zGoal
%Length of segment 1: 0.37, segment 2:0.374, segment 3:0.2295 
while ((xchange > .02 || xchange < -.02) || (zchange < -.02 || zchange > .02))    
        in1 = 0.370*cos(theta1); %These equations are stated in the link provided
        in2 = 0.374*cos(theta1+theta2);
        in3 = 0.2295*cos(theta1+theta2+theta3);
        in4 = -0.370*sin(theta1);
        in5 = -0.374*sin(theta1+theta2); 
        in6 = -0.2295*sin(theta1+theta2+theta3); 
        jacob = [in1+in2+in3, in2+in3, in3; in4+in5+in6, in5+in6, in6; 1,1,1];
        invJacob = inv(jacob); 
        xcurrent = .3708 * sin(theta1) + .374 * sin(theta1+theta2) + .229 * sin(theta1+theta2+theta3) 
        zcurrent = .3708 * cos(theta1) + .374 * cos(theta1+theta2) + .229 * cos(theta1+theta2+theta3)        
        xIncrement = (xGoal - xcurrent)/100; 
        zIncrement = (zGoal - zcurrent)/100; 
        increMatrix = [xcurrent; zcurrent; 1]; %dx/dz/phi 
        change = invJacob * increMatrix; %dtheta1/dtheta2/dtheta3  
        theta1 = theta1 + change(1)  
        theta2 = theta2 + change(2)  
        theta3 = theta3 + change(3)
        xcurrent = .3708 * sin(theta1) + .374 * sin(theta1+theta2) + .229 * sin(theta1+theta2+theta3)  
        zcurrent = .3708 * cos(theta1) + .374 * cos(theta1+theta2) + .229 * cos(theta1+theta2+theta3)          
        xchange = xcurrent - xGoal
        zchange = zcurrent - zGoal
end        

Below is my Python code, which goes into an infinite loop and gives no results. I've looked over the differences between it and the MATLAB code, and they look the exact same to me. I have no clue what is wrong. I would be forever grateful if somebody could take a look and point it out.

def sendArm(xGoal, yGoal, zGoal, right, lj):
    ycurrent = xcurrent = zcurrent = 0
    theta1 = 0.1
    theta2 = 0.1
    theta3 = 0.1
    xcurrent, zcurrent = forwardKinematics(theta1, theta2, theta3)
    xchange = xcurrent - xGoal
    zchange = zcurrent - zGoal
    while ((xchange > 0.05 or xchange < -0.05) or (zchange < -0.05 or zchange > 0.05)):
        in1 = 0.370*math.cos(theta1) #Equations in1-6 are in the pdf I linked to you (inv kinematics section)
        in2 = 0.374*math.cos(theta1+theta2)
        in3 = 0.2295*math.cos(theta1+theta2+theta3)
        in4 = -0.370*math.sin(theta1)
        in5 = -0.374*math.sin(theta1+theta2)
        in6 = -0.2295*math.sin(theta1+theta2+theta3)
        jacob = matrix([[in1+in2+in3,in2+in3,in3],[in4+in5+in6,in5+in6,in6], [1,1,1]]) #Jacobian
        invJacob = inv(jacob) #inverse of jacobian
        xcurrent, zcurrent = forwardKinematics(theta1, theta2, theta3)
        xIncrement = (xGoal - xcurrent)/100 #dx increment
        zIncrement = (zGoal - zcurrent)/100 #dz increment
        increMatrix = matrix([[xIncrement], [zIncrement], [1]])
        change = invJacob*increMatrix #multiplying both matrixes
        theta1 = theta1 + change.item(0)
        theta2 = theta2 + change.item(1)
        theta3 = theta3 + change.item(2)
        xcurrent, zcurrent = forwardKinematics(theta1, theta2, theta3)
        xchange = xcurrent - xGoal
        zchange = zcurrent - zGoal
        print "Xchange: %f ZChange: %f" % (xchange, zchange)
    print "Goals %f %f %f" % (theta1, theta2, theta3)
    right.set_joint_positions(theta1) #First pitch joint
    right.set_joint_positions(theta2) #Second pitch
    right.set_joint_positions(theta3) #Third Pitch joint


def forwardKinematics(theta1, theta2, theta3):
    xcurrent = .3708 * math.sin(theta1) + .374 * math.sin(theta1+theta2) + .229 * math.sin(theta1+theta2+theta3)
    zcurrent = .3708 * math.cos(theta1) + .374 * math.cos(theta1+theta2) + .229 * math.cos(theta1+theta2+theta3)            
    return xcurrent, zcurrent
$\endgroup$
  • $\begingroup$ Does matrix multiplication work in python the same way as in Matlab? change = invJacob*increMatrix #multiplying both matrixes $\endgroup$ – 50k4 Nov 10 '15 at 10:13
  • $\begingroup$ is operator priority working in Matlab and in python the same way? xcurrent = .3708 * math.sin(theta1) + .374 * math.sin(theta1+theta2) $\endgroup$ – 50k4 Nov 10 '15 at 10:57
  • $\begingroup$ By "gives no results" are you saying the 'print' statement is never evaluated? If not, then what is the output for xchange and zchange? $\endgroup$ – Brian Lynch Nov 10 '15 at 11:17
  • $\begingroup$ The Python code prints the xchange and zchange correctly but doesn't exit the while loop. It goes on forever. And thank you 50k4 I will verify them both $\endgroup$ – Iche Nov 10 '15 at 17:07
  • $\begingroup$ You could also try adding mre parantheses to your while cndition. I do not knowmoperator precedence in pyton... $\endgroup$ – 50k4 Nov 10 '15 at 23:31
1
$\begingroup$

I found out what was wrong. The pdf I linked (as well as many others) state that you need to multiply your inverse jacobian matrix by a step x/y/z distance, but that results in an infinite loop. Multiplying the inverse jacobian matrix by the overall change in distance from x/zcurrent to the goals made the python code work smoothly. I'll keep this question up in case anybody wants to see the code.

$\endgroup$
  • $\begingroup$ Could you explain the difference between step and overall change a bit more clearly? $\endgroup$ – 50k4 Nov 11 '15 at 10:39
  • $\begingroup$ Sure. When I finished calculating the inverse Jacobian, I needed to multiply it by a certain distance (x,y,z) toward the goal in order to arrive at theta values. What the link I provided implies is that it is required to multiply the inverse Jacobian by a very small change, (dx, dy, dz). When I did that (denoted by: xIncrement = (xGoal-xCurrent)/100, same for zIncrement), it just lead to an infinite loop. I was able to arrive at a direct solution by just saying xIncrement = xGoal, no need for a small step value. $\endgroup$ – Iche Nov 12 '15 at 4:29
  • $\begingroup$ On stack exchange, it is better to edit your answer to add information requested in comments, rather than adding more comments. Comments are for helping to improve questions and answers, and are distracting, so we try to keep them to a minimum. If all of the information needed to answer the question is contained within it, the comments can be tidied up (deleted). $\endgroup$ – Mark Booth May 29 '17 at 11:29
0
$\begingroup$

The problem is not with the code, but with the point you want to send the arm in the simulation. Whats going is the goal's points constitute a cartesian distance larger than the manipulator can catch. It occurs cause in python may have some numerical errors when the simulation is running.

$\endgroup$
  • 2
    $\begingroup$ Welcome to Robotics Felipe Stark. Thanks for your answer but we are looking for comprehensive answers that provide some explanation and context. Very short answers cannot do this, so please edit your answer to explain why it is right. Answers that don't include explanations may be removed. $\endgroup$ – Mark Booth May 29 '17 at 11:31

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.