dualmanipulation: remove manually adding initial trajectory point (fixes #325)
dualmanipulation: fix neighstatefn "DualArmConstraint"

For one, we cannot compare joints of vprev(vnew) with vsolution directly,
we need to use SubtractDOFValues to consider circular joints.

Secondly, we first check if vprev+vdelta fulfils the constraint, before
using FindIkSolution and check that solution. Otherwise, RRT's _InitPlan
might already fail, because params->Validate() fails (the IK solution
is further away than expected). Maybe this is because FindIkSolution does
not return the closest solution and we would need to filter all solutions,
but so far this method works.

Also removed some unused variables.
Merge pull request #326 from kb0n/dualmanipulation_fix

Dualmanipulation fix