How does removeInvalidContactResults work?

[rjoomen]

This PR is not necessarily PR, but a question as to how removeInvalidContactResults() is supposed to work. I compared the Trajopt_ifopt version (committed here) with the Trajopt version and I do not really understand the differences. I therefore modified the Trajopt_ifopt version to match Trajopt as a starting point for discussion, @Levi-Armstrong.

[Levi-Armstrong]

The current state should be correct. There are some difference between the legacy and ifopt version but this function needs to work for both a discrete(single trajectory joint state) and continues(cast or interpolated discrete) checker. The purpose is to remove any contact results which are associated with a fixed state. If the state is fixed then it cannot move and adding cost/constraints cause instability and it would fail find a solution.

[rjoomen]

Yes, I understand. But if I rewrite your logic like this for easier understanding:

if (position_vars_fixed[0])
{
  if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time1)
    return false;

  if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time1)
    return false;
}

if (position_vars_fixed[1])
{
  if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time0)
    return false;

  if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time0)
    return false;
}

return true;

This means that if:

• either state is CCType_Between
• or state 0 is fixed but one of the types is CCType_Time1
• or state 1 is fixed but one of the types is CCType_Time0

the contact is not removed. Is that correct?

[Levi-Armstrong]

Yes, I understand. But if I rewrite your logic like this for easier understanding:

if (position_vars_fixed[0])
{
  if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time1)
    return false;

  if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time1)
    return false;
}

if (position_vars_fixed[1])
{
  if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time0)
    return false;

  if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Between ||
      r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time0)
    return false;
}

return true;

This means that if:

* either state is CCType_Between

* or state 0 is fixed but one of the types is CCType_Time1

* or state 1 is fixed but one of the types is CCType_Time0

the contact is not removed. Is that correct?

I think what you are proposing is valid and agree that it is easier to understand.