heelstrikepwJc.m

```
function [xnew,lambdas] = heelstrikepwJc(w, xminus)
%deneme heel strike, bu gercek bir fonsikyon degil sonra sil
% calculates the new state following foot contact.
% Angular momentum is conserved about the impact point for the
% whole machine, and about the hip joint for the trailing leg.
% After conservation of angular momentum is applied, the legs
% are switched.
% State vector: qstance, qswing, qdotstance, qdotswing
%
% This version uses the constraint Jacobian, and allows for an
% optional second output, lambdas
% by Art Kuo for ME 646
% Expects the following globals: L gamma Il C M alpha R Mp Ip g
L = 1
gamma = 0
Il = 0
C = 0
M = 1
R = 0
Mp = 1
Ip = 0
sg = sin(gamma);
cg = cos(gamma);
q1 = xminus(1); q2 = xminus(2); % q1 is angle of stance leg ccw wrt vertical
u1 = xminus(3); u2 = xminus(4); % q2 is angle of swing leg ccw wrt vertical
q2 = -q1;
c1 = cos(q1); c2 = cos(q2); c12 = cos(q1-q2);
s1 = sin(q1); s2 = sin(q2); s12 = sin(q1-q2);
% The constraint Jacobian will be used to perform the augmented
% Newton-Euler method, in terms of the "maximal" coordinates X.
% But since our simulation already uses the minimal state vector x,
% we first need to convert from x to X:
% Calculate pose Jacobian, giving center of mass motions of each segment
% so that Jp*u yields something like this:
% [x1dot; y1dot; th1dot; xpdot; ypdot; thpdot; x2dot; y2dot;
% th2dot]
% referring to accelerations in x, y, and theta for segments 1, 2 and
% the pelvis p (which we're treating as a separate segment)
Jp = [-(R+(C-R)*c1) 0 ; % velocity of stance leg COM x
-(C-R)*s1 0 ; % velocity of stance leg COM y
1 0 ; % angular velocity of stance leg
-(R+(L-R)*c1) 0 ; % velocity of pelvis x
-(L-R)*s1 0 ; % velocity of pelvis y
1 0 ; % angular velocity of pelvis
-(R+(L-R)*c1) (L-C)*c2 ; % velocity of swing leg COM x
-(L-R)*s1 (L-C)*s2 ; % velocity of swing leg COM y
0 1 ]; % angular velocity of swing leg
% Let Xdot be the vector containing the full velocities of all segments:
% stance leg, pelvis, and swing leg.
Xdotold = Jp * [u1;u2];
% Now perform calculations for Xdotnew, using a big mass matrix:
bigM = diag([M M Il Mp Mp Ip M M Il]); % diagonal matrix of segment masses
% Notice that bigM * Xdotold is the momentum of the system before impact
% Let's also put together a matrix of constraints for what happens after
% the system hits the ground. We want the segments still to be stuck
% together, and we want the leading foot to be stuck to the
% ground after an inelastic impact. Set up a constraint Jacobian so that the
% constraints are satisfied with Jc * Xdot = 0
% where Jc treats all degrees of freedom in terms of before impact
Jc = [1 0 -(L-C)*c1 -1 0 0 0 0 0 ; % trailing leg glued to pelvis
0 1 -(L-C)*s1 0 -1 0 0 0 0 ; %
0 0 1 0 0 -1 0 0 0 ; % pelvis rotates with leg
0 0 0 -1 0 0 1 0 -(L-C)*c2; % leading leg glued to pelvis
0 0 0 0 -1 0 0 1 -(L-C)*s2;
0 0 0 0 0 0 1 0 R+(C-R)*c2; % leading foot x glued to ground
0 0 0 0 0 0 0 1 (C-R)*s2 ]; % leading foot y glued to ground
% Note that gravity produces no impulse during the collision
rhs = [bigM*Xdotold; zeros(7,1)];
blockmatrix = [bigM Jc'; Jc zeros(7,7)];
blocklhs = blockmatrix \ rhs; % solve for the new velocities
% blocklhs contains the new Xdots, plus the constraint forces
Xdotnew = blocklhs(1:9);
lambdas = blocklhs(10:end); % these are internal forces lambda
% The Xdots of most interest are the angular velocities of
% the trailing and leading legs
utrail = Xdotnew(3);
ulead = Xdotnew(9);
unew = [ulead; utrail];
xnew = [xminus(2); xminus(1); unew(1); unew(2)];
% be sure test whether this produces the correct output
```