def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimum_distance=min_distance)
context = self.plant.CreateDefaultContext()
self.plant.SetFreeBodyPose(
context, B1.body(), RigidTransform([0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), RigidTransform([0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = np.linalg.norm(
X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(ik.q()), q_val))
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(context, B1.body(),
Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(context, B2.body(),
Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.plant.SetPositions(context, self.prog.GetSolution(ik.q()))
self.assertGreater(get_min_distance_actual(), min_distance - tol)
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(
context, B1.body(), Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.plant.SetPositions(context, self.prog.GetSolution(ik.q()))
self.assertGreater(get_min_distance_actual(), min_distance - tol)
def test_AddDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
distance_lower = 0.1
distance_upper = 0.2
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
inspector = self.scene_graph.model_inspector()
frame_id1 = inspector.GetGeometryIdByName(
self.plant.GetBodyFrameIdOrThrow(
self.plant.GetBodyByName("body1").index()),
pydrake.geometry.Role.kProximity, "two_bodies::body1_collision")
frame_id2 = inspector.GetGeometryIdByName(
self.plant.GetBodyFrameIdOrThrow(
self.plant.GetBodyByName("body2").index()),
pydrake.geometry.Role.kProximity, "two_bodies::body2_collision")
ik.AddDistanceConstraint(
geometry_pair=(frame_id1, frame_id2),
distance_lower=distance_lower, distance_upper=distance_upper)
context = self.plant.CreateDefaultContext()
self.plant.SetFreeBodyPose(
context, B1.body(), RigidTransform([0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), RigidTransform([0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = np.linalg.norm(
X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), distance_lower - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), distance_lower - tol)
self.assertLess(get_min_distance_actual(), distance_upper + tol)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(ik.q()), q_val))
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(
context, B1.body(), Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(ik.q()), q_val))
self.assertEqual(len(w), 2)
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(context, B1.body(),
Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(context, B2.body(),
Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(self.prog.Solve(),
mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(self.prog.GetSolution(ik.q()), q_val))
self.assertEqual(len(w), 2)
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
self.plant.SetFreeBodyPose(
context, B1.body(), RigidTransform([0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), RigidTransform([0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(ik.q()), q_val))
