SP 12

Kristopher Doll
ME 581
Project Report: Kinematic Analysis of Recumbent Tilting Tricycle Design
Introduction and Summary:
Three wheeled human powered vehicles offer a number of

practical advantages compared to traditional or recumbent bicycles;
however, rigid frame tricycles tend to tip over when turning at relatively
high speeds and require extremely low rider position. The ability to tilt
or lean when turning like a bicycle would eliminate this problem.
A possible design of a tilting tricycle has been created (figure 1)

and is evaluated in this project. The objective is to create an inverse
kinematic model of this vehicle in MATLAB which is capable of solving
for the position and orientation of each link. Vehicle parameters can be
specified and a kinematically driven tilted turn is simulated to
determine effect of the design on steering performance.
A solidworks model was created to explore the effect of

physical space and manufacturability limitations on the components
used in the vehicle. The solidworks model is also used to validate the
MATLAB model by comparing the behavior of the two models. Figure 1: Tilting Tricycle
However, the MATLAB model is significantly more capable of handling
the problem of optimizing the vehicle geometry. Ackermann steering is far more difficult to verify in the
solidworks model, and changing the geometric parameters is relatively time consuming.
Further investigation may involve converting the framework constructed for this project into a
forward dynamic model to simulate a turn and better assess the vehicle performance.
Methodology and Description of Model:
The MATLAB model features the following links:

1: Ground
2: Frame w/ Rear Wheel
3: Rocker
4: Left Side Column
5: Right Side Column
6: Left Kingpin
7: Right Kingpin
8: Left Wheel
9: Right Wheel
Figure 2: Tilting Mechanism
Since a dynamic simulation was not performed, the leaning of the rider was not modeled. If this
were to be modeled, the rider’s upper body would be link 10, which would pivot about the hips,
modeled as a point on the frame. In addition, if this were a dynamic simulation, the rear wheel would
be modeled as link 11 since its rotational inertia would provide a stabilizing effect on the system.
The bottom tie rods and the steering tie rod are simplified using double revolute constraints
since their mass is small compared to the other moving components. Although the two bottom ties
would actually be connected using revolute joints on a physical prototype of this vehicle, modeling them
using revolutes would be unnecessary and may over-constrain the assembly.
The position/orientation vector is as follows:

q = {r2; p2; r3; p3; r4; p4; r5; p5; r6; p6; r7; p7; r8; p8; r9; p9}
The following points are used in the model:

A) Rear wheel contact
B) Center Rocker Pivot
C) Center Tie Rod Pivot
D) Left Rocker Pivot
E) Right Rocker Pivot
F) Left Tie Pivot
G) Right Tie Pivot
H) Left Kingpin Revolute
I) Right Kingpin Revolute
J) Left Steering Arm Rod End
K) Right Steering Arm Rod End
L) Left Axle-Kingpin Bearing
M) Right Axle-Kingpin Bearing
N) Left Wheel Contact Patch
O) Right Wheel Contact Patch
Description of Coordinate Frames
The global coordinate frame

coincides with the ground Figure 3: Link 2 Points and Coordinates
coordinate frame and is oriented so
that the Z axis points in the forward
direction of the vehicle, Y points up in the vertical direction, and X points to the left side of the vehicle.
The global origin is located on the ground, directly under point B when the vehicle is in the upright,
unturned configuration.
Not all of the coordinate frames are

centered about the mass centroids of the links
since this project will not involve any dynamic
simulation. The location of mass centroids and
masses of the links would depend largely on the
physical design of each component and
hardware acquired from vendors.
Link 2 coordinate frame origin is at point

B (Figure 3). z2 is parallel to the bottom section
of the frame (which is parallel to the ground
when the vehicle is un-tilted and steering
straight). Figure 4: Link 3 Points and Coordinates
The link 3 frame is at the center of the rocker bar as
shown in figure 4. z3 is along the revolute pin at point B.
The local coordinate frame of link 4 is located at point D

(Figure 5). There is a double prime coordinate frame at H in
order to account for the caster angle of the vehicle. This frame
involves a rotation about the x4 axis. Note that link 5 is
symmetric to link 4. The two differences are that x axis and
caster angle are in opposite directions for link 5.
Link 6 frame is centered about the intersection of the

kingpin axis with the axle of the wheel, figure 6. The y6 axis is
along the kingpin axis of rotation. x6 is in the plane spanned by
y6 and the wheel axis. A double prime frame is used for the
revolute joint with the wheel. This frame is rotated about z6 by
the amount of the kingpin angle so that x6 is along the wheel
axle. Link 7 is symmetric to link 6, and the x axes always point to Figure 5: Link 4
the general left side of the vehicle.
The link 8 local frame is centered at the mass center of

the wheel which does not include the stub axle (Figure 7). x8 is
along the axle. z8 points forward and is parallel to the ground.
Link 9 is symmetric to link 8.
The local coordinate locations of points on each link is

computed by the MATLAB script based on user specified
parameters, such as rocker height, wheel track, and certain easily
measurable dimensions that are either determined by physical
space constraints of parts or that need to be optimized in the Figure 6: Link 6
design. For example, the length of the steering tie rod is not user
specified, but is computed by the script to make the wheels
aligned when the vehicle is steering straight. In addition, the
kingpin angle is calculated so that the axis of the kingpin
intersects the contact patch of the tire when the caster angle is
zero.
The MATLAB model in this project does not allow for

camber or toe. It is assumed that camber is unnecessary since
the wheels will tilt during turns, and toe is neglected for ease of
point location computation and so that it does not affect the
interpretation of wheel turn angles.
Figure 7: Link 8
The following of kinematic constraints are employed in the model:
 Revolute at B between Frame(2) and Rocker(3)
 Revolute at D between Rocker(3) and Left Side Column(4)
 Revolute at E between Rocker(3) and Right Side Column(5)
 Revolute at H between Left Side Column(4) and Left Kingpin (6)
 Revolute at I between Right Side Column(5) and Right Kingpin (7)
 Revolute at L between Left Kingpin(6) and Left Wheel(8)
 Revolute at M between Right Kingpin(7) and Right Wheel(9)
 Double Spherical between F on 4 and C on 2
 Double Spherical between G on 5 and C on 2
 Double Spherical between J on 6 and K on 7
 Point N on Left Wheel(8) is on the ground (y=0) and z8 is parallel to ground
 Point O on Right Wheel(9) is on the ground (y=0) and z9 is parallel to ground
 Point A on Frame(2) is on ground and does not move forward (This is an approximation which
assumes the base of the frame remains roughly parallel to the ground )
 Point B on Frame(2) has x coordinate of 0 and z2 is in YZ plane (perpendicular to X)
Note that z8 and z9 are made parallel to the ground by constraining them to be perpendicular to the
normal of the ground.
Driver Constraints:
 Move x coordinate of point C on Frame(2) to actuate tilting
 Move z coordinate of point N on Left Wheel(8) relative to point L to turn
In addition to kinematic and driver constraints, there are Euler parameter constraints that ensure that
each set of Euler parameters is of unit length.
Measured performance variables:
Wheel track is computed from the distance vector between the two ground contact patches.
Wheel turn angle is determined by from the inner product between the z8 axis (z9 for right
wheel) and the global Z axis. This is possible because z8 and z9 are constrained to be parallel to the
ground.
The correct Ackermann turn angle for the right wheel is found by calculating the turning radius
based on the orientation of the left and rear wheels and then computing the angle of the right wheel so
that its axis coincides with the center of the turn.
Simulation
The script can be modified to drive the tilt angle and left wheel angle according to any arbitrary
function. The simulation conducted for this project employs simply linear interpolation from a starting
tilt and turn configuration to a final configuration along a certain number of steps. A left turn was
performed by moving point N on the left wheel axle in the Z direction. This method does not yield a
linear increase in the wheel turn angle, especially since the vehicle is tilting simultaneously.
Results of Simulation
Track will decrease by approx. 1cm

when the vehicle is tilted to 15 degrees and
1cm when the wheels are rotated to yield a
5m turning radius. Figure 8 shows the
combined effect of tilt angle and wheel
rotation on wheel track. This combined 2 cm
change in track width could potentially pose a
problem when this vehicle turns.
Another problem identified using this

model is that the steering is far from ideal
Ackermann. This is evident in the plot of left
and right wheel turning angles (Figure 9).
Figure 8: Wheel Track as a function of Tilt Angle (and Turn)
The right wheel (which is on the outside of the
turn) actually turns slightly more than the left
wheel; this can be verified by watching the
simulation animation.
To correct both of these issues, the

linkage geometry will need to be calibrated. It
may be possible to implement an optimization
routine within MATLAB to find the best
geometric parameters given desired vehicle
dimensions.
It has been shown that this MATLAB

model is a powerful tool for analyzing the
potential performance of this vehicle design.
This model will be eventually expanded to
perform a full dynamic simulation of a turn to
address all the concerns of this design. Figure 9: Comparison of Wheel Turning Angles
MATLAB Code:
%tri_main.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This script simulates the motion of a linkage mechanism. Use tri_ini to
%specify geometry and make initial position guesses. tri_phi computes the
%geometry, constraint jacobians, and evaluates the residuals of the
%constraints. tri_kin solves the mechanism at each time step in the
%simulation.
%
% 5/1/2012 Kristopher Doll
% Written for C05 assignment of ME581
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
tri_ini
%tilt=0.04;
%turn=0.045; %Express turn in terms of an angle
t=0;
steps=100;
turn_min=0;
turn_max=0.0;
tilt_min=0;
tilt_max=0.04;
turn_inc=(turn_max-turn_min)/(steps-1);
tilt_inc=(tilt_max-tilt_min)/(steps-1);
turn=turn_min; tilt=tilt_min;
tri_phi
%Establish initial locations

residuals=1; counter=0;
while and(residuals>1e-5,counter<=20)
q=q-inv(JAC)*PHI;
tri_phi
residuals=max(abs(PHI));
counter=counter+1;
end
a_z8=zeros(1,steps); a_z9=a_z8; tire_space=a_z8; a_tilt=a_z8; a_z9c=a_z8;
figure(1)
for i=1:steps
turn=turn_min+(i-1)*turn_inc;
tilt=tilt_min+(i-1)*tilt_inc;
residuals=1; counter=0;
while and(residuals>1e-5,counter<=20)
q=q-inv(JAC)*PHI;
tri_phi
residuals=max(abs(PHI));
counter=counter+1;
end
%Animation
clf
xlim([-1,1]); ylim([0, 2]); zlim([-2,1])
body2=[r2A r2B r2C r2A];
body3=[r3D r3E];
body4=[r4D r4F r4H];
body5=[r5E r5G r5I];
tierod1=[r2C r4F];
tierod2=[r2C r5G];
body6=[r6H r6L r6J];
body7=[r7I r7M r7K];
steerrod=[r6J r7K];
tire8=A8*circle; tire8(1,:)=tire8(1,:)+r8(1);
tire8(2,:)=tire8(2,:)+r8(2); tire8(3,:)=tire8(3,:)+r8(3);
tire9=A9*circle; tire9(1,:)=tire9(1,:)+r9(1);
tire9(2,:)=tire9(2,:)+r9(2); tire9(3,:)=tire9(3,:)+r9(3);
body8=[r8L r8 r8N tire8];
body9=[r9M r9 r9O tire9];
line(body2(1,:),body2(2,:),body2(3,:),'Color','k');
line(body3(1,:),body3(2,:),body3(3,:),'Color','c');
line(body4(1,:),body4(2,:),body4(3,:),'Color','r');
line(body5(1,:),body5(2,:),body5(3,:),'Color','r');
line(body6(1,:),body6(2,:),body6(3,:),'Color','m');
line(body7(1,:),body7(2,:),body7(3,:),'Color','m');
line(tierod1(1,:),tierod1(2,:),tierod1(3,:),'Color','g');
line(tierod2(1,:),tierod2(2,:),tierod2(3,:),'Color','g');
line(steerrod(1,:),steerrod(2,:),steerrod(3,:),'Color','b');
pause(0.00001)
%Calculate each wheel angle

