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)*cos(q)-sin(q)*sin(q)+sin(q)*q+cos(q), sin(q)*cos(q)+cos(q)*sin(q)-cos(q)*q+sin(q)> - <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, .., 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)*cos(q)-sin(q)*sin(q)+sin(q)*q+cos(q) - 1,sin(q)*cos(q)+cos(q)*sin(q)-cos(q)*q+sin(q) -1>;
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?