Creating a Torque Cost Function

[MartinezMoritz]

Hi!, I'm trying to create a torque cost function based on the paper of STOMP. To do this I'm trying to base on the stomp_cost_function.h but is quite complicated to understand at all.

Is there any hint about the way at how I can create an easy and simple cost function and later (when I understand more the code) can make it even complex?

Thanks!

[jrgnicho]

I would look at the stomp_3dof example

[Ogunniran]

New location of stomp_3dof is this, were you able to create a working version with torque cost function? I need help with this also.