I am having trouble generating the correct trajectory for my robot.

Hi,

I am trying to get the Forward Kinematic (FK), the Inverse Kinematic (IK) and the Jacobian (J) for my closed loop robot. I have broken up the joints to create the rigidBodyTree. Here is my problem.

Originally, I used 4 sine wave generators to provide the movement to my actuated joints to perform the motion I would like it to. Then I fed it to the GetTransform block (GT) and padded the other non actuating joints as zero. This worked well and allow me visualise the motion of the robot. However, when I included the the trajectory from a signal editor, I cannot get my robot to perform the motion I want. The my desired motion is for the entired part of the robot to rotate around the x axis, and the end effector tip to pivot in an arc motion in the xy plane.

When I used the signal editor, the robot only moves and roll in the x axis, which is one of the movements. The other movements can’t be seen (i.e pivoting). I tried setting values for the signals in the signal editor to be higher (i.e at 1 it’s 20 instead of 5 and at 2 it’s 50 instead of 10) to see better movement and it worked. Problem is, it stills only rotate in the X axis. I’ve tried using the original sine wave generators (the ones with the values that allows me to have the desired movement) and feed it into a mux before feeding that connection into the Coordinate Transformation block to get a 4×4 Homogenous Transformation block before connecting it to the Pose port of IK. That still give me the same error. I also tried exporting the sine waves data as xls and then import them into the Signal Editor. That still gives me the same issue.

I then tried to visualise the movements of the end effector and hopefully get those values to be waypoints, hoping to control the robot better. I used the Transform Sensor and a couple of other To Workspace blocks to output the values for post processing. After the desired simulation and movement (I connected them to the model where my sine wave generators connect directly to the joints of the robot to give the desired movements), I had a script and post processed it. I got it to save into a variable called "mytraj". I connected that then to the model with the IK and it still gives me the error. I went into the variable itself and increases every values by timing it by 10 or 20 to see if anything will happen. And sure enough it did the same issue. Only rotation around the X axis, nothing else. The more I increase the value, the more degree or obvious it rotates around the X axis and nothing else. How can I please solve this issue?

This is the code for the waypoints from the visualisation:
X_positions = out.X.signals.values;
Y_positions = out.Y.signals.values;
Z_positions = out.Z.signals.values;
time = out.X.time; % Time vector

figure;
plot(X_positions, Z_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot(X_positions, Y_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot(Y_positions, Z_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot(Z_positions, X_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot(Y_positions, X_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot(Z_positions, Y_positions, ‘color’, [60 100 175]/255);
xlabel(‘Y Coordinate (cm)’);
ylabel(‘Z Coordinate (cm)’);
grid on;

figure;
plot3(X_positions, Y_positions, Z_positions, ‘b’, ‘LineWidth’, 2);
xlabel(‘X Coordinate’);
ylabel(‘Y Coordinate’);
zlabel(‘Z Coordinate’);
title(‘End Effector Trajectory’);
grid on;

% Plot the full trajectory
figure;
plot3(X_positions, Y_positions, Z_positions, ‘b’, ‘LineWidth’, 2);
xlabel(‘X Coordinate’);
ylabel(‘Y Coordinate’);
zlabel(‘Z Coordinate’);
grid on;
hold on;

% Plot the waypoints
plot3(waypoints(:,1), waypoints(:,2), waypoints(:,3), ‘ro’, ‘MarkerSize’, 8);
title(‘End Effector Trajectory with Waypoints’);
hold off;

% Number of waypoints you want (e.g., 20)
num_waypoints = 20;

% Interpolate the time values from 0 to max time over the number of waypoints
waypoints_time = linspace(0, max(time), num_waypoints);

% Select waypoints from X, Y, and Z positions by downsampling
waypoint_indices = round(linspace(1, length(X_positions), num_waypoints)); % Get indices for waypoints
waypoints = [X_positions(waypoint_indices), Y_positions(waypoint_indices), Z_positions(waypoint_indices)];

% Combine interpolated time and waypoints to form the trajectory matrix
traj2ii = [waypoints_time’, waypoints];

trajectory_data = [time, X_positions, Y_positions, Z_positions];

My other question is: For now I have decided to connect the output config of FK to the pose of IK since output of FK is a 4×4 homogenous matrix. I directly control the joints of the robot with my sine wave generators. Is this a correct set up? Basically, sine generators to robot subsystem which connects to FK which connects to IK which is connected to the Jacobian block. Additionally, I have 4 actuated joints which I can clearly see are moving. However, my IK only shows 3 out of 11 joints values. It should be 4 since I am controlling and putting values to 4 joints. The other 7 are understandable since they’re non actuating and I have padded it with zeros.

I have attached all the appropriate figures in this post. Please help me with this.Hi,