four_bar_mechanism.rar_Four Bar Mechanism_matlab 四杆机构_matlab四杆机构

function [ motor_goal_position ] = four_bar_mechanism(joint_angle) % 4BAR_LINK_MECHANISM Summary of this function goes here % Detailed explanation goes here %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Initila length of each 4 bar mechanism on every joint % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %--- hip joint ---% hip_upper_link=320/1000; % unit:m hip_lower_link=83/1000; hip_slide_link_initial=307/1000; hip_compensate_angle=15*pi/180; hip_initial_angle=75*pi/180; %--- knee joint ---% knee_upper_link=285.97/1000; % unit:m knee_lower_link=62.1/1000; knee_slide_link_initial=242.17/1000; knee_compensate_angle=40*pi/180; knee_initial_angle=50*pi/180; %--- motor goal position calculation ---% goal_position_1=goal_position(knee_upper_link,knee_lower_link,knee_slide_link_initial,knee_initial_angle,-joint_angle(1)); goal_position_2=goal_position(hip_upper_link,hip_lower_link,hip_slide_link_initial,hip_initial_angle,joint_angle(2)); goal_position_3=goal_position(hip_upper_link,hip_lower_link,hip_slide_link_initial,hip_initial_angle,joint_angle(3)); goal_position_4=goal_position(knee_upper_link,knee_lower_link,knee_slide_link_initial,knee_initial_angle,-joint_angle(4)); motor_goal_position=[goal_position_1,goal_position_2,goal_position_3,goal_position_4];