def draw_robot_with_center_of_mass(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 angles = ReadStateParser(read_state_string).get_all_joint_radians() painter = Painter(azimuth=0, elevation=0) robot_model = RobotModel(painter) center_of_mass = compute_center_of_mass_from_read_state_string(angles, painter, robot_model,\ draw_robot=True, draw_center_of_mass=True) # robot_model.draw_coordinates() # draw coordinates lastly because we want them on top layer robot_model.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()
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)