def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) 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() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6) 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_rotation_matrix(self): # - Constructors. R = mut.RotationMatrix() self.assertTrue( np.allclose(mut.RotationMatrix(other=R).matrix(), np.eye(3))) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3))) self.assertTrue( np.allclose(mut.RotationMatrix.Identity().matrix(), np.eye(3))) R = mut.RotationMatrix(R=np.eye(3)) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(quaternion=Quaternion.Identity()) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0])) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) # - Nontrivial quaternion. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) R = mut.RotationMatrix(quaternion=q) q_R = R.ToQuaternion() self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz())) # - Inverse. R_I = R.inverse().multiply(R) self.assertTrue(np.allclose(R_I.matrix(), np.eye(3))) if six.PY3: self.assertTrue( np.allclose(eval("R.inverse() @ R").matrix(), np.eye(3)))
def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_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) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6)
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_rotation(), Quaternion)) self.assertTrue(isinstance(value.get_translation(), np.ndarray)) # - Value. self.assertTrue( np.allclose(value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue( np.allclose(value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue( np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
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 = 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_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) 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_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_rotation_matrix(self): # - Constructors. R = mut.RotationMatrix() self.assertTrue(np.allclose( mut.RotationMatrix(other=R).matrix(), np.eye(3))) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) self.assertTrue(np.allclose(copy.copy(R).matrix(), np.eye(3))) self.assertTrue(np.allclose( mut.RotationMatrix.Identity().matrix(), np.eye(3))) R = mut.RotationMatrix(R=np.eye(3)) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(quaternion=Quaternion.Identity()) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) R = mut.RotationMatrix(rpy=mut.RollPitchYaw(rpy=[0, 0, 0])) self.assertTrue(np.allclose(R.matrix(), np.eye(3))) # - Nontrivial quaternion. q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5]) R = mut.RotationMatrix(quaternion=q) q_R = R.ToQuaternion() self.assertTrue(np.allclose(q.wxyz(), q_R.wxyz())) # - Inverse. R_I = R.inverse().multiply(R) self.assertTrue(np.allclose(R_I.matrix(), np.eye(3))) if six.PY3: self.assertTrue(np.allclose( eval("R.inverse() @ R").matrix(), np.eye(3)))
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance( value.get_isometry(), Isometry3)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.matrix()))
def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) 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_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 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) 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_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) self.assertEqual(len(w), 2)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry(road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_transform(), RigidTransform)) self.assertTrue(isinstance( value.get_rotation(), Quaternion)) self.assertTrue(isinstance( value.get_translation(), np.ndarray)) # - Value. with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), np.eye(4, 4))) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = RigidTransform(quaternion=q, p=p) value.set_translation(p) value.set_rotation(q) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) with catch_drake_warnings(expected_count=1): self.assertTrue(np.allclose( value1.get_isometry().matrix(), X_expected.GetAsMatrix4())) self.assertTrue(np.allclose( value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4())) # Test mutation via RigidTransform p2 = [10, 20, 30] q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8])) X2_expected = RigidTransform(quaternion=q2, p=p2) value.set_transform(X2_expected) self.assertTrue(np.allclose( value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
def transform_from_dict(data): pos, _ = get_data_from_dict(data, ['pos', 'position', 'translation']) orientation, key = get_data_from_dict(data, ['quat', 'quaternion', 'rpy']) if key == "rpy": return RigidTransform(RollPitchYaw(orientation), pos) else: return RigidTransform(Quaternion(orientation), pos)
def CalcOutput(self, context, output): t = context.get_time() pos_vec = self.traj_pos.value(t) rot_mat_vec = self.traj_rot.value(t) rb = RigidTransform(Quaternion(rot_mat_vec), pos_vec) output.SetFrom(Value[RigidTransform](rb))
def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) pose = RigidTransform(quaternion=q, p=p) bundle.set_transform(kIndex, pose) self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() == pose.GetAsMatrix34()).all()) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def abstract_handler(self, msg, name, lcmMessage): ''' Function for handling LCM messages originating from a different channel than the main one msg: the returning LCM message field: the field name/path name: name of corresponding shape x, y, z: indices for each of the 3 dimensions of the location where the shape is to be drawn. When the shape is an axis then x will be the first index of the quaternion array and the other 3 will be the next indices in sequence ''' self.handle_checkBox(name) attribute = msg field = lcmMessage.field # if there is a %d in the field replace it with the actual index if ("%d" in field): arr = self.getVector(attribute, lcmMessage.index_field) if (lcmMessage.index_element in arr): i = arr.index(lcmMessage.index_element) field = field.replace("%d", str(i)) if ("%d" not in field): # parse field to get the appropriate information. This is done by # recursively searching for the right attribute in the given message's field attribute = self.getVector(attribute, field) currShape = self.shapes[name] x = lcmMessage.x # special case of an axis data if (currShape.type == "axes"): # get quaternion array, normalize it, and get the corresponding rotation matrix quaternion = [] for i in range(x, x + 4): quaternion.append(attribute[i]) norm = np.linalg.norm(quaternion) quaternion = [x / norm for x in quaternion] rot_matrix = Quaternion(quaternion).rotation() pt_world = self.plant.CalcPointsPositions( self.context, self.plant.GetFrameByName(currShape.frame), currShape.point, self.plant.world_frame()) next_loc = pt_world.transpose()[0] self.drawShape(self.shapes[name], next_loc, msg, rotation_matrix=rot_matrix) else: next_loc = [attribute[x], attribute[x + 1], attribute[x + 2]] self.drawShape(self.shapes[name], next_loc, msg) else: print(lcmMessage.index_element + "is not present in " + lcmMessage.index_field + " and so it cannot be visualized") print("")
def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) 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_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater( p_ST_W.dot(n_W), np.linalg.norm(p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue( np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0])) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(matrix=np.eye(3)) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3))) # - Converting changes in orientation self.assertTrue( np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]), np.zeros((3, 3)))) self.assertTrue( np.allclose( rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]), [0, 0, 0])) self.assertTrue( np.allclose( rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]), [0, 0, 0])) self.assertTrue( np.allclose( rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]), [0, 0, 0])) self.assertTrue( np.allclose( rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent( rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0])) self.assertTrue( np.allclose( rpy.CalcRpyDDtFromAngularAccelInChild(rpyDt=[0, 0, 0], alpha_AD_D=[0, 0, 0]), [0, 0, 0]))
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue( np.allclose(mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0])) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3)))
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue(np.allclose( mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0])) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3)))
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 _to_pose(position, quaternion): """Given pose parts of an lcmt_viewer_geometry_data, parse it into a RigidTransform.""" (p_x, p_y, p_z) = position (q_w, q_x, q_y, q_z) = quaternion return RigidTransform(R=RotationMatrix(quaternion=Quaternion( w=q_w, x=q_x, y=q_y, z=q_z, ), ), p=(p_x, p_y, p_z))
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_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) 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() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) self.assertTrue(np.allclose(result.GetSolution(self.q), q_val))
def test_AddPointToPointDistanceConstraint(self): p_B1P1 = np.array([0.2, -0.4, 0.9]) p_B2P2 = np.array([1.4, -0.1, 1.8]) distance_lower = 0.1 distance_upper = 0.2 self.ik_two_bodies.AddPointToPointDistanceConstraint( frame1=self.body1_frame, p_B1P1=p_B1P1, frame2=self.body2_frame, p_B2P2=p_B2P2, distance_lower=distance_lower, distance_upper=distance_upper) 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() p_WP1 = self._body1_xyz(q_val) + body1_rotmat.dot(p_B1P1) p_WP2 = self._body2_xyz(q_val) + body2_rotmat.dot(p_B2P2) distance = np.linalg.norm(p_WP1 - p_WP2) self.assertLess(distance, distance_upper + 3e-6) self.assertGreater(distance, distance_lower - 3e-6) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) self.assertTrue(np.allclose(result.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 = 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_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)
def get_q_object_final(self, q_object_init, ee_init, ee_final): """ Input: q_object_init: [quaternion, xyz] ee_init: [x y z r p y] ee_final: [x y z r p y] xyz_iiwa: [x, y, z] Return: q_object_final: [quaternion, xyz] """ xyz_obj = np.array(q_object_init[4:]).transpose() # xyz_obj[0] -= np.array(xyz_iiwa) # convert translation from world frame to robot base frame quaternion_obj = Quaternion(np.array(q_object_init[:4])) rot_obj = RotationMatrix(quaternion_obj) X_WO = RigidTransform(rot_obj, xyz_obj) xyz_ee_init = np.array(ee_init[:3]).transpose() rot_ee_init = RollPitchYaw(ee_init[3], ee_init[4], ee_init[5]).ToRotationMatrix() X_WE1 = RigidTransform(rot_ee_init, xyz_ee_init) xyz_ee_final = np.array(ee_final[:3]).transpose() rot_ee_final = RollPitchYaw(ee_final[3], ee_final[4], ee_final[5]).ToRotationMatrix() X_WE2 = RigidTransform(rot_ee_final, xyz_ee_final) X_EO = X_WE1.inverse().multiply(X_WO) X_WO2 = X_WE2.multiply(X_EO) wxyz_obj_final = X_WO2.rotation().ToQuaternion().wxyz() xyz_obj_final = X_WO2.translation() # xyz_ee_obj_init = xyz_obj - xyz_ee_init # rot_ee_init_final = rot_ee_init.multiply(rot_ee_final.transpose()) # xyz_ee_obj_final = np.matmul(rot_ee_init_final.matrix(), xyz_ee_obj_init) # # xyz_ee_obj_final += np.array(xyz_iiwa) # convert translation from robot base frame to world frame # xyz_obj_final = xyz_ee_final + xyz_ee_obj_final # rot_obj_final = rot_ee_init_final.multiply(rot_obj) # quaternion_obj_final = rot_obj_final.ToQuaternion() # wxyz_obj_final = quaternion_obj_final.wxyz() q_object_final = [] for i in range(4): q_object_final.append(wxyz_obj_final[i]) for i in range(3): q_object_final.append(xyz_obj_final[i]) return q_object_final
def test_roll_pitch_yaw(self): # - Constructors. rpy = mut.RollPitchYaw(rpy=[0, 0, 0]) self.assertTrue(np.allclose( mut.RollPitchYaw(other=rpy).vector(), [0, 0, 0])) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(roll=0, pitch=0, yaw=0) self.assertTupleEqual( (rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle()), (0, 0, 0)) rpy = mut.RollPitchYaw(R=mut.RotationMatrix()) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) rpy = mut.RollPitchYaw(matrix=np.eye(3)) self.assertTrue(np.allclose(rpy.vector(), [0, 0, 0])) q_I = Quaternion() rpy_q_I = mut.RollPitchYaw(quaternion=q_I) self.assertTrue(np.allclose(rpy_q_I.vector(), [0, 0, 0])) # - Additional properties. self.assertTrue(np.allclose(rpy.ToQuaternion().wxyz(), q_I.wxyz())) R = rpy.ToRotationMatrix().matrix() self.assertTrue(np.allclose(R, np.eye(3))) # - Converting changes in orientation self.assertTrue(np.allclose(rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]), np.zeros((3, 3)))) self.assertTrue(np.allclose( rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]), [0, 0, 0])) self.assertTrue(np.allclose( rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]), [0, 0, 0])) self.assertTrue(np.allclose( rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]), [0, 0, 0])) self.assertTrue(np.allclose( rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent( rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0, 0, 0])) self.assertTrue(np.allclose(rpy.CalcRpyDDtFromAngularAccelInChild( rpyDt=[0, 0, 0], alpha_AD_D=[0, 0, 0]), [0, 0, 0]))
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
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 add_sensor(self, camera_name, sensor_config): """ Adds Rgbd sensor to the diagram :param camera_name: :type camera_name: :param sensor_config: :type sensor_config: :return: :rtype: """ builder = self.builder sg = self.sg width = sensor_config['width'] height = sensor_config['height'] fov_y = sensor_config['fov_y'] z_near = sensor_config['z_near'] z_far = sensor_config['z_far'] renderer_name = self._renderer_name depth_camera_properties = DepthCameraProperties( width=width, height=height, fov_y=fov_y, renderer_name=renderer_name, z_near=z_near, z_far=z_far) parent_frame_id = sg.world_frame_id() # This is in right-down-forward convention pos = np.array(sensor_config['pos']) quat_eigen = Quaternion(sensor_config['quat']) # camera to world transform T_W_camera = RigidTransform(quaternion=quat_eigen, p=pos) # add camera system camera = builder.AddSystem( RgbdSensor(parent_frame_id, T_W_camera, depth_camera_properties, show_window=False)) builder.Connect(sg.get_query_output_port(), camera.query_object_input_port()) self._rgbd_sensors[camera_name] = camera
def test_api(self): plant = MultibodyPlant(time_step=0.01) model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) plant.Finalize() context = plant.CreateDefaultContext() options = ik.GlobalInverseKinematics.Options() global_ik = ik.GlobalInverseKinematics(plant=plant, options=options) self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram) self.assertIsInstance(global_ik.get_mutable_prog(), mp.MathematicalProgram) body_index_A = plant.GetBodyIndices(model_instance)[0] body_index_B = plant.GetBodyIndices(model_instance)[1] self.assertEqual( global_ik.body_rotation_matrix(body_index=body_index_A).shape, (3, 3)) self.assertEqual( global_ik.body_position(body_index=body_index_A).shape, (3, )) global_ik.AddWorldPositionConstraint( body_index=body_index_A, p_BQ=[0, 0, 0], box_lb_F=[-np.inf, -np.inf, -np.inf], box_ub_F=[np.inf, np.inf, np.inf], X_WF=RigidTransform()) global_ik.AddWorldRelativePositionConstraint( body_index_B=body_index_B, p_BQ=[0, 0, 0], body_index_A=body_index_A, p_AP=[0, 0, 0], box_lb_F=[-np.inf, -np.inf, -np.inf], box_ub_F=[np.inf, np.inf, np.inf], X_WF=RigidTransform()) global_ik.AddWorldOrientationConstraint( body_index=body_index_A, desired_orientation=Quaternion(), angle_tol=np.inf) global_ik.AddPostureCost( q_desired=plant.GetPositions(context), body_position_cost=[1] * plant.num_bodies(), body_orientation_cost=[1] * plant.num_bodies()) gurobi_solver = GurobiSolver() if gurobi_solver.available(): global_ik.SetInitialGuess(q=plant.GetPositions(context)) result = gurobi_solver.Solve(global_ik.prog()) self.assertTrue(result.is_success()) global_ik.ReconstructGeneralizedPositionSolution(result=result)
def test_rigid_transform(self): def check_equality(X_actual, X_expected_matrix): # TODO(eric.cousineau): Use `IsNearlyEqualTo`. self.assertIsInstance(X_actual, mut.RigidTransform) self.assertTrue( np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix)) # - Constructors. X_I = np.eye(4) check_equality(mut.RigidTransform(), X_I) check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I) check_equality(copy.copy(mut.RigidTransform()), X_I) R_I = mut.RotationMatrix() p_I = np.zeros(3) rpy_I = mut.RollPitchYaw(0, 0, 0) quaternion_I = Quaternion.Identity() angle = np.pi * 0 axis = [0, 0, 1] angle_axis = AngleAxis(angle=angle, axis=axis) check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I) check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I) check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I) check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I) check_equality(mut.RigidTransform(R=R_I), X_I) check_equality(mut.RigidTransform(p=p_I), X_I) # - Accessors, mutators, and general methods. X = mut.RigidTransform() X.set(R=R_I, p=p_I) X.SetFromIsometry3(pose=Isometry3.Identity()) check_equality(mut.RigidTransform.Identity(), X_I) self.assertIsInstance(X.rotation(), mut.RotationMatrix) X.set_rotation(R=R_I) self.assertIsInstance(X.translation(), np.ndarray) X.set_translation(p=np.zeros(3)) self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I)) self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3])) self.assertIsInstance(X.GetAsIsometry3(), Isometry3) check_equality(X.inverse(), X_I) self.assertIsInstance( X.multiply(other=mut.RigidTransform()), mut.RigidTransform) self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray) if six.PY3: self.assertIsInstance( eval("X @ mut.RigidTransform()"), mut.RigidTransform) self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
def get_box_from_geom(scene_graph, visual_only=True): # https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py # https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm mock_lcm = DrakeMockLcm() DispatchLoadMessage(scene_graph, mock_lcm) load_robot_msg = lcmt_viewer_load_robot.decode( mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT")) box_from_geom = {} for body_index in range(load_robot_msg.num_links): # 'geom', 'name', 'num_geom', 'robot_num' link = load_robot_msg.link[body_index] [source_name, frame_name] = link.name.split("::") model_index = link.robot_num visual_index = 0 for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x # 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type' if visual_only and (geom.color[3] == 0): continue visual_index += 1 if geom.type == geom.BOX: assert geom.num_float_data == 3 [width, length, height] = geom.float_data extent = np.array([width, length, height]) / 2. elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 [radius] = geom.float_data extent = np.array([radius, radius, radius]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 [radius, height] = geom.float_data extent = np.array([radius, radius, height/2.]) # In Drake, cylinders are along +z else: continue link_from_box = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3() box_from_geom[model_index, frame_name, visual_index-1] = \ (BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom)) return box_from_geom
def __init__(self, server, tf_buffer, joints): super().__init__() # TODO(sloretz) do I need to store allllll of this in the context? # if so, how tf do I get the context in callbacks? self._tf_buffer = tf_buffer # System will publish transforms for these joints only self._joints = tuple(joints) if 0 == len(self._joints): raise ValueError('Need at least one joint') for joint in self._joints: if not Joint_.is_subclass_of_instantiation(type(joint)): raise TypeError('joints must be an iterable of Joint_[T]') self._server = server self._joint_states = [0.0] * len(self._joints) # Map joint names to indexes in joint_states self._joint_indexes = {j.name(): i for j, i in zip(self._joints, range(len(self._joints)))} self._joint_prev_orientation = {j.name(): Quaternion() for j in self._joints} self._joint_axis_in_child = {} for joint in self._joints: if RevoluteJoint_.is_subclass_of_instantiation(type(joint)): server.insert( self._make_revolute_marker(joint), feedback_callback=functools.partial(self._revolute_feedback, joint)) else: # TODO(sloretz) support more than revolute joints raise TypeError('joints must be an iterable of RevoluteJoint_[T]') server.applyChanges() self.DeclareVectorOutputPort( 'joint_states', BasicVector_[float](len(self._joint_states)), self._do_get_joint_states)
def _revolute_feedback(self, joint, feedback): if feedback.event_type != InteractiveMarkerFeedback.POSE_UPDATE: return expected_frame = joint.child_body().body_frame().name() if expected_frame != feedback.header.frame_id: # TODO(sloretz) fix tf2_geometry_msgs_py :( # transformed_point = self._tf_buffer.transform(point_stamped, self._frame_id) print("TODO accept feedback in different frame") return qw = feedback.pose.orientation.w qx = feedback.pose.orientation.x qy = feedback.pose.orientation.y qz = feedback.pose.orientation.z new_orientation = Quaternion(qw, qx, qy, qz) prev_orientation = self._joint_prev_orientation[joint.name()] orientation_diff = prev_orientation.inverse().multiply(new_orientation) diff_aa = AngleAxis(orientation_diff) joint_axis = self._joint_axis_in_child[joint.name()] dot = numpy.dot(joint_axis, diff_aa.axis()) if dot > 0.999: angle_inc = diff_aa.angle() elif dot < -0.999: angle_inc = -1 * diff_aa.angle() else: angle_inc = 0. angle = self._joint_states[self._joint_indexes[joint.name()]] + angle_inc if angle > joint.position_upper_limit(): angle = joint.position_upper_limit() elif angle < joint.position_lower_limit(): angle = joint.position_lower_limit() self._joint_states[self._joint_indexes[joint.name()]] = angle self._joint_prev_orientation[joint.name()] = new_orientation
def test_quaternion_slerp(self): # Test empty constructor. pq = PiecewiseQuaternionSlerp() self.assertEqual(pq.rows(), 4) self.assertEqual(pq.cols(), 1) self.assertEqual(pq.get_number_of_segments(), 0) t = [0., 1., 2.] # Make identity rotations. q = Quaternion() m = np.identity(3) a = AngleAxis() R = RotationMatrix() # Test quaternion constructor. pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test matrix constructor. pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test axis angle constructor. pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test rotation matrix constructor. pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test append operations. pq.Append(time=3., quaternion=q) pq.Append(time=4., rotation_matrix=R) pq.Append(time=5., angle_axis=a)
def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) 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() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6) 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 _convert_mesh(geom): # Given a LCM geometry message, forms a meshcat geometry and material # for that geometry, as well as a local tf to the meshcat geometry # that matches the LCM geometry. # For MESH geometry, this function checks if a texture file exists next # to the mesh file, and uses that to provide the material information if # present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry # not satisfying the above, this function uses the geom.color field to # create a flat color for the object. In the case of other geometry types, # both fields are returned as None. meshcat_geom = None material = None element_local_tf = RigidTransform( RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsMatrix4() if geom.type == geom.BOX: assert geom.num_float_data == 3 meshcat_geom = meshcat.geometry.Box(geom.float_data) elif geom.type == geom.SPHERE: assert geom.num_float_data == 1 meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0]) elif geom.type == geom.CYLINDER: assert geom.num_float_data == 2 meshcat_geom = meshcat.geometry.Cylinder( geom.float_data[1], geom.float_data[0]) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi/2., [1, 0, 0]) element_local_tf[0:3, 0:3] = ( element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3])) elif geom.type == geom.MESH: meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file( geom.string_data[0:-3] + "obj") # Handle scaling. # TODO(gizatt): See meshcat-python#40 for incorporating scale as a # field rather than a matrix multiplication. scale = geom.float_data[:3] element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale)) # Attempt to find a texture for the object by looking for an # identically-named *.png next to the model. # TODO(gizatt): Support .MTLs and prefer them over png, since they're # both more expressive and more standard. # TODO(gizatt): In the long term, this kind of material information # should be gleaned from the SceneGraph constituents themselves, so # that we visualize what the simulation is *actually* reasoning about # rather than what files happen to be present. candidate_texture_path_png = geom.string_data[0:-3] + "png" if os.path.exists(candidate_texture_path_png): material = meshcat.geometry.MeshLambertMaterial( map=meshcat.geometry.ImageTexture( image=meshcat.geometry.PngImage.from_file( candidate_texture_path_png))) else: print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format( geom.type)) return meshcat_geom, material if material is None: def rgb_2_hex(rgb): # Turn a list of R,G,B elements (any indexable list # of >= 3 elements will work), where each element is # specified on range [0., 1.], into the equivalent # 24-bit value 0xRRGGBB. val = 0 for i in range(3): val += (256**(2 - i)) * int(255 * rgb[i]) return val material = meshcat.geometry.MeshLambertMaterial( color=rgb_2_hex(geom.color[:3]), transparent=geom.color[3] != 1., opacity=geom.color[3]) return meshcat_geom, material, element_local_tf
def test_quaternion_slerp(self): # Test empty constructor. pq = PiecewiseQuaternionSlerp() self.assertEqual(pq.rows(), 4) self.assertEqual(pq.cols(), 1) self.assertEqual(pq.get_number_of_segments(), 0) t = [0., 1., 2.] # Make identity rotations. q = Quaternion() m = np.identity(3) a = AngleAxis() R = RotationMatrix() # Test quaternion constructor. pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test matrix constructor. pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test axis angle constructor. pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test rotation matrix constructor. pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R]) self.assertEqual(pq.get_number_of_segments(), 2) np.testing.assert_equal(pq.value(0.5), np.eye(3)) # Test append operations. pq.Append(time=3., quaternion=q) pq.Append(time=4., rotation_matrix=R) pq.Append(time=5., angle_axis=a) # Test getters. pq = PiecewiseQuaternionSlerp( breaks=[0, 1], angle_axes=[a, AngleAxis(np.pi / 2, [0, 0, 1])]) np.testing.assert_equal(np.array([1, 0, 0, 0]), pq.orientation(time=0).wxyz()) np.testing.assert_allclose( np.array([0, 0, np.pi / 2]), pq.angular_velocity(time=0), atol=1e-15, rtol=0, ) np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=0)) np.testing.assert_allclose( np.array([np.cos(np.pi / 4), 0, 0, np.sin(np.pi / 4)]), pq.orientation(time=1).wxyz(), atol=1e-15, rtol=0, ) np.testing.assert_allclose( np.array([0, 0, np.pi / 2]), pq.angular_velocity(time=1), atol=1e-15, rtol=0, ) np.testing.assert_equal(np.zeros(3), pq.angular_acceleration(time=1))