def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(
p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(
p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all())
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
np.testing.assert_array_equal(self.plant.GetPositions(
self.ik_two_bodies.context()), q_val)
self.assertIs(self.ik_two_bodies.get_mutable_context(),
self.ik_two_bodies.context())
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
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(self.q), q_val))
self.assertEqual(len(w), 2)
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(quaternion=Quaternion(1.0 / 3, 2.0 /
3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(frameAbar=self.body1_frame,
R_AbarA=R_AbarA,
frameBbar=self.body2_frame,
R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(self.prog.Solve(),
mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
self.assertEqual(len(w), 2)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = self.prog.Solve()
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
