Skip to content

Commit 89661ad

Browse files
Levi-ArmstrongLevi-Armstrong
authored
Fix trajopt ifopt collision with fixed states (#372)
1 parent 7b93537 commit 89661ad

File tree

6 files changed

+61
-24
lines changed

6 files changed

+61
-24
lines changed

‎trajopt_common/include/trajopt_common/collision_utils.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,9 +47,11 @@ std::size_t cantorHash(int shape_id, int subshape_id);
4747
* @brief Remove any results that are invalid.
4848
* Invalid state are contacts that occur at fixed states or have distances outside the threshold.
4949
* @param contact_results Contact results vector to process.
50+
* @param position_vars_fixed Indicate if a state is fixed
5051
*/
5152
void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results,
52-
const Eigen::Vector3d& data);
53+
const Eigen::Vector3d& data,
54+
const std::array<bool, 2>& position_vars_fixed);
5355

5456
/**
5557
* @brief Extracts the gradient information based on the contact results

‎trajopt_common/src/collision_utils.cpp‎

Lines changed: 37 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -67,13 +67,44 @@ std::size_t cantorHash(int shape_id, int subshape_id)
6767
return static_cast<std::size_t>(1 / 2.0 * (shape_id + subshape_id) * (shape_id + subshape_id + 1) + subshape_id);
6868
}
6969

70-
void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results, const Eigen::Vector3d& data)
70+
void removeInvalidContactResults(tesseract_collision::ContactResultVector& contact_results,
71+
const Eigen::Vector3d& data,
72+
const std::array<bool, 2>& position_vars_fixed)
7173
{
72-
auto end = std::remove_if(
73-
contact_results.begin(), contact_results.end(), [=, &data](const tesseract_collision::ContactResult& r) {
74-
/** @todo Is this correct? (Levi)*/
75-
return (!((data[0] + data[1]) > r.distance));
76-
});
74+
auto end = std::remove_if(contact_results.begin(),
75+
contact_results.end(),
76+
[=, &data, &position_vars_fixed](const tesseract_collision::ContactResult& r) {
77+
/** @todo Is this correct? (Levi)*/
78+
if ((!((data[0] + data[1]) > r.distance)))
79+
return true;
80+
81+
if (!position_vars_fixed[0] && !position_vars_fixed[1])
82+
return false;
83+
84+
if (position_vars_fixed[0])
85+
{
86+
if (r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_None &&
87+
r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_Time0)
88+
return false;
89+
90+
if (r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_None &&
91+
r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_Time0)
92+
return false;
93+
}
94+
95+
if (position_vars_fixed[1])
96+
{
97+
if (r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_None &&
98+
r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_Time1)
99+
return false;
100+
101+
if (r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_None &&
102+
r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_Time1)
103+
return false;
104+
}
105+
106+
return true;
107+
});
77108

78109
contact_results.erase(end, contact_results.end());
79110
}

‎trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -139,9 +139,10 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator
139139
CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
140140
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
141141

142-
void CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
142+
void CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results,
143+
const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
143144
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
144-
tesseract_collision::ContactResultMap& dist_results);
145+
const std::array<bool, 2>& position_vars_fixed);
145146
};
146147

147148
/**
@@ -190,9 +191,10 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator
190191
CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
191192
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
192193

193-
void CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
194+
void CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results,
195+
const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
194196
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
195-
tesseract_collision::ContactResultMap& dist_results);
197+
const std::array<bool, 2>& position_vars_fixed);
196198
};
197199
} // namespace trajopt_ifopt
198200
#endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_EVALUATOR_H

‎trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
9292
}
9393

9494
auto data = std::make_shared<trajopt_common::CollisionCacheData>();
95-
CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map);
95+
CalcCollisionsHelper(data->contact_results_map, dof_vals0, dof_vals1, position_vars_fixed);
9696

9797
for (const auto& pair : data->contact_results_map)
9898
{
@@ -165,9 +165,10 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
165165
return data;
166166
}
167167

168-
void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
168+
void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results,
169+
const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
169170
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
170-
tesseract_collision::ContactResultMap& dist_results)
171+
const std::array<bool, 2>& position_vars_fixed)
171172
{
172173
// The first step is to see if the distance between two states is larger than the longest valid segment. If larger
173174
// the collision checking is broken up into multiple casted collision checks such that each check is less then
@@ -186,7 +187,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<cons
186187
// Don't include contacts at the fixed state
187188
// Don't include contacts with zero coeffs
188189
const auto& zero_coeff_pairs = collision_config_->collision_coeff_data.getPairsWithZeroCoeff();
189-
auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) {
190+
auto filter = [this, &zero_coeff_pairs, &position_vars_fixed](tesseract_collision::ContactResultMap::PairType& pair) {
190191
// Remove pairs with zero coeffs
191192
if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end())
192193
{
@@ -199,7 +200,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<cons
199200
pair.first.second);
200201
double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second);
201202
const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff };
202-
trajopt_common::removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */
203+
trajopt_common::removeInvalidContactResults(pair.second, data, position_vars_fixed);
203204
};
204205

