def __init__(self,
lengths,
ee1x=None,
ee1y=None,
ee1_angles=None,
ee2x=None,
ee2y=None,
ee2_angles=None,
ee1_grappled=False,
ee2_grappled=False):
"""
Constructor for RobotConfig - we suggest using make_robot_config_from_ee1() or make_robot_config_from_ee2()
to construct new instances of RobotConfig rather than calling this function directly.
"""
self.lengths = lengths
self.ee1_grappled = ee1_grappled
self.ee2_grappled = ee2_grappled
if ee1x is not None and ee1y is not None and ee1_angles is not None:
points = [(ee1x, ee1y)]
net_angle = Angle(radians=0)
for i in range(len(ee1_angles)):
x, y = points[-1]
net_angle = net_angle + ee1_angles[i]
x_new = x + (lengths[i] * math.cos(net_angle.in_radians()))
y_new = y + (lengths[i] * math.sin(net_angle.in_radians()))
points.append((x_new, y_new))
self.ee1_angles = ee1_angles
# 1st angle is last angle of e1_angles + pi, others are all -1 * e1_angles (in reverse order)
self.ee2_angles = [math.pi + net_angle] + \
[-ee1_angles[i] for i in range(len(ee1_angles) - 1, 0, -1)]
self.points = points
elif ee2x is not None and ee2y is not None and ee2_angles is not None:
points = [(ee2x, ee2y)]
net_angle = Angle(radians=0)
for i in range(len(ee2_angles)):
x, y = points[0]
net_angle = net_angle + ee2_angles[i]
x_new = x + (lengths[-i - 1] *
math.cos(net_angle.in_radians()))
y_new = y + (lengths[-i - 1] *
math.sin(net_angle.in_radians()))
points.insert(0, (x_new, y_new))
# 1st angle is last angle of e2_angles + pi, others are all -1 * e2_angles (in reverse order)
self.ee1_angles = [math.pi + sum(ee2_angles)] + \
[-ee2_angles[i] for i in range(len(ee2_angles) - 1, 0, -1)]
self.ee2_angles = ee2_angles
self.points = points
else:
raise Exception(
"Could not create RobotConfig - Insufficient information given"
)
def ass_key2list(self, in_id):
# not include the last flag of ee
arr = []
tmp = in_id.split(' ')
segNum = int((len(tmp) - 3) / 2)
for i in range(len(tmp) - 1):
if i < 2:
qq = float(tmp[i].replace(';', ''))
qq = qq * 10000
elif i > 1 and i <= 1 + segNum:
qq = float(tmp[i].replace(';', ''))
tmpAng = Angle(degrees=qq)
qq = tmpAng.in_radians()
else:
qq = float(tmp[i].replace(';', ''))
arr.append(qq)
return arr