def __init__(self, theta=0, size=(0.1, 0.1, 0.0)):
self.theta = theta % 2 # double check
self.size = np.array(size)
assert self.size.shape == (
3,
), "Size must be a 3 element vector! : this is a 2D module but takes in a three dimensional size vector for now. Third entry is ignored"
self.connection_axis = np.array([0., 0., 1.])
self.orientation = Rot.from_axis(self.connection_axis,
-self.theta * (np.pi / 2.))
# NOTE: The fudge factor is to avoid colliding with the plane once
# spawned
self.position = np.array([0., self.size[2] / 2. + 0.002,
0.]) # uses only x and y
self._children = {}
self.controller = m_controller.Controller()
# relative scales
self.radius = 0.25
self.angle = math.pi / 2
self.type = "CIRCLE"
self.MIN_RADIUS = 0.25
self.MAX_RADIUS = 0.5
self.MIN_ANGLE = math.pi / 4
self.MAX_ANGLE = math.pi * 2
self.torque = 50
def __init__(self, theta=0):
self.theta = theta % 2
self.connection_axis = np.array([0., 0., -1.])
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * np.pi)
self.position = np.array([0., 0., 20.])
self.connection_type = Connection
self._children = {}
def __init__(self, theta=0):
self.theta = theta % 4
self.connection_axis = np.array([-1., 0., 0.])
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * (np.pi / 2.0))
# NOTE: The fudge factor is to avoid colliding with the plane once
# spawned
self.position = np.array([0., 0., SIZE[1] / 2.0 + 0.002])
self.connection_id = 0
self.connection_type = Connection
self._children = {}
def __init__(self, theta=0, size=(0.061, 0.061, 0.061)):
self.theta = theta % 4
self.size = np.array(size)
assert self.size.shape == (3, ), "Size must be a 3 element vector!"
self.connection_axis = np.array([0., 0., -1.])
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * (np.pi / 2.))
# NOTE: The fudge factor is to avoid colliding with the plane once
# spawned
self.position = np.array([0., 0., self.size[2] / 2. + 0.002])
self.connection_type = Connection
self._children = {}
def update(self, parent=None, pos=None, direction=None):
# Update own orientation first in case we have been previously
# connected
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * (np.pi / 2.))
# Update position in case parent is None
self.position = np.array([0., 0., SIZE[1] / 2.0 + 0.002])
# Reset connection in case parent is None
self.connection = None
# Call super to update orientation
super().update(parent, pos, direction)
# If parent is not None we need to update position and connection point
if self.parent is not None:
# Update center position for self
self.position = pos - (direction * SIZE[0]) / 2.
# Calculate connection points for joint
conn = np.array([-SIZE[1] / 2., 0., 0.])
parent_conn = parent.orientation.T.rotate(pos - parent.position)
self.connection = (parent_conn, conn)
# Update potential children
self.update_children()
def update(self, parent=None, pos=None, direction=None):
"""Update configuration for the module.
This function should update the parent pointer and internal position of
the module. The 'pos' argument is the central point where this module
connects to parent. The 'direction' argument is a vector pointing in
the direction the module should be attached at.
If no arguments are given it indicates an update back to center
position."""
if (parent is not None and self.parent is not None
and parent != self.parent):
raise ModuleAttached("This module already has a parent: {}".format(
self.parent))
self.parent = parent
# NOTE: We accept 'None' parents which could indicate a detachment of
# this module
if parent is not None:
# The below equation rotates the 'connection_axis' parameter to
# look in the same direction as 'direction'
direction *= -1.
conn_axis = parent.orientation.rotate(self.connection_axis)
v = np.cross(conn_axis, direction)
c = conn_axis.dot(direction)
skew = np.array([[0., -v[2], v[1]], [v[2], 0., -v[0]],
[-v[1], v[0], 0.]])
if 1. + c != 0.:
orient = Rot(
np.identity(3) + skew + skew.dot(skew) * (1. / (1. + c)))
else:
# This means that 'conn_axis' and 'direction' point in the same
# direction, to convert we can find the vector perpendicular to
# 'conn_axis' and rotate by pi
if abs(conn_axis[0]) > abs(conn_axis[2]):
flip = np.array([-conn_axis[1], conn_axis[0], 0.])
else:
flip = np.array([0., -conn_axis[2], conn_axis[1]])
orient = Rot.from_axis(flip, np.pi)
# Update own orientation
self.orientation = orient + parent.orientation + self.orientation
def update(self, parent=None, pos=None, direction=None):
# Update own orientation first in case we have been previously
# connected
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * (np.pi / 2.))
# Update position in case parent is None
self.position = np.array([0., 0., self.size[2] / 2. + 0.002])
# Reset connection in case parent is None
self.connection = None
# Call super to update orientation
super().update(parent, pos, direction)
# If parent is not None we need to update position and connection point
if self.parent is not None:
# Update center position for self
# NOTE: We add a little fudge factor to avoid overlap
self.position = pos - (direction * self.size * 1.01) / 2.
# Calculate connection points for module
conn = np.array([0., 0., -self.size[2] / 2.])
parent_conn = parent.orientation.T.rotate(pos - parent.position)
self.connection = (parent_conn, conn)
# Update potential children
self.update_children()
def update(self, parent=None, pos=None, direction=None):
# Update own orientation first in case we have been previously
# connected
self.orientation = Rot.from_axis(self.connection_axis,
self.theta * np.pi)
# Update position in case parent is None
self.position = np.array([0., 0., 20.])
# Reset connection in case parent is None
self.connection = None
# Call super to update orientation
super().update(parent, pos, direction)
# If parent is not None we need to update position and connection point
if self.parent is not None:
# Update center position for self
self.position = pos - (direction * (20. + 14.5))
# Calculate connection points for joint
conn = np.array([-4.2, 0., 0.])
# NOTE: We add a little fudge factor to avoid overlap
parent_conn = parent.orientation.T.rotate(pos - parent.position)
# parent_conn = pos - parent.position
self.connection = (parent_conn, conn)
# Update potential children
self.update_children()
def rotate(self, theta):
"""Update rotation about connection axis"""
self.theta = (self.theta + theta) % 4
axis = self.connection_axis
self.orientation += Rot.from_axis(axis, self.theta * (np.pi / 2.))
self.update_children()
def rotate(self, theta):
"""Update internal rotation about connection axis"""
self.theta = (self.theta + theta) % 2
self.orientation += Rot.from_axis(self.connection_axis,
self.theta * np.pi)
self.update_children()