def forward_kinematics(robot_name=None, read_state_string=None):
assert robot_name is not None or read_state_string is not None
if read_state_string is None:
read_state_string = RealRobotClient().read_state()
assert 'nan' not in read_state_string
r = RobotModel(Painter())
angles = ReadStateParser(read_state_string).get_all_joint_radians()
r.draw_neck_and_head(neck_transversal_radians=angles['neck_transversal'],
neck_lateral_radians=angles['neck_lateral'])
r.draw_left_arm(
left_shoulder_lateral_radians=angles['left_shoulder_lateral'],
left_shoulder_frontal_radians=angles['left_shoulder_frontal'],
left_elbow_lateral_radians=angles['left_elbow_lateral'])
r.draw_right_arm(
right_shoulder_lateral_radians=angles['right_shoulder_lateral'],
right_shoulder_frontal_radians=angles['right_shoulder_frontal'],
right_elbow_lateral_radians=angles['right_elbow_lateral'])
r.draw_left_lower_limp(
left_hip_transversal_radians=angles['left_hip_transversal'],
left_hip_frontal_radians=angles['left_hip_frontal'],
left_hip_lateral_radians=angles['left_hip_lateral'],
left_knee_lateral_radians=angles['left_knee_lateral'],
left_ankle_lateral_radians=angles['left_ankle_lateral'],
left_ankle_frontal_radians=angles['left_ankle_frontal'])
r.draw_right_lower_limp(
right_hip_transversal_radians=angles['right_hip_transversal'],
right_hip_frontal_radians=angles['right_hip_frontal'],
right_hip_lateral_radians=angles['right_hip_lateral'],
right_knee_lateral_radians=angles['right_knee_lateral'],
right_ankle_lateral_radians=angles['right_ankle_lateral'],
right_ankle_frontal_radians=angles['right_ankle_frontal'])
r.draw_torso()
r.draw_coordinates(
) # draw coordinates lastly because we want them on top layer
r.show()
def forward_kinematics(robot_name=None, read_state_string=None):
assert robot_name is not None or read_state_string is not None
if read_state_string is None:
read_state_string = RealRobotClient().read_state()
assert 'nan' not in read_state_string
r = RobotModel(Painter())
angles = ReadStateParser(read_state_string).get_all_joint_radians()
r.draw_neck_and_head(neck_transversal_radians=angles['neck_transversal'], neck_lateral_radians=angles['neck_lateral'])
r.draw_left_arm(left_shoulder_lateral_radians=angles['left_shoulder_lateral'], left_shoulder_frontal_radians=angles['left_shoulder_frontal'], left_elbow_lateral_radians=angles['left_elbow_lateral'])
r.draw_right_arm(right_shoulder_lateral_radians=angles['right_shoulder_lateral'], right_shoulder_frontal_radians=angles['right_shoulder_frontal'], right_elbow_lateral_radians=angles['right_elbow_lateral'])
r.draw_left_lower_limp(left_hip_transversal_radians=angles['left_hip_transversal'], left_hip_frontal_radians=angles['left_hip_frontal'], left_hip_lateral_radians=angles['left_hip_lateral'], left_knee_lateral_radians=angles['left_knee_lateral'], left_ankle_lateral_radians=angles['left_ankle_lateral'], left_ankle_frontal_radians=angles['left_ankle_frontal'])
r.draw_right_lower_limp(right_hip_transversal_radians=angles['right_hip_transversal'], right_hip_frontal_radians=angles['right_hip_frontal'], right_hip_lateral_radians=angles['right_hip_lateral'], right_knee_lateral_radians=angles['right_knee_lateral'], right_ankle_lateral_radians=angles['right_ankle_lateral'], right_ankle_frontal_radians=angles['right_ankle_frontal'])
r.draw_torso()
r.draw_coordinates() # draw coordinates lastly because we want them on top layer
r.show()
# r._painter.draw_point(r.draw_right_arm(radians(0), radians(60), radians(0), color='b-')[-1])
# r.draw_left_lower_limp()
# r.draw_right_lower_limp(right_knee_lateral_radians=radians(150))
r.draw_left_lower_limp(
# LeftHipTransversal().outward(0).radians,
radians(-0),
LeftHipFrontal().outward(0).angle,
LeftHipLateral().forward(0).radians,
LeftKneeLateral().backward(0).radians,
# radians(150),
LeftAnkleLateral().forward(0).radians,
LeftAnkleFrontal().outward(-0).radians)
r.draw_right_lower_limp(
RightHipTransversal().inward(0).radians,
# radians(-0),
RightHipFrontal().outward(0).angle,
RightHipLateral().forward(0).radians,
RightKneeLateral().backward(0).radians,
RightAnkleLateral().forward(0).radians,
RightAnkleFrontal().outward(-0).radians)
r.draw_torso()
r.draw_coordinates()
r.show()
# r._painter.show(block=False)
# r.draw_left_lower_limp(radians(-0), radians(0), radians(-30), radians(0), 0, 0)
# r.draw_left_lower_limp(radians(-0), radians(0), radians(-90), radians(0), 0, 0)
# r._painter.show(block=True)
# r.draw_left_lower_limp()
# r.draw_right_lower_limp(right_knee_lateral_radians=radians(150))
r.draw_left_lower_limp(
# LeftHipTransversal().outward(0).radians,
radians(-0),
LeftHipFrontal().outward(0).angle,
LeftHipLateral().forward(0).radians,
LeftKneeLateral().backward(0).radians,
# radians(150),
LeftAnkleLateral().forward(0).radians,
LeftAnkleFrontal().outward(-0).radians)
r.draw_right_lower_limp(
RightHipTransversal().inward(0).radians,
# radians(-0),
RightHipFrontal().outward(0).angle,
RightHipLateral().forward(0).radians,
RightKneeLateral().backward(0).radians,
RightAnkleLateral().forward(0).radians,
RightAnkleFrontal().outward(-0).radians)
r.draw_torso()
r.draw_coordinates()
r.show()
# r._painter.show(block=False)
# r.draw_left_lower_limp(radians(-0), radians(0), radians(-30), radians(0), 0, 0)
# r.draw_left_lower_limp(radians(-0), radians(0), radians(-90), radians(0), 0, 0)
# r._painter.show(block=True)