%dot product between z8 and z1
a_z8(i)=acos(h8'*h1)/d2r;
a_z9(i)=acos(h9'*h1)/d2r;
%Calculate distance between tires.
d89=r9O-r8N;
tire_space(i)=sqrt(d89'*d89);
%Tilt angle
a_tilt(i)=asin(tilt/d_piv)/d2r;
%Ackermann Steering Error based on left wheel

radius1=wheelbase/tan(a_z8(i)*d2r)+0.5*track;
%Correct right wheel angle
a_z9c(i)=atan(wheelbase/(radius1+0.5*track))/d2r;
end
figure(2)
plot(a_z8,a_z9,a_z8,a_z9c)
title('Turning Angle of Front Wheels')
xlabel('Left Wheel Turn (Degrees)')
ylabel('Right Wheel (Degrees)')
legend('Actual', 'Ideal Ackermann')
figure(3)
plot(a_tilt,tire_space)
title('Instantaneous Track')
xlabel('Tilt Angle (Degrees)')
ylabel('Track (m)')
%tri_ini.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This subroutine used to specify initial estimates for coordinate frame
%position/orientation and local coordinates of points.
%
%The subroutine requires no inputs.
%
% Written for the final project of ME581
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
d2r=pi/180; %Conversion factor for degrees to radians

I3=eye(3);
%Global Coordinate frame origin is at the projection of the center of the

%rocker onto the ground when cart is in starting position with wheels
%straight and frame upright.
%Input Vehicle parameters, meters, angles in radians

wheelbase=1.75;
track=1;
wheeldia=0.5;
%Camber is set to equal zero since wheels will tilt on turns.
a_cast=10*d2r; %caster angle in y-z plane of links 4 & 5
%toe is also ignored for this simulation
h_piv=0.35; %height of the lower pivot point on the frame
d_piv=0.15; %distance between pivot points on the frame
d_tie=0.02; %sum of bottom tie rod lengths minus rocker length
%Tie rods and rocker will be parallel when frame is upright in this model
l_arm=0.1; %tie rod arm length off the kingpin
a_arm=15*d2r; %angle of tie rod arm on kingpin. zero is parallel to wheel
d_arm=0.05; %distance of arm from axle along kingpin centerline
d_cast=0.1; %distance of caster pivot from axle along kingpin centerline
d_axle=0.05; %distance of wheel center from kingping centerline
d_king=0.05; %distance of rocker pivot from kingpin centerline
%original inclination of kingpin from vertical in the X-Y plane

a_king=atan(d_axle/0.5*wheeldia);
l_rock=track-2*((h_piv+d_piv)*tan(a_king)+d_king/cos(a_king));
l_tie=(l_rock+d_tie)/2;
l_steer=track-
2*((wheeldia+d_arm*cos(a_cast))*sin(a_king)+l_arm*(sin(a_arm)*cos(a_king)-
cos(a_arm)*sin(a_cast)*sin(a_king)))+0.023;
%Constant Local Body-Fixed Locations of Points, m

r1A=[0;0;-wheelbase];
s2pA=[0;-h_piv-d_piv;-wheelbase];
s2pB=[0;0;0];
s2pC=[0;-d_piv;0];
s3pB=[0;0;0];
s3pD=[l_rock*0.5;0;0];
s3pE=[-l_rock*0.5;0;0];
s4pD=[0;0;0];
s4pF=[-d_piv*sin(a_king)+0.5*d_tie*cos(a_king);-d_piv*cos(a_king)-
0.5*d_tie*sin(a_king);0];
s4pH=[d_king;-(h_piv+d_piv+d_king*sin(a_king)-d_cast*cos(a_king)*cos(a_cast)-
0.5*wheeldia)/cos(a_king);0];
s5pE=[0;0;0];
s5pG=[-s4pF(1);s4pF(2);s4pF(3)];
s5pI=[-s4pH(1);s4pH(2);s4pH(3)];
s6pH=[0;d_cast;0];
s6pJ=[-l_arm*sin(a_arm);d_axle;-l_arm*cos(a_arm)];
s6pL=[0;0;0];
s7pI=[0;d_cast;0];
s7pK=[-s6pJ(1);s6pJ(2);s6pJ(3)];
s7pM=[0;0;0];
s8pL=[-d_axle;0;0];
s8pN=[0;-wheeldia/2;0];
s9pM=[ d_axle;0;0];
s9pO=s8pN;
%Estimated Global Position/Orientation of Coordinate Frames

q2_ini=[0; h_piv+d_piv; 0; 1;0;0;0];
q3_ini=q2_ini;
q4_ini=[ l_rock/2;q2_ini(2);0; cos( a_king/2);0;0;1*sin( a_king/2)];
q5_ini=[-l_rock/2;q2_ini(2);0; cos(-a_king/2);0;0;1*sin(-a_king/2)];
q6_ini=[ track/2-d_axle;wheeldia/2;d_cast*sin(a_cast); cos(
a_king/2);0;0;1*sin( a_king/2)]; %rotation is approx.
q7_ini=[-track/2+d_axle;wheeldia/2;d_cast*sin(a_cast); cos(-
a_king/2);0;0;1*sin(-a_king/2)];
q8_ini=[ track/2;wheeldia/2;d_cast*sin(a_cast); 1;0;0;0];
q9_ini=[-track/2;wheeldia/2;d_cast*sin(a_cast); 1;0;0;0];
q=[q2_ini; q3_ini; q4_ini; q5_ini; q6_ini; q7_ini; q8_ini; q9_ini];
fp=[1;0;0];
gp=[0;1;0];
hp=[0;0;1];
%Double Prime coordinate frames in terms of local body coordinates.
f4p2=[1;0;0];
g4p2=[0;cos(a_cast);-sin(a_cast)];
h4p2=[0;sin(a_cast);cos(a_cast)];
C4=[f4p2,g4p2,h4p2];
f5p2=[1;0;0];
g5p2=[0;cos(a_cast);-sin(a_cast)];
h5p2=[0;sin(a_cast);cos(a_cast)];
C5=[f5p2,g5p2,h5p2];
f6p2=[cos(a_king);-sin(a_king);0];
g6p2=[sin(a_king);cos(a_king);0];
h6p2=[0;0;1];
C6=[f6p2,g6p2,h6p2];
f7p2=[cos(a_king);sin(a_king);0];
g7p2=[-sin(a_king);cos(a_king);0];
h7p2=[0;0;1];
C7=[f7p2,g7p2,h7p2];
%Make skew symmetric matrices

S2pAs=skew_sym(s2pA); S2pBs=skew_sym(s2pB); S2pCs=skew_sym(s2pC);
S3pBs=skew_sym(s3pB); S3pDs=skew_sym(s3pD); S3pEs=skew_sym(s3pE);
S4pDs=skew_sym(s4pD); S4pFs=skew_sym(s4pF); S4pHs=skew_sym(s4pH);
S5pEs=skew_sym(s5pE); S5pGs=skew_sym(s5pG); S5pIs=skew_sym(s5pI);
S6pHs=skew_sym(s6pH); S6pJs=skew_sym(s6pJ); S6pLs=skew_sym(s6pL);
S7pIs=skew_sym(s7pI); S7pKs=skew_sym(s7pK); S7pMs=skew_sym(s7pM);
S8pLs=skew_sym(s8pL); S8pNs=skew_sym(s8pN);
S9pMs=skew_sym(s9pM); S9pOs=skew_sym(s9pO);
fps=skew_sym([1;0;0]);
gps=skew_sym([0;1;0]);
hps=skew_sym([0;0;1]);
f4p2s=skew_sym(f4p2); g4p2s=skew_sym(g4p2); h4p2s=skew_sym(h4p2);

[E1,G1,A1,f1,g1,h1]=make_ega([1;0;0;0]);
%Create the image of the tires

res=100; %resolution of the tire circle
circle=zeros(3,res);
dtheta=2*pi/res; theta=-pi/2;
for i=1:1:res
circle(2,i)=0.5*wheeldia*sin(theta);
circle(3,i)=0.5*wheeldia*cos(theta);
theta=theta+dtheta;
end
%tri_phi.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This subroutine computes global distances, rotation matrices, the
%constraint residuals, and the Jacobian matrix relating constraints to the
%change in q, the coordinate frame origin global position vector.
%
%The subroutine requires q, R, w2, t, and the points' body fixed local
%coordinates as inputs.
%
% Written for ME581 Final Project
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Rip r and phi from q

r2=q(1:3);
p2=q(4:7);
r3=q(8:10);
p3=q(11:14);
r4=q(15:17);
p4=q(18:21);
r5=q(22:24);
p5=q(25:28);
r6=q(29:31);
p6=q(32:35);
r7=q(36:38);
p7=q(39:42);
r8=q(43:45);
p8=q(46:49);
r9=q(50:52);
p9=q(53:56);
[E2,G2,A2,f2,g2,h2]=make_ega(p2);
%Double Prime frames in global coordinates

fgh=A4*C4; g4_2=fgh(:,2);
fgh=A5*C5; g5_2=fgh(:,2);
fgh=A6*C6; f6_2=fgh(:,1);
fgh=A7*C7; f7_2=fgh(:,1);
%Compute point locations in global coordinates

r2A=r2+A2*s2pA; r2B=r2+A2*s2pB; r2C=r2+A2*s2pC;
r3B=r3+A3*s3pB; r3D=r3+A3*s3pD; r3E=r3+A3*s3pE;
r4D=r4+A4*s4pD; r4F=r4+A4*s4pF; r4H=r4+A4*s4pH;
r5E=r5+A5*s5pE; r5G=r5+A5*s5pG; r5I=r5+A5*s5pI;
r6H=r6+A6*s6pH; r6J=r6+A6*s6pJ; r6L=r6+A6*s6pL;
r7I=r7+A7*s7pI; r7K=r7+A7*s7pK; r7M=r7+A7*s7pM;
r8L=r8+A8*s8pL; r8N=r8+A8*s8pN;
r9M=r9+A9*s9pM; r9O=r9+A9*s9pO;
%Tie rod vectors
d67=r7K-r6J;
d24=r4F-r2C;
d25=r5G-r2C;
%Define Constraints
PHI=zeros(56,1);
%Revolute at B
PHI(1:3)=r3B-r2B;
PHI(4)=h2'*f3;
PHI(5)=h2'*g3;
%Revolute at D
PHI(6:8)=r4D-r3D;
PHI(9)=h3'*f4;
PHI(10)=h3'*g4;
%Revolute at E
PHI(11:13)=r5E-r3E;
PHI(14)=h3'*f5;
PHI(15)=h3'*g5;
%Revolute at H
PHI(16:18)=r6H-r4H;
PHI(19)=g4_2'*f6;
PHI(20)=g4_2'*h6;
%Revolute at I
PHI(21:23)=r7I-r5I;
PHI(24)=g5_2'*f7;
PHI(25)=g5_2'*h7;
%Revolute at L
PHI(26:28)=r8L-r6L;
PHI(29)=f6_2'*g8;
PHI(30)=f6_2'*h8;
%Revolute at M
PHI(31:33)=r9M-r7M;
PHI(34)=f7_2'*g9;
PHI(35)=f7_2'*h9;
%Double Spherical CF
PHI(36)=d24'*d24-l_tie^2;
%Double Spherical CG
PHI(37)=d25'*d25-l_tie^2;
%Double Spherical JK
PHI(38)=d67'*d67-l_steer^2;
%Ground Contact at N
PHI(39)=r8N(2);
PHI(40)=h8'*g1;
%Ground Contact at O
PHI(41)=r9O(2);
PHI(42)=h9'*g1;
%Ground Contact at A and No Forward Motion

PHI(43:44)=r2A(2:3)-r1A(2:3);
%Point B and z2 constrainted in YZ plane
PHI(45)=r2B(1);
PHI(46)=h2'*f1;
%Driver Constraints
PHI(47)=r2C(1)-tilt;
PHI(48)=r8N(3)-r6L(3)-turn;
%Euler Parameters
PHI(49)=p2'*p2-1;
PHI(50)=p3'*p3-1;
PHI(51)=p4'*p4-1;
PHI(52)=p5'*p5-1;
PHI(53)=p6'*p6-1;
PHI(54)=p7'*p7-1;
PHI(55)=p8'*p8-1;
PHI(56)=p9'*p9-1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Create the Jacobian Matrix

JAC=zeros(56,56);
%Revolute at B, i=2, j=3
JAC(1:3,1:3)=-I3;
JAC(1:3,4:7)= 2*A2*S2pBs*G2;
JAC(1:3,8:10)=I3;
JAC(1:3,11:14)=-2*A3*S3pBs*G3;
JAC(4,4:7)= -2*fp'*A3'*A2*hps*G2;
JAC(4,11:14)=-2*hp'*A2'*A3*fps*G3;
JAC(5,4:7)= -2*gp'*A3'*A2*hps*G2;
JAC(5,11:14)=-2*hp'*A2'*A3*gps*G3;
%Revolute at D, i=3, j=4
JAC(6:8,8:10)=-I3;
JAC(6:8,11:14)= 2*A3*S3pDs*G3;
JAC(6:8,15:17)=I3;
JAC(6:8,18:21)=-2*A4*S4pDs*G4;
JAC(9,11:14)= -2*fp'*A4'*A3*hps*G3;
JAC(9,18:21)= -2*hp'*A3'*A4*fps*G4;
JAC(10,11:14)=-2*gp'*A4'*A3*hps*G3;
JAC(10,18:21)=-2*hp'*A3'*A4*gps*G4;
%Revolute at E, i=3, j=5
JAC(11:13,8:10)=-I3;
JAC(11:13,11:14)= 2*A3*S3pEs*G3;
JAC(11:13,22:24)=I3;
JAC(11:13,25:28)=-2*A5*S5pEs*G5;
JAC(14,11:14)= -2*fp'*A5'*A3*hps*G3;
JAC(14,25:28)= -2*hp'*A3'*A5*fps*G5;
JAC(15,11:14)= -2*gp'*A5'*A3*hps*G3;
JAC(15,25:28)= -2*hp'*A3'*A5*gps*G5;
%Revolute at H, i=4, j=6

JAC(16:18,8:10)=-I3;
JAC(16:18,11:14)= 2*A4*S4pHs*G4;
JAC(16:18,29:31)=I3;
JAC(16:18,32:35)=-2*A6*S6pHs*G6;
JAC(19,11:14)= -2*fp'*A6'*A4*g4p2s*G4;
JAC(19,32:35)= -2*g4p2'*A4'*A6*fps*G6;
JAC(20,11:14)= -2*hp'*A6'*A4*g4p2s*G4;
JAC(20,32:35)= -2*g4p2'*A4'*A6*hps*G6;
%Revolute at I, i=5, j=7
JAC(21:23,22:24)=-I3;
JAC(21:23,25:28)= 2*A5*S5pIs*G5;
JAC(21:23,36:38)=I3;
JAC(21:23,39:42)=-2*A7*S7pIs*G7;
JAC(24,25:28)= -2*fp'*A7'*A5*g5p2s*G5;
JAC(24,39:42)= -2*g5p2'*A5'*A7*fps*G7;
JAC(25,25:28)= -2*hp'*A7'*A5*g5p2s*G5;
JAC(25,39:42)= -2*g5p2'*A5'*A7*hps*G7;
%Revolute at L, i=6, j=8

JAC(26:28,29:31)=-I3;
JAC(26:28,32:35)= 2*A6*S6pLs*G6;
JAC(26:28,43:45)=I3;
JAC(26:28,46:49)=-2*A8*S8pLs*G8;
JAC(29,32:35)= -2*gp'*A8'*A6*f6p2s*G6;
JAC(29,46:49)= -2*f6p2'*A6'*A8*gps*G8;
JAC(30,32:35)= -2*hp'*A8'*A6*f6p2s*G6;
JAC(30,46:49)= -2*f6p2'*A6'*A8*hps*G8;
%Revolute at M, i=7, j=9
JAC(31:33,36:38)=-I3;
JAC(31:33,39:42)= 2*A7*S7pMs*G7;
JAC(31:33,50:52)=I3;
JAC(31:33,53:56)=-2*A9*S9pMs*G9;
JAC(34,39:42)= -2*gp'*A9'*A7*f7p2s*G7;
JAC(34,53:56)= -2*f7p2'*A7'*A9*gps*G9;
JAC(35,39:42)= -2*hp'*A9'*A7*f7p2s*G7;
JAC(35,53:56)= -2*f7p2'*A7'*A9*hps*G9;
%Double Spherical CF i=2, j=4

JAC(36,1:3)= -2*d24';
JAC(36,4:7)= 4*d24'*A2*S2pCs*G2;
JAC(36,15:17)=2*d24';
JAC(36,18:21)=-4*d24'*A4*S4pFs*G4;
%Double Spherical CG i=2, j=5
JAC(37,1:3)= -2*d25';
JAC(37,4:7)= 4*d25'*A2*S2pCs*G2;
JAC(37,22:24)= 2*d25';
JAC(37,25:28)=-4*d25'*A5*S5pGs*G5;
%Double Spherical JK i=6, j=7
JAC(38,29:31)=-2*d67';
JAC(38,32:35)= 4*d67'*A6*S6pJs*G6;
JAC(38,36:38)= 2*d67';
JAC(38,39:42)=-4*d67'*A7*S7pKs*G7;
%Ground Contact at N
JACr=I3; JACp=-2*A8*S8pNs*G8;
JAC(39,43:45)=JACr(2,:);
JAC(39,46:49)=JACp(2,:);
%i=8, j=1
JAC(40,46:49)= -2*gp'*A1'*A8*hps*G8;
%Ground Contact at O
JACr=I3; JACp=-2*A9*S9pOs*G9;
JAC(41,50:52)=JACr(2,:);
JAC(41,53:56)=JACp(2,:);
%i=9, j=1
JAC(42,53:56)= -2*gp'*A1'*A9*hps*G9;
%Ground Contact at A and No Forward Motion

JACr=I3; JACp=-2*A2*S2pAs*G2;
JAC(43:44,1:3)=JACr(2:3,:);
JAC(43:44,4:7)=JACp(2:3,:);
%Point B and z2 constrainted in YZ plane

JACr=I3; JACp=-2*A2*S2pBs*G2;
JAC(45,1:3)=JACr(1,:);
JAC(45,4:7)=JACp(1,:);
%i=2, j=1
JAC(46,4:7)= -2*fp'*A1'*A2*hps*G2;
%Driver Constraints
JACr=I3; JACp=-2*A2*S2pCs*G2;
JAC(47,1:3)=JACr(1,:);
JAC(47,4:7)=JACp(1,:);
JACr=-I3; JACp=-2*A6*S6pLs*G6;
JAC(48,29:31)=JACr(3,:);
JAC(48,32:35)=JACp(3,:);
JACr= I3; JACp=-2*A8*S8pNs*G8;
JAC(48,43:45)=JACr(3,:);
JAC(48,46:49)=JACp(3,:);
%Euler Parameters
JAC(49,4:7)= 2*p2';
JAC(50,11:14)=2*p3';
JAC(51,18:21)=2*p4';
JAC(52,25:28)=2*p5';
JAC(53,32:35)=2*p6';
JAC(54,39:42)=2*p7';
JAC(55,46:49)=2*p8';
JAC(56,53:56)=2*p9';
Off Road Recumbent Trike
Front Suspension Design and Analysis
Chris Evans
ME 581
May 3, 2012
Introduction
A recumbent trike is a 3 wheeled human powered vehicle. There are two wheels in the front
and a single wheel in the back. The rider sets on a seat in a semi reclined position. This
position allows most of the riders weight to be supported by his back. A picture of a
recumbent trike is shown in below in figure 1.
This project explores front suspension design for an off road recumbent trike. Riding in the
reclined position improves comfort by spreading your weight over a larger area. However,
one of the downsides to a recumbent trike is you cannot use your legs as suspension (i.e. you
cannot lift your body off the seat when hitting large bumps). Suspension is especially
important for riding off road. While riding off road you will encounter many bumps that
would make riding a rigid trike uncomfortable. I will explore geometry for a simple light
weight front suspension system that provides good travel and stability. Pro/Engineer
Wildfire 2, 3-d design software was used to model all the suspension components.
Pro/Mechanism software was then used to connect each moving piece of the suspension
system using kinematic constraints. Pro/Mechanism then allows the user to simulate the
kinematic motion of the suspension. Pro/Engineer Mechanism Dynamics Option was then
used (MDO) to evaluate forces at the joints.
Figure 1: Example of a Recumbent Trike
Design Requirements
The purpose of this project is to design a double a-arm suspension system for a recumbent
trike. The suspension will have a minimum travel of 4 inches. The steering mechanism will
minimize bump steer and allow for a sharp turning radius. The sharp turning radius is
needed to navigate through forest paths without running into trees and other obstacles. The
system must be lightweight. The goal is to keep the total weight under 50 lbs(~2x a standard
2 wheel mountain bike). Therefore the weight for all the suspension components needs to be
8 lbs or less. The last challenge will be to build a suspension system that provides stability
on rough terrain. Because an off road trike needs to have sufficient ground clearance,
stability is a concern. The higher center of gravity could create problems when making high
speed turns and riding on along side hills.
Design
A picture of the suspension design is shown in figure 2. The double a-arm suspension system
consists of a lower a-arm, an upper a-arm, a steering knuckle, a steering knuckle pivot block,
a tie rod, and a steering pivot. There is also a shock absorber to allow the suspension system
to support the weight of the vehicle and the operator. The lower a-arm and upper a-arm are
connected to the frame and the steering knuckle pivot block using revolute joints. Center to
center distance on each a-arm link is 6.5 inches. The lower a-arm also connects to the base
of the shock with a revolute joint. The upper part of the shock is connected to the frame with
a revolute joint. The shock was positioned in a location to get the minimum change in force
through the vertical motion of the suspension. The steering knuckle pivots in the steering
knuckle pivot block with a revolute joint. This motion allows the vehicle to turn. The tie rod
connects the operator steering mechanism to the steering knuckle with two spherical joints.
The steering pivot rotates on the frame using a revolute joint. The wheel is allowed to spin
on the wheel axle.
Figure 2: Suspension Components
The suspension components were designed using the Pro/Engineer solid modeling software.
The geometry was created by modeling each part then assembling them into an assembly
model using actual kinematic constraints. Each individual part typically started out as either
a solid rectangle or solid cylinder base feature. Features were then added or subtracted from
those base features using tools in the solid modeling software. The design process is an
iterative process where you continually modify the parts until you develop a geometry that
meets your requirements. The geometry shown in figure 2 was developed to the point where
it was ready to evaluate its performance against the design requirements. All of the parts are
manufactured from aluminum, and the calculated weight for a single suspension system is
5.8 lbs. This would make the total weight for the front suspension components 11.6 lbs. All
the suspension components will be manufactured from aluminum due to its light weight and
good strength characteristics.
Kinematic Analysis
The mechanism used the kinematic constraints are shown in figure 3. A mobility analysis of
the kinematic constraints shows this mechanism has a mobility of 3. The steering knuckle
can travel vertically (1) and it can rotate (2). There is an additional mobility where the tie
rod (link 8) can rotate along its axis on the spherical joints (3).
Figure 3: Kinematic Constraints
The kinematic analysis looked at 3 things. First, were there any problems with components
interfering with each other over the range of motion? This check was accomplished by
manually moving the mechanism to its limits and visually checking for interference between
suspension components. This design did not have any problems with interference. The next
item was to determine the amount of bump steer present in the suspension mechanism.
The bump steer measurement was accomplished by holding the wheel axle perpendicular to
the frame while the suspension was moved vertically a total of 4 inches. The angle of the
operator steering pivot was measured to determine amount of bump steer it would see as the
suspension moved vertically. This measurement over the vertical travel of the suspension is
shown in figure 4. The graph shows a maximum bump steer of around 6.25 degrees.
Bump Steer
-1
-2
Degrees
-3
-4
-5
-6
-7
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
Vertical Displacement (in)
Figure 4: Bump Steer Measurement
Last item that was evaluated was determining the output of the steering with a 45 degree
input in both directions. Turning the input mechanism 45 degrees to the right resulted in a 25
degree angle at the steering axle. Turning the input mechanism 45 degrees to the left resulted
in a 37 degree angle at the steering axle.
Dynamic Analysis
The dynamic analysis explored the forces that the suspension system would experience
during operation. The mechanism was driven by holding the axle perpendicular to the frame
and parallel to the ground. A constant velocity driver moved the axle in the vertical
direction. The shock has a spring rate of 350 lb/in. It is preloaded to start with an initial force
of 50 lbf. The results show the maximum force generated by the shock is 552 lbf after the
suspension travels 4 inches in the vertical direction. The reaction at revolute joints A and B
over the 4 inches of suspension travel are shown in the figure 5.
Connection Reaction at Joints A and B
400
350
Reaction Force (lbf)
300
250
200
150
100
50
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
Vertical Distance
Connection Reaction at A Connection Reaction at B
Figure 5: Connection Reaction Force Lower A-Arm (Link 2)

The reaction at revolute joints D and G are shown in figure 6, and the direction of that
reaction at a single point around 3.5 in is shown with arrows in figure 7. One concern have
with these force results is the influence of the driver boundary condition. These result may
be misleading because the method used to drive this mechanism in the software could be
carrying some of the load. By holding the axle parallel to the ground and perpendicular to
the frame while driving it will likely prevent the correct load from being applied to the upper
a-arm.
Connection Reaction at Joints D and G
60
Reaction Force (lbf)
50
40
30
20
10
0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5
Vertical Distance
Connection Reaction at G Connection Reaction at D
Figure 6: Connection Reaction Force, Upper A-Arm (Link 5)
Figure 7: Reaction Force Direction, Upper A-Arm (Link 5)
Conclusion
This project explored the first iteration of this suspension design. Overall these initial results
were good, but additional work needs completed to improve this design and dynamics model.
The weight of 5.8 lbs for each individual suspension system or a total of 11.6 lbs for both
front suspension systems exceeds the requirement. Additional effort will be needed to try
and reduce this weight by looking at the forces and removing additional material in areas
where it is not needed. One place where weight could be reduced is in the steering knuckle
pivot block. The steering geometry needs modified to improve the turning capabilities.
Further investigation is needed to see if the reaction result on the upper and lower a-arms are
affected by the driver. Since the driver keeps the wheel axle perfectly horizontal it is
possible that it is carrying some of the load that should be exerted on the upper and lower
links. There is an additional need to determine a method to quantify the stability of this
design while cornering at high speed and while operating on steep slopes. This is important
as the design progresses to ensure this design is stable.
KINEMATIC ANALYSIS AND FABRICATION OF
A THEO JANSEN MECHANISM
ME 581 SIMULATION OF MECHANICAL SYSTEMS

FINAL PROJECT
Submitted by:
Saurabh Harsh
Table of Contents
1 Introduction ............................................................................................................................. 2
2 Analysis ................................................................................................................................... 3
2.1 Constraints........................................................................................................................ 5
2.2 Blue Print Information ..................................................................................................... 5
2.3 Jacobian ............................................................................................................................ 6
3 Results ..................................................................................................................................... 7
4 Fabrication ............................................................................................................................... 8
5 Conclusion ............................................................................................................................... 9
6 Appendix ............................................................................................................................... 10
Fig. 1 Model of a Theo Jansen Mechanism(Source: http://www.iaacblog.com)........................... 2

Fig. 2 Linkage diagram of Theo Jansen Mechanism ..................................................................... 3
Fig. 3 Blue Print and Coordinate Systems used for Kinematic Analysis ...................................... 4
Fig. 4 Images taken during fabrication .......................................................................................... 8
1
1 Introduction
The mechanism used in this project imitates the walking pattern of a four legged animal. This
mechanism was developed by Theo Jansen and is popularly known as Theo Jansen Mechanism.
This mechanism has achieved great popularity on the internet and is commonly used for projects
because of the fact that Theo Jansen has shared the length of the links on public domain rather
than patenting it or making it proprietary. The ratio of the length of the links in this project is
used from http://www.mekanizmalar.com/theo_jansen.html
Fig. 1 Model of a Theo Jansen Mechanism(Source: http://www.iaacblog.com)
2
2 Analysis
The mechanism has only revolute joints and one set of the mechanism consists of two units
attached to the same crank. For analysis only one of the units can be considered. In the figure
below there are 14 links and 19 revolute joints. At some locations more than two links are joined
with a revolute and one should be careful while counting the number of revolute joints.
4 10
11
3
1
13
8
12
6 2
5 9
14
7
Fig. 2 Linkage diagram of Theo Jansen Mechanism

No. of links = 14
No. of Revolute Joints = 19
Mobility = (14-1)x3 – 19x2 = 1
3
y4
D
x4
4.6
4
x8 x3
3 6.7 y1
E
y3 A
y8 C 1 x1
4.85 y2
4.77 2
2
4.6 B
8
6 y6
x6 x2
x5
4.6 6.7
F 5
4.85 y5
x7 G
7
y7
No. of Links = 8
No. of Revolutes = 10
Mobility = (8-1)x3 – 10x2 = 1
7.5 No. of Constraints = 20(Kinematic) +
1(Driver)
Order of Jacobian = 21
Fig. 3 Blue Print and Coordinate Systems used for Kinematic Analysis
4
2.1 Constraints
2.2 Blue Print Information
A B C D E F G H
Link 2 [0,0] [2,0]
Link 3 [0,0] [4.6,0] [0,4.85]
Link 4 [6.7,0] [0,0]
Link 5 [0,0] [6.7,0]
Link 6 [0,0] [4.6,0]
Link 7 [4.85,0] [0,0] [0,7.5]
Link 8 [0,0] [0,4.6]
r1C = [-4.77,0]
r1A = [0,0]
5
2.3 Jacobian
6
3 Results
-5000
det(jac)
-10000
-15000
0 50 100 150 200 250 300 350 400
Crank Angle
-5
-10
-15
-15 -10 -5 0 5
7
4 Fabrication
The dimensions used for kinematic analysis were also used for the fabrication of the mechanism.
The links were made from plywood and machine screws and nuts were used to create the
revolute joint. The original plan was to make four units representing four legs of an animal but at
any point when two legs were moving forward the other two would be moving backward which
may have caused problems. To get a proper gait it is necessary to have six units on each side
which means one leg should be replaced by three mechanism. Hence in all to get a good working
model one needs to make twelve units. I attempted to make a working model with twelve units
but in the end fell short of time and resources. I could only finish making six when I realized
other problems with running it from a servo. Hence for demonstration I completed two units
being driven with a single crank. While manually running the mechanism one can feel it locking
near crank angle 180deg.
Fig. 4 Images taken during fabrication
8
5 Conclusion
The Theo Jansen mechanism is an ingenious set of revolutes that gives a very good walking
pattern but proper care should be taken while fabrication to avoid mechanism lock up as shown
above. A flywheel arrangement or inertia from the other units can also be helpful to avoid lock
up. The mechanism has other possible configurations too which are numerically possible but
practically not, so it’s necessary to plot the linkages for each solution to make sure one has the
feasibly correct solution. Also because of the feet getting lifted up it is necessary to have more
than one mechanisms running in place of a leg to avoid any kind of imbalance in the gait.
9
6 Appendix
% ME 581 Theo Jansen Mechanism
% This program runs a Newton raphson iteration algorithm
% for each crank angle
%% Initialization
clc
clear all
theojansen_ini
% The mechanism behaves weird for crank angle 0 and since my initial values
% were for crank -90 I was having trouble running it through an entire
% cycle. So I ran it from -90 to 0 and then used the values from 0 to run
% it through almost an entire cycle
qini = [0 0 0.0175 -4.77 0 1.2132 -3.1598
4.309 -0.6918 2 0.0349 -8.7227 -4.77 0
11.3635 -3.1154 -4.2922 -9.7824 -9.3132 1.6978
9.7927]’;
q = qini;
%%
figure;
final = 360;
djac = zeros(final+1,1);
k = 1;
for j = 0:final
crank = j;
if (mod(crank,360)==0)
q = qini;
end
theojansen_phi
while (max(abs(phi))>0.001)
res = jac\phi;
q = q - res;
theojansen_phi
end
% Plotting
trace = [r2B'; r2A'; r1C'; r8E'; r8F'; r7G';...
r2B'; r3D'; r1C'; r7G'; r7H’; r7F'; r8E'; r3D'];
leg(k,:) = r7H';
plot(trace(:,1),trace(:,2),'LineWidth',2);
hold on
plot(leg(:,1),leg(:,2),'rx','MarkerSize',10);
hold off
axis equal
axis([-15 5 -15 5])
phi;
djac(k) = det(jac);
drawnow;
k = k+1;
end
figure
plot(djac);
10
11
% theojansen_ini.m
% Initializing file for Theo Jansen Mechanism
% Saurabh Harsh 4/28/12
% 90deg rotation matrix

R = [0 -1;
1 0 ];
d2r = pi/180;
% Point C and A on link 1 is fixed

r1C = [-4.77 0]'; % Will stay constant
r1A = [0 0]'; % Will stay constant
% Blue print information for Kinematics

s2pA = [0 0]';
s2pB = [2 0]';
s3pC = [0 0]';
s3pE = [0 4.85]';
s3pD = [4.6 0]';
s4pD = [0 0]';
s4pB = [6.7 0]';
s5pB = [0 0]';
s5pG = [6.7 0]';
s6pC = [0 0]';
s6pG = [4.6 0]';
s7pG = [0 0]';
s7pF = [4.85 0]';
s7pH = [0 7.5]';
s8pE = [0 0]';
s8pF = [0 4.6]';
12
% theojansen_phi.m
% Constraint vector calculation
% Run this to evaluate constraints at each step
% Theo Jansen Mechanism
% Saurabh Harsh 4/28/12
%% Rip ris and Phiis from q defined in ini file

r1 = [0 0]';
r2 = q(1:2);
r3 = q(4:5);
r4 = q(7:8);
r5 = q(10:11);
r6 = q(13:14);
r7 = q(16:17);
r8 = q(19:20);
theta2 = q(3);
theta3 = q(6);
theta4 = q(9);
theta5 = q(12);
theta6 = q(15);
theta7 = q(18);
theta8 = q(21);
%%------------------------------------------
A1 = [1 0;
0 1]; %theta1 is zero always
[A2,B2] = getA(theta2);
%% Positions wrt Global CS

r2A = r2 + A2*s2pA;
r3C = r3 + A3*s3pC;
r6C = r6 + A6*s6pC;
r2B = r2 + A2*s2pB;
r4B = r4 + A4*s4pB;
r5B = r5 + A5*s5pB;
r3D = r3 + A3*s3pD;
r4D = r4 + A4*s4pD;
r3E = r3 + A3*s3pE;
r8E = r8 + A8*s8pE;
r8F = r8 + A8*s8pF;
r7F = r7 + A7*s7pF;
r7G = r7 + A7*s7pG;
r6G = r6 + A6*s6pG;
r5G = r5 + A5*s5pG;
r7H = r7 + A7*s7pH;
%% Constraint vector, Should approach zero with iterations

phi = zeros(9,1);
13
phi(1:2) = r1A-r2A;
phi(3:4) = r1C-r3C;
phi(5:6) = r1C-r6C;
phi(7:8) = r2B-r4B;
phi(9:10) = r4B - r5B;
phi(11:12) = r3D - r4D;
phi(13:14) = r3E - r8E;
phi(15:16) = r8F - r7F;
phi(17:18) = r7G - r6G;
phi(19:20) = r7G - r5G;
phi(21) = theta2 - mod(crank,360)*d2r;
%-------------------------------------------------------
%% Jacobian Calculation
jac = zeros(21,21);
jac(1:2,1:3) = [-eye(2) -B2*s2pA];
jac(3:4,4:6) = [-eye(2) -B3*s3pC];
jac(5:6,13:15) = [-eye(2) -B6*s6pC];
jac(7:8,1:3) = [eye(2) B2*s2pB];
jac(7:8,7:9) = [-eye(2) -B4*s4pB];
jac(9:10,7:12) = [eye(2) B4*s4pB -eye(2) -B5*s5pB];
jac(11:12,4:9) = [eye(2) B3*s3pD -eye(2) -B4*s4pD];
jac(13:14,4:6) = [eye(2) B3*s3pE];
jac(13:14,19:21) = [-eye(2) -B8*s8pE];
jac(15:16,16:21) = [-eye(2) -B7*s7pF eye(2) B8*s8pF];
jac(17:18,13:18) = [-eye(2) -B6*s6pG eye(2) B7*s7pG];
jac(19:20,10:18) = [-eye(2) -B5*s5pG zeros(2) zeros(2,1) eye(2) B7*s7pG];
jac(21,3) = 1; % for driving link constraint
14
Construction and Inverse Kinematics of a 5 DOF
Robotic Arm
ME581 Project
Submitted to Dr. Sommer
By Evan Hendrick
Motive/Background:
One of the top uses of linkage motion control today is in industrial robots such as the ABB robots or
other 5 or 6 DOF robotic arms. These robots are readily used in manufacturing and machining due to
the wide range of motion and ability to repeat tasks accurately. During my undergrad I had the
opportunity to program one of these robots which allowed me to see first-hand the usefulness of the
robotic arms. When programming the arm, I found it much easier to control and program using
Cartesian linear control rather than controlling the joints one by one. I For my project in ME581, I
therefore chose to not only construct a 5 DOF robotic arm, but also to program it to move using position
control and Cartesian coordinate inputs.
Materials and Setup:
For the Materials, I wanted to use Servo motors as I have experience in using them for position control.
One weakness in servos however, is that the less expensive ones did not have the holding torque that I
would need to be able to make a large enough robotic arm that could be useful for anything. After
some research, I found some multiple rotation servos that were geared to provide more torque.
 For the base which would rotate the arm around a vertical axis, I used the SPG805A-BM-360
Servo Power Gearbox (link 1: Ground, price:$179.98):
Fig 1: SPG805A-BM-360 Servo Power Gearbox
 For two of the links and rotary joints, I used SPG785A-5.0 Servo Power Gearbox (Hitec), one with
a 5:1 full metal gear pair (link 2, $129.98) and one with a 4:1 plastic gear pair (link 3, $99.98):
Fig. 2: SPG785A-5.0 Servo Power Gearbox (Hitec)
 I connected these together using a 90 degree aluminum mounting plate ($12.99) and aluminum
end cap mounts ($7.99x2=$15.98) as shown below:
Fig. 3: Mounting plate
 Link 4 was made using a 12” brushed aluminum channel ($14.95):
Fig. 4: Aluminum channel link

 A HSR-5990TG (Titanium gear) servo ($124.99) was attached to the end of link 4 using an
aluminum multipurpose servo bracket ($11.95). I used this servo, because I originally used a
plastic geared servo, but the first time the arm lost power, the gripper hit the ground and
destroyed the plastic gears. This servo provides titanium gears and a high torque (417 oz-in):
Fig. 5: HSR-5990TG and mounting hardware
 I then attached the HSR-5990TG servo to the Lynxmotion Wrist Rotate upgrade (Heavy Duty)
($44.66) and attached the Lynxmotion Little Grip Kit ($40.98) to that:
Fig 6: Lynxmotion Little Grip Kit
 For controlling, I used the Arduino Mega 2560 Microcontroller ($56.50) because I had used it
before and it had enough Pulse Width Modulation outputs to control each of the servos:
Fig. 7: Arduino Mega 2560

 As a user interface to control the robot, I used MATLAB and it’s GUI creator GUIDE pictured
below. I then used MATLAB to provide inputs for the Arduino which output commands to the
servos.
 All of this culminated in my robotic arm pictured below (Total:$732.94):
Fig. 8: 5 DOF Robotic Arm
Procedure/Analysis:
The first thing I did was I constructed the arm and run the wires to the servos. I then needed to do was
to test each of the servos to see how they work. I provided inputs to each of the servos, one at a time to
determine which command made them go to zero degrees (all servos at zero degrees made the arm
stand straight up), and which command made the servos move to 90 degrees. From here, I could form
equations to interpolate commands for other angles. Below is a table of my findings:
Table 1: Joint Equations
0 -90
joint degrees degrees equation
1 81 64 anglein=round((angle)*(81-64)/-90+81)
2 100 73 anglein=round((angle)*(100-73)/90+100)
Figure 9: Arm with links and angles specified
The next step is using inverse kinematics to calculate joint positions needed to move to specified x,y,z
coordinates. Below is the MATLAB code I used to calculate the angles for each joint given the x, y, and z
input (see Appendix for hand calculations and the full code for Arduino and MATLAB)
xpos=xposinput
zpos=zposinput
ypos=yposinput+EF+DE;
Adeg=atand(zpos/xpos)
L=zpos/sind(Adeg);
if ypos<AB
rho=atand(L/(AB-ypos));
rhoobtuse=0;
elseif ypos==AB
rho=90;
elseif ypos>AB
rho=atand(L/abs(AB-ypos));
rhoobtuse=1;
end
if rho>0
BD=L/sind(rho);
elseif rho<=0
BD=L/sind(-rho-.01);
end
Theta=zeros(1,10)
Theta(1)=45
for i=2:10%Iterate for theta solution
angle=(BD/BC-CD/BC*sind(acosd(BC/CD*cosd(Theta(i-1)))))
Theta(i)=asind(angle);
end
theta=Theta(10);
Thetaj(j)=theta;
if rhoobtuse==0
Bdeg=-theta+rho-90;
else
Bdeg=-theta-rho+90;
end
gamma=acosd(BC/CD*cosd(theta));
Cdeg=-(180-theta-gamma)
%%%%%%%%%%%%%%%%%%%%%%%%
Ddeg=-180-(Bdeg+Cdeg);
GUI:
The Graphical User Interface was created in order to allow the user to control the robot in the x,y,z plane
by specifying the position of the end effecter (gripper). Also while controlling the robot through
specifications of x, y, and z positions, the user can set waypoints for the robot to follow. Once all the
waypoints have been set, the user can select to run through the waypoints, allowing the user to program
the robot and then run the programmed actions. The GUI is set up as seen below with controls for x
position, y position, z position, gripper wrist rotation, and gripper opening/closing. There are also buttons
to go into telemetric mode where the user can control the robot by using the controls, to set waypoints, to
clear waypoints, to go into waypoint mode where the robot will move through the waypoints, and a button
to have the robot stand up vertical where little to no power is used.
Fig. 10: GUI for 5 DOF robotic arm

Conclusion:
I believe this project was a success as I was able to meet my goals of controlling a 5 DOF rotary joint arm
by specifying Cartesian coordinates allowing the arm to move linearly. There were some difficulties
however. One difficulty I had was in calculating the joint to Cartesian inverse kinematics. It took a good
amount of calculations and head scratching. I made it a little easier on myself by restricting the arm’s
movements to the 1st quadrant allowing only 90 degrees of rotation around the base. I believe there
would not be much more work needed to allow 360 degree rotation and is something for future work.
Another issue I ran into was that as the arm reaches out, the weight of the arm puts more strain on the
servo motors making the arm sag more than specified. A small correction factor was added to
compensate for this. Another possibility would have been using springs to reduce the necessary torque.
A problem with using MATLAB in conjunction with the Arduino is that the servos loose power as
MATLAB connects to the Arduino and the arm falls and the gripper will hit the ground which is why I
had to switch to a titanium geared servo for the wrist. For now I just try to remember to catch the arm as I
start up the MATLAB program, but springs might help this issue as well.
Appendix:
MATLAB Code:
function varargout = robo_cartesian_controller(varargin)

% ROBO_CARTESIAN_CONTROLLER MATLAB code for robo_cartesian_controller.fig
% ROBO_CARTESIAN_CONTROLLER, by itself, creates a new
ROBO_CARTESIAN_CONTROLLER or raises the existing
% singleton*.
%
% H = ROBO_CARTESIAN_CONTROLLER returns the handle to a new
ROBO_CARTESIAN_CONTROLLER or the handle to
% the existing singleton*.
%
% ROBO_CARTESIAN_CONTROLLER('CALLBACK',hObject,eventData,handles,...)
calls the local
% function named CALLBACK in ROBO_CARTESIAN_CONTROLLER.M with the given
input arguments.
%
% ROBO_CARTESIAN_CONTROLLER('Property','Value',...) creates a new
ROBO_CARTESIAN_CONTROLLER or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before robo_cartesian_controller_OpeningFcn gets
called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to robo_cartesian_controller_OpeningFcn
via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help

robo_cartesian_controller
% Last Modified by GUIDE v2.5 30-Apr-2012 14:57:05
% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @robo_cartesian_controller_OpeningFcn, ...
'gui_OutputFcn', @robo_cartesian_controller_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before robo_cartesian_controller is made visible.

function robo_cartesian_controller_OpeningFcn(hObject, eventdata, handles,
varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to robo_cartesian_controller (see
VARARGIN)
% Choose default command line output for robo_cartesian_controller

handles.output = hObject;
handles.robot=1;
if handles.robot==1
initalize=1;
if initalize==1;
install_arduino
% connect the board
handles.a=arduino('COM4');
end
initalize=0;
servoin=10;
angle=81;
digitalWrite(handles.a,servoin,angle);
servoin=8;
angle=100;
servoin=7;
angle=105;
servoin=6;
angle=68;
servoin=5;
angle=78;
servoin=4;
angle=10;
end
handles.vid = videoinput('winvideo', 1);
%initalizing distances and positions
% Update handles structure

guidata(hObject, handles);
% UIWAIT makes robo_cartesian_controller wait for user response (see
UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = robo_cartesian_controller_OutputFcn(hObject, eventdata,
handles)
% varargout cell array for returning output args (see VARARGOUT);
% Get default command line output from handles structure

varargout{1} = handles.output;
% --- Executes on slider movement.

function xslider_Callback(hObject, eventdata, handles)
% hObject handle to yslider (see GCBO)
% Hints: get(hObject,'Value') returns position of slider

% get(hObject,'Min') and get(hObject,'Max') to determine range of
slider
% --- Executes during object creation, after setting all properties.

function xslider_CreateFcn(hObject, eventdata, handles)
% handles empty - handles not created until after all CreateFcns called
% Hint: slider controls usually have a light gray background.

if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function zslider_Callback(hObject, eventdata, handles)

slider

function zslider_CreateFcn(hObject, eventdata, handles)

end

function yslider_Callback(hObject, eventdata, handles)

slider

function yslider_CreateFcn(hObject, eventdata, handles)

end

function slider5_Callback(hObject, eventdata, handles)
% hObject handle to slider5 (see GCBO)

slider

function slider5_CreateFcn(hObject, eventdata, handles)

end
% --- Executes on button press in TelemetricButton.

function TelemetricButton_Callback(hObject, eventdata, handles)
% hObject handle to TelemetricButton (see GCBO)
AB=7.27;%in inches
BC=6.16;
CD=8.68;
DE=2;
EF=2.42;
timesteps=5;
j=1;
robot=handles.robot;
if robot
a=handles.a;
end
%displaying video from webcam
axes(handles.axes4)
vidRes = get(handles.vid, 'VideoResolution');
imageRes = fliplr(vidRes);
hImage = imshow(zeros(imageRes));
axis image;
preview(handles.vid, hImage);
%initalizing arm vertical
while get(handles.TelemetricButton,'Value')
Rbuttonvalue=get(handles.ReturnButton,'Value');
if Rbuttonvalue && handles.robot
servoin=10;
angle=81;
servoin=8;
angle=100;
servoin=7;
angle=105;
servoin=6;
angle=68;
servoin=5;
angle=78;
servoin=4;
angle=10;
set(handles.ReturnButton,'Value',0)
set(handles.TelemetricButton,'Value',0)
break
end
%inputting commands from GUI sliders
xposinitial=get(handles.xslider,'Value');
set(handles.xval,'String',num2str(xposinitial));
yposinitial=get(handles.yslider,'Value');
set(handles.yval,'String',num2str(yposinitial));
zposinitial=get(handles.zslider,'Value');
set(handles.zval,'String',num2str(zposinitial));
%setting up control displays

axes(handles.axes3)
grid on
plot(zposinitial,xposinitial,'o')
axis([0,10,0,10])
axes(handles.axes2)
cla
rectangle('Position',[2,yposinitial,1,10])
axis([0,5,0,5])
Thetaj(1)=20;
%inputting coordinates
handles.xposinput=xposinitial;
handles.yposinput=yposinitial;
handles.zposinput=zposinitial;
xpos=handles.xposinput;
zpos=handles.zposinput;
ypos=handles.yposinput;
if xpos>0 && zpos>0
Adeg=atand(zpos/xpos);
elseif xpos==0
Adeg=90;
elseif zpos==0
Adeg=0;
end
Adeg
ypos=handles.yposinput+EF+DE;
if Adeg>0
L=zpos/sind(Adeg);
elseif Adeg==0
L=xpos/cosd(Adeg);
end
L
CF=get(handles.CFslider,'Value');
if CF<3
CF=3
end
CF
ypos=ypos+L/CF;
if ypos<AB
rhoobtuse=0;
elseif ypos==AB
rho=90;
elseif ypos>AB
rhoobtuse=1;
end
if rho>0
BD=L/sind(rho);
elseif rho<=0
end
Theta=zeros(1,10);
Theta(1)=45;
for i=2:10
angle=(BD/BC-CD/BC*sind(acosd(BC/CD*cosd(Theta(i-1)))))
Theta(i)=real(asind(angle))
end
theta=Theta(10);
Thetaj(j)=theta;
if rhoobtuse==0
Bdeg=-theta+rho-90;
else
Bdeg=-theta-rho+90;
end
Cdeg=-(180-theta-gamma)
%%%%%%%%%%%%%%%%%%%%%%%%
Edeg=0;
Fdeg=0;
theangles(1,j)=Adeg;
theangles(2,j)=Bdeg;
theangles(3,j)=Cdeg;
theangles(4,j)=Ddeg;
theangles(5,j)=Edeg;
theangles(6,j)=rho;
theangles(7,j)=theta;
%plotting data
sind(Adeg)*(1.5-1.8+2);
Apos=[0,0,0];
Bpos=[Apos(1)+0,Apos(2)+AB,sind(Adeg)*0];
Cpos=[cosd(Adeg)*(Bpos(1)+cosd(90+Bdeg)*BC),Bpos(2)+sind(90+Bdeg)*BC,sind(Ade
g)*(Bpos(3)+cosd(90+Bdeg)*BC)];
Dpos=[(Cpos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg)*CD),Cpos(2)+sind(90+Bdeg+Cdeg)*C
D,(Cpos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg)*CD)];
Epos=[(Dpos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg)*DE),Dpos(2)+sind(90+Bdeg+Cd
eg+Ddeg)*DE,(Dpos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg)*DE)];
Fpos=[(Epos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg+Edeg)*EF),Epos(2)+sind(90+Bd
eg+Cdeg+Ddeg+Edeg)*EF,(Epos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg+Edeg)*EF)];
axes(handles.axes1)
hold on
cla
plot3(Apos(3),Apos(1),Apos(2),'co',Bpos(3),Bpos(1),Bpos(2),'ro',Cpos(3),Cpos(
1),Cpos(2),'bo',Dpos(3),Dpos(1),Dpos(2),'ko',Epos(3),Epos(1),Epos(2),'mo',Fpo
s(3),Fpos(1),Fpos(2),'go',[Apos(3),Bpos(3)],[Apos(1),Bpos(1)],[Apos(2),Bpos(2
)],'r',[Bpos(3),Cpos(3)],[Bpos(1),Cpos(1)],[Bpos(2),Cpos(2)],'b',[Cpos(3),Dpo
s(3)],[Cpos(1),Dpos(1)],[Cpos(2),Dpos(2)],'k',[Dpos(3),Epos(3)],[Dpos(1),Epos
(1)],[Dpos(2),Epos(2)],'m',[Epos(3),Fpos(3)],[Epos(1),Fpos(1)],[Epos(2),Fpos(
2)],'g');%handles.zposinput,handles.xposinput,handles.yposinput,'mo',
xlim([-1,12]);
ylim([-3,12]);
zlim([0,15]);
axis square
%sending commands to the robotic arm

if robot==1
servoin=10;
angle=Adeg;
if angle>0 || angle<90
anglein=round((angle)*(81-64)/-90+81);
end
digitalWrite(a,servoin,anglein);
servoin=8;
angle=Bdeg;
if angle<15 || angle>-90
anglein=round((angle)*(100-73)/90+100);
end
servoin=7;
angle=Cdeg;
end
servoin=6;
angle=Ddeg;
end
servoin=5;
% angle=Edeg;
angle=2*(-get(handles.GripperRotateslider,'Value')+.5)*90;
if anglein<0
anglein=0;
end
% angle=Fdeg;
% anglein=angle;
servoin=4;
anglein=round(get(handles.Gripperslider,'Value')*150+10);
if anglein<0
anglein=0;
end
end
pause(.01)
j
end
% --- Executes on button press in WayPointButton.

function WayPointButton_Callback(hObject, eventdata, handles)
% hObject handle to WayPointButton (see GCBO)
%This allows the robot to run through the recorded waypoints

if handles.robot
a=handles.a;
end
%displaying video
axes(handles.axes4)
vidRes = get(handles.vid, 'VideoResolution');
imageRes = fliplr(vidRes);
hImage = imshow(zeros(imageRes));
axis image;
preview(handles.vid, hImage);
%retrieving waypoints
waypoints=get(handles.WayPointTable,'Data');
for k=1:size(waypoints,1)-1
xposinitial=waypoints(k,1);
xposfinal=waypoints(k+1,1);
AB=7.27;%in inches
BC=6.16;
CD=8.68;
DE=2;
EF=2.42;
timesteps=5;
j=1;
robot=handles.robot;
%creating 5 steps linearly between each waypoint
xpositer=[xposinitial:(xposfinal-xposinitial)/(timesteps-1):xposfinal];
if xposfinal==xposinitial
xpositer=ones(1,timesteps)*xposinitial;
end
yposinitial=waypoints(k,2);
yposfinal=waypoints(k+1,2);
ypositer=[yposinitial:(yposfinal-yposinitial)/(timesteps-1):yposfinal];
if yposfinal==yposinitial
ypositer=ones(1,timesteps)*yposinitial;
end
zposinitial=waypoints(k,3);
zposfinal=waypoints(k+1,3);
zpositer=[zposinitial:(zposfinal-zposinitial)/(timesteps-1):zposfinal]
if zposfinal==zposinitial
zpositer=ones(1,timesteps)*zposinitial;
end
grid;
Thetaj(1)=20;
for j=2:timesteps+1
xposinput=xpositer(j-1);
yposinput=ypositer(j-1);
zposinput=zpositer(j-1);
xpos=xposinput
zpos=zposinput
yposinput
ypos=yposinput+EF+DE;
if xpos>0 && zpos>0

Adeg=atand(zpos/xpos);
elseif xpos==0
Adeg=90;
elseif zpos==0
Adeg=0;
end
Adeg;
if Adeg>0
L=zpos/sind(Adeg);
elseif Adeg==0
L=xpos/cosd(Adeg);
end
L;
CF=get(handles.CFslider,'Value');
if CF<3
CF=3;
end
CF;
ypos=ypos+L/CF;
if ypos<AB
rhoobtuse=0;
elseif ypos==AB
rho=90;
elseif ypos>AB
rhoobtuse=1;
end
if rho>0
BD=L/sind(rho);
elseif rho<=0
end
Theta=zeros(1,10);
Theta(1)=45;%Thetaj(j-1)
for i=2:10
angle=(BD/BC-CD/BC*sind(acosd(BC/CD*cosd(Theta(i-1)))));
Theta(i)=asind(angle);
end
theta=Theta(10);
Thetaj(j)=theta;
if rhoobtuse==0
Bdeg=-theta+rho-90;
else
Bdeg=-theta-rho+90;
end
Cdeg=-(180-theta-gamma);
%%%%%%%%%%%%%%%%%%%%%%%%
Edeg=0;
Fdeg=0;
theangles(1,j)=Adeg;
theangles(2,j)=Bdeg;
theangles(3,j)=Cdeg;
theangles(4,j)=Ddeg;
theangles(5,j)=Edeg;
theangles(6,j)=rho;
theangles(7,j)=theta;
%plotting what the arm should look like
sind(Adeg)*(1.5-1.8+2);
Apos=[0,0,0];
Bpos=[Apos(1)+0,Apos(2)+AB,sind(Adeg)*0];
Cpos=[cosd(Adeg)*(Bpos(1)+cosd(90+Bdeg)*BC),Bpos(2)+sind(90+Bdeg)*BC,sind(Ade
g)*(Bpos(3)+cosd(90+Bdeg)*BC)];
Dpos=[(Cpos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg)*CD),Cpos(2)+sind(90+Bdeg+Cdeg)*C
D,(Cpos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg)*CD)];
Epos=[(Dpos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg)*DE),Dpos(2)+sind(90+Bdeg+Cd
eg+Ddeg)*DE,(Dpos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg)*DE)];
Fpos=[(Epos(1)+cosd(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg+Edeg)*EF),Epos(2)+sind(90+Bd
eg+Cdeg+Ddeg+Edeg)*EF,(Epos(3)+sind(Adeg)*cosd(90+Bdeg+Cdeg+Ddeg+Edeg)*EF)];
axes(handles.axes1)
hold on
cla
plot3(zposinput,xposinput,yposinput,'mo',Apos(3),Apos(1),Apos(2),'co',Bpos(3)
,Bpos(1),Bpos(2),'ro',Cpos(3),Cpos(1),Cpos(2),'bo',Dpos(3),Dpos(1),Dpos(2),'k
o',Epos(3),Epos(1),Epos(2),'mo',Fpos(3),Fpos(1),Fpos(2),'go',[Apos(3),Bpos(3)
],[Apos(1),Bpos(1)],[Apos(2),Bpos(2)],'r',[Bpos(3),Cpos(3)],[Bpos(1),Cpos(1)]
,[Bpos(2),Cpos(2)],'b',[Cpos(3),Dpos(3)],[Cpos(1),Dpos(1)],[Cpos(2),Dpos(2)],
'k',[Dpos(3),Epos(3)],[Dpos(1),Epos(1)],[Dpos(2),Epos(2)],'m',[Epos(3),Fpos(3
)],[Epos(1),Fpos(1)],[Epos(2),Fpos(2)],'g');
xlim([-1,28]);
ylim([-3,28]);
axis square
%sending commands to the robotic arm

