Skip to content

Commit

Permalink
Add BodySphereInOneOfPolytopes constraint.
Browse files Browse the repository at this point in the history
hongkai-dai committed Oct 12, 2018
1 parent a1157b7 commit a620cd2
Showing 6 changed files with 139 additions and 12 deletions.
16 changes: 4 additions & 12 deletions attic/multibody/dev/BUILD.bazel
Original file line number Diff line number Diff line change
@@ -26,15 +26,16 @@ drake_cc_library(
"//multibody:rigid_body_tree_construction",
"@gtest//:without_main",
],
data = [
"//manipulation/models/iiwa_description:models",
"//multibody:models",
],
)

drake_cc_googletest(
name = "global_inverse_kinematics_test",
timeout = "moderate",
srcs = ["test/global_inverse_kinematics_test.cc"],
data = [
"//manipulation/models/iiwa_description:models",
],
tags = gurobi_test_tags() + [
# Takes too long to run with Valgrind.
"no_memcheck",
@@ -50,9 +51,6 @@ drake_cc_googletest(
name = "global_inverse_kinematics_reachable_test",
timeout = "moderate",
srcs = ["test/global_inverse_kinematics_reachable_test.cc"],
data = [
"//manipulation/models/iiwa_description:models",
],
tags = gurobi_test_tags() + [
# Takes too long to run with Valgrind.
"no_memcheck",
@@ -68,9 +66,6 @@ drake_cc_googletest(
name = "global_inverse_kinematics_collision_avoidance_test",
timeout = "moderate",
srcs = ["test/global_inverse_kinematics_collision_avoidance_test.cc"],
data = [
"//manipulation/models/iiwa_description:models",
],
tags = gurobi_test_tags() + [
# Takes too long to run with Valgrind.
"no_memcheck",
@@ -86,9 +81,6 @@ drake_cc_googletest(
name = "global_inverse_kinematics_feasible_posture_test",
timeout = "long",
srcs = ["test/global_inverse_kinematics_feasible_posture_test.cc"],
data = [
"//manipulation/models/iiwa_description:models",
],
tags = gurobi_test_tags() + [
"no_asan",
# Takes too long to run with Valgrind.
Original file line number Diff line number Diff line change
@@ -38,6 +38,65 @@ void CheckPtCollisionFreeBinary(
EXPECT_EQ(in_one_free_region, 1);
}

struct Box {
Box(const Eigen::Vector3d& m_center, const Eigen::Vector3d& m_size)
: center{m_center}, size{m_size} {}
Eigen::Vector3d center;
Eigen::Vector3d size;
};

GTEST_TEST(GlobalInverseKinematicsTest, BodySphereInOneOfPolytopesTest) {
auto single_body = ConstructSingleBody();

GlobalInverseKinematics::Options options;
options.num_intervals_per_half_axis = 2;
GlobalInverseKinematics global_ik(*single_body, options);

// Add a sphere of radius 0.5 in one of the polytopes.
const double radius = 0.5;
const Eigen::Vector3d p_BQ(0.1, 0.3, 0.4);
const int link_idx = 1;

std::vector<Box> boxes;
boxes.emplace_back(Eigen::Vector3d(0.2, 0.4, 1.2),
Eigen::Vector3d(1.1, 1.2, 1.3));
// This box is not large enough to contain the sphere.
boxes.emplace_back(Eigen::Vector3d(0.1, -0.5, 1.3),
Eigen::Vector3d(0.9, 1.5, 1.4));
std::vector<
std::pair<Eigen::Matrix<double, Eigen::Dynamic, 3>, Eigen::VectorXd>>
polytopes;
for (const auto& box : boxes) {
Eigen::Matrix<double, Eigen::Dynamic, 3> A(6, 3);
A << Eigen::Matrix3d::Identity(), -Eigen::Matrix3d::Identity();
Eigen::VectorXd b(6);
b << box.center + box.size / 2, box.size / 2 - box.center;
polytopes.push_back(std::make_pair(A, b));
}
auto z =
global_ik.BodySphereInOneOfPolytopes(link_idx, p_BQ, radius, polytopes);

solvers::GurobiSolver solver;
const auto result = solver.Solve(global_ik);
EXPECT_EQ(result, solvers::SolutionResult::kSolutionFound);

const auto p_WB_sol =
global_ik.GetSolution(global_ik.body_position(link_idx));
const auto R_WB_sol =
global_ik.GetSolution(global_ik.body_rotation_matrix(link_idx));

const Eigen::Vector3d p_WQ_sol = p_WB_sol + R_WB_sol * p_BQ;
const double tol = 1E-6;
for (int i = 0; i < 3; ++i) {
EXPECT_LE(p_WQ_sol(i) + radius,
boxes[0].center(i) + boxes[0].size(i) / 2 + tol);
EXPECT_GE(p_WQ_sol(i) - radius,
boxes[0].center(i) - boxes[0].size(i) / 2 - tol);
}
EXPECT_TRUE(
CompareMatrices(global_ik.GetSolution(z), Eigen::Vector2d(1, 0), tol));
}

TEST_F(KukaTest, CollisionAvoidanceTest) {
// Suppose there is a box with length 0.15m, located at [-0.5, 0, 0.4]
// The goal is to reach the goal position, without colliding with the box
12 changes: 12 additions & 0 deletions attic/multibody/dev/test/global_inverse_kinematics_test_util.cc
Original file line number Diff line number Diff line change
@@ -34,6 +34,18 @@ std::unique_ptr<RigidBodyTree<double>> ConstructKuka() {
return rigid_body_tree;
}

std::unique_ptr<RigidBodyTree<double>> ConstructSingleBody() {
std::unique_ptr<RigidBodyTree<double>> single_object =
std::make_unique<RigidBodyTree<double>>();
const std::string model_path =
FindResourceOrThrow("drake/multibody/models/box.urdf");
parsers::urdf::AddModelInstanceFromUrdfFile(
model_path, drake::multibody::joints::kQuaternion, nullptr,
single_object.get());

return single_object;
}

KukaTest::KukaTest()
: rigid_body_tree_(ConstructKuka()),
global_ik_(*rigid_body_tree_), // Test with default options.
Original file line number Diff line number Diff line change
@@ -9,6 +9,8 @@ namespace drake {
namespace multibody {
std::unique_ptr<RigidBodyTree<double>> ConstructKuka();

std::unique_ptr<RigidBodyTree<double>> ConstructSingleBody();

class KukaTest : public ::testing::Test {
public:
KukaTest();
32 changes: 32 additions & 0 deletions attic/multibody/global_inverse_kinematics.cc
Original file line number Diff line number Diff line change
@@ -413,6 +413,38 @@ GlobalInverseKinematics::BodyPointInOneOfRegions(
return z;
}

solvers::VectorXDecisionVariable
GlobalInverseKinematics::BodySphereInOneOfPolytopes(
int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
double radius,
const std::vector<std::pair<Eigen::Matrix<double, Eigen::Dynamic, 3>,
Eigen::VectorXd>>& polytopes) {
DRAKE_DEMAND(radius >= 0);
const int num_polytopes = static_cast<int>(polytopes.size());
const auto z = NewBinaryVariables(num_polytopes, "z");
// z1 + ... + zn = 1
AddLinearEqualityConstraint(Eigen::RowVectorXd::Ones(num_polytopes), 1, z);

const auto y =
NewContinuousVariables<3, Eigen::Dynamic>(3, num_polytopes, "y");
const Vector3<symbolic::Expression> p_WQ =
p_WBo_[body_index] + R_WB_[body_index] * p_BQ;
// p_WQ = y.col(0) + ... + y.col(n)
AddLinearEqualityConstraint(
p_WQ - y.cast<symbolic::Expression>().rowwise().sum(),
Eigen::Vector3d::Zero());

for (int i = 0; i < num_polytopes; ++i) {
DRAKE_DEMAND(polytopes[i].first.rows() == polytopes[i].second.rows());
AddLinearConstraint(
polytopes[i].first * y.col(i) <=
(polytopes[i].second - polytopes[i].first.rowwise().norm() * radius) *
z(i));
}

return z;
}

// Approximate a quadratic constraint (which could be formulated as a Lorentz
// cone constraint) xᵀx ≤ c² by
// -c ≤ xᵢ ≤ c
30 changes: 30 additions & 0 deletions attic/multibody/global_inverse_kinematics.h
Original file line number Diff line number Diff line change
@@ -256,6 +256,36 @@ class GlobalInverseKinematics : public solvers::MathematicalProgram {
int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
const std::vector<Eigen::Matrix3Xd>& region_vertices);

/**
* Adds the constraint that a sphere rigidly attached to a body has to be
* within one of the given bounded polytopes.
* If the i'th polytope is described as
* bᵢ_lb ≤ Aᵢ * x ≤ bᵢ_ub
* Then a sphere with center position p_WQ and radius r is within the i'th
* polytope, if
* Aᵢ * p_WQ ≤ bᵢ - aᵢr
* where aᵢ(j) = Aᵢ.row(j).norm()
* To constrain that the sphere is in one of the n polytopes, we introduce the
* binary variable z ∈{0, 1}ⁿ, together with continuous variables yᵢ ∈ ℝ³, i
* = 1, ..., n, such that
* p_WQ = y₁ + ... + yₙ
* Aᵢ * yᵢ ≤ (bᵢ - aᵢr)zᵢ
* z₁ + ... +zₙ = 1
* @param body_index The index of the body to which the sphere is attached.
* @param p_BQ The position of the sphere center in the body frame B.
* @param radius The radius of the sphere.
* @param polytopes. polytopes[i] = (Aᵢ, bᵢ). We assume that Aᵢx≤ bᵢ is a
* bounded polytope. It is the user's responsibility to guarantee the
* boundedness.
* @retval z The newly added binary variables. If the sphere is in the i'th
* polytope, then z(i) = 1
*/
solvers::VectorXDecisionVariable BodySphereInOneOfPolytopes(
int body_index, const Eigen::Ref<const Eigen::Vector3d>& p_BQ,
double radius,
const std::vector<std::pair<Eigen::Matrix<double, Eigen::Dynamic, 3>,
Eigen::VectorXd>>& polytopes);

/**
* Adds joint limits on a specified joint.
* @param body_index The joint connecting the parent link to this body will be

0 comments on commit a620cd2

Please sign in to comment.