Dualmanipulation fix

I use a robot with 6 DOF, 4 joints are circular.
Dualmanipulation failed for 2 reasons:

1) The calculated IK solution was checked against vprev by subtracting the joint values. This fails with circular joints, when the values close to 2PI (e.g. 0.0001 and 6.20000). I use ```SubtractDOFValues``` now.

2) ```_InitPlan``` failed, because ```PlannerParams::Validate()``` failed with
```
openrave (Assert): [virtual void OpenRAVE::PlannerBase::PlannerParameters::Validate() const:787] dist <= 1000*g_fEpsilon, (eval 3.84959e-06 <= 1e-12) 
```
I guess that was because although vdelta was all "zeros", a IK solution was still calculated and used. What I do now is: Only calculate new IK solution if vprev+vdelta does not fulfil the constraint.

My question now is: What is _neighstatefn(...) exactly supposed to do?
The way I understood it, we pass current config (vprev) and the delta to the desired next config (vdelta). Then the function should update vprev and return "true" if it found a next config that is close to vprev+vdelta. If that is correct, then I'd say my "fixes" are valid, otherwise there are probably still some conditions left unconsidered.

I didn't change anything about the constraing-checking itself, it looks a little "fragile" to me though.