if robot==1
servoin=10;
angle=Adeg;
if angle>0 || angle<90
anglein=round((angle)*(81-64)/-90+81);
end
servoin=8;
angle=Bdeg;
end
servoin=7;
angle=Cdeg;
end
servoin=6;
angle=Ddeg;
end
servoin=5;
angle=2*(-waypoints(k,5)+.5)*90;
if anglein<0
anglein=0;
end
servoin=4;
anglein=round(waypoints(k,4)*150+10);
if anglein<0
anglein=0;
end
end
pause(.5)
j
end
end
% --- Executes on button press in ReturnButton.

function ReturnButton_Callback(hObject, eventdata, handles)
% hObject handle to ReturnButton (see GCBO)

function CFslider_Callback(hObject, eventdata, handles)
% hObject handle to CFslider (see GCBO)

slider

function CFslider_CreateFcn(hObject, eventdata, handles)
% hObject handle to CFslider (see GCBO)

end

function Gripperslider_Callback(hObject, eventdata, handles)
% hObject handle to Gripperslider (see GCBO)

slider

function Gripperslider_CreateFcn(hObject, eventdata, handles)
% hObject handle to Gripperslider (see GCBO)

end

function GripperRotateslider_Callback(hObject, eventdata, handles)
% hObject handle to GripperRotateslider (see GCBO)

slider
function GripperRotateslider_CreateFcn(hObject, eventdata, handles)
% hObject handle to GripperRotateslider (see GCBO)

end
% --- Executes on button press in InsertButton.

function InsertButton_Callback(hObject, eventdata, handles)
% hObject handle to InsertButton (see GCBO)
% Hint: get(hObject,'Value') returns toggle state of InsertButton

xpos=handles.xposinput;
zpos=handles.zposinput;
ypos=handles.yposinput;
grip=get(handles.Gripperslider,'Value');
wrist=get(handles.GripperRotateslider,'Value');
wpmatrix=get(handles.WayPointTable,'Data');
whichrow=size(wpmatrix,1);
wpmatrix(whichrow+1,:)=[xpos,ypos,zpos,grip,wrist];
set(handles.WayPointTable,'Data',wpmatrix)
% --- Executes on button press in ClearButton.

function ClearButton_Callback(hObject, eventdata, handles)
% hObject handle to ClearButton (see GCBO)
set(handles.WayPointTable,'Data',[])
% --- Executes on button press in livebutton.

function livebutton_Callback(hObject, eventdata, handles)
% hObject handle to livebutton (see GCBO)
% Hint: get(hObject,'Value') returns toggle state of livebutton

Arduino Code:
/* Analog and Digital Input and Output Server for MATLAB */

/* Giampiero Campa, Copyright 2009 The MathWorks, Inc */
/* This file is meant to be used with the MATLAB arduino IO

package, however, it can be used from the IDE environment
(or any other serial terminal) by typing commands like:
0e0 : assign digital pin #4 (e) as input

0f1 : assign digital pin #5 (f) as output
0n1 : assign digital pin #13 (n) as output
1c : read digital pin #2 (c)

1e : read digital pin #4 (e)
2n0 : set digital pin #13 (d) low
2n1 : set digital pin #13 (n) high
2f1 : set digital pin #5 (f) high
2f0 : set digital pin #5 (f) low
4f2 : set digital pin #5 (f) to 50=ascii(2) over 255
4fz : set digital pin #5 (f) to 122=ascii(z) over 255
3a : read analog pin #0 (a)
3f : read analog pin #5 (f)
99 : return script type (1 basic, 2 motor)

*/
#include <Servo.h>
Servo servo10;
Servo servo9;
Servo servo8;
Servo servo7;
Servo servo6;
Servo servo5;
Servo servo4;
int pos = 0;
void setup() {
/* Make sure all pins are put in high impedence state and
that their registers are set as low before doing anything.
This puts the board in a known (and harmless) state */
int i;
for (i=0;i<20;i++) {
pinMode(i,INPUT);
digitalWrite(i,0);
}
/* initialize serial */
Serial.begin(115200);
servo10.attach(10);
servo9.attach(9);
servo8.attach(8);
servo7.attach(7);
servo6.attach(6);
servo5.attach(5);
servo4.attach(4);
}
void loop() {
/* variables declaration and initialization */
static int s = -1; /* state */

static int pin = 13; /* generic pin number */
int val = 0; /* generic value read from serial */

int agv = 0; /* generic analog value */
int dgv = 0; /* generic digital value */
if (Serial.available() >0) {
val = Serial.read();
switch (s) {
/* s=-1 means NOTHING RECEIVED YET ******************* */

case -1:
/* calculate next state */

if (val>47 && val<58) {
/* the first received value indicates the mode
49 is ascii for 1, ... 90 is ascii for Z
s=0 is change-pin mode
s=10 is DI; s=20 is DO; s=30 is AI; s=40 is AO;
s=50 is servo status; s=60 is aervo attach/detach;
s=70 is servo read; s=80 is servo write
s=90 is query script type (1 basic, 2 motor)
*/
s=10*(val-48);
}
break; /* s=-1 (initial state) taken care of */
/* s=0 or 1 means CHANGE PIN MODE */

case 0:
/* the second received value indicates the pin
from abs('c')=99, pin 2, to abs('t')=116, pin 19 */
if (val>98 && val <117) {
pin=val-97; /* calculate pin */
s=1; /* next we will need to get 0 or 1 from serial */
}
else {
s=-1; /* if value is not a pin then return to -1 */
}
break; /* s=0 taken care of */
case 1:
/* the third received value indicates the value 0 or 1 */
if (val>47 && val <50) {
/* set pin mode */
if (val==48) {
pinMode(pin,INPUT);
}
else {
pinMode(pin,OUTPUT);
}
}
s=-1; /* we are done with CHANGE PIN so go to -1 */
/* s=10 means DIGITAL INPUT ************************** */
case 10:
if (val>98 && val <117) {
dgv=digitalRead(pin); /* perform Digital Input */
Serial.println(dgv); /* send value via serial */
}
s=-1; /* we are done with DI so next state is -1 */
/* s=20 or 21 mean DIGITAL OUTPUT ******************** */
case 20:
if (val>98 && val <117) {
s=21; /* next we will need to get 0 or 1 from serial */
}
else {
}
case 21:
/* the third received value indicates the value 0 or 1 */
if (val>-1 && val <180) {
switch(pin){
case 10:
servo10.write(val);
break;
case 9:
servo9.write(val);
break;
case 8:
servo8.write(val);
break;
case 7:
servo7.write(val);
break;
case 6:
servo6.write(val);
break;
case 5:
servo5.write(val);
break;
case 4:
servo4.write(val);
break;
}
}
// dgv=val-48; /* calculate value */

// digitalWrite(pin,dgv); /* perform Digital Output */
// }
s=-1; /* we are done with DO so next state is -1 */
/* s=30 means ANALOG INPUT *************************** */
case 30:
from abs('a')=97, pin 0, to abs('f')=102, pin 6,
note that these are the digital pins from 14 to 19
located in the lower right part of the board */
if (val>96 && val <103) {
agv=analogRead(pin); /* perform Analog Input */
Serial.println(agv); /* send value via serial */
}
s=-1; /* we are done with AI so next state is -1 */
/* s=40 or 41 mean ANALOG OUTPUT ********************* */
case 40:
if (val>98 && val <117) {
s=41; /* next we will need to get value from serial */
}
else {
}
case 41:
/* the third received value indicates the analog value */
analogWrite(pin,val); /* perform Analog Output */
s=-1; /* we are done with AO so next state is -1 */
/* s=90 means Query Script Type (1 basic, 2 motor) */

case 90:
if (val==57) {
/* if string sent is 99 send script type via serial */
Serial.println(1);
}
s=-1; /* we are done with this so next state is -1 */
} /* end switch on state s */

} /* end if serial available */
} /* end loop statement */
Hand
Calculations:
Analysis and
Construction of RSUP
Mechanism
Anthony Jones
ME 581
Spring ’12 Semester
The goal of this project was to design both a physical model and a MATLAB model for a 3D
Kinematic Mechanism I chose to model a RSUP mechanism, seen below in Figure 1.
Figure 1: RSUP Mechanism
The model has a motor giving a constant rotation at point A, a revolute arm AB, a ‘yoke’ BC and
a prism with a center at C and D, that slides in a slot in the Ground body. Point A is a revolute joint,
point B is a spherical joint, C creates a universal joint, and D creates a prismatic joint.
To physically construct the mechanism, I ordered several joints from McMaster Carr. The slot I
used was a special u‐stock piece of Aluminum from an older project, with a width of one inch. The slot
was bolted down to the perpendicular piece of sheetmetal being used for the ground. I used one inch
stock Aluminum bars to create the base of the prism. The holes for the part of the universal joint along
the z‐axis were made by measuring and drilling two holes in a thinner piece of aluminum, then
measuring and bending the two ends, so that the holes were in line with each other. A quarter inch rod
(.25” diameter) was used as the physical axis of the universal joint along the z‐axis, after drilling a small
hole perpendicularly through the rod.
For the other half of the universal joint, I disassembled the McMaster Carr universal joint. The
rod was place in between the two prongs of the removed half of the McMaster joint, so that the pre‐
drilled hole in the axis lined up with the two holes in the prongs. A nail was used as the axis to connect
the rod and the half‐joint.
Figure 2: Universal Joint and

Prism/Slot Joint
The Spherical Joint was also a stock part from McMaster Carr. After finding that the joint offered
a smaller range of motion than I had anticipated, I settled with attaching the threaded end of the
spherical joint in line with the rod that was attached to the free end of the universal joint. Then, a short
piece of aluminum rod was run through the opening in the spherical joint and held in place. A makeshift
revolute arm had to be constructed, much shorter than anticipated (due to the spherical joint’s
limitation), and a hole was drilled through the back of the perpendicular body for running a crankshaft
through. The crank was then attached to the hole drilled in the other end of the revolute arm.
Motors and servo motors were tested for powering the mechanism, but the servos proved to be
too weak to muscle the spherical joint, and I was unable to successfully mount the motor to the arm.
Figure 3: Constructed Mechanism
Above is a view of the final mechanism. I would have greatly preferred to have gotten a final
model identical to the one I used in the MATLAB model, and one powered by a constant speed power
source. However, trying to trouble shoot both sides of the project led to shortcoming on both ends.
Figure 4: Coordinate Frame System
The coordinates are pretty simple. The origin of 2 is at the motor axis, with the h2 term parallel
to the axis. The 3 frame is at the universal joint’s center, with the z3’ along the center of the bar, and the
x3’ term through the axle holes. The 4 frame is different, with the x4’ term through the holes of the
prism, and z4’ is along the global x axis. (This is done to prevent the prism from being laid out identically
to the ground, it causes problems with the p4 term).
The constraints for the Revolute A are to keep the global coordinates of r1A and r2A identical, and the
h2 and h1 terms parallel. Spherical B is just making r3B and r2B at the same point. Universal C is the
same, keeping r3C and r4C at the same point, but adding that the f3 and the f4 stay perpendicular at all
times. For Prismatic D, the first two constraints keep the prism’s f4 term parallel to h1. The second two
concern the distance between D4 and D1. The last keeps the prism from rotating about the f1 axis.
The Jacobian is pretty straight‐forward. There may have been an issue with the Prismatic terms
involving the derivative of the constraints that had d14 terms. Regardless, the PHI was able to converge,
and the Jacobian went through, for the initial conditions. However, problems showed up trying to get
the code to work with an appropriate driver term, in the PHI and Jacobian terms. The code couldn’t
handle when the angle of the revolute arm passed 360 degrees.
The code of the project is below. Hopefully the code can be fixed at a later date, and the
kinematic code can be fully tested. The results seemed promising at first glance, but I can’t guarantee
that they were all successful.
% Final_init ADJ
% general constants
d2r = pi / 180;
R = [ 0 -1 ; 1 0 ]; % Eq 2.6.5, page 42
% mechanism constants - original local coordiante frames

% cm
length2 = 9;
length3 = 25;
length4 = 10;
%initial distances
a = 10;
b = 12;
c = 17.5;
s2pA = [ 0 0 0 ]';
s2pB = [ length2 0 0 ]';
s3pB = [ 0 0 length3 ]';
s3pC = [ 0 0 0 ]';
s4pC = [ 0 0 0 ]';
s4pD4 = [ 0 0 0 ]';
r1A = [ a b 0 ]';
r1D1 = [ 0 0 0 ]';
u2 = [ 0 0 1 ]';
chi2 = 120*d2r;
u4 = [ 0 1 0 ]';
chi4 = 90*d2r;
% initial guesses - estimated by eye

% r2
q(1,1) = a;
q(2,1) = b;
q(3,1) = 0;
% p2
q(4,1) = cos(chi2/2);
q(5,1) = u2(1,1)*sin(chi2/2);
q(6,1) = u2(2,1)*sin(chi2/2);
q(7,1) = u2(3,1)*sin(chi2/2);
% r3
q(8,1) = 0;
q(9,1) = 0;
q(10,1) = c;
% p3
h3 = [ (a-length2*sin(30*d2r)) (b+length2*cos(30*d2r)) -c]';
h3norm = h3/norm(h3);
f3 = [ h3(3) 0 -h3(1) ]';
f3norm = f3/norm(f3);
A3 = [ f3norm cross(h3norm,f3norm) h3norm];
q(11,1) = sqrt(trace(A3)+1)/2;
q(12,1) = 1/(4*q(11,1))*(A3(3,2)-A3(2,3));
q(13,1) = 1/(4*q(11,1))*(A3(1,3)-A3(3,1));
q(14,1) = 1/(4*q(11,1))*(A3(2,1)-A3(1,2));
%r4
q(15,1) = 0;
q(16,1) = 0;
q(17,1) = c;
%p4
q(18,1) = cos(chi4/2);
q(19,1) = u4(1,1)*sin(chi4/2);
q(20,1) = u4(2,1)*sin(chi4/2);
q(21,1) = u4(3,1)*sin(chi4/2);
%crank
phi_start = 120 * d2r;
w2 = 30*2*pi/60; % 60 rpm CCW, convert to rad/sec
t = 0;
% Final Phi ADJ
% global location of local frames and rotation matrices
r2 = q(1:3);
p2 = q(4:7);
r3 = q(8:10);
p3 = q(11:14);
r4 = q(15:17);
p4 = q(18:21);
[E2,G2,A2,f2,g2,h2] = make_ega(p2);
%B2 = A2 * R;
%B3 = A3 * R;
%B4 = A4 * R;
A1 = eye(3);
f1 = [ 1 0 0 ]';
g1 = [ 0 1 0 ]';
h1 = [ 0 0 1 ]';
% global locations of points

