Question: Solve nonlinear vector function

Hello All, 

I am trying to solve a nonlinear equation as a part of inverse kinematic for a robot tip. 

The robot is mostly redundant, and thus, the optimization package is the best to use. 

I looked the NLPSolve function in matrix form. 

the equation(s) am trying to solve is of the following format

Tip - Desired = 0 

Tip is a 6x1 vector and is a function (or expression) of q1 ..., qn number of joints. 

Desired is 6x1 numerical values representing the desired Tip position and orientation. 

for example, for a planar robot with only x,y coordinates

<cos(q[1])*cos(q[3])-sin(q[1])*sin(q[3])+sin(q[1])*q[2]+cos(q[1]), sin(q[1])*cos(q[3])+cos(q[1])*sin(q[3])-cos(q[1])*q[2]+sin(q[1])> - <1,1> = 0

I am a bit confused on how to use the NPLSolve command in matrix form. 

I understand that I need to formulate a procedure p(v) where v is a vector (here it will be v := <q[1], .., q[n]> ) , however, I am not solving one objective function rather 6 of them for 6-degrees of freedom. 

I tried something like

    <cos(q[1])*cos(q[3])-sin(q[1])*sin(q[3])+sin(q[1])*q[2]+cos(q[1]) - 1,sin(q[1])*cos(q[3])+cos(q[1])*sin(q[3])-cos(q[1])*q[2]+sin(q[1]) -1>;
    end proc;
    Optimization[NLPSolve](3,p,initialpoint = evalf(q_start),maximize = false)
where q_start is 3x1 vector giving the initial point. 

but I always get 

Error, (in Optimization:-NLPSolve) non-numeric result encountered


How can I solve for set of equations?



Please Wait...