four_bar_mechanism.m

四杆机构运动求解

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];