Hummm, I’m not sure exactly where you’re referring to. After the first figure, where com_0 and com_1 are defined?

]]>hmmm yup those should be the same, unfortunately I think you might have an error in your calculations!

As I understand it you’ve got something like T01 * T12 * x, where x = [0, 0], rather than T01 * x with x = [l1*cos(q1), l1*sin(q1)]?

In both cases you should end up with equivalent expressions.

Good luck!

I implemented this pretty much straight from ‘Control-limited differential dynamic programming’ by Yuval Tassa, Nicolas Mansard, and Emo Todorov (2014), where they’re implementing iLQR.

And you’re right, iLQR and iLQG are not the same! Todorov talks about the differences in the paper where they introduced iLQG: http://maeresearch.ucsd.edu/skelton/publications/weiwei_ilqg_CDC43.pdf

Possibly the confusion is that I mentioned I used iLQG code provided by Dr. Todorov to get this implementation going?

Does that help clear things up?

sorry about the poor commenting in dif_end! That first part, with xe, all that’s being computed is the end-effector position. It’s a really convoluted (sorry) way of calculating (target – ee), where ee is the end effector position, with `x = l0 * cos(q0) + l1 * cos(q0 + q1) + l2 * cos(q0 + q1 + q2)`

and `y = l0 * sin(q0) + l1 * sin(q0 + q1) + l2 * sin(q0 + q1 + q2)`

.

and then the second part is computing the derivative of (target – ee)^2, which is the position part of the cost function, and the entire final state cost function.

Hope that clears it up!

I think that you forgot the link lengths when stating the coordinates of the respective CoMs. ]]>