Port casadi robot to updated dec vars

Rewrite step and cost so they are using updated decision vars

• fwd kin by cpin
• step and cost
• test in multiple shooting