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)