Basics of Rigid Body Kinematics

inertial frame -> fixed ->
moving frame -> point 's motion has two parts: translation and rotation of the frame

Rotations: typo?? -> ???

r -> coordinate in x-y Cartesian
q -> local positions and angles

fix some point and body moving up

---

% rotational matrices calculated in previous problem set
R_B1 = [1,0,0;0,cos(alpha),-sin(alpha);0,sin(alpha),cos(alpha)];
R_12 = [cos(beta),0,sin(beta);0,1,0;-sin(beta),0,cos(beta)];
R_23 = [cos(gamma),0,sin(gamma);0,1,0;-sin(gamma),0,cos(gamma)];

alpha = sym('alpha','real');
beta = sym('beta','real');
gamma = sym('gamma','real');

% rotational matrices calculated in previous problem set
R_B1 = [1,0,0;0,cos(alpha),-sin(alpha);0,sin(alpha),cos(alpha)];
R_12 = [cos(beta),0,sin(beta);0,1,0;-sin(beta),0,cos(beta)];
R_23 = [cos(gamma),0,sin(gamma);0,1,0;-sin(gamma),0,cos(gamma)];

% write down the 3x1 relative position vectors for link length l_i=1
r_B1_inB = [0;1;0];
r_12_in1 = [0;0;-1];
r_23_in2 = [0;0;-1];
r_3F_in3 = [0;0;-1];

% write down the homogeneous transformation matrices
H_B1 = [1,0,0,0;
        0,cos(alpha),-sin(alpha),1;
        0,sin(alpha),cos(alpha),0;
        0,0,0,1];
H_12 = [cos(beta),0,sin(beta),0;
        0,1,0,0;
        -sin(beta),0,cos(beta),-1;
        0,0,0,1];
H_23 = [cos(gamma),0,sin(gamma),0;
        0,1,0,0;
        -sin(gamma),0,cos(gamma),-1;
        0,0,0,1];

% create the cumulative transformation matrix
H_B3 = H_B1*H_12*H_23; 

% find the foot point position vector
r_BF_inB = r_B1_inB + R_B1*r_12_in1 + R_B1*R_12*r_23_in2 + R_B1*R_12*R_23*r_3F_in3;

r_BF_inB = @(alpha,beta,gamma)[...
    -sin(beta + gamma) - sin(beta);...
  sin(alpha)*(cos(beta + gamma) + cos(beta) + 1) + 1;...
  -cos(alpha)*(cos(beta + gamma) + cos(beta) + 1)];
 
J_BF_inB = @(alpha,beta,gamma)[...
                                              0,             - cos(beta + gamma) - cos(beta),            -cos(beta + gamma);...
 cos(alpha)*(cos(beta + gamma) + cos(beta) + 1), -sin(alpha)*(sin(beta + gamma) + sin(beta)), -sin(beta + gamma)*sin(alpha);...
 sin(alpha)*(cos(beta + gamma) + cos(beta) + 1),  cos(alpha)*(sin(beta + gamma) + sin(beta)),  sin(beta + gamma)*cos(alpha)];

% given are the functions 
%   r_BF_inB(alpha,beta,gamma) and
%   J_BF_inB(alpha,beta,gamma) 
% for the foot positon respectively Jacobian
  
r_BF_inB = @(alpha,beta,gamma)[...
    - sin(beta + gamma) - sin(beta);...
  sin(alpha)*(cos(beta + gamma) + cos(beta) + 1) + 1;...
  -cos(alpha)*(cos(beta + gamma) + cos(beta) + 1)];

J_BF_inB = @(alpha,beta,gamma)[...
                                              0,             - cos(beta + gamma) - cos(beta),            -cos(beta + gamma);...
 cos(alpha)*(cos(beta + gamma) + cos(beta) + 1), -sin(alpha)*(sin(beta + gamma) + sin(beta)), -sin(beta + gamma)*sin(alpha);...
 sin(alpha)*(cos(beta + gamma) + cos(beta) + 1),  cos(alpha)*(sin(beta + gamma) + sin(beta)),  sin(beta + gamma)*cos(alpha)];

% write an algorithm for the inverse differential kinematics problem to
% find the generalized velocities dq to follow a circle in the body xz plane
% around the start point rCenter with a radius of r=0.5 and a 
% frequeny of 0.25Hz. The start configuration is q =  pi/180*([0,-60,120])',
% which defines the center of the circle
q0 = pi/180*([0,-60,120])';
dq0 = zeros(3,1);
rCenter = r_BF_inB(q0(1),q0(2),q0(3));
radius = 0.5;
f = 0.25;
rGoal = @(t) rCenter + radius*[sin(2*pi*f*t),0,cos(2*pi*f*t)]';
drGoal = @(t) 2*pi*f*radius*[cos(2*pi*f*t),0,-sin(2*pi*f*t)]';

% define here the time resolution
deltaT = 0.01;
timeArr = 0:deltaT:1/f;

% q, r, and rGoal are stored for every point in time in the following arrays
qArr = zeros(3,length(timeArr));
rArr = zeros(3,length(timeArr));
rGoalArr = zeros(3,length(timeArr));

q = q0;
dq = dq0;
for i=1:length(timeArr)
    t = timeArr(i);
    % data logging, don't change this!
    q = q+deltaT*dq;
    qArr(:,i) = q;
    rArr(:,i) = r_BF_inB(q(1),q(2),q(3));
    rGoalArr(:,i) = rGoal(t);
    
    % controller: 
    % step 1: create a simple p controller to determine the desired foot
    % point velocity
    v = ...;
    % step 2: perform inverse differential kinematics to calculate the
    % generalized velocities
    dq = ...;
    
end

plotTrajectory(timeArr, qArr, rArr, rGoalArr);

---

https://homes.cs.washington.edu/~todorov/courses/cseP590/06_JacobianMethods.pdf