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()
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)