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
Improve this question
$\endgroup$
6
  • $\begingroup$ Does matrix multiplication work in python the same way as in Matlab? change = invJacob*increMatrix #multiplying both matrixes $\endgroup$
    – 50k4
    Nov 10, 2015 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, 2015 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, 2015 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, 2015 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, 2015 at 23:31

2 Answers 2

Reset to default
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.

Improve this answer
$\endgroup$
3
  • $\begingroup$ Could you explain the difference between step and overall change a bit more clearly? $\endgroup$
    – 50k4
    Nov 11, 2015 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, 2015 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, 2017 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.

Improve this answer
$\endgroup$
1
  • 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, 2017 at 11:31

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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