diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 9405fbff..dabe4abe 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -68,11 +68,7 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( contact_manager_ = env_->getContinuousContactManager(); contact_manager_->setActiveCollisionObjects(manip_active_link_names_); - contact_manager_->setCollisionMarginData(collision_config_->contact_manager_config.margin_data); - // Increase the default by the buffer - contact_manager_->setDefaultCollisionMarginData( - collision_config_->contact_manager_config.margin_data.getMaxCollisionMargin() + - collision_config_->collision_margin_buffer); + contact_manager_->applyContactManagerConfig(collision_config_->contact_manager_config); } CollisionCacheData::ConstPtr @@ -243,11 +239,7 @@ LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( contact_manager_ = env_->getDiscreteContactManager(); contact_manager_->setActiveCollisionObjects(manip_active_link_names_); - contact_manager_->setCollisionMarginData(collision_config_->contact_manager_config.margin_data); - // Increase the default by the buffer - contact_manager_->setDefaultCollisionMarginData( - collision_config_->contact_manager_config.margin_data.getMaxCollisionMargin() + - collision_config_->collision_margin_buffer); + contact_manager_->applyContactManagerConfig(collision_config_->contact_manager_config); } CollisionCacheData::ConstPtr diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp index 868a7a3a..f0d2b4b1 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -67,11 +67,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(std::shared_p contact_manager_ = env_->getDiscreteContactManager(); contact_manager_->setActiveCollisionObjects(manip_active_link_names_); - contact_manager_->setCollisionMarginData(collision_config_->contact_manager_config.margin_data); - // Increase the default by the buffer - contact_manager_->setDefaultCollisionMarginData( - collision_config_->contact_manager_config.margin_data.getMaxCollisionMargin() + - collision_config_->collision_margin_buffer); + contact_manager_->applyContactManagerConfig(collision_config_->contact_manager_config); } CollisionCacheData::ConstPtr