How to deal with this optimization problem?

I read in the paper that the nonlinear constrained optimization problem is solved by the classic Newton-Raphson iterative method. But i don't really know how! Help me, please!

[Ced]

"classic" newton-raphson and "nonlinear constrained problem" sounds a bit contradictory to be, but opinions vary.

Short Version: My best guess would be a Sequential Quadratic Program (SQP). There are quite a few available solvers out there. Matlab also has one, but depending on the size of your problem, you might want a compiled language for speed.

(21) is what you are optimizing, taking steps in tau

(22) is your equality constraint

(23) are your inequality constraints

Slightly more detailed version:

At first glance, I would say that they solve (21) using the Newton-Algorithm. For that time step, q_i and qdot_i are evaluated at tau_i, so you would have an optimization problem with a known q_i, qdot_i. You still need to include the constraints (23) though. One possibility would be to add Lagrange multipliers to (21) and solve that. This should eventually lead to a quadratic program. To extract xdot (I'm assuming x = [ q ; q_dot ] ), they then evaluate the dynamics, but project the result onto the set of possible states using the Jacobians J in (22).

[August Chen]

Dear Ced, Thanks for your answer.I tried to figure this optimization problem out with "fmincon" function.But faced with this "FMINCON requires the following inputs to be of data type double: 'Aeq','Beq'." Maybe the cause is that Tau is a three elements vector and Xdot_i+1 is a two dimensions vector.

And this is myfun.I don't really know whether I could wirte this function contains the relation between Tau_i and Tau_i+1(I am not familiar with Matlab and the explaination in Matlab'Help also confuses me in "fmincon") Actually, I want to find a solution look like "solve" that I can just type equqtions of constraints (22,23) and get minimun Q (21).Sounds like i was making a daydream,LOl.I just wikied SQP, I will have a try with your method.Anyway, Thanks your help.Have a good day dear Ced.

[Ced]

Sure, no problem. It's nice to see a problem more from the robotics world and less from the image processing one for a change ;).

Looking at your function, note that Tau_a = D*ddq + ... = D*inv(D)*x + ... = x + ...

This might help in getting the quadratic form necessary for a QP solver.

For low dimensional problems, you might be able to solve it using fmincon. I have not used it much, so I don't know the details of the inner workings. But other optimization functions in matlab (including the QP solvers) have a similar syntax, so understanding one will help you understand the other.

I'll try and summarize what fmincon needs. This is the most general form you can call the function:

x = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon,options)

1. fun has to be a function with 1 input variable (not necessarily scalar though). In your case, this would be Q(x). So, in each time step, you would have

Q_to_minimize = @(x)Q(x,Tau_H,Tau_L,C,G,dq,dt); % function of x only

Note that Tau_H, ... dt are known in each time step

2. x0: starting value for tau, I think that's clear

3. A,b: these are the matrices for the linear inequality constraints. You could plug in eq (23), but there is a special field for bounds, so set them to A = [], b = []. See point 5.

4. Aeq, beq: these are the matrices for the linear equality constraints Aeq*x = beq. I don't know what you plugged in here, but my guess is that it wasn't a matrix. tau having a different dimension that x is not a problem, J*inv(D)*tau has the same dimension as x. (or should... if not, there is a mistake somewhere!)

5. lb, ub: These are lower and upper bounds for x, i.e. Tau_H and Tau_L.

6. This would be a field for nonlinear constraints. Luckily, I think all your constraints can be rewritten in linear form.

7. options: This is an object where you can define certain parameters of the optimization, e.g. particular algorithm and such. I would ignore it for now.

PS: Maybe this is what you meant, but "doc fmincon" gives you a much more elaborate documentation. "help fmincon" is only a summary.

PPS: Good luck