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()
robot_server_host = "localhost"
robot_server_port = 1313
udp_server = socketserver.UDPServer((robot_server_host, robot_server_port), VirtualRobotRequestHandler)
Thread(target=udp_server.serve_forever).start()
if __name__ == '__main__':
start_robot_server_thread()
painter = Painter(azimuth=-30, elevation=60)
robot_model = RobotModel(painter)
# robot_model.draw_initial_pose()
robot_model.draw_neck_and_head()
RobotMemory.left_arm_joints = robot_model.draw_left_arm()
RobotMemory.right_arm_joints = robot_model.draw_right_arm()
RobotMemory.torso_corner_positions = robot_model.draw_torso()
def func(i):
# Note that draw_left/right_lower_limp() is called two times.
# The param `draw` is false for the first time, which makes the func only do math work.
# If we want super-high performance, refactor those 2 funcs.
RobotMemory.left_lower_limp_joints = robot_model.draw_left_lower_limp(*RobotMemory.left_lower_limp_angles, draw=False)
RobotMemory.right_lower_limp_joints = robot_model.draw_right_lower_limp(*RobotMemory.right_lower_limp_angles, draw=False)
left_lower_limp_lines = robot_model.draw_left_lower_limp(*RobotMemory.left_lower_limp_angles, get_lines=True)
right_lower_limp_lines = robot_model.draw_right_lower_limp(*RobotMemory.right_lower_limp_angles, get_lines=True)
center_of_mass = compute_center_of_mass_given_joints(
from util.painter import Painter
from robot_model import RobotModel
from joint.joint_angle import *
r = RobotModel(Painter())
r.draw_neck_and_head()
r.draw_left_arm()
# r.draw_left_arm(radians(-30), radians(60), radians(-30))
# r._painter.draw_point(r.draw_left_arm(radians(0), radians(-60), radians(0), color='b-')[-1])
r.draw_right_arm()
# r.draw_right_arm(radians(0), radians(30), radians(160), color='b-')
# 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,
__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()
from util.painter import Painter
from robot_model import RobotModel
from joint.joint_angle import *
r = RobotModel(Painter())
r.draw_neck_and_head()
r.draw_left_arm()
# r.draw_left_arm(radians(-30), radians(60), radians(-30))
# r._painter.draw_point(r.draw_left_arm(radians(0), radians(-60), radians(0), color='b-')[-1])
r.draw_right_arm()
# r.draw_right_arm(radians(0), radians(30), radians(160), color='b-')
# 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,
robot_server_port = 1313
udp_server = socketserver.UDPServer((robot_server_host, robot_server_port),
VirtualRobotRequestHandler)
Thread(target=udp_server.serve_forever).start()
if __name__ == '__main__':
start_robot_server_thread()
painter = Painter(azimuth=-30, elevation=60)
robot_model = RobotModel(painter)
# robot_model.draw_initial_pose()
robot_model.draw_neck_and_head()
RobotMemory.left_arm_joints = robot_model.draw_left_arm()
RobotMemory.right_arm_joints = robot_model.draw_right_arm()
RobotMemory.torso_corner_positions = robot_model.draw_torso()
def func(i):
# Note that draw_left/right_lower_limp() is called two times.
# The param `draw` is false for the first time, which makes the func only do math work.
# If we want super-high performance, refactor those 2 funcs.
RobotMemory.left_lower_limp_joints = robot_model.draw_left_lower_limp(
*RobotMemory.left_lower_limp_angles, draw=False)
RobotMemory.right_lower_limp_joints = robot_model.draw_right_lower_limp(
*RobotMemory.right_lower_limp_angles, draw=False)
left_lower_limp_lines = robot_model.draw_left_lower_limp(
*RobotMemory.left_lower_limp_angles, get_lines=True)
