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
self.ik_two_bodies.AddOrientationConstraint(
frameA=self.body1_frame, frameB=self.body2_frame,
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_AB = body1_rotmat.transpose().dot(body2_rotmat)
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 = pydrake.math.RotationMatrix(
quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = pydrake.math.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)