r2A = r2 + A2*s2pA;
r2B = r2 + A2*s2pB;
r3B = r3 + A3*s3pB;
r3C = r3 + A3*s3pC;
r4C = r4 + A4*s4pC;
r4D4 = r4 + A4*s4pD4;
f1p = f1;
g1p = g1;
h1p = h1;
f2p = f1;
g2p = g1;
h2p = h1;
f3p = f1;
g3p = g1;
h3p = h1;
f4p = f1;
g4p = g1;
h4p = h1;
% revolute constraints
% Rev A
PHI(1:3,1) = r1A - r2A;
PHI(4,1) = f2'*h1;
PHI(5,1) = g2'*h1;
% Spherical B
PHI(6:8,1) = r2B - r3B;
% Universal C
PHI(9:11,1) = r3C - r4C;
PHI(12,1) = f4'*f3;
% Prismatic D
d14 = r4D4 - r1D1;
PHI(13,1) = h1'*h4;
PHI(14,1) = h1'*g4;
PHI(15,1) = f1'*d14;
PHI(16,1) = g1'*d14;
PHI(17,1) = f1'*g4;
% Euler Parameters
PHI(18,1) = p2'*p2-1;
PHI(19,1) = p3'*p3-1;
PHI(20,1) = p4'*p4-1;
% driver constraint
Theta = atan2((f1'*g2),(f1'*f2));
x = 2*acos(p2(1))*180/pi
PHI(21,1) = Theta - phi_start- w2*t;
% Jacobian by rows
JAC = zeros(21,21);
JAC(1:3,1:7) = [ -eye(3) 2*A2*skew_sym(s2pA)*G2 ];

JAC(4,4:7) = [ -2*h1p'*A1'*A2*skew_sym(f2p)*G2 ];
JAC(5,4:7) = [ -2*h1p'*A1'*A2*skew_sym(g2p)*G2 ];
JAC(6:8,1:7) = [ eye(3) -2*A2*skew_sym(s2pB)*G2 ];
JAC(6:8,8:14) = [ -eye(3) 2*A3*skew_sym(s3pB)*G3 ];
JAC(9:11,8:14) = [ eye(3) -2*A3*skew_sym(s3pC)*G3 ];

JAC(12,11:14) = [ -2*f4p'*A4'*A3*skew_sym(f3p)*G3 ];
JAC(9:11,15:21) = [ -eye(3) 2*A4*skew_sym(s4pC)*G4 ];

JAC(12,18:21) = [ -2*f3p'*A3'*A4*skew_sym(f4p)*G4 ];
JAC(13,18:21) = [ -2*h1p'*A1'*A4*skew_sym(h4p)*G4 ];
JAC(14,18:21) = [ -2*h1p'*A1'*A4*skew_sym(g4p)*G4 ];
JAC(15,15:21) = [ f1p'*A1' -2*f1p'*A1'*A4*skew_sym(s4pD4)*G4 ];
JAC(16,15:21) = [ g1p'*A1' -2*g1p'*A1'*A4*skew_sym(s4pD4)*G4 ];
JAC(17,18:21) = [ -2*f1p'*A1'*A4*skew_sym(g4p)*G4 ];
JAC(18,4:7) = [2*p2'];
JAC(19,11:14) = [2*p3'];
JAC(20,18:21) = [2*p4'];
JAC(21,4) = -2/sqrt(1-p2(1)^2);
% Final_kin ADJ
% Newton-Raphson position solution
assy_tol = 1e-5;
Final_phi;
while max(abs(PHI)) > assy_tol,
PHI;
q = q - inv(JAC) * PHI;
Final_phi;
end
% velocity
velrhs = zeros(21,1);
velrhs(21) = w2;
qd = inv(JAC) * velrhs;
% global velocities of points
r2d = qd(1:3);
r3d = qd(8:10);
r4d = qd(15:17);
p2d = qd(4:7);
p3d = qd(11:14);
p4d = qd(18:21);
w1p = 0;
w2p = 2*G2*p2d;
w3p = 2*G3*p3d;
w4p = 2*G4*p4d;
% acceleration
accrhs = zeros(21,1);
accrhs(1:3) = A2*skew_sym(w2p)*skew_sym(w2p)*s2pA;
accrhs(4) = -h1p'*(A1'*A2*skew_sym(w2p)*skew_sym(w2p))*f2p;
accrhs(5) = -h1p'*(A1'*A2*skew_sym(w2p)*skew_sym(w2p))*g2p;
accrhs(6:8) = A3*skew_sym(w3p)*skew_sym(w3p)*s3pB-
A2*skew_sym(w2p)*skew_sym(w2p)*s2pB;
accrhs(9:11) = A4*skew_sym(w4p)*skew_sym(w4p)*s4pC-
A3*skew_sym(w3p)*skew_sym(w3p)*s3pC;
accrhs(12) = -f3p'*(A3'*A4*skew_sym(w4p)*skew_sym(w4p) +
2*skew_sym(w3p)'*A3'*A4*skew_sym(w4p) +
skew_sym(w3p)*skew_sym(w3p)*A3'*A4)*f4p;
accrhs(13) = -h4p'*(skew_sym(w4p)*skew_sym(w4p)*A4'*A1)*h1p;
accrhs(14) = -g4p'*(skew_sym(w4p)*skew_sym(w4p)*A4'*A1)*h1p;
accrhs(15) = f1p'*A1'*(-A4*skew_sym(w4p)*skew_sym(w4p)*s4pD4);
accrhs(16) = g1p'*A1'*(-A4*skew_sym(w4p)*skew_sym(w4p)*s4pD4);
accrhs(17) = -g4p'*(skew_sym(w4p)*skew_sym(w4p)*A4'*A1)*f1p;
accrhs(18) = -2*(p2d)'*p2d;
accrhs(19) = -2*(p3d)'*p3d;
accrhs(20) = -2*(p4d)'*p4d;
accrhs(21) = 2*p2(1)*p2d(1)^2*(1-p2(1)^2)^-1.5;
qdd = inv(JAC) * accrhs;
r2dd = qdd(1:3);
r3dd = qdd(8:10);
r4dd = qdd(15:17);
p2dd = qdd(4:7);
p3dd = qdd(11:14);
p4dd = qdd(18:21);
%Final_main ADJ 5/1/12
clear
assy_tol = 1e-5;
Final_init;
%timer loop
keep = [];
for t = 0 : .05 : 1,
Final_kin;
r2x = r2B(1);
r2y = r2B(2);
r4z = r4D4(3);
keep = [ keep ; t r4dd(3) r4(3) r4d(3) r2x r2y r4z r3dd(3) r3(3) r3d(3)];
end
p2x = keep(:,5);
p2y = keep(:,6);
p4z = keep(:,7);
% plot
time = keep(:,1);
Acceleration = keep(:,2)*-1;
figure(1)
plot(time, Acceleration )
ylabel('acceleration (cm/s^2)');
xlabel('time');
title('Acceleration vs Time');
figure(2)
Velocity = keep(:,4);
plot(time,Velocity)
ylabel('velocity (cm/s)');
xlabel('time (s)');
title('Velocity vs Time');
figure(3)
Position = keep(:,3);
plot(time,p4z)
ylabel('Global D4Z (cm)');
xlabel('time (s)');
title('Global Z Position of Prism vs Time');
figure(4)
plot(p2x,p2y)
ylabel('Global B2Y');
xlabel('Global B2X');
title('Rotation of B2');
axis square;
Kinematic Position and Instant Center Analysis of
Prosthetic Polycentric Knee Mechanisms
ME 581 Final Project
Michael MacDonald
Anthony Ligouri
5/3/2012
Introduction:
The human knee is a complex mechanism that plays multiple important roles that are essential
to efficient gait. It acts as a shock absorber at heel strike and allows sufficient toe clearance during
swing, just to name a few. The knee actually rotates and translates about 3 axes, an axis through the
femoral condyles, an axis along the long axis of the femur, and an axis passing posteriorly to anteriorly
through the knee [4], making it a joint that rotates around and instant screw rather than an instant
center. Modern prosthetic devices struggle to copy these complex kinematics, and mostly attempt to
emulate the motion in the sagittal plane, which becomes motion about an instant center. Another
setback of modern prosthetic knees is the inability to provide active stability control, which is provided
in the natural knee by the quadriceps muscles and hamstrings muscles in the thigh. Stability in
prosthetic knees is largely determined by the location of the line of action of the ground reaction force
(GRF) with respect to the center of rotation of the knee [1]. This is most important during heel strike
and weight acceptance, where the GRF should act anteriorly to the knee’s instant center, creating an
extension moment that keeps the leg from collapsing under the weight of the individual. If this moment
is not sufficient, an amputee using an articulated knee must create an extension moment at the knee by
extending the hip [1].
The objective of this study is to analyze the position kinematics and instant center locus, or
centrode, of different prosthetic knee devices in MATLAB using methods develop developed by Ed Haug
which were presented to the authors in ME 581 at The Pennsylvania State University in order to quantify
different aspects of knee stability during the gait cycle.
It would be useful to engineers and prosthetists to perform analysis on the entire realm of
prosthetic knee mechanisms, and to compare them to the natural human knee, however, several factors
kept us from accomplishing this goal. Primarily, it would be impossible to analyze every type of knee
mechanism within the time frame of this study. Secondly, with access to only a small selection of real
devices to photograph, it is exceedingly difficult to find scalable, sagittal plane photos of each
mechanism to be digitized. Actual images of a human knee that show the origin and insertion of the
anterior and posterior cruciate ligaments, which would allow us to calculate the link lengths of the 4-bar
mechanism within the human knee, can be difficult to read correctly and should only be done by trained
medical professionals for any type of medical analysis. Therefore, short of dissecting a cadaver knee and
taking measurements, this data was unavailable to us. Consequently, this analysis will focus on only 2
prosthetic knee mechanisms, the Freada 2SR320, a 4-bar type linkage, and the Otto Bock EBS 360, a 5-
Bar linkage. The methods for analysis can easily be extended to other knee mechanisms, such as the
widely used Otto Bock C-Leg and linkages that are under development, such as the 6-bar linkage
presented by Jin et al [2].
Methods:
In order to apply Haug’s method of kinematic analysis to the prosthetic knee mechanisms, it was
necessary to gather photos in a sagittal plane view that could be properly digitized and scaled for
analysis. An extensive search was performed online for knee mechanisms with clear sagittal plane
views. One four bar knee mechanism, the Freada 2sr320, was found to meet these requirements with a
decent picture resolution. Multiple patented knee designs were examined as well as manufacturers’
websites, but it proved to be difficult to find good planar views of the knee mechanism. After limited
success in an internet search, pictures were taken of actual prosthetic knees at KCF Technologies in
State College, PA. Pictures of the OttoBock EBS360 and the OttoBock C-leg were taken with a scaling
ruler. Once the required photos were gathered, they were converted to ‘tps’ files using the program
TpsUtil. This allowed the photos to be digitized in the software program Tpsdig2. Each photo was
imported into tpsdig2 and points were placed on all mechanism joints. The photos were then saved as
tps files and converted to text files for easy importation to Matlab. The header and other information
beside the joint locations were removed from the text file. These digitized photos are shown below in
figure 1.
Figure 1 – photos of knee mechanisms analyzed
After digitizing the photos, they were loaded in Matlab and the joint locations were assigned.
The ground link (link 1) was assigned as the link to which the femur is connected. This link is assumed to
not rotate as the knee goes through flexion and extension within the gait cycle. The initial position of
the linkage is assumed to be in full extension. The generalized coordinates for each mechanism were
established in a Matlab file with estimations of the angles of each respective coordinate frame. The x
axis of each coordinate frame was established as running axially along each link towards the next joint,
while the y axis was established transversely to each respective link. The joint coordinates were
estimated in global coordinates with respect to point A on the ground link as shown in figure 1. The
generalized coordinates are displayed below in figure 2 and 3.
 0 
  cm 
 r2   0 
    60 
 2  2.500  
 r3 
q      4.206cm
9 x1
3   189 
 r4   
   0.677cm
 4   4.735 
 104 
 
Figure 2. Initial guesses for the generalized coordinates of the four bar mechanism
 0 
  cm 
 0 

 2 
r   283 
  1.95  
 2   cm
 r3   10.488 

3 
  180 
q      2.55  
12x1
 r4   cm
 4   10.775 
   60 
r5    
5    2.125cm 
  9.925 
 
 {100} 
Figure 3. Initial guesses for the generalized coordinates of the five bar mechanism
Upon establishing the generalized coordinates, a constraint vector was created for each
mechanism. For the four bar linkage, the constraints are straight forward as there is only one degree of
freedom and four revolute joint constraints. The five bar was a little more challenging as it has two
degrees of freedom due to the small link (link 4) having the ability to rotate in either direction for a
specified motion. In reality, this link is constrained by a damping mechanism as the OttoBock EBS360 is
actually a dynamic system. For the purposes of this project, it was viewed as a kinematically determined
mechanism in order to analyze its motion over a gait cycle. A kinematic constraint was placed on this
link that prevented links 4 and 5 from becoming parallel and thereby forcing the link to rotate inwards
and never enter the unrealistic “elbow out” position.
The driver constraint used was derived from an experimental knee angle curve established in
literature [4]. A polynomial fit was performed on this curve using several data points collected at key
events in the gait cycle. This curve was used to define the angle between the shank and thigh links (1
and 3) for an entire gait cycle. In order to get a vector oriented in the direction of the shank, a local
rotation matrix [C] was established between the local body 3 coordinate system and the direction of the
shank. This rotation matrix was multiplied by the global rotation matrix for body 3 [A3], which provided
unit vectors in the direction of the shank. The angle between the unit vector running axially along the
shank and the unit vector running axially along the thigh (global y), was then set as the driver constraint
with the angle changing as defined by the aforementioned knee angle curve. The knee angle versus gait
cycle plot found in literature is shown below in figure 4 along with the polynomial approximation used
as a driver function. The constraint vector for the four bar mechanism is shown in figure 5 and the
constraint vector for the five bar mechanism is shown in figure 6.
Figure 4. Knee angle versus gait cycle from literature and polynomial approximation
 r2  A2  s2 ' A r1A 

 
 
REV _ A   r2 A  r1A  r3  A3  s3 ' B r2  A2  s2 ' B 
     
 REV _ B   r3 B  r2 B   
C
  REV _ C    r4   r3 
C C
  r4  A4  s4 ' r3  A3  s3 '   0
C
     
 REV _ D   T r4 ''  r1 
D D
  
  DRIVER  g1 * g 3  cos(   )  r4  A4  s4 ' D r1D 
 
 
 g1T * g 3 ''  cos(   ) 
Figure 5. Constraint vector for the 4 bar mechanism

Defining the unit vector axis along the shank for the driver constraint:
Qˆ  [ A3 ][C]
  8.5
cos( )  sin( )
[C]   
 sin( ) cos( ) 
g ''  Qˆ (:,2)
3
g1T * g 3 ''  cos(   )
Where α = angle between y axis along shank and y axis of body 3
 r2  A2  s2 ' A r1A 

 
 
r2 A  r1A   r3  A3  s3 '  r2  A2  s2 ' 
       
 B B


 REV _ A     
   r3 B  r2 B  
 REV _ B   r4  A4  s4 '  r3  A3  s3 '
   C
    C
  REV _ C    r4 D  r3 D   

C C  
  0
    r   r    
r5  A5  s5 ' D r4 D
5 4
 REV _ D     
  DRIVER   T r5 ''  r1 
E E
  
g1 * g 3  cos(   )  
 r5  A5  s5 ' E r1E 
 
 
 g1 * g 3  cos(   )
T ''

Figure 6. Constraint vector for 5 bar mechanism
After establishing the constraint vectors, the Jacobian of each respective linkage was defined.
The Jacobians for each mechanism are shown on the next page in figures 7 and 8.
 I 2  B2  s2 ' A 02 x2  02 x1  02 x2  02 x1  
 
 
 I 2   B2  s2 ' B I 2  B3  s3 ' B 02 x2  02 x1  
 
 
   q  0 2 x 2 

02 x1   I 2   B3  s3 'C I 2  B4  s4 ' 
C
 
0  02 x1  02 x2  02 x1  I 2  D
B4  s4 ' 
 2x2
 
 
01x 2  1 01x2  0 01x2  0 
Figure 7. Jacobian for the four bar mechanism
 I 2  B2  s2 ' A 02 x2  02 x1  02 x2  02 x1  02 x2  02 x1  

 
 
 I 2   B2  s2 ' B I 2  B3  s3 ' B 02 x2  02 x1  02 x2  02 x1  
 
 
0 
 2x2
02 x1   I 2   B3  s3 'C I 2  B4  s4 'C 02 x2  02 x1  
 
q   
0 
 2x2 02 x1  02 x2  02 x1   I 2   B4  s4 ' D I 2  B5  s5 ' 
D 
 

01x 2  01x1  01x2  02 x1  01x2   { f } *{ f } 0  
4
'
5 1x 2
'

{ f 4 } *{ f 5 } 
 
 
01x 2  01x1  01x2  1 01x2  01x1  01x2  01x1  
Figure 8. Jacobian for the five bar mechanism

To check that the constraint and jacobian vectors were created correctly, a newton-raphson
numerical solution was applied and convergence of the constraint vector for each mechanism was
plotted versus the number of iterations of the newton-raphson process. It is expected that the
constraint vector will converge to zero within five iterations of the newton-raphson solution. The
convergence plots are shown in the results section of this report.
Finding the instant center location between the thigh and shank links for the four bar linkage is
fairly straight forward. Primary instant centers for each revolute joint are known and the secondary
instant centers can be found graphically according to Kennedy’s theorem. A line can be formed through
instant centers 12 and 23 (joints A and B), and another line can be formed through instant centers 34
and 14 (joints C and D). The instant center between links 1 and 3 (thigh and shank) will be located at the
point at which the lines intersect.
Finding the instant center of link 5 is not as straight forward. Since there are five primary instant
centers and two degrees of freedom, the secondary instant centers cannot be found directly. The
simplest method to find the location of the secondary instant centers in a five bar linkage is to use the
angular velocity ratio method [3]. In this method, the ratio of angular velocity of the two links whose
instant center is desired can be used to find the instant center. These links already have two primary
instant centers on either side of an intermediate link. This means that the desired instant center is
located on a line between these two primary instant centers. The distance between the two primary
instant centers is known and the ratio of the distance from each respective primary center to the new
instant center is equal to the ratio of the angular velocity of each respective link. This method is difficult
to explain verbally, but can be seen more clearly when displayed graphically. The equation for this
method appears as:
Unfortunately, since the driver constraint for knee angle is not time driven, the angular velocity
of each link is not known. A simplification could be made to the gait cycle in which the cycle takes place
over an evenly spaced sequence of time, but this is unrealistic and as a result not very useful in analysis.
If the time between key events in the gait cycle is known, then a more accurate time relation could be
formed for the driver constraint. Unfortunately, there was no time to find or collect such data to
include in this report, so the instant center between the thigh and shank links of the five bar mechanism
was not found.
Results:
Initially, a position solution for 0% gait cycle (the initial position) was performed in order to
check the convergence of the constraints. The solution was run over 10 Newton Raphson solutions in
case any constraints took more than the expected number of iterations (about 5) to converge. The
magnitude of the residuals vs. the number of iterations (Figure 9) is shown below. It is evident that all
the constraints are driven to 0 by 10 iterations except for 1. This constraint that is not yet 0 is the driver
constraint, which has a value of .0065 after 10 iterations. By 30 iterations, it has a value of .0014 and
would probably reach some asymptotic value by 40 or 50 iterations. For this study, we considered 10
iterations to be acceptable, as this translates to an error or only .65% between the actual angle of the
knee mechanism and the angle it is being driven to.
Freada 2SR320 Convergence

0.5
0.4
0.3
0.2
0.1
Residuals
-0.1
-0.2
-0.3
-0.4
-0.5
1 2 3 4 5 6 7 8 9 10
Number of Newton Raphson Iterations
Figure 9. Convergence of the four bar knee mechanism
In order to visualize the initial position of the 4 bar mechanism, the position of the 4 revolute
joints were plotted along with the joint center at that position (Figure 10). It can be seen that the
position of the links is exactly as shown in the picture from which the data was taken, and the center of
rotation of link 3 with respect to link 1 is at the intersection of lines extending from links 2 and 4, as it
should be. This serves as a validation of sorts that our model is correct.
Initial Postion of the Freada 2SR320 4 bar and Corresponding Instant Center
3
1
y position (cm)
-1
-2
-3
-4
-6 -5 -4 -3 -2 -1 0 1 2 3
x position (cm)
Figure 10. Geographical layout of the four bar knee mechanism
Moving the knee through the range of motion experienced during 1 gait cycle, performing a
position solution and finding the instant center for each percentage step gives the instant center locus,
or centrode, for link 3 (the tibia) with respect to link 1 (the femur) shown below (Figure 11). There are 3
distinct arcs observable here, two that are “concave up” and one that is “convex up”. It is obvious that
this is vastly different from the centrode of the 4-bar presented in class. An explanation of this is
attempted below in the discussion.
Figure 11. Centrode of the thigh and shank links for the four bar mechanism over one gait cycle
A similar analysis was performed on the initial position of the Otto Bock EBS 360. For this
mechanism, all the constraints converged to zero in only 5 Newton Raphson iterations (Figure 11).
Otto Bock EBS 360 Convergence
0.4
0.2
0
Residuals
-0.2
-0.4
-0.6
-0.8
-1
1 1.5 2 2.5 3 3.5 4 4.5 5
Number of Newton Raphson Iterations
Figure 12. Constraint convergence of the five bar mechanism
A similar attempt was made to visualize the initial position of the 5-bar mechanism, however,
because all 5 links are connected in a single closed loop, intermediate centers needed to find the instant
center of link 3 with respect to link 1 need to be found via a “ratio of angular velocities” method
presented in Pennock (2006) [3]. Unfortunately, we are unable to use this method. The driver is not a
function of time, but a function of gait cycle percentage, and it is possible and very likely that the time
step between percentages is not constant. Furthermore, the function of this varying time step is
unknown, making a transformation of the driver from a function of gait cycle percentage to a function of
time impossible. Consequently, we were not able to calculate the joint center for the EBS 360, which is
an unfortunate result. However, we were able to plot the locations of the joint positions to assure
ourselves the model was configured correctly (Figure 12)
Initial Postion of the Otto Bock EBS 360
0
-1
-2
-3
-4
y position (cm)
-5
-6
-7
-8
-9
-10
-8 -6 -4 -2 0 2 4
x position (cm)
Figure 12. Graphical layout of the Ottobock EBS 360
Discussion and Conclusion:
Figure 11 shows the centrode of the Freada 2SR320 4-bar knee mechanism, which displays 3
distinct arcs as described above. We know this is not the centrode of a stereotypical 4 bar as presented
in class. We attempt to explain these arcs by recognizing that over the total range of motion of these
type of 4-bar knee mechanisms, there are periods where links 2 and 4 do not cross, during which the
instant center of rotation of link 3 with respect to link 1 is outside the mechanism as in Figure 10 above,
and other periods where links 2 and 4 cross, during which the instant center of link 3 lies within the
mechanism at the intersection of links 2 and 4. This would explain the top concave arc and convex arc,
which would occur during the crossed and uncrossed periods, respectively. Currently, we cannot offer
an explanation for the smaller concave arc shown in Figure 11.
This model is obviously in need of some method of validation, which would make for some useful future
work on this subject. One method of validation may be to take a video recording of an actual knee
mechanism moving through the same range of motion as persecribed in this model and graphically find
the instant center of rotation of link 3 with respect to link 1. This could be accomplished, either by hand
or on any number of imaging programs, by drawing a line between the joints on link 2 and link 4 and
extrapolating the lines to a point where they intersect and finding the coordinates of that point. If it
traces out the same centrode as the model, it would be validated. Secondary validation could be
performed by making an animation in MATLAB of the linkage and instant center as it moves through the
range of motion prescribed by the driver, which could be examined for realistic behavior and even
compared to the graphical validation method previously described.
However, even with a good method of model validation, we are unable to draw many conclusions about
the inherent stability of the knee without more information. As stated above, stability of a prosthetic
knee is largely dependent on the location of the line of action of the GRF with respect to the instant
center. In order to determine when this specific mechanism is in a stable extension position we would
need a data set giving the equation for a line describing the line of action of the GRF with respect to the
femur (ground) during stance. With that, we could determine the moment arm of the GRF to the instant
center and be able to calculate the extension (or flexion) moment of the GRF. To determine how much
extension moment the subject would need to produce at the hip, the moment of the subject’s center of
mass (COM) about the instant center would also be required to perform a moment balance about the
instant center. This data was not available for this study and would most likely require collection of
additional data from like subject in the gait lab using a force plate along with a motion capture system.
As explained above, the centrode for the Ottobock EBS 360 5-bar mechanism could not be found using
the methods available. However, it could be found if the driver used was a function time rather than a
function of gait percentage. In order to obtain this data, we would once again need to collect data of a
live subject in the gait lab, recording the knee angle vs. time over one complete gait cycle. This data
could then be used for the driver, and valid angular velocities for the links could be calculated, allowing
us to use the methods in Pennock (2006) to find the instant centers.
We also realize that in a 2 degree of freedom linkage like the Otto Bock EBS 360, there are other factors
that determine stability other than the location of the instant center of rotation and line of action of the
GRF. Simply from visual inspection, one can imagine if a moment was put on link 3 to put the knee into
flexion, link 4 would collapse into link 3 or link 5 until it made contact unless some force was put on it.
Likewise, if an extension moment was put on link 3, the x axis of links 4 and 5 would eventually become
collinear and may pop to an “elbow out” configuration upon flexion. Without some force on link 4, the
mechanism would not be stable. In our model, we constrained the movement of link 4 to enforce this
stability. In reality there are springs and dampers within the mechanism that provide forces to prevent
this collapse. These were not used in our model, and it is possible if they were included that they could
provide more primary instant centers that would make finding the instant center of link 3 with respect
to link 1 possible without use of the ratio of angular velocity method.
In conclusion, much insight was gained in the modeling of polycentric knee mechanisms. Several key
pieces of data are needed to quantify stability in the knee including the location of the instant center of
rotation, the line of action of the GRF, the line of action of the COM, and a complete mechanism model
that has enough primary instant centers to find the instant center of interest if a non-time-dependent
driver is used. It was realized during the preparation of this report and presentation that the instant
center of the links of interest of the 6-bar presented by Jin et al (2003) could have been easily found by
use of the graphical method and would have made a better subject of analysis than the Otto Bock EBS
360. Interesting future work in this area would include analysis of said 6-bar, and the Otto Bock C-Leg,
which also contains 4 links, but instead of links 3 and 4 connected by a revolute joint, they are
connected by a cylindrical join.
Special thanks goes out to KCF Technologies, State College PA, for allowing us to photograph several
different prosthetic knee mechanisms for analysis.
The code used in this modeling simulations is appended at the end of the document.
References
[1] Radcliffe, C.W. (1994) Four bar prosthetic knee mechanisms: kinematics, alignment, and prescription
criteria. Prosthetics and Orthotics International. 18:159-173
[2] Jin, D., Zhang, R., et. al. (2003) Kinematic and Dynamic performance of prosthetic knee joint using six
bar mechanism. Journal of Rehabilitation Research and Development. 40:39-48
[3] Pennock, G.R. (2006) Curvature theory for a two degree of freedom planar linkage. Mechanism and
Machine Theory. 43(5):525-548
[4] Perry, J. & Burnfield, J. (2010) Gait Analysis: Normal and Pathological Function 2nd edition. SLACK
incorporated. Pgs 85:103
Matlab Code Appendix
Freada 2SR320 initializing
%% 581 project
% Mike and Tony
% Prosthetic knee mechanism analysis
% Freada 2SR320 ini file
clc
d2r = pi/180;
R = [0 -1;1 0];
%% Load data files
freada_SR = load('2SR320_pts.txt');
scale = 1/34; %centimeters per pixel as determined in tpsdig
pointA = freada_SR(1,:)*scale;
pointB = freada_SR(2,:)*scale;
pointC = freada_SR(3,:)*scale;
pointD = freada_SR(4,:)*scale;
pointQ = freada_SR(5,:)*scale;
%% Initial guesses for generalized coordinates