205206
if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS &&
@@ -329,7 +330,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
329330
}
330331

331332
auto data = std::make_shared<trajopt_common::CollisionCacheData>();
332-
CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map);
333+
CalcCollisionsHelper(data->contact_results_map, dof_vals0, dof_vals1, position_vars_fixed);
333334
for (const auto& pair : data->contact_results_map)
334335
{
335336
using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, trajopt_common::GradientResultsSet>;
@@ -401,9 +402,10 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
401402
return data;
402403
}
403404

404-
void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
405+
void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(tesseract_collision::ContactResultMap& dist_results,
406+
const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
405407
const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
406-
tesseract_collision::ContactResultMap& dist_results)
408+
const std::array<bool, 2>& position_vars_fixed)
407409
{
408410
// If not empty then there are links that are not part of the kinematics object that can move (dynamic environment)
409411
if (!diff_active_link_names_.empty())
@@ -417,7 +419,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const
417419
// Don't include contacts at the fixed state
418420
// Don't include contacts with zero coeffs
419421
const auto& zero_coeff_pairs = collision_config_->collision_coeff_data.getPairsWithZeroCoeff();
420-
auto filter = [this, &zero_coeff_pairs](tesseract_collision::ContactResultMap::PairType& pair) {
422+
auto filter = [this, &zero_coeff_pairs, &position_vars_fixed](tesseract_collision::ContactResultMap::PairType& pair) {
421423
// Remove pairs with zero coeffs
422424
if (std::find(zero_coeff_pairs.begin(), zero_coeff_pairs.end(), pair.first) != zero_coeff_pairs.end())
423425
{
@@ -432,7 +434,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const
432434
const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff };
433435

434436
// Don't include contacts at the fixed state
435-
trajopt_common::removeInvalidContactResults(pair.second, data);
437+
trajopt_common::removeInvalidContactResults(pair.second, data, position_vars_fixed);
436438
};
437439

438440
// The first step is to see if the distance between two states is larger than the longest valid segment. If larger

‎trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -215,7 +215,7 @@ enum class SQPStatus
215215
NLP_CONVERGED, /**< NLP Successfully converged */
216216
ITERATION_LIMIT, /**< SQP Optimization reached iteration limit */
217217
PENALTY_ITERATION_LIMIT, /**< SQP Optimization reached penalty iteration limit */
218-
TIME_LIMIT, /**< SQP Optimization reached reached limit */
218+
OPT_TIME_LIMIT, /**< SQP Optimization reached reached limit */
219219
QP_SOLVER_ERROR, /**< QP Solver failed */
220220
CALLBACK_STOPPED /**< Optimization stopped because callback returned false */
221221
};

‎trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
104104
if (elapsed_time > params.max_time)
105105
{
106106
CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time);
107-
status_ = SQPStatus::TIME_LIMIT;
107+
status_ = SQPStatus::OPT_TIME_LIMIT;
108108
break;
109109
}
110110

@@ -127,7 +127,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem)
127127
}
128128

129129
// If status is iteration limit or time limit we need to exit penalty iteration loop
130-
if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::TIME_LIMIT)
130+
if (status_ == SQPStatus::ITERATION_LIMIT || status_ == SQPStatus::OPT_TIME_LIMIT)
131131
break;
132132

133133
// Set status to running

0 commit comments

Comments
 (0)