1function [V_b_Ib_ref, qdot_ref, activeTasks] = simulink_inverseKinematics(q, pf_ref, pr_ref, g_joints_home, T_I_f, T_I_b, T_I_r, obstacleSurfacePoints, CA_active, modelParams)
2num_joints = modelParams.num_joints;
3bodyTwists = modelParams.bodyTwists;
4q_max = modelParams.q_max;
5q_min = modelParams.q_min;
6
7
8
9q_normalized = (q - q_min)./(q_max - q_min);
10alpha = 0.001;
11W_jointLimit = alpha * (2*q_normalized - 1)./((1-q_normalized).^2 .* q_normalized.^2);
12
13
14
15base = 2;
16sf = 2*base + 2;
17g_joints_home_f = g_joints_home(:,:,sf:end);
18bodyTwists_f = bodyTwists(:, sf - 1:end);
19num_joints_f = num_joints - 2*base;
20Jf = calculateGeometricJacobian(g_joints_home_f, num_joints_f, bodyTwists_f);
21
22
23eb = 2*base + 1;
24num_joints_b = num_joints - num_joints_f;
25g_joints_home_b = zeros(4,4,num_joints_b + 1);
26for i = 1:eb
27 g_joints_home_b(:,:,i) = g_joints_home(:,:, eb + 1 - i);
28end
29bodyTwists_b = fliplr(bodyTwists(:, 1:eb - 1));
30Jr = calculateGeometricJacobian(g_joints_home_b, num_joints_b, bodyTwists_b);
31
32
33Jb = [eye(6), zeros(6,4)];
34
35
36if CA_active == 1
37 [V_b_Ib_ref, qdot_ref, activeTasks] = CLIK_collisionAvoidance(T_I_f, T_I_b, T_I_r, obstacleSurfacePoints, Jf, Jb, Jr);
38else
39 [qdot_ref] = CLIK_pathFollowing(Jf, Jr, T_I_f, T_I_r, pf_ref, pr_ref);
40 V_b_Ib_ref = zeros(6,1);
41 activeTasks = 1;
42end
43
44end
45
46