def test_normalize_vector(self): '''Test vector normalization''' for k in range(10): rand_vec = np.random.random(k) # Make a random k-length vector # Ignore the astronomically unlikely case that a vector has near 0 norm if not np.isclose(np.sum(rand_vec), 0): normalized = normalize(rand_vec) norm = np.linalg.norm(normalized) # Test that the norm is 1 np.testing.assert_almost_equal(norm, 1.0, err_msg="The normalized vector did not have length 1")
def test_normalize_vector(self): '''Test vector normalization''' for k in range(10): rand_vec = np.random.random(k) # Make a random k-length vector # Ignore the astronomically unlikely case that a vector has near 0 norm if not np.isclose(np.sum(rand_vec), 0): normalized = normalize(rand_vec) norm = np.linalg.norm(normalized) # Test that the norm is 1 np.testing.assert_almost_equal( norm, 1.0, err_msg="The normalized vector did not have length 1")
def set_pose_server(self, srv): '''Set the pose of the submarine TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id ''' rospy.logwarn("Manually setting position of simulated vehicle") position = sub8_utils.rosmsg_to_numpy(srv.pose.position) self.body.setPosition(position) rotation_q = sub8_utils.rosmsg_to_numpy(srv.pose.orientation) rotation_norm = np.linalg.norm(rotation_q) velocity = sub8_utils.rosmsg_to_numpy(srv.twist.linear) angular = sub8_utils.rosmsg_to_numpy(srv.twist.angular) self.body.setLinearVel(velocity) self.body.setAngularVel(angular) # If the commanded rotation is valid if np.isclose(rotation_norm, 1., atol=1e-2): self.body.setQuaternion(sub8_utils.normalize(rotation_q)) else: rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation") return SimSetPoseResponse()