Skip to content

Commit

Permalink
Signed distance to capsule calculations with Autodiff support (RobotL…
Browse files Browse the repository at this point in the history 
…ocomotion#12617)
  • Loading branch information
@tehbelinda @DamrongGuoy
tehbelinda authored and DamrongGuoy committed Jan 27, 2020
1 parent c3bcbc3 commit 68fce04
Show file tree
Hide file tree
Showing 5 changed files with 240 additions and 31 deletions.
169 changes: 144 additions & 25 deletions geometry/proximity/distance_to_point_callback.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,6 @@ struct CallbackData {
std::vector<SignedDistanceToPoint<T>>& distances;
};


/** @name Functions for computing distance from point to primitives
This family of functions compute the distance from a point Q to a primitive
shape. Refer to QueryObject::ComputeSignedDistanceToPoint() for more details.
Expand All @@ -102,39 +101,51 @@ struct CallbackData {
@param is_grad_w_unique[out] True if the value in `grad_W` is unique. */
//@{

/** Overload of ComputeDistanceToPrimitive() for sphere primitive. */
/** Computes distance from point to sphere with the understanding that all
quantities are measured and expressed in the sphere's frame, S. Otherwise, the
semantics of the parameters are as documented as above. */
template <typename T>
void ComputeDistanceToPrimitive(const fcl::Sphered& sphere,
const math::RigidTransform<T>& X_WG,
const Vector3<T>& p_WQ, Vector3<T>* p_GN,
T* distance, Vector3<T>* grad_W,
bool* is_grad_W_unique) {
void SphereDistanceInSphereFrame(const fcl::Sphered& sphere,
const Vector3<T>& p_SQ, Vector3<T>* p_SN,
T* distance, Vector3<T>* grad_S,
bool* is_grad_W_unique) {
const double radius = sphere.radius;
const Vector3<T> p_GQ_G = X_WG.inverse() * p_WQ;
const T dist_GQ = p_GQ_G.norm();

const T dist_SQ = p_SQ.norm();
// The gradient is always in the direction from the center of the sphere to
// the query point Q, regardless of whether the point Q is outside or inside
// the sphere G. The gradient is undefined if the query point Q is at the
// center of the sphere G.
// the sphere S. The gradient is undefined if the query point Q is at the
// center of the sphere S.
//
// If the query point Q is near the center of the sphere G within a
// If the query point Q is near the center of the sphere S within a
// tolerance, we arbitrarily set the gradient vector as documented in
// query_object.h (QueryObject::ComputeSignedDistanceToPoint).
const double tolerance = DistanceToPointRelativeTolerance(radius);
// Unit vector in x-direction of G's frame.
const Vector3<T> Gx = Vector3<T>::UnitX();
*is_grad_W_unique = (dist_GQ > tolerance);
// Gradient vector expressed in G's frame.
const Vector3<T> grad_G = *is_grad_W_unique ? p_GQ_G / dist_GQ : Gx;

// p_GN is the position of a witness point N in the geometry frame G.
*p_GN = T(radius) * grad_G;
// Unit vector in x-direction of S's frame.
const Vector3<T> Sx = Vector3<T>::UnitX();
*is_grad_W_unique = (dist_SQ > tolerance);
// Gradient vector expressed in S's frame.
*grad_S = *is_grad_W_unique ? p_SQ / dist_SQ : Sx;

// p_SN is the position of a witness point N in the geometry frame S.
*p_SN = T(radius) * (*grad_S);

// Do not compute distance as ∥p_SQ∥₂, because the gradient of ∥p_SQ∥₂ w.r.t.
// p_SQ is p_SQᵀ/∥p_SQ∥₂ which is not well defined at p_SQ = 0. Instead,
// compute the distance as p_NQ_S.dot(grad_S).
*distance = (p_SQ - *p_SN).dot(*grad_S);
}

// Do not compute distance as ∥p_GQ∥₂, because the gradient of ∥p_GQ∥₂ w.r.t.
// p_GQ is p_GQᵀ/∥p_GQ∥₂ which is not well defined at p_GQ = 0. Instead,
// compute the distance as p_NQ_G.dot(grad_G).
*distance = (p_GQ_G - *p_GN).dot(grad_G);
/** Overload of ComputeDistanceToPrimitive() for sphere primitive. */
template <typename T>
void ComputeDistanceToPrimitive(const fcl::Sphered& sphere,
const math::RigidTransform<T>& X_WG,
const Vector3<T>& p_WQ, Vector3<T>* p_GN,
T* distance, Vector3<T>* grad_W,
bool* is_grad_W_unique) {
const Vector3<T> p_GQ_G = X_WG.inverse() * p_WQ;
Vector3<T> grad_G;
SphereDistanceInSphereFrame(sphere, p_GQ_G, p_GN, distance, &grad_G,
is_grad_W_unique);

// Gradient vector expressed in World frame.
*grad_W = X_WG.rotation() * grad_G;
Expand Down Expand Up @@ -162,6 +173,92 @@ void ComputeDistanceToPrimitive(const fcl::Halfspaced& halfspace,
*is_grad_W_unique = true;
}

/** Overload of ComputeDistanceToPrimitive() for capsule primitive. */
template <typename T>
void ComputeDistanceToPrimitive(const fcl::Capsuled& capsule,
const math::RigidTransform<T>& X_WG,
const Vector3<T>& p_WQ, Vector3<T>* p_GN,
T* distance, Vector3<T>* grad_W,
bool* is_grad_W_unique) {
const double radius = capsule.radius;
const double half_length = capsule.lz / 2;

// If the query point Q is closest to the end caps of the capsule, then we can
// re-use the distance to sphere calculations since they are effectively the
// same. Since our capsule is aligned with the local z-axis, we can simply
// compare the z co-ordinates in the capsule's frame G to determine which
// section the query point Q falls in.
// z
// ^ ●●
// | ● ● Top end cap
// |--------●------●-------------------
// | ● ●
// | ● ● Spine
// | ● ●
// |--------●------●-------------------
// | ● ● Bottom end cap
// | ●●

const Vector3<T> p_GQ = X_WG.inverse() * p_WQ;
if (p_GQ.z() >= half_length || p_GQ.z() <= -half_length) {
// Represent the end cap of the capsule using a sphere S of the same radius.
const fcl::Sphered sphere_S(radius);
// The sphere is defined centered on the origin of frame S. Frame S and G
// are related by a simple translation (their bases are perfectly aligned).
// So, a vector quantity expressed in frame G is the same as when expressed
// in S.
const Vector3<T> p_GS{
0, 0, (p_GQ.z() >= half_length) ? half_length : -half_length};
// The query point measured w.r.t. the sphere origin (equivalently expressed
// in G or S).
const Vector3<T> p_SQ = p_GQ - p_GS;
// Position vector of the nearest point N expressed in S's frame.
Vector3<T> grad_S;
Vector3<T> p_SN;
SphereDistanceInSphereFrame(sphere_S, p_SQ, &p_SN, distance, &grad_S,
is_grad_W_unique);
*grad_W = X_WG.rotation() * grad_S; // grad_S = grad_G because R_GS = I.
*p_GN = p_GS + p_SN; // p_SN = p_SN_G because R_GS = I.
} else {
// The query point Q projects onto (and is nearest to) the spine. The
// gradient is perpendicular to the spine and points from N' to Q (where
// N' is the point on the spine nearest Q). Equivalently, the gradient is
// in the same direction as the vector from G's origin to R, where point R
// is the projection of Q onto G's x-y plane. And M is the nearest point
// on the capsule's surface to R. The distance between Q and N is the same
// as between R and M.
//
// This allows us to solve for the gradient, distance, and nearest point
// by computing the distance from R to a sphere with the same radius and
// centered on G's origin. We can then shift M to N by adding the
// z-component back into N.
//
// z
// ^ ● ●
// | ● ●
// | ● ●
// | ● |---N---> Q
// | ● | ●
// | ● G---M---> R
// | ● | ●
// | ● | ●
// | ● ●
// | ● ●
// | ● ●

// TODO(SeanCurtis-TRI): For further efficiency, consider doing these
// calculations in 2D and then promoting them back into 3D.
const Vector3<T> p_GR{p_GQ.x(), p_GQ.y(), 0};
const fcl::Sphered sphere_S(radius);
Vector3<T> p_GM;
Vector3<T> grad_G;
SphereDistanceInSphereFrame(sphere_S, p_GR, &p_GM, distance, &grad_G,
is_grad_W_unique);
*p_GN << p_GM.x(), p_GM.y(), p_GQ.z();
*grad_W = X_WG.rotation() * grad_G;
}
}

// TODO(DamrongGuoy): Add overloads for all supported geometries.

//@}
Expand Down Expand Up @@ -210,6 +307,22 @@ class DistanceToPoint {
is_grad_W_unique};
}

/** Overload to compute distance to a capsule. */
SignedDistanceToPoint<T> operator()(const fcl::Capsuled& capsule) {
// TODO(SeanCurtis-TRI): This would be better if `SignedDistanceToPoint`
// could be default constructed in an uninitialized state and then
// pointers to its contents could be passed directly to ComputeDistance...
// This would eliminate the inevitable copy in the constructor.
T distance{};
Vector3<T> p_GN_G, grad_W;
bool is_grad_W_unique{};
ComputeDistanceToPrimitive(capsule, X_WG_, p_WQ_, &p_GN_G, &distance,
&grad_W, &is_grad_W_unique);

return SignedDistanceToPoint<T>{geometry_id_, p_GN_G, distance, grad_W,
is_grad_W_unique};
}

/** Overload to compute distance to a box. */
SignedDistanceToPoint<T> operator()(const fcl::Boxd& box) {
// Express the given query point Q in the frame of the box geometry G.
Expand Down Expand Up @@ -499,6 +612,7 @@ struct ScalarSupport<double> {
case fcl::GEOM_BOX:
case fcl::GEOM_CYLINDER:
case fcl::GEOM_HALFSPACE:
case fcl::GEOM_CAPSULE:
return true;
default:
return false;
Expand All @@ -514,6 +628,7 @@ struct ScalarSupport<Eigen::AutoDiffScalar<DerType>> {
case fcl::GEOM_SPHERE:
case fcl::GEOM_BOX:
case fcl::GEOM_HALFSPACE:
case fcl::GEOM_CAPSULE:
return true;
default:
return false;
Expand Down Expand Up @@ -584,6 +699,10 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
distance = distance_to_point(
*static_cast<const fcl::Halfspaced*>(collision_geometry));
break;
case fcl::GEOM_CAPSULE:
distance = distance_to_point(
*static_cast<const fcl::Capsuled*>(collision_geometry));
break;
default:
// Returning false tells fcl to continue to other objects.
return false;
Expand Down
11 changes: 11 additions & 0 deletions geometry/proximity/distance_to_shape_callback.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,6 +143,11 @@ class DistancePairGeometry {
SphereShapeDistance(sphere_A, halfspace_B);
}

void operator()(const fcl::Sphered& sphere_A,
const fcl::Capsuled& capsule_B) {
SphereShapeDistance(sphere_A, capsule_B);
}

//@}

private:
Expand Down Expand Up @@ -320,6 +325,12 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd& a,
distance_pair(sphere_S, halfspace_O);
break;
}
case fcl::GEOM_CAPSULE: {
const auto& capsule_O =
*static_cast<const fcl::Capsuled*>(o_geometry);
distance_pair(sphere_S, capsule_O);
break;
}
default: {
// We don't have a closed form solution for the other geometry, so we
// call FCL GJK/EPA.
Expand Down
14 changes: 13 additions & 1 deletion geometry/proximity/test/distance_sphere_to_shape_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,10 @@ template <>
void DistancePairGeometry<float>::operator()(const fcl::Sphered&,
const fcl::Boxd&) {}

template <>
void DistancePairGeometry<float>::operator()(const fcl::Sphered&,
const fcl::Capsuled&) {}

template <>
void DistancePairGeometry<float>::operator()(const fcl::Sphered&,
const fcl::Cylinderd&) {}
Expand All @@ -61,6 +65,7 @@ namespace {

using Eigen::Vector3d;
using fcl::Boxd;
using fcl::Capsuled;
using fcl::CollisionObjectd;
using fcl::Cylinderd;
using fcl::Halfspaced;
Expand Down Expand Up @@ -346,11 +351,14 @@ TEST_F(DistancePairGeometryTest, SphereSphereDouble) {
EXPECT_TRUE((ResultsMatch<double, Sphered>(Sphered(1.3))));
}


TEST_F(DistancePairGeometryTest, SphereBoxDouble) {
EXPECT_TRUE((ResultsMatch<double, Boxd>(Boxd{1.3, 2.3, 0.7})));
}

TEST_F(DistancePairGeometryTest, SphereCapsuleDouble) {
EXPECT_TRUE((ResultsMatch<double, Capsuled>(Capsuled{1.3, 2.3})));
}

TEST_F(DistancePairGeometryTest, SphereCylinderDouble) {
EXPECT_TRUE((ResultsMatch<double, Cylinderd>(Cylinderd{1.3, 2.3})));
}
Expand All @@ -363,6 +371,10 @@ TEST_F(DistancePairGeometryTest, SphereBoxAutoDiffXd) {
EXPECT_TRUE((ResultsMatch<AutoDiffXd, Boxd>(Boxd{1.3, 2.3, 0.7})));
}

TEST_F(DistancePairGeometryTest, SphereCapsuleAutoDiffXd) {
EXPECT_TRUE((ResultsMatch<AutoDiffXd, Capsuled>(Capsuled{1.3, 2.3})));
}

TEST_F(DistancePairGeometryTest, SphereCylinderAutoDiffXd) {
EXPECT_TRUE((ResultsMatch<AutoDiffXd, Cylinderd>(Cylinderd{1.3, 2.3})));
}
Expand Down
71 changes: 71 additions & 0 deletions geometry/proximity/test/distance_to_point_callback_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,6 +290,71 @@ GTEST_TEST(DistanceToPoint, Box) {
}
}

// Simple smoke test for signed distance to Capsule. It does the following:
// Perform test of three different points w.r.t. a capsule: outside, on
// surface, and inside.
// Do it with AutoDiff relative to the query point's position.
// Confirm the *values* of the reported quantity.
// Confirm that the derivative of distance (extracted from AutoDiff) matches
// the derivative computed by hand (grad_W).
GTEST_TEST(DistanceToPoint, Capsule) {
const double kEps = 6 * std::numeric_limits<double>::epsilon();

// Provide some arbitary pose of the capsule G in the world.
const RotationMatrix<double> R_WG(
AngleAxis<double>(M_PI / 5, Vector3d{1, 2, 3}.normalized()));
const Vector3d p_WG{0.5, 1.25, -2};
const RigidTransform<double> X_WG(R_WG, p_WG);
const fcl::Capsuled capsule(0.7, 1.3);
PointShapeAutoDiffSignedDistanceTester<fcl::Capsuled> tester(&capsule, X_WG,
kEps);
// We want to test the 3 sections for when the point Q is nearest to:
// 1. The top end cap of the capsule.
// 2. The spine of the capsule.
// 3. The bottom end cap of the capsule.
// In each section, we pick a direction away from the capsule that *isn't*
// aligned with the frame basis and prepare the witness point N accordingly.
const Vector3d vhat_NQ_Gs[3] = {
Vector3d{2, -3, 6}.normalized(), // Upwards and away.
Vector3d{2, -3, 0}.normalized(), // Perpendicularly outwards.
Vector3d{2, -3, -6}.normalized() // Downwards and away.
};
const Vector3d p_GN_Gs[3] = {
capsule.radius * vhat_NQ_Gs[0] + Vector3d{0, 0, capsule.lz / 2},
capsule.radius * vhat_NQ_Gs[1] + Vector3d{0, 0, capsule.lz / 4},
capsule.radius * vhat_NQ_Gs[2] + Vector3d{0, 0, -capsule.lz / 2}};

for (int i = 0; i < 3; ++i) {
const Vector3d& vhat_NQ_G = vhat_NQ_Gs[i];
const Vector3d& p_GN_G = p_GN_Gs[i];

// Case: point lies *outside* the capsule.
{
const Vector3d p_NQ_G = 1.5 * vhat_NQ_G;
EXPECT_TRUE(tester.Test(p_GN_G, p_NQ_G));
}

// Case: point lies *on* the capsule.
{
const Vector3d p_NQ_G = Vector3d::Zero();
EXPECT_TRUE(tester.Test(p_GN_G, p_NQ_G));
}

// Case: point lies *in* the capsule.
{
const Vector3d p_NQ_G = -(0.5 * capsule.radius) * vhat_NQ_G;
EXPECT_TRUE(tester.Test(p_GN_G, p_NQ_G, true /* is inside */));
}

// Case: point lies on the capsule's spine.
{
const Vector3d p_NQ_G = -capsule.radius * vhat_NQ_G;
EXPECT_TRUE(tester.Test(p_GN_G, p_NQ_G, true /* is inside */,
false /* ill defined */));
}
}
}

// Simple smoke test for signed distance to Halfspace. It does the following:
// Perform test of three different points w.r.t. a halfspace: outside, on
// surface, and inside.
Expand Down Expand Up @@ -501,6 +566,12 @@ void TestScalarShapeSupport() {
EXPECT_EQ(distances.size(), 1);
}

// Capsule
{
run_callback(make_shared<fcl::Capsuled>(1.0, 2.0));
EXPECT_EQ(distances.size(), 1);
}

// Convex
// TODO(SeanCurtis-TRI): Add convex that is *not* supported; create a small
// utility test to generate a tetrahedron.
Expand Down
Loading

0 comments on commit 68fce04

Please sign in to comment.