% q = [x2 y2 phi2 x3 y3 phi3 x4 y4 phi4]'
r1 = [0 0]; %rA
r2 = [0 0]; %origin of link 2 (at A)
r3 = [pointB(1)-pointA(1) pointB(2)-pointA(2)]; %origin of link 3 (at B)
r4 = [pointC(1)-pointA(1) pointC(2)-pointA(2)]; %origin of link 4 (at C)
theta1 = 0;
theta2 = -60*d2r; %Angle estimation made using tpsdig
theta3 = 189*d2r;
theta4 = 104*d2r;
q = [r2 theta2 r3 theta3 r4 theta4]';
%% Blueprint info for joint locations
% Link 1
s1pA = [0 0]';
s1pD = [pointD(1)-pointA(1) pointD(2)-pointA(2)]';
% Link 2
s2pA = [0 0]';
% Magnitude of the distance between A and B
AB = sqrt((pointB(1)-pointA(1))^2 + (pointB(2)-pointA(2))^2);
s2pB = [AB 0]';
% Link 3
s3pB = [0 0]';
% Magnitude of the distance between B and C
BC = sqrt((pointC(1)-pointB(1))^2 + (pointC(2)-pointB(2))^2);
s3pC = [BC 0]';
% Blueprint information for point Q to be used in driver constraint
s3pQ = [pointQ(1)-pointB(1), pointQ(2)-pointB(2)]';
a=8.5*d2r; %angle between y3 and y3'' (long axis of tibia)
C=[cos(a), -sin(a); sin(a), cos(a)]; %rotation matrix for defining point Q
s3ppQ = s3pB + C*s3pQ;
% Link 4
s4pC = [0 0]';
% Magnitude of the distance between C and D
CD = sqrt((pointD(1)-pointC(1))^2 + (pointD(2)-pointC(2))^2);
s4pD = [CD 0]';
Freada 2SR320 constraint
%% 581 project
% Mike and Tony
%Constraint file
% Freada 2SR320
clc
theta=0;
%% Rip from the generalized coordinates

r2 = q(1:2);
phi2 = q(3);
r3 = q(4:5);
phi3 = q(6);
r4 = q(7:8);
phi4 = q(9);
%% Attitude Matrices
A2 = [cos(phi2) -sin(phi2); sin(phi2) cos(phi2)];
%B attitude matrices are rotated 90 degrees from A
B2 = R*A2;
B3 = R*A3;
B4 = R*A4;
%% Create global location for coordinate points
r1A = s1pA;
r1D = s1pD;
r2A = r2 + A2*s2pA;
r2B = r2 + A2*s2pB;
r3B = r3 + A3*s3pB;
r3C = r3 + A3*s3pC;
r3Q = r3 + A3*s3ppQ; %
r4C = r4 + A4*s4pC;
r4D = r4 + A4*s4pD;
%Define constraint matrix
PHI = zeros(9,1);
PHI(1:2) = r2A - r1A; %constraint for revolute A (i=1,j=2)
PHI(3:4) = r3B - r2B; %constraint for revolute B (i=2,j=3)
PHI(5:6) = r4C - r3C; %constraint for revolute C (i=3,j=4)
PHI(7:8) = r4D - r1D; %constraint for revolute D (i=1,j=4)
%Driver
UnitQ=A3*C;
g3_pp=UnitQ(:,2); %unit vector along y3'' (tibia)
g1=[0,1]';
PHI(9)=g1'*g3_pp-cos(pi-theta);
%Define the Jacobian

JAC = zeros(9,9);
%For rev A:
JAC(1:2,1:3) = [eye(2) B2*s2pA];
%For rev B:
JAC(3:4,1:3) = [-eye(2) -B2*s2pB];
JAC(3:4,4:6) = [eye(2) B3*s3pB];
%For rev C:
JAC(5:6,4:6) = [-eye(2) -B3*s3pC];
JAC(5:6,7:9) = [eye(2) B4*s4pC];
%For rev D:
JAC(7:8,7:9) = [eye(2) B4*s4pD];
%For driver:
JAC(9,6)= 1;
Freada 2SR320 position solution

%% 581 project
% Mike and Tony
% Position solution
% Freada 2SR320
%% Newton Raphson
freada_2sr320_ini
freada_2sr320_phi
n=10;
for i=1:n
q=q-inv(JAC)*PHI;
freada_2sr320_phi
end
Freada 2SR320 instant center

%% 581 project
% Mike and Tony
% Finding instant center
% Freada 2SR320
%% Line for 1 - 2
%point 2 = r1A
x2=r1A(1);
y2=r1A(2);
%point 1 = r2B
x1=r2B(1);
y1=r2B(2);
m12=(y2-y1)/(x2-x1);
b12=-((y2-y1)/(x2-x1))*x1+y1;
% y=m12*x+b12
%% Line for 3 - 4
%point 2 = r4C
x2=r4C(1);
y2=r4C(2);
%point 1 = r4D
x1=r4D(1);
y1=r4D(2);
m34=(y2-y1)/(x2-x1);
b34=-((y2-y1)/(x2-x1))*x1+y1;
% y=m34*x+b34
%% find x and y
x=(b34-b12)/(m12-m34);
y=m34*x+b34;
Freada 2SR320 main
%% 581 project
% Mike and Tony
% Main File
% Freada 2SR320
close all
clear all
clc
%% Get Driver data

ME581final_driver_data
%% Step through GCP, compute new position solution, find instant center
%storage for instant center data

x_ctr=zeros(1,length(GCP));
y_ctr=zeros(1,length(GCP));
for k=1:length(GCP)
theta=angle(k); % knee angle for corresponding point in gait cycle
pos_soln_4bar; % initializes and finds a new position solution for
each joint angle
int_ctr_4bar; % finds x and y coords for the instant center at that
position
plot(x,y,'o');
axis([-5 5 -5 5])
%F(k)=getframe;
x_ctr(k)=x; % stores x and y before moving on to the next position
y_ctr(k)=y;
X=[r1A(1) r2B(1) r3C(1) r4D(1) r1A(1)];
Y=[r1A(2) r2B(2) r3C(2) r4D(2) r1A(2)];
hold on
plot(X,Y)
axis equal
% G(k)=getframe;
hold off
end
figure(2)
plot(x_ctr, y_ctr, 'o')
axis equal
title('Envelope of the Knee Center through 1 Gait Cycle')
xlabel('x coordinate of knee center')
ylabel('y coordinate of knee center')
%figure(3)
%movie(F,1,1)
% figure(4)
% movie(G,1,3)
Otto Bock EBS 360 initializing

%% 581 project
% Mike and Tony
% Otto Bock EBS 3R60 ini file
clc
d2r = pi/180;
R = [0 -1;1 0];
%% Load data files
five_bar = load('ob_ebs360_points.txt');
scale = 1/80; %centimeters per pixel as determined in tpsdig
pointA = five_bar(1,:)*scale;
pointB = five_bar(2,:)*scale;
pointC = five_bar(3,:)*scale;
pointD = five_bar(4,:)*scale;
pointE = five_bar(5,:)*scale;
pointQ = five_bar(6,:)*scale;
%% Initial guesses for generalized coordinates

% q = [x2 y2 phi2 x3 y3 phi3 x4 y4 phi4]'
r1 = [0 0]; %rA
r2 = [0 0]; %origin of link 2 (at A)
r3 = [pointB(1)-pointA(1) pointB(2)-pointA(2)]; %origin of link 3 (at C)
r4 = [pointC(1)-pointA(1) pointC(2)-pointA(2)]; %origin of link 4 (at D)
r5 = [pointD(1)-pointA(1) pointD(2)-pointA(2)]; %origin of link 5 (at E)
theta1 = 0;
theta2 = 283*d2r; %Angle estimation made using tpsdig
theta3 = 180*d2r;
theta4 = 60*d2r;
theta5 = 100*d2r;
q = [r2 theta2 r3 theta3 r4 theta4 r5 theta5]';
%% Blueprint info for joint locations
% Link 1
s1pA = [0 0]';
s1pE = [pointE(1)-pointA(1) pointE(2)-pointA(2)]';
% Link 2
s2pA = [0 0]';
% Magnitude of the distance between A and B
AB = sqrt((pointB(1)-pointA(1))^2 + (pointB(2)-pointA(2))^2);
s2pB = [AB 0]';
% Link 3
s3pB = [0 0]';
% Magnitude of the distance between B and C
BC = sqrt((pointC(1)-pointB(1))^2 + (pointC(2)-pointB(2))^2);
s3pC = [BC 0]';
% Blueprint information for point Q to be used in driver constraint
s3pQ = [pointQ(1)-pointB(1), pointQ(2)-pointB(2)]';
a=-4.5*d2r; %angle between y3 and y3'' (long axis of tibia)
C=[cos(a), -sin(a); sin(a), cos(a)]; %rotation matrix for defining point Q
s3ppQ = s3pB + C*s3pQ;
% Link 4
s4pC = [0 0]';
% Magnitude of the distance between C and D
CD = sqrt((pointD(1)-pointC(1))^2 + (pointD(2)-pointC(2))^2);
s4pD = [CD 0]';
% Link 5
s5pD = [0 0]';
% Magnitude of the distance between D and E
DE = sqrt((pointE(1)-pointD(1))^2 + (pointE(2)-pointD(2))^2);
s5pE = [DE 0]';
Otto Bock EBS 360 constraint

%% 581 project
% ebs_360 prosthetic knee 5 bar
% Constraint file
% Otto Bock EBS 3R60
clc
theta=0;
%% Rip from the generalized coordinates
r2 = q(1:2);
phi2 = q(3);
r3 = q(4:5);
phi3 = q(6);
r4 = q(7:8);
phi4 = q(9);
r5 = q(10:11);
phi5 = q(12);
%% Attitude Matrices
%B attitude matrices are rotated 90 degrees from A
B2 = R*A2;
B3 = R*A3;
B4 = R*A4;
B5 = R*A5;
%% Create global location for coordinate points
r1A = s1pA;
r1E = s1pE;
r2A = r2 + A2*s2pA;
r2B = r2 + A2*s2pB;
r3B = r3 + A3*s3pB;
r3C = r3 + A3*s3pC;
r3Q = r3 + A3*s3ppQ; %
r4C = r4 + A4*s4pC;
r4D = r4 + A4*s4pD;
r5D = r5 + A5*s5pD;
r5E = r5 + A5*s5pE;
%Define constraint matrix

PHI = zeros(12,1);
PHI(1:2) = r2A - r1A; %constraint for revolute A (i=1,j=2)
PHI(3:4) = r3B - r2B; %constraint for revolute B (i=2,j=3)
PHI(5:6) = r4C - r3C; %constraint for revolute C (i=3,j=4)
PHI(7:8) = r5D - r4D; %constraint for revolute D (i=4,j=5)
PHI(9:10) = r5E - r1E; %constraint for revolute E (i=1,j=5)
% constraint to keep the revolute btw 4 and 5 "elbow in"
f4=A4(:,1); % i=4
f5=A5(:,1); % j=5
PHI(11)>=f4'*R'*f5; %keeps the sum of the cos of the angle less than 0
%Driver
UnitQ=A3*C;
g3_pp=UnitQ(:,2); %unit vector along y3'' (tibia)
g1=[0,1]';
PHI(12)=g1'*g3_pp-cos(pi-theta); % i=1 j=3
%Define the Jacobian

JAC = zeros(12,12);
%For rev A:
JAC(1:2,1:3) = [eye(2) B2*s2pA];
%For rev B:
JAC(3:4,1:3) = [-eye(2) -B2*s2pB];
JAC(3:4,4:6) = [eye(2) B3*s3pB];
%For rev C:
JAC(5:6,4:6) = [-eye(2) -B3*s3pC];
JAC(5:6,7:9) = [eye(2) B4*s4pC];
%For rev D:
JAC(7:8,7:9) = [-eye(2) -B4*s4pD];
JAC(7:8,10:12) = [eye(2) B5*s5pD];
%For rev E:
JAC(9:10,10:12) = [eye(2) B5*s5pE];
%For parallel constraint btw links 4 and 5
JAC(11, 9) = -f4'*f5;
JAC(11, 12) = f4'*f5;
%For driver:
JAC(12,6)= 1;
Otto Bock EBS 360 position solution

%% 581 project
% Mike and Tony
% Position solution
%% Newton Raphson
ebs_360_ini
ebs_360_phi
n=20;
for i=1:n
q=q-inv(JAC)*PHI;
ebs_360_phi
end
Otto Bock EBS 360 main
%% 581 project
% ebs_360 prosthetic knee 5 bar
% Main
% Otto Bock EBS 3R60
close all
clear all
clc
%% Get Driver data

ME581final_driver_data
%% Step through GCP, compute new position solution, find instant center
%storage for instant center data

x_ctr=zeros(1,length(GCP));
y_ctr=zeros(1,length(GCP));
for k=1:length(GCP)
theta=angle(k); % knee angle for corresponding point in gait cycle
pos_soln_5bar; % initializes and finds a new position solution for each
joint angle
X=[r1A(1) r2B(1) r3C(1) r4d(1) r5e(1) r1A(1)];
Y=[r1A(2) r2B(2) r3C(2) r4d(2) r5e(2) r1A(2)];
plot(X,Y);
H=getframe;
end
movie(H)
Convergence plot code

%% Newton Raphson
% ebs_360_ini
% ebs_360_phi
freada_2sr320_ini
freada_2sr320_phi
n=10;
PHIvec=zeros(length(PHI),n);
for i=1:n
PHIvec(:,i)=PHI;
q=q-inv(JAC)*PHI;
% ebs_360_phi
freada_2sr320_phi
end
figure
hold on
for i=1:9
plot(PHIvec(i,:))
end
title('Freada 2SR320 Convergence')
xlabel('Number of Newton Raphson Iterations')
ylabel('Residuals')
axis([1 10 -0.5 0.5])
Driver data synthesis code
%Anthony Ligouri
%ME 581 Final Project
%Generation of Driver data via interpolation code and poly fit functions
%Data Source: Gait Analysis, Perry & Burnfield
gcp=[0,12,39,50,62,70,95,100]; %percent gait cycle
knee_angle=[5,20,5,10,40,60,0,5]; %angle in degrees between the thigh and

shank
% figure(1)
% plot(GCP, knee_angle)
P=polyfit(gcp,knee_angle,7);
GCP=(0:1:100);
angle=polyval(P,GCP);
% hold on
plot(GCP, angle)
title('Knee flexion angle vs. gait cycle percentage')
xlabel('Gait cycle percentage')
ylabel('knee angle (degrees)')
Four-bar Automation
Josh Norman
05/03/2012
ME581: Dr. Sommer

Motivation:
This project was motivated by the fact that four-bar linkages have a wide variety of
applications: from manufacturing to suspensions to basic geometrical manipulation (ex: turning a
crank results in an approximate straight line, etc). However, the procedure behind solving the
kinematics of a four bar can be quite tedious, especially in the beginning phases when it is up to
the user to determine the object’s blueprints, often from just a digital image alone. It is a fairly
trivial task to assign local coordinate systems to each object and then locate the two revolutes on
each link with respect to these coordinates. It is less trivial to determine the location of reference
points (the points of interest which are not revolutes and are not found in either the constraints or
Jacobian) with respect to these local origins. However, most of the legwork behind these tasks is
fairly simple geometry that just requires some amount of attentiveness to detail on the part of the
user, as one simple sine or cosine mistake for example can cause the solution to diverge. It
therefore makes sense to automate this process using an application that can handle mathematical
operations with ease, in this case Matlab, to come up with a series of programs that minimize the
amount of work on the part of the user, and thus at the same time help to ensure accurate results
on the first try.

Four-bar Examples:
Figure 1: Webcutter mechanism
Figure 2: Bicyle suspension (blue is ground)
Figure 3: Hoeken’s linkage (approximate straight-line mechanism)

Goal:
The goals of this project are threefold:
1. Streamline the process behind solving the kinematics of a four-bar mechanism.
2. Come up with a way to visualize the results in an intuitive manner.
3. Enable the user to interact with the visualization in real-time.
Method:
Both Matlab (R2010a) and tpsDig2 were used for the automation process. Minimal effort
on the part of the user goes into using the Matlab program. The main legwork behind this
automation method involves following a few rules when digitizing the mechanism via tpsDig2.
Once that has been completed, the Matlab code is relatively easy to implement.
tpsDig2:
 For each four-bar mechanism, there will be four .tps files (note the miniscule on the
“.tps” as Matlab will not recognize “.TPS”).
 name_points.tps
 name_link2.tps
 name_link3.tps
 name_link4.tps
These four files must all be placed in Matlab’s current working directory, along with all
of the Matlab scripts, to be addressed later. name refers to the user’s desired name ID for
the mechanism, such as “Webcutter” or “Hoekens” for example.

 For the name_points.tps file:
Figure 4: Order to select landmarks in name_points.tps file
The landmarks selected using tpsDig2 must begin at the GND revolute on the driver link.
The next three points will follow the path of the mechanism to each of its remaining
revolutes, ending at the other GND revolute. After the four revolutes have been selected
(the simple four-bar always has exactly four revolutes), any number of reference points
may be selected, as seen at points 5 and 6 above in Figure 4. Note: at least one
reference point must be selected for the Matlab program to work. It is up to the user
to remember which of the three link (link 2, 3, or 4) each of the reference points belong
to, as the Matlab program will not determine this automatically. However, when the
program is run, it will automatically recognize the number of reference points that must
be accounted for and ask the user to enter the corresponding link number one at a time for
each reference point.

 For the 3x name_link#.tps files:
Figure 5: Order to select landmarks in name_link#.tps files
Figure 5 shows an example for the order in which to select the points for a given link, in
this case for link3 of a webcutter mechanism. The first two points selected must be the
revolutes of the link. They must be selected in the same order in which they were chosen
in the name_points.tps file. The remaining points are up to the user to decide, as they
will shape the outline of that link. They must be selected in order as one travels around
the perimeter of the object. They can be selected CW or CCW, either will work. The last
point chosen does not need to match the first point, as the Matlab program will
automatically join those two points to close the loop.

Matlab Programs:
There are a total of six .m files. They are listed below, along with a brief description, in
the order in which they are run on execution:
1. fourBar_run Controls the execution of the other .m files. Only this
program needs to be run on execution.
2. fourBar_ini Determines the blueprint and initial guesses
3. fourBar_phi Solves the kinematics
4. fourBar_outlines Translates and rotates the link outlines for use in animation
5. fourBar_animation Animates the mechanism
6. slider GUI which allows the user to interact with the animation.
Also requires a slider.fig file which controls the GUI’s appearance
See Appendix 1 for the complete text of all .m files.

Results:
Below, the images resulting from execution of this method are presented for each of the
mechanisms in Figures 1-3. In these images, the crank was rotated from 0-360 degrees in 1
degree increments.
Webcutter
Figure 1.1: Resulting image of webcutter rotated through 360 degrees.

Bike Suspension
Figure 1.2: Resulting Image of bike suspension rotated through 360 degrees.
Hoeken’s Linkage
Figure 1.3: Resulting image of Hoeken’s linkage rotated through 360 degrees.
Conclusions:
The four-bar automation process works well and was tested for all three mechanisms seen
in Figures 1, 2, and 3. The pros of this method are that it requires only an image of the
mechanism and some legwork in setting up the .tps files. The Matlab program can perform the
rest of the calculations given a few inputs from the user during the running of the script. Thus,
this method is easy to use. The cons of this method are that it only works for a generalized case,
that is, for four-bar mechanisms containing only revolutes. Also, because the method relies
heavily on the image that is digitized, a high resolution image is needed for more accurate
results, as it is typically not the size of the links that matters when trying to capture the purpose
of the mechanism, but the ratios between the lengths of the links. Finally, a more recent version
of Matlab is required, as the function “clearvars” does not exist in older versions (such as
R2007).
In the future, the GUI should be extended so that tpsDig2 is not required, but instead the
image can be digitized within Matlab itself. Also, this program would make for a good
executable file. This would enable users who do not have access to Matlab to run the analysis.
Appendix 1: .m Files
%% fourBar_run.m
%
% Written By: Josh Norman
% Date: 05/01/12
% Program Summary: Controls the execution of all other .m scripts which are
% executed in the automation process. The user must run only this file.
clc; clear all; close all;
%% List Four Bar Types To Choose From

wildcard_txt = '*points.tps';
files_struct = dir(wildcard_txt); %list all *points.txt files in structure
files_cell = struct2cell(files_struct); %list these files in cell
names_cell = files_cell(1,:); %select first row of cell b/c it contains names
names_char = char(names_cell); %convert this cell row of names to characters
[m,n] = size(names_char); %use m to know how many files are available
for i = 1:m
typeNameIndex = strfind(names_char(i,:),'_points'); %first position in string where
"_points" is found
typeName = [names_char(i,1:typeNameIndex-1), char(abs(' ')*ones(1,50))]; %retrieve
only what occurs before "_points" %Pad with 100 extra blanks
typeName = typeName(1:50); %make string exactly 50 characters long
typeOptionsVec = [sprintf('%i) %s',i,typeName), char(abs(' ')*ones(1,60))]; %create
character array of file options to choose from % Padded
typeOptions(i,1:60) = typeOptionsVec(1:60);
end
disp('Recognized Four Bar Options')

disp(' ')
disp(typeOptions) %print list of available files to workspace
fprintf('\n\n')
typeChoice = input('Enter Type # To Run --> ');
typeID = deblank(typeOptions(typeChoice,:));
typeIDIndex = strfind(typeID,')');
typeID = typeID(typeIDIndex+2:length(typeID));
%% Run .m Files Sequentially

fourBar_phi;
fourBar_outlines;
fourBar_animate;
%% fourBar_ini.m
%
%Written By: Josh Norman
%Date: 04/30/2012
%
%Program Summary: Automatically determines blueprints and initial guesses.
%% Constants
d2r = pi/180;
r2d = 1/d2r;
R = [0 -1;1 0];
numRev = 4; % # of revolutes (Always 4 on a Four Bar)
%% Coordinates from .txt file

num_headerlines = 1; %number of headerlines in .tps file
strPoints = sprintf('%s_points.tps',typeID);
fid = fopen(strPoints); %open .tps file
xycell = textscan(fid,'%f %f','headerlines',num_headerlines); %read the data into 2 columns
[ X, Y ]
xymat = cell2mat(xycell);
fclose(fid);
xScaled = xymat(:,1)/SF; %SCALED x-coords of revolutes [cm]

yScaled = xymat(:,2)/SF; %SCALED y-coords of revolutes [cm]
%% Blueprint
%Revolutes (exactly two links per point)
r1A = [0;0]; %Always [0; 0] %First point is GND
s2pA = [0;0]; %Always [0; 0]
s2pB = [norm([xScaled(2)-xScaled(1),yScaled(2)-yScaled(1)],2); 0];
s3pB = [0;0]; %Always [0; 0]
s3pC = [norm([xScaled(3)-xScaled(2),yScaled(3)-yScaled(2)],2); 0];
s4pC = [0;0]; %Always [0; 0]
s4pD = [norm([xScaled(4)-xScaled(3),yScaled(4)-yScaled(3)],2); 0];
r1D = [xScaled(4)-xScaled(1);yScaled(4)-yScaled(1)]; %Last point is GND
%Reference Points
for i=1:numRef
delx1 = xScaled(refOnLink(1,i))-xScaled(refOnLink(1,i)-1);
dely1 = yScaled(refOnLink(1,i))-yScaled(refOnLink(1,i)-1);
theta1 = atan2(dely1,delx1)*r2d; %angle between horizontal and instance's x-axis
delx2 = xScaled(numRev+i)-xScaled(refOnLink(1,i)-1);
dely2 = yScaled(numRev+i)-yScaled(refOnLink(1,i)-1);
theta2 = atan2(dely2,delx2)*r2d; %angle between horizontal and instance's coord rotated
around its revolute
d12 = norm([delx2,dely2],2); %distance between instances reference point and its revolute
thetadiff = theta2-theta1;
s123pPQR(1:2,i) = [cosd(thetadiff)*d12; sind(thetadiff)*d12];

end
%% Initial Guesses
%Link 2
q2_ini = [0;0;phi2_desired]; %initial guess for link 2
%Link 3
delxAB = xScaled(2)-xScaled(1);
delyAB = yScaled(2)-yScaled(1);
dAB = norm([delxAB,delyAB],2);
thetaAB = atan2(delyAB,delxAB)*r2d;
delxBC = xScaled(3)-xScaled(2);
delyBC = yScaled(3)-yScaled(2);
dBC = norm([delxBC,delyBC],2);
thetaBC = atan2(delyBC,delxBC)*r2d;
q3_ini = [dAB*cosd(thetaAB); dAB*sind(thetaAB); thetaBC*d2r]; %initial guess for link 3
%Link 4
delxAC = xScaled(3)-xScaled(1);
delyAC = yScaled(3)-xScaled(1);
dAC = norm([delxAC,delyAC],2);
thetaAC = atan2(delyAC,delxAC)*r2d;
delxCD = xScaled(4)-xScaled(3);
delyCD = yScaled(4)-xScaled(3);
dCD = norm([delxCD,delyCD],2);
thetaCD = atan2(delyCD,delxCD)*r2d;
q4_ini = [dAC*cosd(thetaAC); dAC*sind(thetaAC); thetaCD*d2r]; %initial guess for link 4
%% q Inital
q = [q2_ini;q3_ini;q4_ini];
%% fourBar_phi.m
%
%Written By: Josh Norman
%Date: 04/30/2012
%
%Program Summary: Rip r and phi from q. Determines PHI (note majiscule)
%matrix and Jacobian matrix. To be used as part of Newton-Raphson
%technique in iterative loop. Determines position, speed, acceleration at
%any crank angle.
close all; clearvars -except typeID;
%% INPUTS
conv_limit = 1E-6; %desired convergence limit
w2 = 60*(1/60)*2*pi; %CCW (+) crank angular velocity [rad/s]
SF = 15.75; %scaling factor [pixel/cm]
%User Inputs (Command Window) - - - - - - - - - - - -

