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)
__author__ = 'zhao'
from inverse_kinematics.lower_limp import InverseKinematicsLeg
from util.painter import Painter
from util.point import Point
from robot_model import RobotModel
is_left = False
# target_position = Point((-8.0, 7.25, -44.699999999999989))
target_position = Point((-8.0, 7.25, -44.7))
painter = Painter()
robot_model = RobotModel(painter)
r_hip_transversal_radians = 0
r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians\
= InverseKinematicsLeg(is_left=is_left, hip_transversal_radians=r_hip_transversal_radians, target_position=target_position, robot_model=robot_model, painter=painter).get_angles()
# r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians = [0] * 5
# painter.draw_point(target_position)
robot_model.draw_neck_and_head()
robot_model.draw_torso()
robot_model.draw_left_arm()
robot_model.draw_right_arm()
robot_model.draw_left_lower_limp()
robot_model.draw_right_lower_limp(right_hip_transversal_radians=r_hip_transversal_radians, right_hip_frontal_radians=r_hip_frontal_radians, right_hip_lateral_radians=r_hip_lateral_radians, right_knee_lateral_radians=r_knee_lateral_radians, right_ankle_lateral_radians=r_ankle_lateral_radians, right_ankle_frontal_radians=r_ankle_frontal_radians)
robot_model.show()
from util.point import Point
from robot_model import RobotModel
is_left = False
# target_position = Point((-8.0, 7.25, -44.699999999999989))
target_position = Point((-8.0, 7.25, -44.7))
painter = Painter()
robot_model = RobotModel(painter)
r_hip_transversal_radians = 0
r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians\
= InverseKinematicsLeg(is_left=is_left, hip_transversal_radians=r_hip_transversal_radians, target_position=target_position, robot_model=robot_model, painter=painter).get_angles()
# r_hip_frontal_radians, r_hip_lateral_radians, r_knee_lateral_radians, r_ankle_lateral_radians, r_ankle_frontal_radians = [0] * 5
# painter.draw_point(target_position)
robot_model.draw_neck_and_head()
robot_model.draw_torso()
robot_model.draw_left_arm()
robot_model.draw_right_arm()
robot_model.draw_left_lower_limp()
robot_model.draw_right_lower_limp(
right_hip_transversal_radians=r_hip_transversal_radians,
right_hip_frontal_radians=r_hip_frontal_radians,
right_hip_lateral_radians=r_hip_lateral_radians,
right_knee_lateral_radians=r_knee_lateral_radians,
right_ankle_lateral_radians=r_ankle_lateral_radians,
right_ankle_frontal_radians=r_ankle_frontal_radians)
robot_model.show()
# 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)
