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(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 = 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)