%Reference Points
strPoints = sprintf('%s_points.tps',typeID);
fid = fopen(strPoints); %open .tps file
num_headerlines = 1;
xycell = textscan(fid,'%f %f','headerlines',num_headerlines); %read the data into 2 columns
[ X, Y ]
xymat = cell2mat(xycell);
fclose(fid);
numRef = length(xymat(:,1))-4; % # of reference points (#landmarks-#revolutes)
disp(sprintf('\n\nNumer of Reference Points Detected = %i',numRef))
for i = 1:numRef
refOnLink(1,i) = input(sprintf('\tReference %i is on Link -> ',i));
end
%Crank
crankMin = input('Enter min crank angle [deg] (0) --> '); %min crank angle [deg]
if isempty(crankMin)
crankMin = 0;
end
crankMax = input('Enter max crank angle [deg] (360) --> '); %max crank angle [deg]
if isempty(crankMax)
crankMax = 360;
end
crankStep = input('Enter crank step [deg] (1) --> '); %crank step angle [deg]
if isempty(crankStep)
crankStep = 1;
end
% - - - - - - - - - - - - - - - - - - - - - - - - - -
%% LOOP THROUGH INDIVIDUAL CRANK ANGLES
phi2_desired_v = (crankMin:crankStep:crankMax)*(pi/180); %desired angle of link2 (driver)
[rad]
for k = 1:length(phi2_desired_v)
phi2_desired = phi2_desired_v(k); %discrete instance of crank angle
fourBar_ini; %run pre-program to initialize q values
%% PRE-PROCESSING
%Begin Iterations
n = 1; %loop counter
YN = 1;
while YN == 1
%constants
r2 = q(1:2);
phi2 = q(3);
r3 = q(4:5);
phi3 = q(6);
r4 = q(7:8);
phi4 = q(9);
%build vector which has all r vectors in it (robust feature to allow for
%reference points)
r234(1:2,1) = r2;
r234(3:4,1) = r3;
r234(5:6,1) = r4;
%rotation matrices
A2 = [cos(phi2) -sin(phi2);sin(phi2) cos(phi2)];
%build matrix which has all rotation matrices in it (robust feature to
%allow for reference points
A234(1:2,1:2) = A2;
A234(1:2,3:4) = A3;
A234(1:2,5:6) = A4;
%derivative wrt q of rotation matrices

B2 = R*A2;
B3 = R*A3;
B4 = R*A4;
%global location of revolutes

r2A = r2+A2*s2pA;
r2B = r2+A2*s2pB;
r3B = r3+A3*s3pB;
r3C = r3+A3*s3pC;
r4C = r4+A4*s4pC;
r4D = r4+A4*s4pD;
for cc = 1:numRef
index = (refOnLink(cc)-1)*2 - 1; %computes index to use from robust vector/matrix
%depending on whether current instance of ref point is on link 2,3,or4
r123PQR(1:2,cc) = r234(index:index+1) + A234(1:2,index:index+1)*s123pPQR(1:2,cc); % [2 x
numRef]
end
%Determine PHI matrix

PHI = zeros(9,1); %initialize matrix
PHI(1:2) = r2A - r1A;
PHI(3:4) = r3B - r2B;
PHI(5:6) = r4C - r3C;
PHI(7:8) = r4D - r1D;
PHI(9) = phi2 - phi2_desired; %driver constraint
%Determine Jacobian matrix

JAC = zeros(9,9); %initialize matrix
JAC(1:2,1:3) = [eye(2) B2*s2pA];
JAC(3:4,1:3) = [-eye(2) -B2*s2pB];
JAC(3:4,4:6) = [eye(2) B3*s3pB];
JAC(5:6,4:6) = [-eye(2) -B3*s3pC];
JAC(5:6,7:9) = [eye(2) B4*s4pC];
JAC(7:8,7:9) = [eye(2) B4*s4pD];
JAC(9,3) = 1;
%% PROCESSING
%Iterations
if max(abs(PHI)) > conv_limit
q = q-inv(JAC)*PHI;
YN=1;
else
YN = 0;
end
n = n+1;
end
loop_count(k,1) = n; %report number of loops required for convergence
fb_q(k,1:9) = q'; %save current q vector as row - to be outputted to file fb_q.txt
%Record coordinate points
allXs(k,1:4) = [q(1), q(4), q(7), r4D(1)]; %Converged x-coords of revolutes. Now add to
that...
allXs(k,5:5+(numRef-1)) = r123PQR(1,:); %... x-coords of ref points
allYs(k,1:4) = [q(2), q(5), q(8), r4D(2)]; %Converged y-coords of revolutes. Now add to
that...
allYs(k,5:5+(numRef-1)) = r123PQR(2,:); %... y-coords of ref points
%Velocity Solution
Vrhs = zeros(9,1);
Vrhs(9,1) = w2;
q_dot = inv(JAC)*Vrhs;
q_dot_v(k,1) = q_dot(9);
%Acceleration Solution
Arhs = zeros(9,1);
Arhs(1:2,1) = [(q_dot(3)^2)*A2*s2pA];
Arhs(3:4,1) = [(q_dot(6)^2)*A3*s3pB - (q_dot(3)^2)*A2*s2pB];
Arhs(5:6,1) = [(q_dot(9)^2)*A4*s4pC - (q_dot(6)^2)*A3*s3pC];
Arhs(7:8,1) = [(q_dot(9)^2)*A4*s4pD];
Arhs(9,1) = 0;
q_dot_dot = inv(JAC)*Arhs;
%% FIGURES
figure(1)
hold on
plot(phi2*(180/pi),max(abs(PHI)),'b.','markersize',7)
hold off
ylabel('Max Residual')
h = gca;
set(h,'fontsize',15)
title('Residual vs Crank Angle')
end
figure(1)
xlim([0,360]);
%% Output Files
% Output q's
hFile = fopen('q.txt','w');
fprintf(hFile,'%.5e %.5e %.5e %.5e %.5e %.5e %.5e %.5e %.5e\n',fb_q');
fclose(hFile);
%% fourBar_outlines.m
%
% Written By: Josh Norman
% Date: 05/01/12
% Program Summary: Receives inputs of fb_link2.tps, fb_link3.tps, fb_link4.tps outlines and
% anti-rotates them to their starting position using phi2_0, phi3_0,
% phi4_0. Then saves starting position link outlines to fb_link2_0.txt,
% fb_link3_0.txt, fb_link4_0.txt
%% Inputs
strLink2 = sprintf('%s_link2.tps',typeID);
filenamesIn = [strLink2;strLink3;strLink4];
filenamesOut = ['link2_0.txt';'link3_0.txt';'link4_0.txt'];
%% Loop through each of 3 links

for linkNum = 1:3
%% Read in x-y coords from file

num_headerlines = 1; %number of headerlines in .tps file
fid2 = fopen(filenamesIn(linkNum,:)); %open .tps file
xyCell = textscan(fid2,'%f %f','headerlines',num_headerlines); %read the data into 2 columns
[ X, Y ]
xyMat = cell2mat(xyCell)/SF; %Scaled raw xy-coords for link
fclose(fid2);
nxy = length(xyMat(:,1))-2; %Number of xy-coords for outline of link (subtract 2 to account
for 2 revolutes)
%% Process xy-coords
%Define first data point to be local origin
xOrig = xyMat(1,1)*ones(nxy+1,1); %make in to column vector of identical vals (add 1 because
we will close the outline's loop later)
yOrig = xyMat(1,2)*ones(nxy+1,1); %make in to column vector of identical vals (add 1 because
we will close the outline's loop later)
%Define second data point to be reference point (other revolute) - used to

%determine angle
xRef = xyMat(2,1)*ones(nxy,1); %make in to column vector of identical vals
yRef = xyMat(2,2)*ones(nxy,1); %make in to column vector of identical vals
%Determine link's initial angle

delx_0 = xRef(1) - xOrig(1); %distance between 2 revolute's x-coords
dely_0 = yRef(1) - yOrig(1); %distance between 2 revolute's y-coords
phi_0 = atan2(dely_0,delx_0); %initial angle of link
%Define local outline to be remaining points

xOut = xyMat(3:(nxy+2),1); %note that nxy2+2 is the length of xyMat2
yOut = xyMat(3:(nxy+2),2); %note that nxy2+2 is the length of xyMat2
%Add 1 more point to outline which equals the first point (close the loop)
xOut(nxy+1,1) = xOut(1,1);
yOut(nxy+1,1) = yOut(1,1);
%Translate links so local origin is at (0,0)

xOutTr = xOut-xOrig;
yOutTr = yOut-yOrig;
xyOutTr = [xOutTr, yOutTr]'; %combine into matrix of two row vectors
%Anti-rotate links so local x-axis is horizontal

Aneg = [cos(-phi_0) -sin(-phi_0);sin(-phi_0) cos(-phi_0)];
xyOutTrRot = Aneg * xyOutTr; %Rotated -phi_0 radians around its origin
%% Output new coords to .txt file

fid3 = fopen(filenamesOut(linkNum,:),'w');
fprintf(fid3,'%4e %4e\n',xyOutTrRot);
fclose(fid3);
close all
hold on
plot(xOutTr,yOutTr,'r')
plot(xyOutTrRot(1,:),xyOutTrRot(2,:),'b');
hold off
axis equal
pause(3)
end
%% fourBar_animate.m - animate web cutter four-bar for ME 581
% requires
% wc_q.txt - npos x 9 matrix of {q}' at sequential positions
% wc_link2.txt - n2 x 2 matrix of local vertices for link 2
% Written By: HJSIII - 03.03.18
% Adapted By: Josh Norman 05/01/12
clc; clf; clearvars -except allXs allYs; close all;
%% load q
allq = load( 'q.txt' );
[ npos, nc ] = size( allq );
%% load images of links

s2p = load( 'link2_0.txt' )'; %[ 2 x n2 ] x-y coords of link2 outline
[ nr, n2 ] = size( s2p ); %get size

%% initial image and handles

hFig2 = figure( 2 );
h2 = plot( s2p(1,:),s2p(2,:) ); %plot link 2 outline @ initial position

axis( [ -20 20 -20 20 ] )
axis square
set(gca,'fontsize',16)
xlabel('X [cm]')
ylabel('Y [cm]')
title('Four Bar Visualization')
hold on
hold off
%% CONTINUOUS LOOP
active = 1;
ncount = 1;
posSlider = 1; %seed initial value for posSlider
while active == 1
%% Run Slider GUI

hgui = slider; %open GUI
global posSlider; %allow GUI's global variable posSlider to be seen by this script
global stopButton;
ipos = round(npos*posSlider); %integer representing which position to diplay in figure
if ipos==0
ipos=1; %round up to 1 if original ipos=0 since later code cannot recognize a zeroith
index of a matrix
end
%% Plot current revolutes & references

iXs = allXs(ipos,:);
iYs = allYs(ipos,:);
hold on
plot(iXs,iYs,'.g','markersize',6)
plot(iXs(1:4),iYs(1:4),'.c','markersize',6)
hold off
%% Plot current configuration

q = allq(ipos,:)'; %pick q row from q matrix to set as current - turn in to column
vector { 9 x 1 }
% link 2
r2 = q(1:2) * ones(1,n2); %transform { 2 x 1 } row into [ 2 x n2 ] matrix - this will be
pivot point of rotation
phi2 = q(3); %angle of rotation
A2 = [ cos(phi2) -sin(phi2) ; sin(phi2) cos(phi2) ]; %rotation matrix
xy2 = r2 + A2*s2p; %link 2 outline rotated coordinates
% link 3
% link 4
% move images
set( h2, 'XData', xy2(1,:) );
set( h2, 'YData', xy2(2,:) );
set( h3, 'XData', xy3(1,:) );
set( h3, 'YData', xy3(2,:) );
set( h4, 'XData', xy4(1,:) );
set( h4, 'YData', xy4(2,:) );
% bottom of animation loop

ncount = ncount+1;
if stopButton==1
active = 0;
end
end
function varargout = slider(varargin)
% SLIDER M-file for slider.fig
% SLIDER, by itself, creates a new SLIDER or raises the existing
% singleton*.
%
% H = SLIDER returns the handle to a new SLIDER or the handle to
% the existing singleton*.
%
% SLIDER('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in SLIDER.M with the given input arguments.
%
% SLIDER('Property','Value',...) creates a new SLIDER or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before slider_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to slider_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES
% Edit the above text to modify the response to help slider
% Last Modified by GUIDE v2.5 30-Apr-2012 19:29:11
% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @slider_OpeningFcn, ...
'gui_OutputFcn', @slider_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end
if nargout
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT
% --- Executes just before slider is made visible.

function slider_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% varargin command line arguments to slider (see VARARGIN)
% Choose default command line output for slider

handles.output = hObject;
% Update handles structure

%UIWAIT makes slider wait for user response (see UIRESUME)

%uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = slider_OutputFcn(hObject, eventdata, handles)
% varargout cell array for returning output args (see VARARGOUT);
% Get default command line output from handles structure

varargout{1} = handles.output;

function slider1_Callback(hObject, eventdata, handles)
global posSlider; %define global variable to be passed to script using this GUI
posSlider = get(hObject,'Value'); %returns position of slider
[msg, msgid] = lastwarn;
s = warning('off',msgid);
%Hints: get(hObject,'Min') and get(hObject,'Max') to determine range of slider

function slider1_CreateFcn(hObject, eventdata, handles)

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
end
% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton2 (see GCBO)
global stopButton
stopButton = 1;
Analysis of the Theo Jansen Walking Linkage
ME 581 Project Report, Spring 2012
Jesse Pentzer
May 3, 2012
1 Introduction
Since the early 1990s, a new form of artificial life has taken up residence along the coasts of the Netherlands.
Creatures such as those shown in Figure 1 can be seen wandering the beaches. The creator of these machines
is a man named Theo Jansen. Born in 1948, Theo Jansen studied physics at the University of Delft, and
has since been involved in many projects involving both art and technology. His Animaris, a term referring
to his beach animals, are large machines constructed of plastic tubing, wood, and plastic sails. Many of
the machines are powered by wind and are able to store energy in recycled plastic bottles [3]. The most
interesting part of these creations, and the topic of this paper, is the linkage that enables these machines to
walk.
A diagram of the Jansen linkage and the topology of the linkage are shown in Figure 2. The linkage
is connected to ground at two points, A and D. The one input to the linkage is the angle of Link 2. The
topology of the linkage shows that three closed loops make up the Jansen mechanism.
The following sections will describe the constraints and blueprint information used in modeling the Jansen
linkage. A comparison between a MATLAB simulation of the linkage and a SolidWorks simulation is used
to verify the accuracy of the constraints. Finally, the dynamics of the linkage are analyzed to determine the
forces at the joints and the torque required for operation.
Figure 1: Theo Jansen creations [1, 2].
J. Pentzer 1
C
G7−8 B2−7
4 3
R 7 R
2 A
E D 8 C
B 1 R 3 R
6 F
5 7 B2−3
R 5 R E
F
G R G6−8
4 2
6
8 R R R
1
D1−6 D1−4 A
H
Figure 2: Diagram and topology of the Jansen linkage.
2 Jansen Linkage Design

The linkage analyzed in this project was designed to be similar in size to those created by Jansen, but made
of aluminum instead of plastic tubing. Jansen designed the relative lengths in the linkage using a type of
genetic algorithm [3]. First, a list of random numbers were generated for each link. The group of numbers
forming linkages close to Jansen’s ideal walking curve were copied and combined into new legs. This process
repeated until the ideal numbers were found. The final eleven numbers needed to create the linkage, which
are available on his website [3], are shown in Table 1 and diagrammed in Figure 3. These linkages were
scaled such that the overall height of the final mechanism for this project was around 1.8 m, giving a scale
factor of 0.0458 for each of the eleven numbers. SolidWorks was used to design each of the linkages out of
3
4 inch thick 1060 aluminum, as shown in Figure 4.
3 Linkage Mass Properties and Dimensions

After sizing the linkages, the next step was to attach coordinate frames and determine blueprint information
for all necessary points. An exploded view of the linkage with all neccesary local frames and points is shown
in Figure 5. The origin of each local frame was placed at the center of mass of the respective link. The
position of the center of mass was determined using the “Mass Properties” tool in SolidWorks. This tool
Table 1: Linkage lengths used in the design of the Jansen linkage.
a 38 b 41.5
c 39.2 d 40.1
e 55.8 f 39.4
g 36.7 h 65.7
i 49 j 50
k 61.9 l 7.8
m 15
J. Pentzer 2
Figure 3: Diagram of Jansen linkage showing labels for each design dimension [3].
Figure 4: SolidWorks model of Jansen linkage created for this project.
J. Pentzer 3
also provides the mass, M , and moment of inertia, J, of each link about the mass center, which are given in
Table 2. Next, the “Measure” tool was used to determine the relative position of each point of interest with
respect to the mass centers. This blueprint information is given in Tables 3 and 4.
Table 2: Mass and mass moment of inertia for each link in Jansen leg.
M (kg) J (kg − m2 )
Link 2 10.41 0.34
Link 3 3.18 0.18
Link 4 5.04 0.36
Link 5 2.59 0.10
Link 6 2.59 0.10
Link 7 3.84 0.31
Link 8 5.55 0.49
Table 3: Constant local body-fixed locations of specific points for links 2-5. Units are millimeters.
{s2 }′ {s3 }′ {s4 }′ {s5 }′
A {0, 0}T
B {211.33, 0}T {352.17, 0}T
C {−352.17, 0}T {172.61, 376.81}T
D {210.67, −206.66}T
E {−354.23, −206.66}T {−277.62, 0}T
F {277.62, 0}T
G
H
Table 4: Constant local body-fixed locations of specific points for links 6-8. Units are millimeters.
{s6 }′ {s7 }′ {s8 }′
A
B {436.12, 0}T
C
D {−276.86, 0}T
E
F {−383.48, 258.31}T
T T
G {276.86, 0} {−436.12, 0} {133.66, 258.31}T
H {242.57, −423.42}T
4 Constraints
The states necessary for representing the position of the linkage, q, are
n oT
{q} = {r2 }T φ2 {r3 }T φ3 {r4 }T φ4 {r5 }T φ5 {r6 }T φ6 {r7 }T φ7 {r8 }T φ8 , (1)
J. Pentzer 4
y1′
y3′ x′2
C B B
x′3 x′1
A
y4′ Link 3
C
Link 2
y2′
x′7
Link 4
x′4 B
E D y7′
D
E
Link 7
Link 5 Link 6
y6′
y5′
G
F
x′6
x′5
y8′
F G
Link 8 x′8
Figure 5: Exploded view of Jansen linkage with coordinate systems and points labeled.
J. Pentzer 5
where ri is the position of the ith linkage mass center and φi is the angle of rotation of the ith link. There are
revolute joints located at points A-G on the Jansen linkage. The mobility equation gives 3∗(8−1)−2∗(10) = 1.
As expected, the linkage has one degree of mobility. A constant angular speed driver is placed on Link 2 so
that the mechanism is kinematically driven. The resulting constraint vector, Φ, is
   

{r1 }A − {r2 }A    {r1 } + [A1 ]{s1 }′A − {r2 } − [A2 ]{s2 }′A  
   
{r3 }B − {r2 }B  {r } + [A ]{s }′B
− {r } − [A ]{s }′B 

  
  

  
 3 3 3 2 2 2 

   
C C  ′C ′C 
{r4 } − {r3 }  {r } + [A ]{s } − {r } − [A ]{s }

  
 

  
 4 4 4 3 3 3 

   
E E  ′E ′E 
{r5 } − {r4 }  {r } + [A ]{s } − {r } − [A ]{s }

  
 

  
 5 5 5 4 4 4 

   
D D  ′D ′D 
{r4 } − {r1 }  {r4 } + [A4 ]{s4 } − {r1 } − [A1 ]{s1 } 

  
 

   
{Φ} = D D = {r6 } + [A6 ]{s6 } − {r1 } − [A1 ]{s1 }
′D ′D (2)
{r6 } − {r1 }
  
 
{r7 }B − {r2 }B  {r7 } + [A7 ]{s7 }′B − {r2 } − [A2 ]{s2 }′B 

 
 

 
 
 

   
G G  ′G ′G 
{r } − {r } {r } + [A ]{s } − {r } − [A ]{s }

  
 

 7 6 
 
 7 7 7 6 6 6 

   
F F  ′F ′F 
{r8 } − {r5 }  {r8 } + [A8 ]{s8 } − {r5 } − [A5 ]{s5 } 

  


  
 

   
G G  ′G ′G 
{r } − {r } {r } + [A ]{s } − {r } − [A ]{s }

  
 

 8 6 
 
 8 8 8 6 6 6 

  
 
φ2 − φ2,start − ω2 φ2 − φ2,start − ω2 t
  
where φ2,start is the initial attitude of Link 2, ω2 is the angular speed of Link 2, and [Ai ] is the attitude
matrix of the ith link, given by " #
cos φi − sin φi
[Ai ] = . (3)
sin φi cos φi
The Jacobian of the constraint matrix, given in (5), is a repetition of similar pieces because all joints are
revolutes. The Jacobian is a 21x21 square matrix, so this is a kinematically driven analysis. The only new
variable in the Jacobian is Bi , which is calculated using
" #" #
0 −1 cos φi − sin φi
[Bi ] = . (4)
1 0 sin φi cos φi
J. Pentzer 6
J. Pentzer
 
−[I2 ] −[B2 ]{s2 }′A [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ]
 

 −[I2 ] −[B2 ]{s2 }′B [I2 ] [B3 ]{s3 }′B [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] 

[02x2 ] [02x1 ] −[I2 ] −[B3 ]{s3 }′C [I2 ] [B4 ]{s4 }′C [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ]
 
 
 ′E ′E


 [02x2 ] [02x1 ] [02x2 ] [02x1 ] −[I2 ] −[B4 ]{s4 } [I2 ] [B5 ]{s5 } [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] 

[02x2 ] [02x1 ] [02x2 ] [02x1 ] [I2 ] [B4 ]{s4 }′D [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ]
 
 
 
[Φq ] = 
 [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [I2 ] [B6 ]{s6 }′D [02x2 ] [02x1 ] [02x2 ] [02x1 ] 

[02x2 ] [02x1 ] −[I2 ] −[B2 ]{s2 }′B [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [I2 ] [B7 ]{s7 }′B [02x2 ] [02x1 ]
 
 
 
′G

 [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] −[I2 ] −[B6 ]{s6 } [I2 ] [B7 ]{s7 }′G [02x2 ] [02x1 ] 

[02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] −[I2 ] −[B5 ]{s5 }′F [02x2 ] [02x1 ] [02x2 ] [02x1 ] [I2 ] [B8 ]{s8 }′F
 
 
 
′G

 [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] [02x2 ] [02x1 ] −[I2 ] −[B6 ]{s6 } [02x2 ] [02x1 ] [I2 ] [B8 ]{s8 }′G 

[01x2 ] 1 [01x2 ] [01x1 ] [01x2 ] [01x1 ] [01x2 ] [01x1 ] [01x2 ] [01x1 ] [02x2 ] [02x1 ] [01x2 ] [01x1 ]
(5)
7
With the constraint vector and Jacobian matrix known, the position of the linkage can now be calculated
using the Newton-Raphson method:
{qk } = {qk−1 } − [Φq ]−1 Φ. (6)
To calculate velocity and acceleration, {q̇} and {q̈}, the right hand sides of equations
[Φq ]{q̇} = ν (7)
and
[Φq ]{q̈} = γ (8)
must be found. The velocity right hand side, ν, is given by

( )
020x1
{ν} = (9)
w2
and the acceleration right hand size, γ, is given by

 

 −φ̇22 A2 s′A 2 

 
φ̇23 A3 s′B 2
3 − φ̇2 A2 s2
′B

 


 

 
φ̇24 A4 s′C 2
4 − φ̇3 A3 s3
′C

 


 

 
2 2
φ̇5 A5 s5 − φ̇4 A4 s′E
′E

 


 4 

 
2 ′D
φ̇ A s

 


 4 4 4 

{γ} = φ̇26 A6 s′D
6 . (10)
 
2 2
φ̇7 A7 s′B − φ̇ ′B
2 A2 s2

 


 7 

 
φ̇27 A7 s′G 2
7 − φ̇6 A6 s6
′G

 


 

 
φ̇28 A8 s′F 2
8 − φ̇5 A5 s5
′F

 


 

 
φ̇28 A8 s′G 2
8 − φ̇6 A6 s6
′G

 


 

 
0
 
The velocity and acceleration are then solved for by simply multiplying the right hand sides by the inverse
of the Jacobian.
5 Analysis Results
This section is discussed in two parts. First, the position, velocity, and acceleration results are presented.
Next, the dynamic analysis is discussed.
5.1 Position, Velocity, Acceleration

The end of the “toe”, or point H, is the most important part of the linkage from a position standpoint. This
is the part of the mechanism that touches the ground and allows the machine to walk. A plot of the linkage
in black with a trace of the toe position in blue is shown in Figure 6. This trace shows the stepping motion
of the linkage, which is extremely flat during the bottom portion. In the MATLAB simulation, there was a
variation of approximately 1 mm during the flat portion. Obviously, the simulation assumes links of perfect
lengths and perfect construction, which would be impossible when actually constructing the linkage.
J. Pentzer 8
1
0.5
Global Y (m)
0
−0.5
−1
−1.5
−2 −1.5 −1 −0.5 0 0.5
Global X (m)
Figure 6: Plot of Jansen linkage in black with trace of toe position in blue.
To verify that the Jansen MATLAB simulation was working correctly it was compared with a SolidWorks
kinematic solution. After designing the linkage in SolidWorks, it is fairly simple plot the location of a point
over time and then export this data to a comma separated variable file, easily importable into MATLAB. A
comparison of MATLAB and SolidWorks results is given in Figure 7. The plot shows the global Y position
of the mass center of Link 8 over time. The two simulations agree extremely well.
Another aspect of the Jansen linkage kinematics that is interesting to analyze is the relationship between
vertical position of the toe and the velocity of the toe in the global X direction. For a smooth stepping
motion, it is desirable that the speed of the toe be fairly constant while it is in contact with the ground.
A comparison of the toe velocity in the global X direction and the toe position in the global Y direction
is shown in Figure 8. The toe is on the ground during the flat portion of the position trace near -1.29 m.
During the step, the velocity varies from 1.6 m/s to 2.1 m/s. The velocity is not constant during the step,
but it is changing slowly and the variation is small.
5.2 Joint Forces and Torques

The first step in analyzing the forces and torques action in the Jansen linkage is defining Q vector, which
contains the outside forces acting on each of the links. When no part of the leg is touching the ground, the
J. Pentzer 9
−0.6
Global Y of Link 8 Center of Mass (m)

MATLAB
−0.65 SolidWorks
−0.7
−0.75
−0.8
−0.85
−0.9
−0.95
−1
0 1 2 3 4 5
Time (s)
Figure 7: Comparison of SolidWorks and MATLAB simulation results.

Velocity of Point H in Global X Direction (m/s)
Position of Point H in Global Y Direction (m)
3 −0.9
2 −0.95
1 −1
0 −1.05
−1 −1.1
−2 −1.15
−3 −1.2
−4 −1.25
−5 −1.3
0 100 200 300
Link 2 Angle (deg)
Figure 8: Comparison of toe velocity, in blue, and toe position, in green.
J. Pentzer 10
only outside forces are the weights of the links, and Q is defined as
n oT 


 0 −M2 g 0  


 n oT 

 
0 −M3 g 0 

 

 



 n oT 





 0 −M4 g 0  

n oT 

{Q} = 0 −M5 g 0 . (11)

 n oT 
 
0 −M6 g 0 

 

 


 n oT 

 
 0 −M7 g 0 

 
 



 n oT 


0 −M8 g 0
 
When the toe, or point H is in contact with the ground, there is another external force pushing upwards.
To determine the magnitude of this force, I first determined how many legs would be required so that at
least one toe is in contact with the ground at all times. The simulation showed that the toe is in contact
with the ground for approximately 90◦ of rotation of the input crank, Link 2. Therefore, to maintain contact
with the ground throughout the input crank rotation, four legs would be required. The external force, F ,
added at the toe when in contact with the ground was defined as the weight of four mechanisms,
F = 4g(M2 + M3 + M4 + M5 + M6 + M7 + M8 ) (12)
The resulting Q matrix is

 n oT 


 0 −M2 g 0 



 n oT 

 
0 −M3 g 0

 


 



 n oT 





 0 −M4 g 0 



 n oT 
{Q} = 0 −M5 g 0 . (13)

 n oT 

 
0 −M6 g 0

 


 


 n oT 

 
0 −M7 g 0

 


 


 n oT 

−M8 g + F {r8H } × {0 F 0}T
 
0
 
The cross product term is added to account for the moment about the mass center of Link 8 caused by the
force F . The calculated torque required to drive the linkage at a constant input speed of 60 RPM is shown
in Figure 9. The input torque varies between 150 N-m and -150 N-m, and the contact force has a relatively
small effect on the input torque. It is interesting to note that the peak torque does not occur when the leg
is on the ground, but rather when the leg is being pulled upwards between steps.
Next, I determined the joint at which the maximum reaction forces occur. At both ground connections,
points A and D, the reaction force peaked at around 4,000 N. The maximum occurred at point A, and is
shown in Figure 10. The contact force has a much larger impact on reaction force than on input torque, as
is shown by the sharp increase in reaction force at 300◦ input angle and the drop off at 60◦ input angle.
J. Pentzer 11
200
No Contact Force
150 Contact Force
Input Torque (N-m)
100
50
−50
−100
−150
0 100 200 300
Link 2 Angle (deg)
Figure 9: Input torque required for 60 RPM operation. Simulation with contact force on toe in red, without
contact force in blue.
Magnitude of Link 2 - Link 1 Reaction Force (N)
4000
No Contact Force
3500 Contact Force
3000
2500
2000
1500
1000
500
0
0 100 200 300
Link 2 Angle (deg)
Figure 10: Magnitude of reaction force between links 2 and 1 at point A.
J. Pentzer 12
6 Conclusions
The results of this project provide a useful tool in the design of vehicle-sized Jansen linkages. The calculated
values of input torque and joint forces can be utilized to size the power source and bearings when building
the actual linkage. Also, utilizing SolidWorks in the design of the mechanism automated the calculation of
link mass, mass center, and mass moment of inertia. SolidWorks was also useful in verifying the MATLAB
simulation. SolidWorks provides outputs of position and forces, but I found that MATLAB was slightly easier
to use when looking at forces in individual joints, and when adding new forces acting on the mechanism at
certain times during operation. One area where SolidWorks excels is in animations. High quality animations
of the mechanism in motion are easy to setup. Also, by clicking on a plot in SolidWorks, the mechanism is
automatically placed in the position corresponding to that instant in time. This is very useful for determining
which positions cause maximum forces or input torques.
References
[1] http://carolineasmussen.com/wp-content/uploads/2012/04/loekvanderklis-strandbeest-06.jpeg, April
2012.
[2] http://www.inhabitat.com/wp-content/uploads/strandbeest9.jpg, April 2012.
[3] http://www.strandbeest.com/, April 2012.
J. Pentzer 13
1 %Main function for simulating Jansen mechanism
2 %Jesse Pentzer
3 %20120219
4
5 clc;
6 clear all;
7 close all;
8
9 JansenConstants;
10
11 q init = [0; 0; 90*pi/180; -0.3; 0.3; -30*pi/180; -0.6; 0; 15*pi/180; -0.8; -0.3; -70*pi/180; -0.4; -0.2; -80*pi/
12 t=0;
13
14 q = q init;
15
16 count = 1;
17
18 if exist('Plots') == 7
19 rmdir('Plots','s');
20 end
21 mkdir('Plots');
22
23 t start = 0.0;
24 t step = 0.01;
25 t end = 3;
26
27
28 q s = zeros(length(t start:t step:t end),21);
29 q dot s = zeros(length(t start:t step:t end),21);
30 q ddot s = zeros(length(t start:t step:t end),21);
31 Jac s = zeros(length(t start:t step:t end),1);
32 Time = zeros(length(t start:t step:t end),1);
33 P p = zeros(length(t start:t step:t end),1);
34 P q = zeros(length(t start:t step:t end),1);
35 V p = zeros(length(t start:t step:t end),3);
36 V q = zeros(length(t start:t step:t end),3);
37 Lambda s = zeros(length(t start:t step:t end),21);
38 H s = zeros(length(t start:t step:t end),2);
39 V s = zeros(length(t start:t step:t end),2);
40
41 for t=t start:t step:t end
42 for i = 1:20
43 [Phi, Jac] = func JansenSystem(q,t);
44
45 q = q-(Jac)ˆ-1*Phi;
46
47 end
48
49 [q dot q ddot a rhs] = func JansenVelAccel(q,Jac);
50
51
52
53 func PlotJansen(figure(1),q,count);
54 Index = find(H s(:,1)6=0);
55 plot(H s(Index,1),H s(Index,2));
56 axis([-2 0.5 -2 1]);
57 Movie(count) = getframe;
58 cla;
59
60 q s(count,:) = q';
61 q dot s(count,:) = q dot';
62 q ddot s(count,:) = q ddot';
63 Jac s(count) = det(Jac);
64 Time(count) = t;
65 [Lambda Q] = func InverseDynamics(q ddot, Jac, q);
66 Lambda s(count,:) = Lambda';
67
68 r8 = q(19:20);
J. Pentzer 14
69 p8 = q(21);
70 A8 = [cos(p8) -sin(p8); sin(p8) cos(p8)];
71 H s(count,:) = r8 + A8*s8pH;
72
73 B8 = R*A8;
74 V s(count,:) = [q dot(19); q dot(20)] + q dot(21)*B8*s8pH;
75
76 count = count + 1;
77 end
78
79
80 figure(2);
81 plot(q s(:,3),Jac s);
82 xlabel('Angle of Link 2 (rad)');
83 ylabel('Determinant of Jacobian');
84
85 Solid8 = dlmread('Link8-1.csv',',', 2, 0);
86 figure(3);
87 plot(Time,q s(:,20),'k','LineWidth',3)
88 hold on;
89 plot(Solid8(:,1),Solid8(:,2)-0.064,'r');
90 xlabel('Time (s)');
91 ylabel('Global Y of Link 8 Center of Mass (m)');
92 legend('MATLAB','SolidWorks');
93
94 figure(4);
95 plot(H s(:,1),H s(:,2));
96 func PlotJansenOnce(gcf,q s(35,:)',count)
97 xlabel('Global X (m)');
98 ylabel('Global Y (m)');
99
100 figure(4);
101 plot(H s(:,1),H s(:,2));
102 func PlotJansenOnce(gcf,q s(35,:)',count)
103 xlabel('Global X (m)');
104 ylabel('Global Y (m)');
105
106 figure(5);
107 plot(Time,V s(:,1));
108 xlabel('Time (s)');
109 ylabel('Global X Velocity (m/s)');
110
111 figure(6);
112 [AX,H1,H2] = plotyy(q s(:,3)*180/pi,V s(:,1),q s(:,3)*180/pi,H s(:,2));
113 set(get(AX(1),'Ylabel'),'String','Velocity of Point H in Global X Direction (m/s)');
114 set(get(AX(2),'Ylabel'),'String','Position of Point H in Global Y Direction (m)');
115 set(AX(1),'XLim',[0 360]);
116 set(AX(2),'XLim',[0 360]);
117 xlabel('Link 2 Angle (deg)');
118
119
120 figure(7);
121 plot(q s(:,3)*180/pi, -Lambda s(:,end));
123 ylabel('Input Torque (N-m)');
124 xlim([0 360]);
125
126 figure(8);
127 plot(q s(:,3)*180/pi, sqrt(Lambda s(:,1).ˆ2+Lambda s(:,2).ˆ2));
129 ylabel('Magnitude of Link 2 - Link 1 Reaction Force (N)');
130 xlim([0 360]);
131
132 if 0
133
134
135 figure(9);
J. Pentzer 15
139 xlim([0 360]);
140
141 figure(10);
145 xlim([0 360]);
146
147 figure(11);
151 xlim([0 360]);
152
153 figure(12);
157 xlim([0 360]);
158
159 figure(13);
163 xlim([0 360]);
164
165 figure(14);
169 xlim([0 360]);
170
171 figure(15);
175 xlim([0 360]);
176
177 figure(16);
181 xlim([0 360]);
182
183 figure(17);
187 xlim([0 360]);
188 end
1 %Blueprint information for Jansen mechanism

2 %Jesse Pentzer
3 %20120219
4
5 %Ground link information
6 r1 = [0, 0]';
7 s1pA = [0, 0]'/1000;
8 s1pD = [-535.40, -109.90]'/1000;
9
10 %Convert the solidworks dimensions into body-fixed coordinates from the
11 %center of mass.
12 %Link 2
13 L2COM = [0 0]'/1000;
14 L2A = [0 0]'/1000;
J. Pentzer 16
15 L2B = [211.33 0]'/1000;
16 s2pA = L2A - L2COM;
17 s2pB = L2B - L2COM;
18
19 %Link 3
20 L3COM = [352.17 0]'/1000;
21 L3C = [0 0]'/1000;
22 L3B = [704.34 0]'/1000;
23 s3pC = L3C - L3COM;
25
26 %Link 4
27 L4COM = [354.23 206.66]'/1000;
28 L4C = [526.84 583.47]'/1000;
29 L4D = [564.90 0]'/1000;
30 L4E = [0 0]'/1000;
31 s4pC = L4C - L4COM;
32 s4pD = L4D - L4COM;
33 s4pE = L4E - L4COM;
34
35 %Link 5
36 L5COM = [277.62 0]'/1000;
37 L5E = [0 0]'/1000;
38 L5F = [555.24 0]'/1000;
39 s5pE = L5E - L5COM;
40 s5pF = L5F - L5COM;
41
42 %Link 6
43 L6COM = [276.86 0]'/1000;
44 L6D = [0 0]'/1000;
45 L6G = [553.72 0]'/1000;
46 s6pD = L6D - L6COM;
47 s6pG = L6G - L6COM;
48
49 %Link 7
50 L7COM = [436.12 0]'/1000;
51 L7G = [0 0]'/1000;
52 L7B = [872.24 0]'/1000;
55
56 %Link 8
57 L8COM = [383.48 -258.31]'/1000;
58 L8F = [0 0]'/1000;
59 L8G = [517.14 0]'/1000;
60 L8H = [626.05 -681.73]'/1000;
61 s8pF = L8F - L8COM;
63 s8pH = L8H - L8COM;
64
65
66 %Driver information
67 p2start = 0*pi/180;
68 w2 = 60*2*pi/60;
69
70 R = [0 -1; 1 0];
71
72 A1 = eye(2);
73 B1 = R*A1;
74
75 %Indexing variables for Jacobian
76 Link2 = 1:3;
77 Link3 = 4:6;
78 Link4 = 7:9;
79 Link5 = 10:12;
80 Link6 = 13:15;
81 Link7 = 16:18;
82 Link8 = 19:21;
J. Pentzer 17
83
84
85 %Link mass properties
86 Mass2 = 10.41;
87 J2 = 0.34;
88 M2 = [Mass2 Mass2 J2];
89
90 Mass3 = 3.18;
91 J3 = 0.18;
93
94 Mass4 = 5.04;
95 J4 = 0.36;
97
98 Mass5 = 2.59;
99 J5 = 0.10;
100 M5 = [Mass5 Mass5 J5];
101
102 Mass6 = 2.59;
103 J6 = 0.10;
104 M6 = [Mass6 Mass6 J6];
105
106 Mass7 = 3.84;
107 J7 = 0.31;
108 M7 = [Mass7 Mass7 J7];
109
110 Mass8 = 5.55;
111 J8 = 0.49;
112 M8 = [Mass8 Mass8 J8];
113
114 M = diag([M2 M3 M4 M5 M6 M7 M8]);
115
116 g = 9.81;
1 function [Phi, Jac] = func JansenSystem(q,t)

2
3 %Jansen constraints and Jacobian
4
5 JansenConstants;
6
7 r2 = q(1:2);
8 p2 = q(3);
9 r3 = q(4:5);
10 p3 = q(6);
11 r4 = q(7:8);
12 p4 = q(9);
13 r5 = q(10:11);
14 p5 = q(12);
15 r6 = q(13:14);
16 p6 = q(15);
17 r7 = q(16:17);
18 p7 = q(18);
19 r8 = q(19:20);
20 p8 = q(21);
21
22
30
31 B2 = R*A2;
32 B3 = R*A3;
J. Pentzer 18
33 B4 = R*A4;
34 B5 = R*A5;
35 B6 = R*A6;
36 B7 = R*A7;
37 B8 = R*A8;
38
39
40 Phi = [r1+A1*s1pA - r2 - A2*s2pA;
41 r3 + A3*s3pB - r2 - A2*s2pB;
42 r4 + A4*s4pC - r3 - A3*s3pC;
43 r5 + A5*s5pE - r4 - A4*s4pE;
44 r4 + A4*s4pD - r1 - A1*s1pD;
45 r6 + A6*s6pD - r1 - A1*s1pD;
46 r7 + A7*s7pB - r2 - A2*s2pB;
47 r7 + A7*s7pG - r6 - A6*s6pG;
48 r8 + A8*s8pF - r5 - A5*s5pF;
49 r8 + A8*s8pG - r6 - A6*s6pG;
50 p2 - p2start - w2*t;];
51
52
53 %Develop Jacobians for each constraint
54 Jac = zeros(length(Phi), length(q));
55 %A revolute joint
56 Jac(1:2, Link2) = [-eye(2) -B2*s2pA];
57
58 %B revolute joint
59 Jac(3:4, Link2) = [-eye(2) -B2*s2pB];
60 Jac(3:4, Link3) = [eye(2) B3*s3pB];
61
62 %C revolute joint
63 Jac(5:6, Link3) = [-eye(2) -B3*s3pC];
64 Jac(5:6, Link4) = [eye(2) B4*s4pC];
65
66 %E revolute joint
67 Jac(7:8, Link4) = [-eye(2) -B4*s4pE];
68 Jac(7:8, Link5) = [eye(2) B5*s5pE];
69
70 %D revolute joint
71 Jac(9:10, Link4) = [eye(2) B4*s4pD];
72
73 %D revolute joint
74 Jac(11:12, Link6) = [eye(2) B6*s6pD];
75
76 %B revolute joint
77 Jac(13:14, Link2) = [-eye(2) -B2*s2pB];
78 Jac(13:14, Link7) = [eye(2) B7*s7pB];
79
80 %G revolute joint
81 Jac(15:16, Link6) = [-eye(2) -B6*s6pG];
82 Jac(15:16, Link7) = [eye(2) B7*s7pG];
83
84 %F revolute joint
85 Jac(17:18, Link5) = [-eye(2) -B5*s5pF];
86 Jac(17:18, Link8) = [eye(2) B8*s8pF];
87
88 %G revolute joint
89 Jac(19:20, Link6) = [-eye(2) -B6*s6pG];
90 Jac(19:20, Link8) = [eye(2) B8*s8pG];
91
92 %Driver constraint
93 Jac(21,3) = 1;
1 function [q dot, q ddot a rhs] = func JansenVelAccel(q,Jac)

2
3 %Jansen velocity and acceleration
4 JansenConstants;
5
J. Pentzer 19
6 r2 = q(1:2);
7 p2 = q(3);
8 r3 = q(4:5);
9 p3 = q(6);
10 r4 = q(7:8);
11 p4 = q(9);
12 r5 = q(10:11);
13 p5 = q(12);
14 r6 = q(13:14);
15 p6 = q(15);
16 r7 = q(16:17);
17 p7 = q(18);
18 r8 = q(19:20);
19 p8 = q(21);
20
21
29
30 B2 = R*A2;
31 B3 = R*A3;
32 B4 = R*A4;
33 B5 = R*A5;
34 B6 = R*A6;
35 B7 = R*A7;
36 B8 = R*A8;
37
38 v rhs = zeros(21,1);
39 v rhs(end) = w2;
40
41 q dot = inv(Jac)* v rhs;
42
43 p2 dot = q dot(3);
44 p3 dot = q dot(6);
45 p4 dot = q dot(9);
46 p5 dot = q dot(12);
47 p6 dot = q dot(15);
48 p7 dot = q dot(18);
49 p8 dot = q dot(21);
50
51 a rhs = zeros(21,1);
52 a rhs(1:2,1) = -p2 dotˆ2*A2*s2pA;
53 a rhs(3:4,1) = p3 dotˆ2*A3*s3pB - p2 dotˆ2*A2*s2pB;
54 a rhs(5:6,1) = p4 dotˆ2*A4*s4pC - p3 dotˆ2*A3*s3pC;
55 a rhs(7:8,1) = p5 dotˆ2*A5*s5pE - p4 dotˆ2*A4*s4pE;
56 a rhs(9:10,1) = p4 dotˆ2*A4*s4pD;
57 a rhs(11:12,1) = p6 dotˆ2*A6*s6pD;
58 a rhs(13:14,1) = p7 dotˆ2*A7*s7pB - p2 dotˆ2*A2*s2pB;
59 a rhs(15:16,1) = p7 dotˆ2*A7*s7pG - p6 dotˆ2*A6*s6pG;
60 a rhs(17:18,1) = p8 dotˆ2*A8*s8pF - p5 dotˆ2*A5*s5pF;
61 a rhs(19:20,1) = p8 dotˆ2*A8*s8pG - p6 dotˆ2*A6*s6pG;
62 a rhs(21,1) = 0;
63
64 q ddot = inv(Jac)* a rhs;
1 function [Lambda Q] = func InverseDynamics(qddot, Jac, q)

2
3 %Inverse dynamics for Jansen mechanism
4
5 JansenConstants;
6
7 Q2 = [0; -Mass2*g; 0];
J. Pentzer 20
8 Q3 = [0; -Mass3*g; 0];
9 Q4 = [0; -Mass4*g; 0];
10 Q5 = [0; -Mass5*g; 0];
11 Q6 = [0; -Mass6*g; 0];
12 Q7 = [0; -Mass7*g; 0];
13
14 r8 = q(19:20);
15 p8 = q(21);
17 H8 = r8 + A8*s8pH;
18
19 Rm = [A8*s8pH; 0];
20 F = (Mass2+Mass3+Mass4+Mass5+Mass6+Mass7)*g*4;
21
22 Mom = cross(Rm,[0;F;0]);
23 if H8(2) < -1.29
24 Q8 = [0; -Mass8*g+F; Mom(3)];
25 else
26 Q8 = [0; -Mass8*g; 0];
27 end
28 Q = [Q2; Q3; Q4; Q5; Q6; Q7; Q8];
29
30 Lambda = (Jac')ˆ(-1)*(Q-M*qddot);
1 function func PlotJansen(Handle,q,count)

2 JansenConstants;
3
4 r2 = q(1:2);
5 p2 = q(3);
6 r3 = q(4:5);
7 p3 = q(6);
8 r4 = q(7:8);
9 p4 = q(9);
10 r5 = q(10:11);
11 p5 = q(12);
12 r6 = q(13:14);
13 p6 = q(15);
14 r7 = q(16:17);
15 p7 = q(18);
16 r8 = q(19:20);
17 p8 = q(21);
18
19
27
28 hold on;
29 %Plot link 2
30 r2A = r2 + A2*s2pA;
31 r2B = r2 + A2*s2pB;
32
33 plot([r2A(1) r2B(1)],[r2A(2) r2B(2)],'r');
34
35 %Plot link 3
36 r3B = r3 + A3*s3pB;
37 r3C = r3 + A3*s3pC;
38
39 plot([r3B(1) r3C(1)],[r3B(2) r3C(2)],'r');
40
41 %Plot link 4
42 r4C = r4 + A4*s4pC;
43 r4D = r4 + A4*s4pD;
J. Pentzer 21
44
45 plot([r4C(1) r4D(1)],[r4C(2) r4D(2)],'k');
46
47 r4E = r4 + A4*s4pE;
48 r4D = r4 + A4*s4pD;
49
50 plot([r4E(1) r4D(1)],[r4E(2) r4D(2)],'k');
51 plot([r4E(1) r4C(1)],[r4E(2) r4C(2)],'k');
52
53 %Plot link 5
54 r5E = r5 + A5*s5pE;
55 r5F = r5 + A5*s5pF;
56
57 plot([r5E(1) r5F(1)],[r5E(2) r5F(2)],'r');
58
59 %Plot link 6
60 r6D = r6 + A6*s6pD;
61 r6G = r6 + A6*s6pG;
62
63 plot([r6D(1) r6G(1)],[r6D(2) r6G(2)],'r');
64
65 %Plot link 7
66 r7B = r7 + A7*s7pB;
67 r7G = r7 + A7*s7pG;
68
69 plot([r7B(1) r7G(1)],[r7B(2) r7G(2)],'r');
70
71 %Plot link 8
72 r8F = r8 + A8*s8pF;
73 r8G = r8 + A8*s8pG;
74 r8H = r8 + A8*s8pH;
75
76 plot([r8F(1) r8G(1)],[r8F(2) r8G(2)],'k');
77 plot([r8F(1) r8H(1)],[r8F(2) r8H(2)],'k');
78 plot([r8H(1) r8G(1)],[r8H(2) r8G(2)],'k');
79
80
81 axis([-2 0.5 -2 1]);
82
83 drawnow;
84 % % pause();
85 %
86 % FileName = 'WebCutter';
87 %
88 % cd('Plots');
89 %
90 % frame = getframe(Handle);
91 % im = frame2im(frame);
92 % [imind,cm] = rgb2ind(im,256);
93 % if count == 1;
94 % imwrite(imind,cm,FileName,'gif', 'Loopcount',inf);
95 % else
96 % imwrite(imind,cm,FileName,'gif','WriteMode','append');
97 % end
98
99
100 % cd('..');
101 % hold off;
102 % cla;
J. Pentzer 22
Ben Rittenhouse
Variable Stroke Mechanism

ME 581 Final Project Report
Ben Rittenhouse
INTRODUCTION
The motivation for this project was a paper by Peter L. Valentine about variable stroke mechanisms for
Ornithopters. In the paper a variable stroke mechanism is proposed that could be used to control the flapping
motion of a flying machine. In the past this type of mechanism was used in steam engines and valve gears but
could be implemented in any machine requiring variable amplitude driving action. The unique aspect of this
variable stroke mechanism is that the slider
oscillates between a fixed lower bound and a
variable upper bound.
This project analyzes the kinematics of

this variable stroke mechanism using the
iterative Newton- Raphson method outlined in
class. This project only performs the position
solutions, not the velocity and acceleration
solutions.
APPROACH
The approach for this project is very
similar to that of Computer Projects 1-3 done
for class. First, the lengths of links and the
estimated initial pose of the mechanism was FIGURE 1. VARIABLE STROKE MECHANISM.
ME 581 Final Project 1

Ben Rittenhouse
computed by digitizing points in TPS Dig. Next, each link and joint is labeled and local coordinate frames are
attached to each link. Next, the constraint vector is defined and the derivative is taken with respect to the
generalized coordinate vector to obtain the Jacobian. The final step is to program these elements into Matlab to
compute the position solution.
MECHANISM LAYOUT
In order to observe how the mechanism behaves as the lever in Fig. 1 is moved, an angle is defined as seen
in Fig. 2. The angle controls the position of point G and will determine the amplitude of the mechanism.
DIGITIZING POINTS
The global position of each point in the schematic was digitized in TPS Dig, then that data was imported into
Matlab and processed to convert pixels to centimeters and calculate the length of each link. The code for
accomplishing this is titled VSM_digitize_points.m and is presented on pgs. 15and 16 at the end of this paper.
LOCAL COORDINATE FRAMES

In order to analyze the mechanism
each link is numbered and each point is
lettered according to Fig. 2. Next, a local
coordinate frame is attached to each link
according to the figure on pg. 9. Each point
is defined according to all local coordinate
frames that it touches, i.e. point C is defined
according to the coordinate frames on links 3
and 4. The code for accomplishing this is
titled VSM_ini.m and is presented on pgs. 17
and 18 at the end of this paper.
FIGURE 2. SIMPLIFIED MECHANISM LAYOUT

Ben Rittenhouse
CONSTRAINTS
The mechanism is defined by a set of constraints such as revolute and pin in slot joints. These constraints
are written mathematically in a constraint vector and are all equal to zero when the mechanism is orientated
correctly. Next the derivative of the constraint vector is taken with respect to the generalized coordinate vector to
obtain the Jacobian. The constraint vector and Jacobain are written symbolically on pg. 10 and code for
implementing it is titled VSM_phi.m and is presented on pgs. 19 and 20 at the end of this paper.
COMPUTING POSITION SOULTION

The position solution is computed via a Newton- Raphson algothrithim for different crank positions in order
to analyize the mechanim’s behavior as it is driven through one full rotation. This process is performed many
times for different values of in order to observe how affects the performance of the mechanism. The code for
accomplishing this is titled WC_main.m and WC_main_theta.m and is presented on pgs. 23 through 26 at the end
of this paper.

Ben Rittenhouse
RESULTS
The position solutions for angles of ranging from -30 to 30 degrees is presented in Fig. 3. Figure 4 shows
the relationship between slider amplitude and . It is interesting to note that the relationship between and slider
amplitude is nearly linear. In addition, it is demonstrated that the slider has one fixed bound and one variable
bound. Also of note, the determinant of the Jacobian is on the order of -1e7 which indicates that the mechanism is
well behaved and not in danger of locking up.
Slider Amplitude as a Function of 
Slider Position as a Function of Crank Angle
14
14
= -30
12 = -24 12
= -18
10 = -12
10
= -6
Slider Amplitude (cm)

=
Slider Position (cm)
8 0
= 6 8
= 12
6
= 18 6
= 24
4
= 30
4
2
2
0
0
-2 -30 -20 -10 0 10 20 30
0 50 100 150 200 250 300 350
 (deg)
 2 (deg)
FIGURE 4.
FIGURE 3.

Ben Rittenhouse
VISUALIZATION IN WORKING MODEL

In order to better visualize what is happening in the mechanism as is changed the model was modeled in
Working Model and simulated. Figures 5 and 6 show the mechanism modeled in Working Model with equal to
-5 and -30 degrees respectively. From these plots it is easy to see why the mechanism has no amplitude at equals
30 degrees. In Fig. 6 points E and F are lined up so that the rotation of link 2 is not transmitted to the slider.
FIGURE 5.

Ben Rittenhouse
FIGURE 6.

Ben Rittenhouse
APPENDICES Variable Stroke Mechanism with all joints highlighted.

Ben Rittenhouse
Simplified Variable Stroke Mechanism layout with point G set to ground (𝜃 not included).
3 B
C
2
I A
D
7
H
5
6 4
G
F

Ben Rittenhouse
Exploded view of mechanism with local coordinates.
x’3
x’7 C3
I7 C4 B3
y’3
H7
y’7 D4 B2
D5
y’2
x’2
H6
A2
y’1
y’4 x’5 A1 x’1

E6 y’5
x’6 x’4
y’6 E5
F4 F1
G6
G1

Constraint vector, Φ, and Jacobian, Φ𝑞 , written symbolically. Ben Rittenhouse
{ } { } { }
{ } { } { }
{ } { } { }
{ } { } { }
{ } { } { } { }
{ } { }
{ } { } { } { }
{ } { } { }
{ } { } { }
{ }
{ { } } { }
[ ] [ ]{ }
[ ] [ ]{ } [ ] [ ]{ }
[ ] [ ]{ } [ ] [ ]{ }
[ ] [ ]{ } [ ] [ ]{ }
[ ] [ ]{ } [ ] [ ]{ }
[ ]
[ ] [ ]{ }
[ ] [ ]{ }
[ ] [ ]{ } [ ] [ ]{ }
[ ]
[ ]
[ ]{ }

Ben Rittenhouse
Mechanism with angle introduced, .

Ben Rittenhouse
Simplified mechanism with angle introduced.
3 B
C
2
I A
D
7
H
5
4
6
E
F
G

Ben Rittenhouse
Plot of determinant vs driver angle.
7
x 10 Determinant of Jacobian as a Function of Crank Angle
-0.95
-1
-1.05
det(JAC)
-1.1
-1.15
-1.2
-1.25
0 50 100 150 200 250 300 350
 2
(deg)

Ben Rittenhouse
Index on Matlab files:

VSM_digitize_points.m 15-16
VSM_ini.m 17-18
VSM_phi.m 19-21
VSM_kin.m 22
VSM_main.m 23-24
VSM_main_theta.m 25-26

Ben Rittenhouse
%% VSM_digitize_points.m
%Created 4/30/2012
%ME581, Final Project
%Calculates x-y positions of joints/ length of links
%% Load Data from TPS-DIG
%set current folder

cd('C:\Users\Benjamin\Dropbox\Classes\ME581\Variable Stroke Mechanism')
%parameters
p2cm = .1; %cm/pixel
%Load data from tps dig file

data = load('VSP_points_txt.txt');
%define orgin as point A

origin = data(10,:);
%define point locations wrt to origin

rI = (data(1,:)-origin)*p2cm;
rJ = (data(2,:)-origin)*p2cm;
rH = (data(3,:)-origin)*p2cm;
rG = (data(4,:)-origin)*p2cm;
rF = (data(5,:)-origin)*p2cm;
rE = (data(6,:)-origin)*p2cm;
rD = (data(7,:)-origin)*p2cm;
rC = (data(8,:)-origin)*p2cm;
rB = (data(9,:)-origin)*p2cm;
rA = (data(10,:)-origin)*p2cm;

Ben Rittenhouse
%define link dimentions

L2 = pdist([rB; rA]);
L3 = pdist([rC; rB]);
L4 = pdist([rF; rC]);
L4_D = pdist([rF; rD]);
L5 = pdist([rE; rD]);
L7 = pdist([rI; rH]);
tri6_HG = pdist([rH; rG]);
tri6_GE = pdist([rG; rE]);
tri6_HE = pdist([rH; rE]);
Lsupport = pdist([rG; rJ]);
%compute initial lever angle

theta_0 = asin((rG(1) - rJ(1))/Lsupport);
%{
%plot points
plot((data(:,1)-origin(1))*p2cm, (data(:,2)-origin(2))*p2cm,'o')
axis square
xlabel('X (cm)')
ylabel ('Y(cm)')
%}
%{
%% Compute mobility
nL = 7; %number of links
nJ1 = 8; %J1 joint
nJ2 = 1; %J2 joint
M = 3*(nL-1) - 2*nJ1 - nJ2
%}

Ben Rittenhouse
%% VSM_ini.m
%Created 4/29/2012
%Defines constraints and makes initial guesses
%parameters
d2r = pi/180;
R = [0 -1; 1 0];
w2 = 60/60*2*pi; %rev/min * min/60sec * 2pi/rev = rad/sec
%compute global position of G

xG = rJ(1) + Lsupport*sin(theta);
yG = rJ(2) - Lsupport*cos(theta);
rG_theta = [xG yG];
%constant local body-fixed locations of specific points

%blueprint info
s1pA = rA';
s1pF = rF';
s1pG = rG_theta';
s2pA = [0;0];
s2pB = [L2;0];
s3pB = [0;0];
s3pC = [0;L3];
s4pC = [0;L4];
s4pD = [0;L4_D];
s4pF = [0;0];
s5pD = [0;L5];
s5pE = [0;0];
s6pE = [tri6_GE;0];
s6pG = [0;0];
s6pH = [10.52;18.08]; %found via hand calculations
s7pH = [0;0];
s7pI = [0;L7];

Ben Rittenhouse
%estimated global pose of coordinate frames

phi2 = 46*d2r;
phi3 = 95*d2r;
phi4 = -3*d2r;
phi5 = 36*d2r;
phi6 = 18*d2r;
phi7 = 87*d2r;
r2 = rA';
r3 = rB';
r4 = rF';
r5 = rE';
r6 = rG';
r7 = rH';
%initial global coordinate vector

q2_ini = [r2; phi2];
q = [q2_ini; q3_ini; q4_ini; q5_ini; q6_ini; q7_ini];

Ben Rittenhouse
%% VSM_phi.m
%Created 4/30/2012
%Calculates constraint vector and jacobian
%rip r and phi from q vector

r2 = q(1:2);
phi2 = q(3);
r3 = q(4:5);
phi3 = q(6);
r4 = q(7:8);
phi4 = q(9);
r5 = q(10:11);
phi5 = q(12);
r6 = q(13:14);
phi6 = q(15);
r7 = q(16:17);
phi7 = q(18);
%compute A matrices
%compute B matrices
B2 = R*A2;
B3 = R*A3;
B4 = R*A4;
B5 = R*A5;
B6 = R*A6;
B7 = R*A7;

Ben Rittenhouse
%compute position information for all points wrt ground origin

r1A = s1pA;
r1F = s1pF;
r1G = s1pG;
r2A = r2 + A2*s2pA;
r2B = r2 + A2*s2pB;
r3B = r3 + A3*s3pB;
r3C = r3 + A3*s3pC;
r4C = r4 + A4*s4pC;
r4D = r4 + A4*s4pD;
r4F = r4 + A4*s4pF;
r5D = r5 + A5*s5pD;
r5E = r5 + A5*s5pE;
r6E = r6 + A6*s6pE;
r6G = r6 + A6*s6pG;
r6H = r6 + A6*s6pH;
r7H = r7 + A7*s7pH;
r7I = r7 + A7*s7pI;
%compute constraint vector

PHI = zeros(18,1);
PHI(1:2) = r2A - r1A;
PHI(3:4) = r3B - r2B;
PHI(5:6) = r4C - r3C;
PHI(7:8) = r5D - r4D;
PHI(9:10) = r6E - r5E;
PHI(11:12) = r4F - r1F;
PHI(13:14) = r6G - r1G;
PHI(15:16) = r7H - r6H;
PHI(17) = r7I(2) + 1.6;
PHI(18) = phi2 - w2*t;
resid = max(abs(PHI));

Ben Rittenhouse
%compute jacobian
int = B7*s7pI; %intermediate variable used in pin-slot constraint
JAC = zeros(18);
JAC(1:2,1:3) = [eye(2), B2*s2pA];
JAC(3:4,1:6) = [-eye(2), -B2*s2pB, eye(2), B3*s3pB];
JAC(5:6,4:9) = [-eye(2), -B3*s3pC, eye(2), B4*s4pC];
JAC(7:8,7:12) = [-eye(2), -B4*s4pD, eye(2), B5*s5pD];
JAC(9:10,10:15) = [-eye(2), -B5*s5pE, eye(2), B6*s6pE];
JAC(11:12,7:9) = [eye(2), B4*s4pF];
JAC(13:14,13:15) = [eye(2), B6*s6pG];
JAC(15:16,13:18) = [-eye(2), -B6*s6pH, eye(2), B7*s7pH];
JAC(17,16:18) = [0, 1, int(2)];
JAC(18,3) = 1;

Ben Rittenhouse
%% VSM_kin.m
%Created 5/1/2012
%Contains loop for position solution
%Does not compute velocity and acceleration solutions
%position solution
for i = 1:10
VSM_phi
q = q - JAC\PHI;
end
%take determinant of jacobian

det_JAC = det(JAC);

Ben Rittenhouse
%% WC_main.m
%Created 4/30/2012
% Run kin file inside time loop to find position info
%set current folder

% Run VSM_digitize_points.m to calculate length of links

VSM_digitize_points
%initialize global variables with blueprint info and initial guesses

theta = theta_0;
%theta = theta_0;
VSM_ini
%simulation parameters
tstep = .01;
tend = 1;
e = 1;
for t = 0:tstep:tend
VSM_kin
%save variables of interest in separate matrix
Q(e,:) = [t, phi2, r7I(1), det_JAC];
e = e+1;
end
t = Q(:,1);
phi2 = Q(:,2);
xI = Q(:,3);
det_JAC = Q(:,4);

Ben Rittenhouse
%plot results
figure(1), clf
plot(phi2/d2r, xI + 76.15)
xlabel('\phi _2 (deg)'), ylabel('Slider Position (cm)')
title('Slider Position as a Function of Crank Angle')
xlim([0 360])
ylim([-.5 10])
figure(2), clf
plot(phi2/d2r,det_JAC)
xlabel('\phi _2 (deg)'), ylabel('det(JAC)')
title('Determinant of Jacobian as a Function of Crank Angle')
xlim([0 360])

Ben Rittenhouse
%% WC_main_theta.m
%Created 4/30/2012
clc, clear all
%set current folder

% Run VSM_digitize_points.m to calculate length of links

VSM_digitize_points
%evaluate mechanism for different values of theta

d2r = pi/180;
s = 1;
for theta = linspace(-30*d2r, 30*d2r, 11)
%create new global variables based on theta

VSM_ini
%calculate position info based on theta

e = 1;
for t = 0:0.01:1
VSM_kin
%save variables of interest in separate matrix
Q(e,:) = [phi2, r7I(1)];
e = e+1;
end
phi2 = Q(:,1);
xI = Q(:,2);
Z(:,:,s) = [phi2 xI];
th(s) = theta;
s = s+1;
end

Ben Rittenhouse
%plot results
offset = 76.15;
phi2 = Z(:,1,1);
figure(1)
plot(phi2/d2r,Z(:,2,1) + offset, phi2/d2r,Z(:,2,2)+offset, phi2/d2r,Z(:,2,3)+offset,
phi2/d2r,Z(:,2,4)+offset, phi2/d2r,Z(:,2,5)+offset...
,phi2/d2r,Z(:,2,6)+offset, phi2/d2r,Z(:,2,7)+offset, phi2/d2r,Z(:,2,8)+offset,
phi2/d2r,Z(:,2,9)+offset, phi2/d2r,Z(:,2,10)+offset...
,phi2/d2r,Z(:,2,11)+offset)
legend(['\theta = ' num2str(th(1)/d2r)],...
['\theta = ' num2str(th(2)/d2r)],...
['\theta = ' num2str(th(11)/d2r)])
xlabel('\phi _2 (deg)'), ylabel('Slider Position (cm)')
title('Slider Position as a Function of Crank Angle')
xlim([0 360])
figure(2)
Amp = offset + [max(Z(:,2,1)); max(Z(:,2,2)); max(Z(:,2,3)); max(Z(:,2,4));...
max(Z(:,2,5)); max(Z(:,2,6)); max(Z(:,2,7)); max(Z(:,2,8));...
max(Z(:,2,9)); max(Z(:,2,10)); max(Z(:,2,11))];
plot(th/d2r, Amp)
xlabel('\theta (deg)'), ylabel('Slider Amplitude (cm)')
title('Slider Amplitude as a Function of \theta')
ylim([0 14])

Analysis of the Klann Linkage used for a Walking Machine
Mike Rothenberger
May 1, 2012
1 Klann Linkage
The Klann linkage was designed by Joseph Klann in 1994 to replace wheeled locomotion by creating a walking
machine that moves by replicating the gait of an animal. In his patent that was published in 2001, Klann
discussed how the linkage could be used for situations where a vehicle with wheels could not drive. The
linkage consists of two rocker arms (link 2 and 5), a leg (link 3), a cranking arm (link 6), and a connecting
arm (link 4) between the leg and the crank. The crank arm and the two rocker arms are attached to the
frame of the machine, or commonly known as the ground (link 1). Figure 1 shows that these links are
connected through a set of revolute joints.
Figure 1: The topology for a single Klann linkage.
The connections from the rocker arms and the crank shaft to the frame of the machine help determine
the final motion of the leg. By positioning these revolute joints in different locations, the reciprocal arc that
the leg follows can significantly change. For this project, these connections to ground were determined as
shown in Figure 2 and were scaled so that the machine is large enough to hold a person.
Figure 2: The three revolute joints connected to the frame.
1
Figure 3: One complete linkage with scaled dimensions for a full sized walking machine.
Figure 3 shows the required lengths of each link, as well as a required locked angle for the leg connector
link. The angle for the leg itself is not required to be locked at a specific value, but should be between 140
and 160 degrees. If the links and revolute joints on the frame are designed in this manner than the final
motion of the leg will look like the trace shown in Figure 4.
Figure 4: The trace of the motion each leg would follow for one rotation of the crankshaft.
2
2 Problem Formulation
The broad purpose of this study was to see how much discomfort a person would experience when riding this
machine and how much torque would be required to drive the crankshaft. To do this the machine needed to
be scaled large enough for a person to ride, and then simulated in a 3D environment that could apply forces
and collisions between the machine and the floor it would walk on. SolidWorks was chosen because of its
ability to meet all of the above requirements. The machine was placed on a flat surface with gravity and the
weight of a person being applied downward on the seat. All of the exact parameters are listed in Table 1.
Table 1: Parameters used for the walking machine and their values.
Parameter Value
Weight of Machine 580 lbs.
Weight of Rider 200 lbs.
Material 1060 Aluminum Alloy
Number of Motors 1
Figure 5 shows the front and side views of the walking machine with the general dimensions to give
a representation of the overall size. An aluminum alloy was chosen as the material to be used for all of
the links and pins, as well as the body and seat. A preliminary study of stresses was conducted to see if
the seat could hold the weight of a 200 lb. person and it could so the alloy was used for all parts. As a
further study it would be interesting to see if the alloy would be able to withstand all of the joint forces for
walking a significant distance. A thick sheet of rubber was placed beneath the machine, and was fixed in
the simulation, while all other pieces were free to move.
Figure 5: The dimensions for a full sized walking machine capable of carrying a person.
3
3 Solution Method
This study was completed by using the motion analysis tools available in SolidWorks with the steps listed
below:
1. Created a new motion study by right clicking the current motion study and selecting the option to
create another one.
2. Switched the animation tab to motion analysis for more accurate and realistic results.
3. Added a driving motor to one of the crank arms (All other crank arms had a parallel mate between
them and the driving crank arm to simulate one motor driving all of the arms).
Figure 6: Screenshot from SolidWorks showing the driving motor on the crank arm.
4. Added gravity in the downward direction (-y axis).
5. Added a downward force to the seat to simulate the weight of a 200 lb. person sitting.
Figure 7: Screenshot from SolidWorks showing the force applied by the seated person.
4
6. Added collision between each of the legs and the floor to keep the machine from falling through the
rubber pad.
7. Clicked the motion study parameter button and changed the animation frames per second to match
the motion analysis frames per second (25 per second).
8. Moved the diamond icon that determines how low the simulation should run to 6 seconds to allow for
2 cycles of the crank arm rotating.
9. Ran the simulation by clicking the calculate button.
After all of these steps were completed the results could be determined by clicking on the results and
plots icon in the toolbar which brings up a menu that lets you decide what to plot. Figure 8 shows that
there are many options including the displacement, velocity, and acceleration of specific links or the applied
forces and torques of motors that drive the links.
It also shows the ability to trace the path of a specific point, which was used to create Figure 4 above.
The linear displacement was helpful to determine how much the seat would move up and down, while the
linear velocity was useful in determining how each leg would speed up in the vertical direction as it struck
the ground.
Figure 8: Screenshot from SolidWorks showing the options in the Results and Plots menu.
After each plot was created, the CSV file could be saved by right clicking on the plot. This file was then
imported into MATLAB so that the more advanced plotting features could be used. Importing the data into
MATLAB required the user to go to the file menu dropdown and select import data. This would bring up
an open file menu where the data file could be selected. After this process the variable editor window would
change and show the data that could be imported.
5
4 Results and Discussion
Figure 9: Vertical displacement of the seat through one crank arm revolution.
Figure 9 shows that the vertical displacement of the seat for one cycle of the crank arm is very small. At
most the seat moves up and down 3.5 cm as the machine moves forward. This shows that the rider will only
experience some vertical travel during the trip. When the plot in Figure 10 is analyzed, it becomes clear
that when the leg accelerates downward during a step it would cause a decent amount of shock for the rider.
Figure 10: Linear velocity of a leg in the vertical direction during one cycle.
6
This model did not include any form of shock absorber for this problem and would account for a significant
jolt during the ride because the leg accelerates up to a velocity of -1 m/s before it hits the ground.
Another possible solution could be to add a motor controller to change the torque values to slow the leg
down when it hits the ground instead of driving the crankshaft at a constant velocity. It would be interesting
to see how that would change the overall forward motion of the vehicle.
Figure 11: Torque required to drive all 8 legs at the constant angular velocity of 20 rpm for both cases.
Figure 11 shows the two cases that the motor torque was recorded from. There was the no contact case
where the walking device was suspended in the air and the body was fixed so the legs could rotate freely.
The plot shows a smooth oscillation between positive and negative torques for the crankshaft because gravity
was still being applied and the weight of the leg would require the torque to change dramatically to keep a
constant angular velocity on the crank arm. The contact case was where the walking device was set on top of
the fixed floor with all of its parts free to move so it could walk forward. The torque curve shows an increase
in the peaks which would be due to the extra force applied to the seat to mimic a 200 lb. person sitting on
it. The abnormalities in the contact case are most likely from the fact that the leg had a curved bottom
which would come in contact with the floor, and SolidWorks struggled to correctly determine the collision
that would occur between these surfaces while also calculating the rest of the motion of the machine. A
possible fix for this situation would be to add another piece to the leg to act as a flat foot that would have
more surface area to contact the floor with.
5 Bibliography
J. C. Klann, “Walking Device,”U.S. Patent 6 260 862, July 17, 2001.
7
Simulating Motions
of Shoulder, Elbow, and Wrist
Yang Xu, Tao Zhou, and Yen-Hsun Wu
1 Introduction
Motion analysis systems are widely used to study human movement. From the
multi-camera setup and markers attached on body landmarks, body segments can be
tracked in three dimensional spaces. In order to extract meaningful information from
locations of the markers, it is necessary to construct a model for the body. There are
many methods for model construction. Most of them use the joint angels between two
segments as elemental variables since joints in human body were mostly treated as
rotational joint. Joint angle analysis, however, is indirect when the orientation of each
segment is the investigating focus.
In this project, we planned to apply the mechanism simulation technique on the

motion of human upper extremity. The simulation could provide deeper understanding
on the mobility, the kinematic constraint, and driver constraint of the human arm.
Through the modeling process, the structure of the upper limb was inspected
thoroughly. The assumption for modeling each body segment can be verified from the
result of simulation. The motion analysis system was used to validate the simulation
result. The comparison between the simulated results and recorded data would give
certain degrees of discrepancy. Based on the discrepancy, it is possible to refine the
assumptions of modeling and to gain insightful information on human bodies.
The project consists of two sections, the modeling and the data recording. The
modeling and simulation procedures followed the technique learnt in the class. The
system first was digitized from the motion analysis system in three dimension
coordinate environment. The local coordinates were then glued to individual segments.
The blue print information was obtained from the digitized sample. The initial
position was used as the initial guess. The mobility of the whole system was more
than one as result more than one driver constraints were needed.
After finish the kinematic driven study, the procedure may be used in investigating the
kinematic properties of a system consisting of both human and robot. Hopefully, it
could provide more insightful information on how human controls their bodies.
2 Equipment Setup
2.1 HapticMaster
HapticMaster is developed by FCS Control Systems (Netherland). It is a three DOF,

force-controlled haptic interface as shown in Fig 1. It provides the user with a crisp
haptic sensation and the power to closely simulate the weight and force found in a
wide variety of human tasks.
Figure 1. The actuator arrangement and the kinematics of the HapticMaster
The mechanism of the robot arm is built for zero backlash, which yields some friction
in the joints. However, the friction is completely eliminated by the control loop, up to
the accuracy of the force sensor. The result is a near backlash-free and smooth moving
behavior at the end effector.
The workspace of the HapticMaster is depicted in Fig. 2. The kinematic chain from
the bottom up yields: base rotation, arm up/down, arm in/out, illustrated in Fig. 1.
This makes 3 degrees of freedom at the end effector, which spans a volumetric
workspace.
Figure 2. The end effector plate workspace of the HapticMaster spans a 3-dimensional space with a volume of
approximately 80 liters.
2.2 Gimbal
The HapticMaster is designed for exchangeable end effectors, to match an appropriate

end effector to the end application. For instance an end effector with 3 additional
rotations can be mounted at the end plate of the robot arm. The gimbal we use can
provide angle of yaw, pitch and roll. The unit of the data is radian.
Figure 3. 3 DOF gimbal
2.3 Qualysis Tracking Manager
Three dimensional (3-D) kinematic data were collected with five motion capture
cameras (figure 2, Qualysis, Gothenburg, Sweden) .Reflective markers were attached
to subject and robot arms with tape. Calibration was conducted before data recording.
Accuracy of each marker was limited within 2mm. Two markers are used to indicate
the center of shoulder, elbow and wrist joints respectively. One was used to indicate
the movement of hand, two to indicate the handle and two to indicate the movement
of robot arms. X axis was set in the frontal axis of subject with right side as positive
direction. Y axis was at the sagittal axis with the anterior side as positive direction,
and positive Z direction was set at upward direction.
Figure 4. Kinematics data capture from five camera
Figure 5. Result of kinematics data capture
3 Method
Here, we strap subject’s trunk. So the truck cannot move and shoulder cannot translate.
Then we take shoulder as spherical joint, elbow as revolute joint and wrist as
universal joint, shown in Fig 6. As for the gimbal, it is 3 DOF device which is a
combination of revolute joint and universal joint. As for the robot, for simplicity, the
rotation movement and translation downwards and upwards are limited. Thus, robot
only moves forwards and backwards, which we take it as prismatic joint. So the
mechanism
chanism contains six joints, as shown in Fig 7. We use clock time to synchronize
the data collected from the robot and tracking system’ s data.
For the system, nL = 6; nJ1 = 3; nJ2 = 2; nJ3 = 1;
Mobility: M = 6 × (nL - 1) – 5×nJ3 – 4×nJ2 – 3×nJ3= 6×5 – 5×3 – 4×2 – 3×1 = 4
Fig 6. structure of upper extremity

Fig 7. mechanism of robot-induced movement system
Fig 8. Generalized coordinates
4 Results and Discussion

Fig 9 – Fig 11 shows the trajectory of elbow. From the figures, we can see that during
forward and backward movement, elbow would waggle and its trajectory does not show
apparent pattern. However, the trajectories of elbow in y and Z direction show typical sine wave.
The possible reason is that brain has better control performance along the two directions.
As for the kinematic part, the constraints did not converge. The possible reason is that we
oversimplify human upper limb. For example, we suppose wrist does not move upward and
downward. That is the reason why we suppose wrist joint can be considered as universal joint.
Then for the elbow joint, when it rotates, there is certain angle between upper arm and forearm. It
is not a revolute joint.
Fig 9. trajectory of elbow in x direction Fig 10. trajectory of elbow in y direction
Fig 11. trajectory of elbow in Z direction

Appendix
ଵ ஺ ଶ ஺
, ଵ ଶ , " 2, # 1

ଶ ஻ ଷ
஻
$ % %, ଶ ଷ , " 3, # 2
ଶ
'ଷ′ ( ଶ′ ଷ ଶ , " 3, # 2
்
ଷ ଶ

ଷ
ଷ′ ( ଶ′ ଷ ଶ , " 3, # 2

்

ଷ ஼ ସ ஼
)*" + +, ଷ ସ , " 4, # 3
ଷ 'ସ′ ( ଷ′
ସ ଷ , " 4, # 3
்

ସ
ସ ஽ ହ ஽ $ - -, ସ ହ , " 5, # 4

ହ ் ସ
ହ′ ( 'ସ′ ହ ସ " 5, # 4
,
ହ ହ′ ( 'ସ′ ହ ସ , " 5, # 4
்

ସ
ହ ா ଺ ா )*" / /, ହ ଺ , " 6, # 5

଺ ହ ଺′ ( ହ′ ଺ ହ , " 6, # 5
்

Φଷହൈଵ ଵ ଵ′ ( '଺′ 1, ଵ ଺ , " 1, # 6

ଵ ் ଺
்
଺
ଵ′ ( ଺′ ଵ ଺ , " 1, # 6

ଵ ଵ଺
'ଵ′ ( ଵ଺
ாி
ଵ଺ ଺ ா ଵ ி , " 1, # 6

் ாி
ଵ ଵ଺ ଵ′ ( ଵ଺ா

ଵ଺ ଺ ா ଵ ி , " 1, # 6
ாி
் ாி

ாி
ଵ ଵ′ ( '଺′ ଵ '଺ , " 1 # 6

்

଺
ଶ ଶ 1 /)2$ 334$$ ଶ

ଷ ் ଷ 1 /)2$ 334$$ ଷ
்
ସ ் ସ 1 /)2$ 334$$ ସ

் 1 /)2$ 334$$ ହ
ହ ் ହ
଺ ଺ 1 /)2$ 334$$ ଺

ସ ସ଴ "$ "$
ହ ହ

଺ ଺ ଺
଴
1
଴
ଷ଴
ଷ
2 ଶ
 −[ I 3 ] 2[ A2 ][ S%2 ]' A [G2 ] 03*3 03*4 03*3 03*4 03*3 03*4 03*3 03*4 
 
 3[ I ] − 2[ A2 ][ S% ]' B [G ]
2 2 − [ I 3 ] 2[ A 3 ][ % ]' B [G ]
S 3 3 03*3 0 3*4 03*3 0 3*4 03*3 03*4 
 01*3 −2{g3 }' T [ A3 ]T [ A2 ][h%2 ]' [G2 ] 01*3 −2{h2 } [ A2 ] [ A3 ][ g3 ] [G3 ] 01*3
'T T
% '
01*4 01*3 01*4 01*3 01*4 
 
 01*3 −2{ f 3 } [ A3 ] [ A2 ][h2 ] [G2 ] −2{h2 }' T [ A2 ]T [ A3 ][ f%3 ]' [G3 ] 01*3
'T T % ' 01*3 01*4 01*3 01*4 01*3 01*4 
 03*3 03*4 [I3 ] −2[ A3 ][ S%3 ]' C [G3 ] −[ I 3 ] 2[ A4 ][ S%4 ]' C [G4 ] 03*3 03*4 03*3 03*4 
 
 01*3 01*4 01*3 −2{ f 4 }' T [ A4 ]T [ A3 ][h%3 ]' [G3 ] 01*3 −2{h3 }' T [ A3 ]T [ A4 ][ f%4 ]' [G4 ] 01*3 01*4 01*3 01*4 
0 03*4 03*3 03*4 [I3 ] −2[ A4 ][ S%4 ]' D [G4 ] −[ I 3 ] 2[ A5 ][ S%5 ]' D [G5 ] 03*3 03*4 
 3*3

 01*3 01*4 01*3 01*4 01*3 −2{g5 }' T [ A5 ]T [ A4 ][ f%4 ]' [G4 ] 01*3 −2{ f 4 }' T [ A4 ]T [ A5 ][ g% 5 ]' [G5 ] 01*3 01*4 
0 0 0 0 0 −2{h }'T
[ A ]T
[ A ][ %
f ]'
[G ] 0 − 2{ f }'T
[ A ]T
[ A ][ % ]' [G ]
h 0 01*4 
 1*3 1*4 1*3 1*4 1*3 5 5 4 4 4 1*3 4 4 5 5 5 1*3

 03*3 03*4 03*3 03*4 03*3 03*4 [I3 ] −2[ A5 ][ S%5 ]' E [G5 ] −[ I 3 ] 2[ A6 ][ S%6 ] [G6 ]
' E

0 01*4 01*3 01*4 01*3 01*4 01*3 −2{h6 }' T [ A6 ]T [ A5 ][h%5 ]' [G5 ] 01*3 −2{h5 }' T [ A5 ]T [ A6 ][h%6 ]' [G6 ]
 1*3
 01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 01*3 −2{g1 }' T [ A1 ]T [ A6 ][ f%6 ]' [G6 ]
 
 01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 01*3 −2{g1 }' T [ A1 ]T [ A6 ][h%6 ]' [G6 ]
 01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 −{h1 }' T [ A1 ]T 2{ f1}' T [ A1 ]T [ A6 ][ s%6 ]' F [G6 ] 
 
 01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 −{ f1 }' T [ A1 ]T 2{h1 }' T [ A1 ]T [ A6 ][ s%6 ]' F [G6 ] 

J =  01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 01*3 −2{h1 }' T [ A1 ]T [ A6 ][ f%6 ]' [G6 ] 

 01*3 2{ p2 }T 01*3 01*4 01*3 01*4 01*3 01*4 01*3 01*4 
0 01*4 01*3 2{ p3 }T 01*3 01*4 01*3 01*4 01*3 01*4 
 1*3

 01*3 01*4 01*3 01*4 01*3 2{ p4 }T 01*3 01*4 01*3 01*4 
0 01*4 01*3 01*4 01*3 01*4 01*3 2{ p5 }T 01*3 01*4 
 1*3 
 01*3 
T
01*4 01*3 01*4 01*3 01*4 01*3 01*4 01*3 2{ p6 }
 
 01*3 01*4 01*3 01*4 01*3 [−2 / 1 − p 4(1) 0 0 0] 01*3
2
01*4 01*3 01*4 
 
 01*3 01*4 01*3 01*4 01*3 01*4 01*3 [ −2 / 1 − p5(1) 0 0 0] 2
01*3 01*4 
0 01*4 01*3 01*4 01*3 01*4 01*3 01*4 (0,1, 0) 01*4 
 1*3

 01*3 01*4 (0, 0,1) 01*4 01*3 01*4 01*3 01*4 01*3 01*4 
 
 
 
 
 
 
 
 
 

SP 12

Uploaded by

Copyright:

Available Formats

You might also like

SP 12

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

SP 12

Uploaded by

Copyright:

Available Formats

Kristopher Doll

Introduction and Summary:

Three wheeled human powered vehicles offer a number of

A possible design of a tilting tricycle has been created (figure 1)

A solidworks model was created to explore the effect of

Methodology and Description of Model:

The MATLAB model features the following links:

The position/orientation vector is as follows:

The following points are used in the model:

Description of Coordinate Frames

The global coordinate frame

Not all of the coordinate frames are

Link 2 coordinate frame origin is at point

The local coordinate frame of link 4 is located at point D

Link 6 frame is centered about the intersection of the

The link 8 local frame is centered at the mass center of

The local coordinate locations of points on each link is

The MATLAB model in this project does not allow for

Measured performance variables:

Track will decrease by approx. 1cm

Another problem identified using this

To correct both of these issues, the

It has been shown that this MATLAB

%Establish initial locations

%Calculate each wheel angle

%Ackermann Steering Error based on left wheel

d2r=pi/180; %Conversion factor for degrees to radians

%Global Coordinate frame origin is at the projection of the center of the

%Input Vehicle parameters, meters, angles in radians

%original inclination of kingpin from vertical in the X-Y plane

%Constant Local Body-Fixed Locations of Points, m

%Estimated Global Position/Orientation of Coordinate Frames

q=[q2_ini; q3_ini; q4_ini; q5_ini; q6_ini; q7_ini; q8_ini; q9_ini];

%Double Prime coordinate frames in terms of local body coordinates.

%Make skew symmetric matrices

f4p2s=skew_sym(f4p2); g4p2s=skew_sym(g4p2); h4p2s=skew_sym(h4p2);

%Create the image of the tires

%Rip r and phi from q

%Double Prime frames in global coordinates

%Compute point locations in global coordinates

%Ground Contact at A and No Forward Motion

%Create the Jacobian Matrix

%Revolute at H, i=4, j=6

%Revolute at L, i=6, j=8

%Double Spherical CF i=2, j=4

%Ground Contact at A and No Forward Motion

%Point B and z2 constrainted in YZ plane

Figure 1: Example of a Recumbent Trike

Figure 2: Suspension Components

Figure 3: Kinematic Constraints

Figure 4: Bump Steer Measurement

Connection Reaction at A Connection Reaction at B

Figure 5: Connection Reaction Force Lower A-Arm (Link 2)

Connection Reaction at Joints D and G

Connection Reaction at G Connection Reaction at D

Figure 6: Connection Reaction Force, Upper A-Arm (Link 5)

Figure 7: Reaction Force Direction, Upper A-Arm (Link 5)