def reset(self):
self.velocity = Twist(0, 0)
self.pose = RigidTransform(Translation(0, 0), Rotation.from_degrees(0))
self.previous_pose = RigidTransform(Translation(0, 0),
Rotation.from_degrees(0))
self.prev_left_pos = 0
self.prev_right_pos = 0
def __init__(self, x, y, theta, position):
self.position_ = position
self.origin_ = Point2D(x, y)
self.theta_ = theta
self.rotation_ = Rotation(theta, AngleUnit.RADIANS)
def __init__(self, translation_=None, rotation_=None):
if translation_ == None:
self.translation = Translation()
else:
self.translation = translation_
if rotation_ == None:
self.rotation = Rotation()
else:
self.rotation = rotation_
def test_do_start(self):
_rot = Rotation()
_round = (Counterweight(), (TacticDefense(), ))
_rot.add(_round)
Combat.do_start()
self.assertTrue(Combat.workers_exist())
self.assertEqual(Combat._round, 1)
self.assertEqual(len(Combat._keys), 1)
def test_do_round(self):
_rot = Rotation()
_round = (Counterweight(), (TacticDefense(), ))
_rot.add(_round)
Combat.start_workers()
Combat.do_round()
Combat.stop()
# test output
keys = [kp.get("key") for kp in _globals.KEY_PRESSES]
self.assertEqual(''.join(keys), "req")
def __init__(self, small_blind: int, player_list: list) -> None:
self.card_deck, self.current_pot, self.small_blind, self.player_list, self.big_blind =\
card_deck, 0, small_blind, player_list, small_blind * 2
self.community_cards_hidden = []
self.community_cards_open = []
for player in self.player_list:
# Sets a player with position 0 to move in the beginning of each round
if player.position == 0:
player.is_turn = True
# creates a rotation
self.current_rotation = Rotation(self.small_blind, self.player_list)
def __init__(self, orient=[0, 0, 0], offset=[0, 0, 0]):
# type: (object, object) -> object
self.offset = np.array(offset)
self.R = Rotation()
m_orient = np.array(orient)
self.globalPoints = None
self.imagePoints = False
if m_orient.size == 3:
self.basis = self.R.getCameraBasis(orient)
else:
self.basis = m_orient
def remove_y_operators_from_circuit(self, start_index:int=0) -> None:
"""
Removes Y operators from pi/8 and measurement blocks. To be called after pi/4 rotations
have been commuted to the end of the circuit.
"""
i = start_index
while i < len(self.ops):
pauli_block = self.ops[i]
if isinstance(pauli_block, Measurement) or (pauli_block.rotation_amount in {Fraction(1,8), Fraction(-1,8)}):
y_op_indices = list()
# Find Y operators and modify them into X operators
for j in range(self.qubit_num):
if pauli_block.ops_list[j] == PauliOperator.Y:
y_op_indices.append(j)
pauli_block.ops_list[j] = PauliOperator.X
if y_op_indices:
right_block_indices = list()
# For even numbers of Y operators, add 2 additional pi/4 rotations (one on each side)
if len(y_op_indices) % 2 == 0:
first_operator = y_op_indices.pop(0)
self.add_single_operator(first_operator, PauliOperator.Z, Fraction(1,4), i)
self.add_single_operator(first_operator, PauliOperator.Z, Fraction(-1,4), i+2)
right_block_indices.append(i+4)
i += 1
# Add 2 pi/4 rotations on each side of the Pauli block
new_block = [PauliOperator.Z if i in y_op_indices else PauliOperator.I for i in range(self.qubit_num)]
self.add_pauli_block(Rotation.from_list(new_block, Fraction(1,4)), i)
self.add_pauli_block(Rotation.from_list(new_block, Fraction(-1,4)), i+2)
right_block_indices.append(i+2)
i += 1
# Commute the right (new) pi/4 rotations towards the end of the circuit
for right_block_index in right_block_indices:
while right_block_index + 1 < len(self.ops):
self.commute_pi_over_four_rotation(right_block_index)
right_block_index += 1
self.ops.pop()
else:
# This is assuming pi/4 rotations (not including ones from this operation)
# have been commuted to the end of the circuit.
break
i += 1
def reset_car(self, ndist, next_index):
''' Reset's the car on the track
ndist - normalized track distance
next_index - index of the next way point
'''
# Compute the starting position and heading
start_point = self.center_line.interpolate(ndist, normalized=True)
start_yaw = math.atan2(
self.center_line.coords[next_index][1] - start_point.y,
self.center_line.coords[next_index][0] - start_point.x)
start_quaternion = Rotation.from_euler('zyx',
[start_yaw, 0, 0]).as_quat()
# Construct the model state and send to Gazebo
model_state = ModelState()
model_state.model_name = 'racecar'
model_state.pose.position.x = start_point.x
model_state.pose.position.y = start_point.y
model_state.pose.position.z = 0
model_state.pose.orientation.x = start_quaternion[0]
model_state.pose.orientation.y = start_quaternion[1]
model_state.pose.orientation.z = start_quaternion[2]
model_state.pose.orientation.w = start_quaternion[3]
model_state.twist.linear.x = 0
model_state.twist.linear.y = 0
model_state.twist.linear.z = 0
model_state.twist.angular.x = 0
model_state.twist.angular.y = 0
model_state.twist.angular.z = 0
self.model_state_client(model_state)
def get_args(self):
if self.op == None: return
pts = self._requirements.get(self.op) # Get requirements
if self.op == 'rotate':
p = PopupWindow(self.pz.frame, "Insira o angulo (em graus)")
self.pz.frame.wait_window(p.top)
p_list = list(sum(self.pz.buffer[:pts], ()))
T = Rotation(float(p.getval()), p_list[0], p_list[1])
if self.op == 'scale':
px = PopupWindow(self.pz.frame, "Insira um fator para x")
self.pz.frame.wait_window(px.top)
py = PopupWindow(self.pz.frame, "Insira um fator para y")
self.pz.frame.wait_window(py.top)
p_list = list(sum(self.pz.buffer[:pts], ()))
T = Scale(float(px.getval()), float(py.getval()), p_list[0],
p_list[1])
if self.op == 'translate':
px = PopupWindow(self.pz.frame, "Insira um offset para x")
self.pz.frame.wait_window(px.top)
py = PopupWindow(self.pz.frame, "Insira um offset para y")
self.pz.frame.wait_window(py.top)
T = Translation(float(px.getval()), float(py.getval()))
if self.op == 'zoom':
self.zoom(self.pz.buffer[:pts])
T = None
self.pz.buffer = self.pz.buffer[pts:] # Remove points of buffer
return T
def test_combat_rotation_property(self):
# test initial
self.assertEqual(Combat.rotation, None)
Combat.rotation = Rotation()
# test _keys private variable for correctness
self.assertTrue(len(Combat._keys) is 0)
def __init__(self,
rotation=True,
translate=True,
scale=True,
horizontal_flip=True,
**kwargs):
self.augmentators = {}
if rotation:
ag = kwargs['ag'] if 'ag' in kwargs else 30
self.rotation = Rotation(ag)
self.augmentators['rotation'] = self.rotation
if translate:
tr = kwargs['tr'] if 'tr' in kwargs else 0.2
self.translate = Translate(tr)
self.augmentators['translate'] = self.translate
if scale:
sc_lower = kwargs['sc_lower'] if 'sc_lower' in kwargs else 0.2
sc_upper = kwargs['sc_upper'] if 'sc_upper' in kwargs else 0.2
self.scale = Scale((sc_lower, sc_upper))
self.augmentators['scale'] = self.scale
if horizontal_flip:
self.horizontal_flip = HorizontalFlip()
self.augmentators['horizontal_flip'] = self.horizontal_flip
def setUp(self):
self._rotation = Rotation()
# Combos
self._rotation.use(Breech(4)).at(1)
self._rotation.use(Whirlwind()).at(2)
self._rotation.use(BloodyHack(6)).at(3)
self._rotation.use(BloodyHack(5)).at(4)
# Abilities
# conq_dps.use( BladeWeave() ).at( 1 )
# conq_dps.use( UseDiscipline() ).at( 2 )
self._rotation.use(Annihilate()).at(3)
self._rotation.use(RendFlesh()).at(4)
_globals.register_keybinds(self._rotation)
def main():
filenum = 1
lidar_pitch = 9.5/180*np.pi
filename = "/home/henry/Documents/projects/avl/Detection/ground/data/points_data_"+repr(filenum)+".txt"
planes, pcds = read_points_data(filename)
tf = Rotation(roll=0.0, pitch=lidar_pitch, yaw=0.0)
for plane in planes:
plane.rotate_around_axis(axis="y", angle=-lidar_pitch)
for pcd in pcds:
pcd.rotate(tf)
pcd = pcds[0]
plane = planes[1]
# print(np.min(pcd.data[0,:]), np.max(pcd.data[0,:]))
vis(plane, pcd, dim_2d=True)
# the onlyfloor points
# pcd = pcds[2]
# plane = planes[2]
# the reestimated plane
sac = RANSAC(Plane3D, min_sample_num=3, threshold=0.1, iteration=50, method="MSAC")
plane2, _, _, _ = sac.ransac(pcd.data.T)
vis(plane2, pcd, dim_2d=True)
plt.show()
def add_single_operator(self, qubit: int, operator_type: PauliOperator, rotation_amount: Fraction, index: int = None) -> None:
"""
Add a single Pauli operator (I, X, Z, Y) to the circuit.
Args:
qubit (int): Targeted qubit
operator_type (PauliOperator): Operator type (I, X, Y, Z)
index (int, optional): Index location. Default: end of the circuit
"""
if index is None:
index = len(self)
new_rotation = Rotation(self.qubit_num, rotation_amount)
new_rotation.change_single_op(qubit, operator_type)
self.add_pauli_block(new_rotation, index)
def test_rigidtransform(self): #test constructor pose1 = RigidTransform() self.assertEqual(pose1.get_translation().get_x(), 0.0) self.assertEqual(pose1.get_translation().get_y(), 0.0) self.assertEqual(pose1.get_rotation().get_theta(), 0.0) pose2 = RigidTransform(Translation(10.0, 15.0), Rotation.from_degrees(45.0)) self.assertEqual(pose2.get_translation().get_x(), 10.0) self.assertEqual(pose2.get_translation().get_y(), 15.0) self.assertEqual(pose2.get_rotation().get_theta(), 45.0) #test transform pose3 = pose1.transform(pose2) self.assertAlmostEqual(pose3.get_translation().get_x(), 10.0) self.assertAlmostEqual(pose3.get_translation().get_y(), 15.0) self.assertAlmostEqual(pose3.get_rotation().get_theta(), 45.0) pose4 = pose2.transform(pose1) self.assertAlmostEqual(pose4.get_translation().get_x(), 10.0) self.assertAlmostEqual(pose4.get_translation().get_y(), 15.0) self.assertAlmostEqual(pose4.get_rotation().get_theta(), 45.0) pose5 = RigidTransform(Translation(10.0, 10.0), Rotation.from_degrees(45.0)) pose6 = pose5.transform(pose2) #used this link to verify: http://www.wolframalpha.com/widgets/view.jsp?id=bd71841fce4a834c804930bd48e7b6cf self.assertAlmostEqual(pose6.get_translation().get_x(), 10-(25/math.sqrt(2))+10*math.sqrt(2)) self.assertAlmostEqual(pose6.get_translation().get_y(), 25/math.sqrt(2)+10*math.sqrt(2)-10*(-1+math.sqrt(2))) self.assertAlmostEqual(pose6.get_rotation().get_theta(), 90.0) pose7 = pose2.transform(pose5) self.assertAlmostEqual(pose7.get_translation().get_x(), -25.0/math.sqrt(2) + 10*math.sqrt(2) + 5.0/2.0*(4+math.sqrt(2))) self.assertAlmostEqual(pose7.get_translation().get_y(), 25.0/math.sqrt(2) + 10*math.sqrt(2) - 5.0/2.0*(-6+5*math.sqrt(2))) self.assertAlmostEqual(pose7.get_rotation().get_theta(), 90.0) #test inverse pose6inverse = pose6.inverse() pose8 = pose6.transform(pose6inverse) self.assertAlmostEqual(pose8.get_translation().get_x(), 0) self.assertAlmostEqual(pose8.get_translation().get_y(), 0) self.assertAlmostEqual(pose8.get_rotation().get_theta(), 0) #test intersection intersection_point = pose1.intersection(pose2) self.assertAlmostEqual(intersection_point.get_x(), -5.0) self.assertAlmostEqual(intersection_point.get_y(), 0.0)
def __init__(self, line_one_, line_two_):
self.line_one = line_one_
self.line_two = line_two_
normal_line_one = RigidTransform(
self.line_one.end,
Rotation.from_translation(self.line_one.slope, True).normal())
normal_line_two = RigidTransform(
self.line_two.start,
Rotation.from_translation(self.line_two.slope, True).normal())
self.center = normal_line_one.intersection(normal_line_two)
center_to_end_dist = Translation.from_translations(
self.center, self.line_one.end).norm()
start_to_center_dist = Translation.from_translations(
self.center, self.line_two.start).norm()
if (center_to_end_dist - start_to_center_dist > 1E-9):
#should never enter here
print("ERROR, CENTER OF ARC IS CALCULATED INCORRECTLY")
self.radius = 7777
else:
self.radius = center_to_end_dist
def then(self, new_pose_measurement):
"""
Uses the given measurement to continue tracking the cube, and returns an updated PoseTrack instance.
:param new_pose_measurement The latest measurement of where the cube is and how it is oriented.
"""
new_pose_measurement_stability = self.last_pose_measurement_stability + 1 \
if new_pose_measurement.front_measurement == self.last_pose_measurement.front_measurement \
else 0
new_facing = self.facing
new_stable_measurement = self.stable_pose_measurement
new_rotations = self.rotations
if new_pose_measurement_stability >= 3:
new_stable_measurement = new_pose_measurement
# if the front side changed, use the new side to figure out which quarter X or Y rotation happened
# TODO: what about quick 180s?
if new_facing.current_front != new_pose_measurement.front_measurement.current_front:
adj = [(new_facing.rotated_by(r), r)
for r in [Rotation(x=0.25), Rotation(x=-0.25), Rotation(y=0.25), Rotation(y=-0.25)]]
for facing, rotation in adj:
if facing.current_front == new_pose_measurement.front_measurement.current_front:
new_facing = facing
new_rotations = Rotation.plus_rotation_simplified(new_rotations, rotation)
# if diagonals are flipped, guess which Z rotation happened based on the last stable measured angle
if new_facing.is_top_right_darker() != new_pose_measurement.front_measurement.is_top_right_darker:
advance = self.stable_pose_measurement.angle < 0
r = Rotation(z=0.25 if advance else -0.25)
new_rotations = Rotation.plus_rotation_simplified(new_rotations, r)
new_facing = new_facing.z()
if not advance:
new_facing = new_facing.z().z()
return PoseTrack(new_facing,
new_stable_measurement,
new_pose_measurement,
new_pose_measurement_stability,
new_rotations)
def exp(twist):
cos_theta = math.cos(twist.dtheta)
sin_theta = math.sin(twist.dtheta)
rotation = Rotation(cos_theta, sin_theta)
#if theta is very small, use taylor series to approximate (we can't divide by zero)
if (math.fabs(twist.dtheta) < zero):
sin_theta_over_theta = 1.0 - math.pow(
twist.dtheta, 2) / 6.0 + math.pow(twist.dtheta, 4) / 120.0
one_minus_cos_theta_over_theta = 1.0 / 2.0 * twist.dtheta - math.pow(
twist.dtheta, 3) / 24.0 + math.pow(twist.dtheta, 5) / 720.0
else:
sin_theta_over_theta = sin_theta / twist.dtheta
one_minus_cos_theta_over_theta = (1.0 - cos_theta) / twist.dtheta
translation = Translation(sin_theta_over_theta * twist.dx,
one_minus_cos_theta_over_theta * twist.dx)
return RigidTransform(translation, rotation)
def get_attention_vector(quat):
"""
get face orientation vector from quaternion
:param quat:
:return:
"""
dcm = R.quat2dcm(quat)
v_front = np.mat([[0], [0], [1]])
v_front = dcm * v_front
v_front = np.array(v_front).reshape(3)
# v_top = np.mat([[0], [1], [0]])
# v_top = dcm * v_top
# v_top = np.array(v_top).reshape(3)
# return np.hstack([v_front, v_top])
return v_front
def racecar_reset(self, ndist, next_index):
rospy.wait_for_service('gazebo/set_model_state')
#random_start = random.random()
prev_index, next_index = self.find_prev_next_waypoints(
self.start_ndist)
# Compute the starting position and heading
#start_point = self.center_line.interpolate(ndist, normalized=True)
start_point = self.center_line.interpolate(self.start_ndist,
normalized=True)
start_yaw = math.atan2(
self.center_line.coords[next_index][1] - start_point.y,
self.center_line.coords[next_index][0] - start_point.x)
start_quaternion = Rotation.from_euler('zyx',
[start_yaw, 0, 0]).as_quat()
# Construct the model state and send to Gazebo
model_state = ModelState()
model_state.model_name = 'racecar'
model_state.pose.position.x = start_point.x
model_state.pose.position.y = start_point.y
model_state.pose.position.z = 0
model_state.pose.orientation.x = start_quaternion[0]
model_state.pose.orientation.y = start_quaternion[1]
model_state.pose.orientation.z = start_quaternion[2]
model_state.pose.orientation.w = start_quaternion[3]
model_state.twist.linear.x = 0
model_state.twist.linear.y = 0
model_state.twist.linear.z = 0
model_state.twist.angular.x = 0
model_state.twist.angular.y = 0
model_state.twist.angular.z = 0
self.racecar_service(model_state)
for joint in EFFORT_JOINTS:
self.clear_forces_client(joint)
#keeping track where to start the car
self.reverse_dir = not self.reverse_dir
self.start_ndist = (self.start_ndist + ROUND_ROBIN_ADVANCE_DIST) % 1.0
self.progress_at_beginning_of_race = self.progress
class CoordinateSystem():
def __init__(self, x, y, theta, position):
self.position_ = position
self.origin_ = Point2D(x, y)
self.theta_ = theta
self.rotation_ = Rotation(theta, AngleUnit.RADIANS)
def origin(self):
return self.origin_
def position(self):
return self.position_
def theta(self):
return self.theta_
def point_to_vector(self, point):
if self.position_ == Position.CENTER:
vector = Vector2D(point, self)
rotated_vector = self.rotation_.rotate_vector(vector)
return rotated_vector
def convert_vector(self, vector):
if self.position_ == Position.TOPLEFT and vector.coordinate_system().position() == Position.CENTER:
original_point = vector.original_point()
new_vector = Vector2D(original_point, self, invert_y = True)
return new_vector
def points_to_vectors(self, points):
vectors = []
for point in points:
vectors.append(self.point_to_vector(point))
return vectors
def __repr__(self):
return "("+ str(self.origin()) + "," + str(self.position()) + ")" + " theta " + str(self.theta_)
def main():
start_time = time.time()
create_cube()
cube_3d_goal = copy.deepcopy(cube_3d)
# pp.pprint(cube_3d)
r = Rotation()
# r.rotation('U2', cube_3d)
r.rotation('F', cube_3d)
r.rotation('U', cube_3d)
r.rotation("B'", cube_3d)
r.rotation("F'", cube_3d)
r.rotation("D'", cube_3d)
# r.rotation("F'", cube_3d)
# r.rotation("D'", cube_3d)
# r.rotation('F', cube_3d)
# r.rotation('B', cube_3d)
# r.rotation("L'", cube_3d)
Solver(cube_3d, cube_3d_goal, r.rotation)
pp.pprint(time.time() - start_time)
WIDTH = 800
HEIGHT = 800
Graph_3D(WIDTH, HEIGHT, 'Cube 3D !', cube_3d)
pyglet.app.run()
def reset_drone(self, x, y, z):
''' Reset's the drone
'''
start_quaternion = Rotation.from_euler('zyx', [0, 0, 0]).as_quat()
# Construct the model state and send to Gazebo
model_state = ModelState()
model_state.model_name = 'drone'
model_state.pose.position.x = x
model_state.pose.position.y = y
model_state.pose.position.z = z
model_state.pose.orientation.x = start_quaternion[0]
model_state.pose.orientation.y = start_quaternion[1]
model_state.pose.orientation.z = start_quaternion[2]
model_state.pose.orientation.w = start_quaternion[3]
model_state.twist.linear.x = 0
model_state.twist.linear.y = 0
model_state.twist.linear.z = 0
model_state.twist.angular.x = 0
model_state.twist.angular.y = 0
model_state.twist.angular.z = 0
self.model_state_client(model_state)
class Homogeneous(object):
def __init__(self):
self.r = Rotation()
self.t = Translation()
def origin(self):
return Matrix([0, 0, 0, 1])
def stationary(self):
return Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
def _build(self, r, t):
return r.col_insert(3, t).row_insert(3, self.origin().T)
def translate_x(self, x):
r = self.r.stationary()
t = self.t.translate_x(x)
return self._build(r, t)
def translate_y(self, y):
r = self.r.stationary()
t = self.t.translate_y(y)
return self._build(r, t)
def translate_z(self, z):
r = self.r.stationary()
t = self.t.translate_z(z)
return self._build(r, t)
def rotate_x(self, x):
r = self.r.rotate_x(x)
t = self.t.stationary()
return self._build(r, t)
def rotate_y(self, y):
r = self.r.rotate_y(y)
t = self.t.stationary()
return self._build(r, t)
def rotate_z(self, z):
r = self.r.rotate_z(z)
t = self.t.stationary()
return self._build(r, t)
class RigidTransform(object):
global zero
zero = 1E-9
def __init__(self, translation_=None, rotation_=None):
if translation_ == None:
self.translation = Translation()
else:
self.translation = translation_
if rotation_ == None:
self.rotation = Rotation()
else:
self.rotation = rotation_
def get_rotation(self):
return self.rotation
def get_translation(self):
return self.translation
def transform(self, other_transformation):
return RigidTransform(
self.translation.translate(
other_transformation.get_translation().rotate(self.rotation)),
self.rotation.rotate(other_transformation.get_rotation()))
def inverse(self):
return RigidTransform(
self.translation.inverse().rotate(self.rotation.inverse()),
self.rotation.inverse())
'''
ethaneade.com/lie_groups.pdf
exponential map for rigid transformations
this basically converts a twist (constant-curvature velocity) to a rigid transformation (pose)
Twist: [[dx ]
[dy = 0] We don't move sideways
[dtheta]]
Rotation: [[cos(dtheta), -sin(dtheta)]
[sin(dtheta), cos(dtheta)]]
Translation: [[sin(dtheta)/dtheta, -(1-cos(dtheta)/dtheta)] * [[dx ]
[(1-cos(dtheta)/dtheta, sin(dtheta)/dtheta)]] [dy = 0]]
'''
@staticmethod
def exp(twist):
cos_theta = math.cos(twist.dtheta)
sin_theta = math.sin(twist.dtheta)
rotation = Rotation(cos_theta, sin_theta)
#if theta is very small, use taylor series to approximate (we can't divide by zero)
if (math.fabs(twist.dtheta) < zero):
sin_theta_over_theta = 1.0 - math.pow(
twist.dtheta, 2) / 6.0 + math.pow(twist.dtheta, 4) / 120.0
one_minus_cos_theta_over_theta = 1.0 / 2.0 * twist.dtheta - math.pow(
twist.dtheta, 3) / 24.0 + math.pow(twist.dtheta, 5) / 720.0
else:
sin_theta_over_theta = sin_theta / twist.dtheta
one_minus_cos_theta_over_theta = (1.0 - cos_theta) / twist.dtheta
translation = Translation(sin_theta_over_theta * twist.dx,
one_minus_cos_theta_over_theta * twist.dx)
return RigidTransform(translation, rotation)
def intersection_(self, transform_a, transform_b):
rotation_a = transform_a.get_rotation()
rotation_b = transform_b.get_rotation()
translation_a = transform_a.get_translation()
translation_b = transform_b.get_translation()
tan_b = rotation_b.get_tan()
#???
t = ((translation_a.get_x() - translation_b.get_x()) * tan_b +
translation_b.get_y() - translation_a.get_y()) / (
rotation_a.get_sin() - rotation_a.get_cos() * tan_b)
return translation_a.translate(rotation_a.to_translation().scale(t))
def intersection(self, other_transform):
other_rot = other_transform.get_rotation()
if (self.rotation.is_parallel(other_rot)):
#should never reach here
return Translation(float("inf"), float("inf"))
if (math.fabs(self.rotation.get_cos()) < math.fabs(
other_rot.get_cos())):
return self.intersection_(self, other_transform)
else:
return self.intersection_(other_transform, self)
def print(self):
print("transform:")
self.translation.print()
self.rotation.print()
def __init__(self):
self.r = Rotation()
self.t = Translation()
class Steering:
'''
构造函数
channelH: 水平舵机的信号通道
min_angleH: 水平舵机最小转角
max_angleH: 水平舵机最大转角
channelV: 垂直舵机的信号通道
min_angleV: 垂直舵机最小转角
max_angleV: 垂直舵机最大转角
init_angleH: 水平舵机初始转角
init_angleV: 垂直舵机初始转角
'''
def __init__(self, channelH, min_angleH, max_angleH,
channelV, min_angleV, max_angleV, init_angleH=0, init_angleV=0):
self.hRotation = Rotation(channelH, min_angleH, max_angleH, init_angleH)
self.vRotation = Rotation(channelV, min_angleV, max_angleV, init_angleV)
def setup(self):
GPIO.setwarnings(False)
self.hRotation.setup()
self.vRotation.setup()
'''
向上步进转动
'''
def up(self):
self.vRotation.positiveRotation()
'''
向下步进转动
'''
def down(self):
self.vRotation.reverseRotation()
'''
向左步进转动
'''
def left(self):
self.hRotation.positiveRotation()
'''
向右步进转动
'''
def right(self):
self.hRotation.reverseRotation()
'''
转动到指定的角度
'''
def specify(self, angleH, angleV):
if (angleH != None):
self.hRotation.specifyRotation(angleH)
if (angleV != None):
self.vRotation.specifyRotation(angleV)
'''
停止舵机
'''
def cleanup(self):
self.hRotation.stop()
self.vRotation.stop()
class Camera:
def __init__(self, orient=[0, 0, 0], offset=[0, 0, 0]):
# type: (object, object) -> object
self.offset = np.array(offset)
self.R = Rotation()
m_orient = np.array(orient)
self.globalPoints = None
self.imagePoints = False
if m_orient.size == 3:
self.basis = self.R.getCameraBasis(orient)
else:
self.basis = m_orient
def set_offset(self, offset):
self.offset = np.array(offset)
def set_NewBasis(self, basis=np.eye(3)):
self.basis = np.array(basis)
def getOffset(self):
return self.offset
def getBasis(self):
return self.basis
# 1
def setGlobalPoints(self, glob):
self.globalPoints = glob
# 2
def getGlobalPoints(self):
return self.globalPoints
# 3
def glob2image(self, glob=None):
if glob is not None:
self.setGlobalPoints(glob)
if glob is None:
assert ("Any points hasn't been found!")
as_camera_see = self.newCameraCords(self.globalPoints)
self.imagePoints = transformCords(as_camera_see)
return self.imagePoints
# 4
def getImagePoints(self):
return self.imagePoints
# 5
def setImagePoints(seld, imageCords):
seld.imagePoints = imageCords
def bindingParams(self, camera):
basis = camera.getBasis()
of = camera.getOffset()
rR = inv(basis) @ self.basis
dT = of - self.offset
return rR.T, dT
# makes global cords as it sees this cam
def newCameraCords(self, gCords):
gCords = np.array(gCords)
cords = np.empty(gCords.shape)
for i in range(gCords.shape[0]):
temp = np.array([inv(self.basis.T) @ (gCords[i] - self.offset)])
cords[i] = temp
return cords
def __init__(self, channelH, min_angleH, max_angleH,
channelV, min_angleV, max_angleV, init_angleH=0, init_angleV=0):
self.hRotation = Rotation(channelH, min_angleH, max_angleH, init_angleH)
self.vRotation = Rotation(channelV, min_angleV, max_angleV, init_angleV)
# limitations under the License.
from __future__ import print_function
import math
import sys
from rotation import Rotation, RotationException, Point3D, PointException
"""
A collection of basic unit tests for 3D rotation,
implemented by rotation.Rotation.
"""
try :
p = Point3D(1,1,1)
rot = Rotation(rz=1, angle=math.pi/2)
print("Angle of rotation (must be pi/2 = 90 deg): {0} deg".format(Rotation.rad2deg(rot.getAngle())))
print("Rotation around the z-axis:")
print("Axis of rotation: {0}".format(rot.getAxis()))
print("{0} --> {1}".format(p, rot.rotate(p)))
print("Expected: (-1, 1, 1)")
print()
print("Rotation around the y-axis:")
rot.setAxis(0, 2, 0)
print("Axis of rotation: {0}".format(rot.getAxis()))
print("{0} --> {1}".format(p, rot.rotate(p)))
print("Expected: (1, 1, -1)")
print()
print("Rotation around the x-axis:")
class Round:
"""
A class for rounds
"""
def __init__(self, small_blind: int, player_list: list) -> None:
self.card_deck, self.current_pot, self.small_blind, self.player_list, self.big_blind =\
card_deck, 0, small_blind, player_list, small_blind * 2
self.community_cards_hidden = []
self.community_cards_open = []
for player in self.player_list:
# Sets a player with position 0 to move in the beginning of each round
if player.position == 0:
player.is_turn = True
# creates a rotation
self.current_rotation = Rotation(self.small_blind, self.player_list)
def get_current_pot(self) -> int:
"""
Returns a string with the current pot
"""
return self.current_pot
def hand_winner(self) -> "Player":
"""
Determines the winner of the hand and adds the pot to the player's bank
if the player is the winner
"""
pass
def round_over(self) -> bool:
"""
Returns whether or not the hand is over
"""
round_over = False
if len(self.current_rotation.get_possible_moves()) == 0:
round_over = True
return round_over
def give_cards(self) -> None:
"""
Shuffles and gives out the cards to the players and places the community cards, collects the blinds.
"""
# Collect the blinds
for player in self.player_list:
if player.position == 1:
player.bank -= self.small_blind
player.to_call = self.small_blind
self.current_rotation.current_pot += self.small_blind
elif player.position == 2:
player.bank -= self.big_blind
self.current_rotation.current_pot += self.big_blind
else:
player.to_call = self.big_blind
# Shuffle the deck
shuffle(self.card_deck)
# Give players their cards
for player in self.player_list:
player.hand.append(self.card_deck.pop())
player.hand.append(self.card_deck.pop())
# Place community cards
for i in range(5):
self.community_cards_hidden.append(self.card_deck.pop())
def give_button(self):
"""
Assigns dealer button to a random player
player with position == 0 is the dealer, position == 1 is SB,
position == 2 is BB
"""
pass
