Skip to content

Commit 6237b81

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Enable continuous collision checking for moving to moving objects
1 parent e752e65 commit 6237b81

File tree

16 files changed

+852
-179
lines changed

16 files changed

+852
-179
lines changed

‎tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_utils.h‎

Lines changed: 267 additions & 58 deletions
Large diffs are not rendered by default.

‎tesseract/tesseract_collision/include/tesseract_collision/core/types.h‎

Lines changed: 40 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -73,25 +73,47 @@ struct ContactResult
7373
{
7474
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
7575

76-
double distance; /**< @brief The distance between two links */
77-
int type_id[2]; /**< @brief A user defined type id that is added to the contact shapes */
78-
std::string link_names[2]; /**< @brief The two links that are in contact */
79-
int shape_id[2]; /**< @brief A link can be made up of multiple shapes */
80-
int subshape_id[2]; /**< @brief Some shapes linke octomap and mesh have subshape (boxes and triangles) */
81-
Eigen::Vector3d nearest_points[2]; /**< @brief The nearest point on both links */
82-
Eigen::Vector3d normal; /**< @brief The normal vector to move the two objects out of contact */
83-
Eigen::Vector3d cc_nearest_points[2]; /**< @brief When using continuous contact checking this is the nearest point on
84-
the object */
85-
double cc_time; /**< @brief This is between 0 and 1 indicating the point of contact */
86-
ContinouseCollisionType cc_type; /**< @brief The type of continuous contact */
76+
/** @brief The distance between two links */
77+
double distance;
78+
/** @brief A user defined type id that is added to the contact shapes */
79+
int type_id[2];
80+
/** @brief The two links that are in contact */
81+
std::string link_names[2];
82+
/** @brief A link can be made up of multiple shapes */
83+
int shape_id[2];
84+
/** @brief Some shapes linke octomap and mesh have subshape (boxes and triangles) */
85+
int subshape_id[2];
86+
/** @brief The nearest point on both links in world coordinates */
87+
Eigen::Vector3d nearest_points[2];
88+
/** @brief The nearest point on both links in local coordinates */
89+
Eigen::Vector3d nearest_points_local[2];
90+
/** @brief The transfrom of link in world coordinates */
91+
Eigen::Isometry3d transform[2];
92+
/** @brief The normal vector to move the two objects out of contact in world coordinates */
93+
Eigen::Vector3d normal;
94+
/** @brief This is between 0 and 1 indicating the point of contact */
95+
double cc_time[2];
96+
/** @brief The type of continuous contact */
97+
std::array<ContinouseCollisionType, 2> cc_type;
98+
/** @brief The transform of link in world coordinates at its desired final location.
99+
* Note: This is not the location of the link at the point of contact but the final location the link when performing
100+
* continuous collision checking. If you desire the location of contact use cc_time and interpolate between
101+
* transform and cc_transform;
102+
*/
103+
Eigen::Isometry3d cc_transform[2];
87104

88105
ContactResult() { clear(); }
89-
/// Clear structure data
106+
107+
/** @brief reset to default values */
90108
void clear()
91109
{
92110
distance = std::numeric_limits<double>::max();
93111
nearest_points[0].setZero();
94112
nearest_points[1].setZero();
113+
nearest_points_local[0].setZero();
114+
nearest_points_local[1].setZero();
115+
transform[0] = Eigen::Isometry3d::Identity();
116+
transform[1] = Eigen::Isometry3d::Identity();
95117
link_names[0] = "";
96118
link_names[1] = "";
97119
shape_id[0] = -1;
@@ -101,10 +123,12 @@ struct ContactResult
101123
type_id[0] = 0;
102124
type_id[1] = 0;
103125
normal.setZero();
104-
cc_nearest_points[0].setZero();
105-
cc_nearest_points[1].setZero();
106-
cc_time = -1;
107-
cc_type = ContinouseCollisionType::CCType_None;
126+
cc_time[0] = -1;
127+
cc_time[1] = -1;
128+
cc_type[0] = ContinouseCollisionType::CCType_None;
129+
cc_type[1] = ContinouseCollisionType::CCType_None;
130+
cc_transform[0] = Eigen::Isometry3d::Identity();
131+
cc_transform[1] = Eigen::Isometry3d::Identity();
108132
}
109133
};
110134

