setGroupStateValidityCallback not automatically applied to internal samplers within UnionConstraintSampler

[prajd3d]

Description

Not sure if this is a bug or feature that needs to be included to avoid surprise (principle of least-surprise)

I am using the "constraint_samplers::ConstraintSamplerManager::selectDefaultSampler" helper function.

auto sampler = constraint_samplers::ConstraintSamplerManager::selectDefaultSampler( planning_scene, jmg->getName(), constraints);

For my use-case, it happens to select a "constraint_samplers::UnionConstraintSampler" because I have IK and Joint constraints. (i.e., sampler is essentially "constraint_samplers::UnionConstraintSampler")

In order to impose some validity constraints, I need to set the callback "setGroupStateValidityCallback" on sampler as follows.

sampler->setGroupStateValidityCallback( ... )

But, it seems that this not enough. I also have to go into the internal samplers and set their validity functions manually as well. So, I end up doing this.

` auto const casted = std::dynamic_pointer_cast(sampler);

      for (auto const & internal_sampler : casted->getSamplers()) {
          RCLCPP_ERROR_STREAM(
              LOGGER,
              "Internal sampler: " << internal_sampler->getName());
          internal_sampler->setGroupStateValidityCallback(
              std::bind(
                  validity_check_function,
                  std::placeholders::_1,
                  std::placeholders::_2,
                  std::placeholders::_3,
                  parameters.enable_validity_check));
      }

`

I do not want to use the error-prone dynamic_pointer_cast. I would prefer to stay away from it.

I wanted to know if I am going wrong somewhere or missing something.

I would like for the UnionConstraintSampler to automatically apply the same validity function to its internal samplers. I am guessing one way to implement this is to override the "setGroupStateValidityCallback" function in UnionConstraintSampler and have it apply the same callback to its internal samplers.

Your environment

• ROS Distro: Foxy
• OS Version: Ubuntu 20.04
• Source or Binary build? Source
• If binary, which release version?
• If source, which branch? foxy

[henningkayser]

@prajd3d thanks for the issue. I'm not sure I entirely understand your question though. The constructed samplers are already configured to sample your constraints using joint state and IK sampling. The validity checks are part of the sampler implementation, so you don't need a validity callback for the default case. However, if you have external constraints that are not covered by the Constraints message (say, collision distance, functional constraints based on several joints, etc...) you need a validity callback that filters the solutions additionally. Is that your use case? If yes, I don't think that there is a better way to implement it, currently.

However... I just noticed that the UnionConstraintSampler doesn't pass the callback and doesn't even seem to make use of it. The only use I find is inside the IKConstraintSampler which doesn't really look right to me, especially since the callback should support arbitrary checks. I'll need to verify if this is really a bug or maybe intentional due to performance reasons.

[prajd3d]

Sorry for the delay. My use-case is exactly what you mentioned. I have external constraints that need to be checked. Maybe the following will explain what I found surprising.

Let's say that I only had to use the IKConstraintSampler, then I would create the sampler and then call the "setGroupStateValidity" function on it. Here the creation and configuration (i.e., setting constraints) of the IKConstraintSampler is decoupled. And, it seems to me that this decoupling is intended. And, rightly so. But, that is not the focus of my issue.

On the other hand, let's say I end up getting a UnionConstraintSampler from the "selectDefaultSampler" function call. In this case, we want the setup and configuration to be decoupled as well. So, when I configure the UnionConstraintSampler to use a state-validity callback, I would expect the samplers that make up the UnionConstraintSampler to be configured with the same callback as well. I was hoping the configuration process would be automatically done. But, in the current API, I have to manually configure each internal sampler with the same callback. And, doing this involves casting operations which I would like to avoid.

So, in other words, I would like the API to be smart enough to configure all internal samplers with the same callback as that of the UnionConstraintSampler.