update link transforms in UnionConstraintSampler::project too

[v4hn]

This extends #186 (0119d58).

Scott Paulin found that the UnionConstraintSampler should explicitly
update the LinkTransforms of the returned samples, because one of the
other samplers could require them to be valid.

Back then, we missed that the sampler also provides a project method
that has the same limitation. So I update the transforms there too.

This should be picked to j/k.

I triggered this today by adding a JointConstraint and an OrientationConstraint
as PathConstraints to a planning request.

Update: The JointConstraint alone is enough to trigger this, according to a mail on the mailing list.

Here's a stack-trace to reveal the calling code:

    quat=..., ks=..., max_attempts=4)
    at /home/v4hn/ros/indigo/moveit/src/moveit/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp:416
    reference_state=..., max_attempts=4, project=true)
    at /home/v4hn/ros/indigo/moveit/src/moveit/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp:562
    max_attempts=4)
    at /home/v4hn/ros/indigo/moveit/src/moveit/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp:586
    max_attempts=4)
    at /home/v4hn/ros/indigo/moveit/src/moveit/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp:159
    gls=0x7fffac051690, new_goal=0x7fffa4005450)

[v4hn]

@scottpaulin do you have a moment to verify this patch?

[scottpaulin]

Looks good! I'm surprised that this can be triggered with only a joint constraint, since the joint constraint sampler does not use the robot state's link transforms.

[v4hn]

On Fri, Dec 23, 2016 at 06:08:56PM -0800, Scott Paulin wrote: Looks good! I'm surprised that this can be triggered with only a joint constraint, since the joint constraint sampler does not use the robot state's link transforms.

Well, PathConstraints also have to be true at the goal, so the *GoalConstraintSampler* has to combine the JointConstraint and the IKConstraints. I suppose this triggers the error.

[davetcoleman]

Thanks!