‎tesseract/tesseract_collision/src/bullet/bullet_cast_bvh_manager.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -325,13 +325,13 @@ void BulletCastBVHManager::setActiveCollisionObjects(const std::vector<std::stri
325325
if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)
326326
{
327327
// Update with active
328-
updateCollisionObjectFilters(active_, *cow, false);
328+
updateCollisionObjectFilters(active_, *cow);
329329

330330
// Get the active collision object
331331
COW::Ptr& active_cow = link2castcow_[cow->getName()];
332332

333333
// Update with active
334-
updateCollisionObjectFilters(active_, *active_cow, true);
334+
updateCollisionObjectFilters(active_, *active_cow);
335335

336336
// Check if the link is still active.
337337
if (!isLinkActive(active_, cow->getName()))
@@ -346,13 +346,13 @@ void BulletCastBVHManager::setActiveCollisionObjects(const std::vector<std::stri
346346
else
347347
{
348348
// Update with active
349-
updateCollisionObjectFilters(active_, *cow, false);
349+
updateCollisionObjectFilters(active_, *cow);
350350

351351
// Get the active collision object
352352
COW::Ptr& active_cow = link2castcow_[cow->getName()];
353353

354354
// Update with active
355-
updateCollisionObjectFilters(active_, *active_cow, true);
355+
updateCollisionObjectFilters(active_, *active_cow);
356356

357357
// Check if link is now active
358358
if (isLinkActive(active_, cow->getName()))

‎tesseract/tesseract_collision/src/bullet/bullet_cast_simple_manager.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -295,13 +295,13 @@ void BulletCastSimpleManager::setActiveCollisionObjects(const std::vector<std::s
295295
COW::Ptr& cow = co.second;
296296

297297
// Update with request
298-
updateCollisionObjectFilters(active_, *cow, false);
298+
updateCollisionObjectFilters(active_, *cow);
299299

300300
// Get the cast collision object
301301
COW::Ptr cast_cow = link2castcow_[cow->getName()];
302302

303303
// Update with request
304-
updateCollisionObjectFilters(active_, *cast_cow, true);
304+
updateCollisionObjectFilters(active_, *cast_cow);
305305

306306
// Add to collision object vector
307307
if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)

‎tesseract/tesseract_collision/src/bullet/bullet_discrete_bvh_manager.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -205,7 +205,7 @@ void BulletDiscreteBVHManager::setActiveCollisionObjects(const std::vector<std::
205205
{
206206
COW::Ptr& cow = co.second;
207207

208-
updateCollisionObjectFilters(active_, *cow, false);
208+
updateCollisionObjectFilters(active_, *cow);
209209
}
210210
}
211211

‎tesseract/tesseract_collision/src/bullet/bullet_discrete_simple_manager.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -191,7 +191,7 @@ void BulletDiscreteSimpleManager::setActiveCollisionObjects(const std::vector<st
191191
{
192192
COW::Ptr& cow = co.second;
193193

194-
updateCollisionObjectFilters(active_, *cow, false);
194+
updateCollisionObjectFilters(active_, *cow);
195195

196196
// Update collision object vector
197197
if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)

‎tesseract/tesseract_collision/test/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,3 +34,4 @@ add_gtest(${PROJECT_NAME}_octomap_mesh_unit collision_octomap_mesh_unit.cpp)
3434
add_gtest(${PROJECT_NAME}_clone_unit collision_clone_unit.cpp)
3535
add_gtest(${PROJECT_NAME}_box_box_cast_unit collision_box_box_cast_unit.cpp)
3636
add_gtest(${PROJECT_NAME}_compound_compound_unit collision_compound_compound_unit.cpp)
37+
add_gtest(${PROJECT_NAME}_sphere_sphere_cast_unit collision_sphere_sphere_cast_unit.cpp)

‎tesseract/tesseract_collision/test/collision_box_box_cast_unit.cpp‎

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -70,24 +70,28 @@ void runTest(ContinuousContactManager& checker)
7070

7171
EXPECT_TRUE(!result_vector.empty());
7272
EXPECT_NEAR(result_vector[0].distance, -0.2475, 0.001);
73-
EXPECT_NEAR(result_vector[0].cc_time, 0.25, 0.001);
74-
EXPECT_TRUE(result_vector[0].cc_type == ContinouseCollisionType::CCType_Between);
73+
EXPECT_NEAR(result_vector[0].cc_time[0], -1.0, 0.001);
74+
EXPECT_NEAR(result_vector[0].cc_time[1], 0.25, 0.001);
75+
EXPECT_TRUE(result_vector[0].cc_type[0] == ContinouseCollisionType::CCType_None);
76+
EXPECT_TRUE(result_vector[0].cc_type[1] == ContinouseCollisionType::CCType_Between);
7577

7678
EXPECT_NEAR(result_vector[0].nearest_points[0][0], -0.5, 0.001);
7779
EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.5, 0.001);
7880
EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
7981

80-
EXPECT_NEAR(result_vector[0].nearest_points[1][0], -1.275, 0.001);
81-
EXPECT_NEAR(result_vector[0].nearest_points[1][1], -0.625, 0.001);
82+
EXPECT_NEAR(result_vector[0].nearest_points[1][0], -0.325, 0.001);
83+
EXPECT_NEAR(result_vector[0].nearest_points[1][1], 0.325, 0.001);
8284
EXPECT_NEAR(result_vector[0].nearest_points[1][2], 0.0, 0.001);
8385

84-
EXPECT_NEAR(result_vector[0].cc_nearest_points[0][0], -0.325, 0.001);
85-
EXPECT_NEAR(result_vector[0].cc_nearest_points[0][1], 0.325, 0.001);
86-
EXPECT_NEAR(result_vector[0].cc_nearest_points[0][2], 0.0, 0.001);
86+
Eigen::Vector3d p0 = result_vector[0].transform[1] * result_vector[0].nearest_points_local[1];
87+
EXPECT_NEAR(p0[0], -1.275, 0.001);
88+
EXPECT_NEAR(p0[1], -0.625, 0.001);
89+
EXPECT_NEAR(p0[2], 0.0, 0.001);
8790

88-
EXPECT_NEAR(result_vector[0].cc_nearest_points[1][0], 2.525, 0.001);
89-
EXPECT_NEAR(result_vector[0].cc_nearest_points[1][1], 3.175, 0.001);
90-
EXPECT_NEAR(result_vector[0].cc_nearest_points[1][2], 0.0, 0.001);
91+
Eigen::Vector3d p1 = result_vector[0].cc_transform[1] * result_vector[0].nearest_points_local[1];
92+
EXPECT_NEAR(p1[0], 2.525, 0.001);
93+
EXPECT_NEAR(p1[1], 3.175, 0.001);
94+
EXPECT_NEAR(p1[2], 0.0, 0.001);
9195
}
9296

9397
TEST(TesseractCollisionUnit, BulletCastSimpleCollisionBoxBoxUnit) // NOLINT

0 commit comments

Comments
 (0)