The problem is my end effector is a plate and I want to keep its normal upwards. I know the two point ik problem at start and goal. But how to do it for the intermediate milestones in motion planning functions?
If you are using the functions in robotPlanning (makeSpace, planToConfig, etc), the IK objective can be passed in the extraConstraints argument.
You can then plan as normal, except the resulting path will only be coarse milestones along the path. To get a finely discretized path you will need to call the ClosedLoopCSpace.discretizePath method.