def __init__(
self,
PARENT_NODE=None,
TARGET_LIST=[],
):
## init internal sub-classes
self.input = KeyboardInput()
# b.Input.connect_from(a.Output)
## init base node for whole crane
self.base_node = avango.gua.nodes.TransformNode(Name="base_node")
self.base_node.Transform.value = \
avango.gua.make_trans_mat(.0, -.1,.0)
PARENT_NODE.Children.value.append(self.base_node)
# rotational axises
rotAxi0 = avango.gua.Vec3(0, 1, 0)
rotAxi1 = avango.gua.Vec3(0, 0, 1)
# FIRST
hinge0 = Hinge()
hinge0.my_constructor(PARENT_NODE=self.base_node,
DIAMETER=.1,
HEIGHT=.01,
ROT_AXIS=rotAxi0,
SF_ROT_INPUT_MAT=[-180, 180])
hinge0.sf_rot_value.connect_from(self.input.sf_rot_input0)
arm0 = Arm(hinge0.matrix, .01, .1)
# SECOND
hinge1 = Hinge()
hinge1.my_constructor(PARENT_NODE=hinge0.matrix,
DIAMETER=.02,
HEIGHT=.01,
ROT_OFFSET_MAT=avango.gua.make_trans_mat(
.0, arm0.length, .0),
ROT_AXIS=rotAxi1,
SF_ROT_INPUT_MAT=[0, 90])
hinge1.sf_rot_value.connect_from(self.input.sf_rot_input1)
arm1 = Arm(hinge1.matrix, .005, .08)
# THIRD
hinge2 = Hinge()
hinge2.my_constructor(PARENT_NODE=hinge1.matrix,
DIAMETER=.02,
HEIGHT=.01,
ROT_OFFSET_MAT=avango.gua.make_trans_mat(
.0, arm1.length, .0),
ROT_AXIS=rotAxi1,
SF_ROT_INPUT_MAT=[-90, 90])
hinge2.sf_rot_value.connect_from(self.input.sf_rot_input2)
arm2 = Arm(hinge2.matrix, .005, .08)
# FOURTH
hook = Hook()
hook.rot_mat = avango.gua.make_trans_mat(.0, arm2.length, .0)
hook.my_constructor(hinge2.matrix, .02, TARGET_LIST)
hinge0.hook = hook
hinge1.hook = hook
hinge2.hook = hook
def __init__(self,
PARENT_NODE = None,
TARGET_LIST = [],
):
self.rotation = 0.0
self.world_pos = (0,0,0)
### resources ###
## init base node for whole crane
self.base_node = avango.gua.nodes.TransformNode(Name = "base_node")
self.base_node.Transform.value = avango.gua.make_trans_mat(0.0,-0.1,0.0)
PARENT_NODE.Children.value.append(self.base_node)
self.targets = TARGET_LIST
## init internal sub-classes
self.input = KeyboardInput()
## ToDo: init first hinge && connect rotation input
# ...
#self.sf_rotation.connect_from(self.input.sf_rot_value1)
self.firsthinge = Hinge()
self.firsthinge.my_constructor(
PARENT_NODE = self.base_node,
DIAMETER = 0.05, # in meter
HEIGHT = 0.01, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
ROT_AXIS = avango.gua.Vec3(0,1,0), # the axis to rotate arround with the rotation input (default is head axis)
ROT_CONSTRAINT = [-180.0, 180.0], # intervall with min and max rotation of this hinge
BASE = True
)
self.firsthhinge_node = self.firsthinge.get_hinge_node()
## ToDo: init first arm-segment
# ...
self.firstarm = Arm(
PARENT_NODE = self.firsthhinge_node,
DIAMETER = 0.005, # in meter
LENGTH = 0.075, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
)
self.firstarm_node = self.firstarm.get_arm_node()
self.firstarm_node.Transform.value = avango.gua.make_trans_mat(0,0.04,0)
## ToDo: init second hinge && connect rotation input
# ...
self.secondhinge = Hinge()
self.secondhinge.my_constructor(
PARENT_NODE = self.firstarm_node,
DIAMETER = 0.02, # in meter
HEIGHT = 0.02, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
ROT_AXIS = avango.gua.Vec3(0,0,1), # the axis to rotate arround with the rotation input (default is head axis)
ROT_CONSTRAINT = [-0.0, 90.0], # intervall with min and max rotation of this hinge
)
self.secondhinge_node = self.secondhinge.get_hinge_node()
self.secondhinge_node.Transform.value = avango.gua.make_trans_mat(0,0.04,0)
## ToDo: init second arm-segment
# ...
self.secondarm = Arm(
PARENT_NODE = self.secondhinge_node,
DIAMETER = 0.005, # in meter
LENGTH = 0.075, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
)
self.secondarm_node = self.secondarm.get_arm_node()
self.secondarm_node.Transform.value = avango.gua.make_trans_mat(0,0.04,0)
## ToDo: init third hinge && connect rotation input
# ...
self.thirdhinge = Hinge()
self.thirdhinge.my_constructor(
PARENT_NODE = self.secondarm_node,
DIAMETER = 0.02, # in meter
HEIGHT = 0.02, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
ROT_AXIS = avango.gua.Vec3(0,0,1), # the axis to rotate arround with the rotation input (default is head axis)
ROT_CONSTRAINT = [-90.0, 90.0], # intervall with min and max rotation of this hinge
)
self.thirdhinge_node = self.thirdhinge.get_hinge_node()
self.thirdhinge_node.Transform.value = avango.gua.make_trans_mat(0,0.04,0)
## ToDo: init third arm-segment
# ...
self.thirdarm = Arm(
PARENT_NODE = self.thirdhinge_node,
DIAMETER = 0.005, # in meter
LENGTH = 0.075, # in meter
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), # the rotation offset relative to the parent coordinate system
)
self.thirdarm_node = self.thirdarm.get_arm_node()
self.thirdarm_node.Transform.value = avango.gua.make_trans_mat(0,0.04,0)
## ToDo: init hook
self.firsthook = Hook()
self.firsthook.my_constructor(
PARENT_NODE = self.thirdarm_node,
SIZE = 0.03,
TARGET_LIST = self.targets,
#TARGET_LIST = []
)
self.firsthook_node = self.firsthook.get_hook_node()
self.firsthook_node.Transform.value = avango.gua.make_trans_mat(0,0.045,0)
def __init__(self,
PARENT_NODE = None,
TARGET_LIST = [],
):
### resources ###
## init base node for whole crane
self.base_node = avango.gua.nodes.TransformNode(Name = "base_node")
self.base_node.Transform.value = avango.gua.make_trans_mat(0.0,-0.1,0.0)
PARENT_NODE.Children.value.append(self.base_node)
## init internal sub-classes
self.input = KeyboardInput()
## ToDo: init first hinge && connect rotation input
self.hinge1 = Hinge()
self.hinge1.my_constructor(PARENT_NODE = self.base_node, DIAMETER = 0.1, HEIGHT = 0.01, \
ROT_OFFSET_MAT = avango.gua.make_identity_mat(), ROT_AXIS = avango.gua.Vec3(0,1,0), \
ROT_CONSTRAINT = [-180.0, 180.0])
self.hinge1.sf_rot_value.connect_from(self.input.sf_rot_value0)
#self.hinge1.connect_from(self.KeyboardInput.Button1)
## ToDo: init first arm-segment
# ...
self.arm1 = Arm(PARENT_NODE = self.hinge1.get_hinge_transform_node(), DIAMETER = 0.01, LENGTH = 0.1, ROT_OFFSET_MAT = avango.gua.make_identity_mat())
## ToDo: init second hinge && connect rotation input
# ...
self.hinge2 = Hinge()
self.hinge2.my_constructor(PARENT_NODE = self.hinge1.get_hinge_transform_node(), DIAMETER = 0.02, HEIGHT = 0.01, \
ROT_OFFSET_MAT = avango.gua.make_rot_mat(90, 1, 0, 0), ROT_AXIS = avango.gua.Vec3(0,0,1), \
ROT_CONSTRAINT = [0.0, 90.0], TRANS_OFFSET = self.arm1.length)
self.hinge2.sf_rot_value.connect_from(self.input.sf_rot_value1)
# # ## ToDo: init second arm-segment
# # # ...
self.arm2 = Arm(PARENT_NODE = self.hinge2.get_hinge_transform_node(), DIAMETER = 0.01, LENGTH = 0.1, ROT_OFFSET_MAT = avango.gua.make_identity_mat())
# # ## ToDo: init third hinge && connect rotation input
# # # ...
self.hinge3 = Hinge()
self.hinge3.my_constructor(PARENT_NODE = self.hinge2.get_hinge_transform_node(), DIAMETER = 0.02, HEIGHT = 0.01, \
ROT_OFFSET_MAT = avango.gua.make_rot_mat(90, 1, 0, 0), ROT_AXIS = avango.gua.Vec3(0,0,1), \
ROT_CONSTRAINT = [-90.0, 90.0], TRANS_OFFSET = self.arm2.length)
self.hinge3.sf_rot_value.connect_from(self.input.sf_rot_value2)
# # ## ToDo: init third arm-segment
# # # ...
self.arm3 = Arm(PARENT_NODE = self.hinge3.get_hinge_transform_node(), DIAMETER = 0.01, LENGTH = 0.1, ROT_OFFSET_MAT = avango.gua.make_identity_mat())
# # ## ToDo: init hook
# # # ...
self.hook = Hook()
self.hook.my_constructor(PARENT_NODE = self.hinge3.get_hinge_transform_node(), SIZE = 0.025, TARGET_LIST = TARGET_LIST, OFFSET = self.arm3.length)
self.hook.sf_mat.connect_from(self.hook.hook_geometry.WorldTransform)
def __init__(
self,
PARENT_NODE=None,
TARGET_LIST=[], # required for bounding box intersection in Hook class
):
## init base node for whole crane
self.base_node = avango.gua.nodes.TransformNode(Name="base_node")
self.base_node.Transform.value = avango.gua.make_trans_mat(
0.0, -0.1, 0.0)
PARENT_NODE.Children.value.append(self.base_node)
## init internal sub-classes
self.input = KeyboardInput()
## first hinge
self.hinge0 = Hinge(
PARENT_NODE=self.base_node,
DIAMETER=0.1,
HEIGHT=0.01,
ROT_OFFSET_MAT=avango.gua.make_identity_mat(),
SF_ROT_INPUT=self.input.sf_rot_input0,
)
## first arm segment
self.arm0 = Arm(
PARENT_NODE=self.hinge0.hinge_node,
LENGTH=0.12,
DIAMETER=0.01,
ROT_OFFSET_MAT=avango.gua.make_identity_mat(),
)
## second hinge
self.hinge1 = Hinge(
PARENT_NODE=self.arm0.arm_end_node,
DIAMETER=0.04,
HEIGHT=0.01,
ROT_OFFSET_MAT=avango.gua.make_rot_mat(90.0, 1, 0, 0),
SF_ROT_INPUT=self.input.sf_rot_input1,
MIN_ANGLE=-45,
MAX_ANGLE=45,
)
## second arm segment
self.arm1 = Arm(
PARENT_NODE=self.hinge1.hinge_node,
LENGTH=0.09,
DIAMETER=0.007,
ROT_OFFSET_MAT=avango.gua.make_rot_mat(45.0, 0, 1, 0) *
avango.gua.make_rot_mat(-90.0, 1, 0, 0),
)
## third hinge
self.hinge2 = Hinge(
PARENT_NODE=self.arm1.arm_end_node,
DIAMETER=0.03,
HEIGHT=0.01,
ROT_OFFSET_MAT=avango.gua.make_rot_mat(90.0, 1, 0, 0),
SF_ROT_INPUT=self.input.sf_rot_input2,
MIN_ANGLE=-135,
MAX_ANGLE=45,
)
## third arm segment
self.arm3 = Arm(
PARENT_NODE=self.hinge2.hinge_node,
LENGTH=0.07,
DIAMETER=0.007,
ROT_OFFSET_MAT=avango.gua.make_rot_mat(45.0, 0, 1, 0) *
avango.gua.make_rot_mat(-90.0, 1, 0, 0),
)
## Hook
self.hook = Hook()
self.hook.my_constructor(
PARENT_NODE=self.arm3.arm_end_node,
SIZE=0.02,
TARGET_LIST=TARGET_LIST,
)
class Robot:
""" Represents the robot, and handles interacting with the servos"""
kit = ServoKit(channels=16, frequency=200)
# When moving slowly:
# ... the delay between each move.
delay = 0.01
# ... the angle increment each move
incrementAngle = 0.5
open = 40
closed = 100
# Horizontal = 0
# Vertical up = +90
# Vertical down = -90
lowerLength = 170.0
upperLength = 170.0
headLength = 120.0
platformHeight = 70.0
# Angle & Servo Settings
# Servos face different ways, and have different effective/useful ranges in the arm.
# We need a way to translate an arm angle that is intuitive/useful for us, into
# the actual angle for the servo. The most intuitive way to think about the angles for a servo is
# to treat it as a corner in a triangle, then:
# 0 is closed
# 90 is a right angle
# 180 is fully open
# Calibration:
# Move the robot so the arm is fully horizontal and stretched out.
# Ideally values here would be lower == 0 and upper,head == 180 but they probably need calibrating.
# Set the calibrationAngle on each arm below to:
# "lower": The current value
# "upper","head: The current value - 180
arms = {
"turntable": Arm([0], Direction.FORWARDS),
"lower": Arm([1, 2], Direction.BACKWARDS, 18.0),
"upper": Arm([3], Direction.FORWARDS, -14.0),
"head": Arm([4], Direction.BACKWARDS, -15.0),
"mouth": Arm([5], Direction.FORWARDS)
}
def move(self, arm, angle):
arm = self.arms[arm]
for servo in arm.servos:
output = angle + arm.calibrationAngle
if arm.direction is Direction.BACKWARDS:
output = 180 - output
print("Moving {servo} to {output}".format(servo=servo,
output=output))
self.kit.servo[servo].angle = output
def slowSetWrapper(self, data):
asyncio.run(self.slowSet(data))
async def slowSet(self, data):
actions = []
for key, value in data.items():
actions.append(self.asyncMove(key, float(value)))
await asyncio.gather(*actions)
async def asyncMove(self, armName, position, startDelay=0):
print("Starting asyncMove for {arm}".format(arm=armName))
await asyncio.sleep(startDelay)
arm = self.arms[armName]
start = int(round(self.kit.servo[arm.servos[0]].angle))
end = position + arm.calibrationAngle
if arm.direction is Direction.BACKWARDS:
end = 180 - end
increment = -1 * self.incrementAngle if start >= end else self.incrementAngle
# print("SetOne: Start: {start}, end {end}, increment {increment}".format(start=start,end=end, increment=increment))
for i in np.arange(start, end, increment):
for servo in arm.servos:
self.kit.servo[servo].angle = i
await asyncio.sleep(self.delay)
# Ensure they end up in the final place, in case increment doesn't
# exactly match the end position
for servo in arm.servos:
self.kit.servo[servo].angle = end
print("...end asyncMove for {arm}".format(arm=armName))
def setPosition(self, length, height):
angles = self._calculateAngles(length, height)
self.move("lower", angles['lower'])
self.move("upper", angles['upper'])
self.move("head", angles['head'])
# Private Methods
def _calculateAngles(self, length, height):
hypotenuseLength = math.sqrt(length**2 + (height + self.headLength)**2)
print(hypotenuseLength)
a1 = math.degrees(
math.acos((self.lowerLength**2 + hypotenuseLength**2 -
self.upperLength**2) /
(2 * self.lowerLength * hypotenuseLength)))
a2 = math.degrees(math.atan((height + self.headLength) / length))
a = a1 + a2
b = math.degrees(
math.acos((self.lowerLength**2 + self.upperLength**2 -
hypotenuseLength**2) /
(2 * self.lowerLength * self.upperLength)))
c1 = math.degrees(
math.acos((self.upperLength**2 + hypotenuseLength**2 -
self.lowerLength**2) /
(2 * self.upperLength * hypotenuseLength)))
c2 = 90 - a2
c = c1 + c2
vals = {
"c1": c1,
"c2": c2,
"lower": a,
"upper": b,
"head": c - 25,
"hypotenuseLength": hypotenuseLength
}
print(vals)
return (vals)