I need help validating my forward kinematic using Simulink and the Robotic Systems Toolbox for a surgical robot.

Hi all,

I am a student and I am working on a surgical eye robot which utilises a remote centre of motion mechanism (RCM). I have found the forward kinematics for it using the DH method and another method. I have also found the Jacobian velocity matrix for it. However, my Jacobian seems a bit off and I am looking to validate it and the forward kinematic using RST. I have exported the CAD model as XML and created an slx file for it. The robot composes of multiple joints. However, in reality, only 4 joints are actuation joints, the rest are closed loop joints that reduce the robots DOF to 1 or 2 DOF (pitch and translation).

I have set up the robots and actuated it fine using a constant block connected a degree to radians block which is then connected to the robot, which is connected to a MATLAB function script which is then connected to a display to show me the homogenous matrix.

I am now at the stage where I am looking to connect a GetTransform block in order to retrieve the forward kinematic. Below is the way I am connecting the blocks. My apologies since I am using a different computer and can't screenshot my layout.

3 Sine blocks and a constant block connected to the robots (I have made the robots connection and blocks a subsystem). From the robot subsystem, it's connected to a mux that takes 4 inputs. The mux is connected to the GetTransform block. The GetTransform block is connected to a Coordinate Transformation Conversion block which is then connected to a scope.

All of my setup seems fine. However, I am getting an error that says the input is a vector of 4 whereas the outputs requires 11. I know this is due to the joints block where I have set up 4 of them to sense the postion and computed the torque automatically and I have 11 joints in total. When I add an additional Simulink to PS and PS to simulink to another joint, I get an error like this: "The input is a vector of 5 whereas the outputs requires 11". As mentioned, only 4 of the joints are actuating joints and joints that I need to find the forward kinematic for. How can I fix this issue without adding more joints to sense the position?

My second question is rather simple. I am using 3R1P configuration. I know I can get a constant block connected to a degrees to radians block to control the revolute arm of the robot. What is a similar setup I can use to move the prismatic joints?

Thank you so much for the help in advance!

Answer 1

[Umar]

Hi @Cuong Huynh Nguyen

You mentioned, “3 Sine blocks and a constant block connected to the robots (I have made the robots connection and blocks a subsystem). From the robot subsystem, it's connected to a mux that takes 4 inputs. The mux is connected to the GetTransform block. The GetTransform block is connected to a Coordinate Transformation Conversion block which is then connected to a scope. All of my setup seems fine. However, I am getting an error that says the input is a vector of 4 whereas the outputs requires 11. I know this is due to the joints block where I have set up 4 of them to sense the postion and computed the torque automatically and I have 11 joints in total. When I add an additional Simulink to PS and PS to simulink to another joint, I get an error like this: "The input is a vector of 5 whereas the outputs requires 11". As mentioned, only 4 of the joints are actuating joints and joints that I need to find the forward kinematic for. How can I fix this issue without adding more joints to sense the position?”

Please see my response by addressing your first query.

The error message indicates that the GetTransform block expects an input vector of size 11, which corresponds to the total number of joints in your robot model. However, your current setup only provides a vector of size 4, which is likely derived from the four actuating joints. Since you have 11 joints in total, it is essential to make sure that the input to the GetTransform block includes all necessary joint states. The four actuating joints are not sufficient to represent the entire robot's configuration. The remaining joints must be accounted for, even if they are not actively controlled. So, to resolve the mismatch, you can modify the Mux input to include additional elements that represent the remaining joints. Here are a few strategies:

Option A: Zero Padding

If the non-actuating joints do not contribute to the transformation, you can pad the input vector with zeros. This way, you maintain the required size of 11 without adding additional sensing or control mechanisms.

% Assuming 'actuatingJoints' is a vector of size 4
actuatingJoints = [joint1_position; joint2_position; joint3_position;   
joint4_position];
% Create a zero vector for the remaining joints
remainingJoints = zeros(7, 1); % 11 total - 4 actuating = 7 non-actuating
% Combine both vectors
inputVector = [actuatingJoints; remainingJoints]; % This will be of size 11

Option B: State Estimation

If the non-actuating joints have states that can be estimated or are known, you can create a mechanism to compute or retrieve their positions. This could involve using a Constant block or a MATLAB Function block to output the known positions of these joints.

function output = getJointStates()
  % Example of known positions for non-actuating joints
  joint5_position = 0.5; % Example value
  joint6_position = 0.3; % Example value
  % Add more as needed
  output = [joint5_position; joint6_position; ...]; % Continue for all non-  
  actuating joints
end

Once you have constructed the input vector of size 11, connect it to the GetTransform block. For more information on this block, please refer to

https://www.mathworks.com/help/robotics/ref/transformtree.gettransform.html?s_tid=doc_ta

Make sure that the Mux block is configured to accept the new input vector correctly. After making these adjustments, run your Simulink model again. Monitor the output in the Scope to ensure that the transformations are computed correctly and that the error regarding vector size is resolved. This will make sure that the input vector to the GetTransform block matches the expected size of 11, you can effectively resolve the error you are encountering. Whether through zero padding or state estimation, it is crucial to account for all joints in your robot model. This approach not only fixes the immediate issue but also enhances the robustness of your simulation by ensuring that all joint states are appropriately represented.

To address your second query regarding, “My second question is rather simple. I am using 3R1P configuration. I know I can get a constant block connected to a degrees to radians block to control the revolute arm of the robot. What is a similar setup I can use to move the prismatic joints?”

To control a prismatic joint in MATLAB, you can use a combination of blocks in Simulink or write a script in MATLAB. Since you are using Simulink, I will focus on it by providing step by step instructions. In Simulink, create a model that includes the following components:

Position Target: Use the State Targets parameters in the Prismatic Joint block to specify the desired position of the prismatic joint. The position can be controlled using a constant input or a time-varying signal, much like you would for angular positions in revolute joints.

Actuation Input: You can provide the actuation force through the input port f for controlling the joint. If you want to specify the position and velocity directly, enable the corresponding input ports in the block parameters.

By utilizing constant values, gain adjustments, and saturation limits, you can make sure that the prismatic joint operates within its physical constraints.

I would also recommend example documentation “Control PR2 Arm Movements Using ROS Actions and Inverse Kinematics” by clicking the link below

https://www.mathworks.com/help/robotics/ug/control-pr2-arm-movements-using-actions-and-ik.html

Here is an example code snippet for ROS if you feel like experimenting with it.

The code snippet below is a basic example to illustrate sending commands to a prismatic joint:

% Create action client for the prismatic joint
prismaticActionClient = rosactionclient('/prismatic_joint_controller/. 
joint_trajectory_action');
waitForServer(prismaticActionClient);

% Set up the goal message
goalMessage = rosmessage('trajectory_msgs/JointTrajectoryGoal');
goalMessage.Trajectory.JointNames = {'prismatic_joint_name'}; % Specify 
your joint name here

% Define trajectory points
point1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
point1.Positions = [0]; % Initial position
point1.Velocities = [0]; % Initial velocity
point1.TimeFromStart = rosduration(1.0); % Time from start

point2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
point2.Positions = [1]; % Desired position
point2.Velocities = [0]; % Desired velocity
point2.TimeFromStart = rosduration(2.0); % Time from start

% Assign points to goal message
goalMessage.Trajectory.Points = [point1, point2];

% Send goal to prismatic joint
sendGoalAndWait(prismaticActionClient, goalMessage);

Additional Suggestions

Calibration:

Make that the prismatic joints are calibrated correctly within your simulation environment to avoid unexpected behavior during motion execution.

Limits and Constraints:

Always define the limits for the prismatic joints to prevent any out-of-bound movements, which could lead to simulation errors or physical damages in real robots.

Testing and Validation:

Before deploying commands to the physical robot, run simulations to validate the behavior of the prismatic joints in various scenarios, ensuring smooth transitions and adherence to trajectory commands.

Hope following these suggestions helps resolve your problem. Please let me know if you have any further questions.

[Cuong]

Hi Umar,

Thank you so much for your response! I have implemented your first suggestion and it seems to work well. The display is now giving me some numerical responses. However, I am running into some errors when I try to set up the inverse kinematic and the GetJacobian block. For best practice, since I only have 4 actuating joints, can I placed the MatLab function block containing the code for it in between the inverse kinematic block and the Jacobian block? I guess what I'm asking is can I use that same function script multiple times. I have attached my setup below and the errors I am getting. I'm almost certain that the errors is due to the fact that I am actuating four joints and have 11 in total, and not have 11 inputs and outputs. My total setup includes a signal editor (though I might move towards using Polynomial Trajectory from RST if this run is successful), the inverse kinematic block, my robot subsystem, MATLAB function block, getTransform block, getJacobian block, matrix multiplication block, some mux and demux and some scopes and displays block. My apologies for the inconvenience! I am quite new to Simulink and I am trying to learn the best practice. Please help me where you can, I would truly appreciate it!

[Cuong]

My initial guess input for the inverse kinematic is a 1x11 array since I have 11 joints. Is this good or should I use the createInputVector script after it to let the inverse kinematic block know to only focus on the 4 actuating joints? My apologies for the plethora of questions!

[Umar]

Hi @Cuong Huynh Nguyen,

No need for apologies, it’s okay to ask bunch of questions.

Let me address your first query, “However, I am running into some errors when I try to set up the inverse kinematic and the GetJacobian block. My initial guess input for the inverse kinematic is a 1x11 array since I have 11 joints. Is this good or should I use the createInputVector script after it to let the inverse kinematic block know to only focus on the 4 actuating joints? “

So, after reading your comments in post, this is what I have comprehended so far to understand your setup.

Actuating vs. Non-Actuating Joints: Since you have four actuating joints and seven non-actuating joints, your function must correctly construct an input vector that accounts for both. The combined vector should be of size 11, where the first four elements represent the actuating joints and the remaining seven are zeros.

MATLAB Function Block: You mentioned placing a MATLAB function block between the inverse kinematic block and the Jacobian block. This is a common practice to preprocess inputs or outputs for subsequent blocks.

Now, this is what I understand by analyzing your errors in the attached screenshot shared by you.

The reason you are encountering these errors is suggesting issues with how outputs from your MATLAB function are being interpreted by subsequent blocks (like the Demux block). Specifically, the “size mismatch” indicates that your function's output does not align with what the downstream blocks expect and the warnings regarding nonuniform distribution highlight that Simulink cannot automatically infer the correct sizes/types of outputs from your function. So, based on the analysis of these errors, I will suggest the following suggestions. Adjust Output Size in MATLAB Function to make sure that your MATLAB function explicitly defines its output size. You can do this by preallocating the output variable:

   function inputVector = createInputVector(Joint1, Joint2, Joint3,
   Joint4)
       % Define output size
       inputVector = zeros(11, 1);  % Preallocate

       % Inputs: positions of actuating joints
       % Outputs: combined input vector of size 11
       actuatingJoints = [Joint1; Joint2; Joint3; Joint4];
       remainingJoints = zeros(7, 1); % 11 total - 4 actuating = 7 
       non-actuating

       % Combined input vector of size 11
       inputVector = [actuatingJoints; remainingJoints];
   end

For more information on zeros function, please refer to

https://www.mathworks.com/help/matlab/ref/zeros.html

Now, addressing your query regarding, “For best practice, since I only have 4 actuating joints, can I placed the MatLab function block containing the code for it in between the inverse kinematic block and the Jacobian block? I guess what I'm asking is can I use that same function script multiple times.”

Yes, you can use the same MATLAB function script multiple times within your model. Each instance will operate independently unless you define shared variables. Also, make sure that the output of your MATLAB function block is correctly connected to the input of the GetJacobian block. Use a Scope or Display block to visualize outputs at various stages for debugging. In addition,double check that all blocks are correctly configured to match expected input/output sizes. For blocks like Demux or Mux, make sure they are set up to handle vectors of size 11.

Since you mentioned, “I am quite new to Simulink and I am trying to learn the best practice”, I would suggest yourself with Simulink documentation on defining inputs/outputs in custom MATLAB functions. This will provide clarity on best practices. If necessary, use additional blocks to reshape or convert data types appropriately. If I were you, I would consider creating a simplified version of your model with just the essential blocks (the MATLAB function and Jacobian) to isolate issues before re-integrating it into your full setup. additional blocks to reshape or convert data types appropriately and then utilizing Simulink’s debugging tools like breakpoints and data logging to trace through your model’s execution.

Also, you mentioned,”My total setup includes a signal editor (though I might move towards using Polynomial Trajectory from RST if this run is successful), the inverse kinematic block, my robot subsystem, MATLAB function block, getTransform block, getJacobian block, matrix multiplication block, some mux and demux and some scopes and displays block.”

If you decide to implement Polynomial Trajectory generation from RST, consider evaluating how it can improve trajectory smoothness compared to your current approach. Polynomial trajectories can minimize jerk and provide smoother transitions between waypoints, which is beneficial in robotic applications requiring precision. As you refine your setup, rigorous testing will be essential. Utilize both simulation environments and real-world tests (if applicable) to validate the performance of your control algorithms and ensure they meet the operational requirements. Hope, you're considering expanding functionality or integrating with other software (e.g., ROS), make sure that the communication protocols are well established, as this could enhance the versatility of your robotic system.

Hopefully, if you follow these recommendations and modifications, you should be able to resolve the errors you're encountering and successfully establish a setup for your inverse kinematics model in Simulink.

Please let me know if you have any further questions, I will be more happy to assist you.

[Cuong]

Hi Umar,

Thank you so much for your insight and advice! It has been very helpful. I was able to solve those errors I had earlier. The problem was that the inverse kinematic block was accounting for 11 joints where as the input for my robot subsystem was 4 joints. The original I script I had used was a conversion of 4 inputs into 1 (11x1) output whereas I was actually meant to reverse it. So I grab 11 inputs from the inverse block and sliced them into the 4 joints that I needed so that gives 1 (11x1) input into 4 outputs.

Thank you for helping me to get there. I am now very close to getting the Jacobian block to work, which was my original goal. However, I am getting some errors with the Matrix Multiply block.

The GetJacobian block takes in all 11 joints from the inverse kinematic (where I connected the input of the GetJacobian to). However, as I am interested in 4 joints, my goal is to create a 6 by 4 matrix (4 joints). I got a script to slice through those 11 joints and get the 4 joints that I need. For my other connection, I have 4 addition outputs that sense the velocity from the robot subsystem. I have connected that to a mux and that mux (input A) is connected to the first input of the Matrix Multiply block, the second input (input B - the 6 by 4 from the sliced Jacobian) is connected to the second input of the Matrix Multiply. The first error was that the mux was producing a "1 dimensional with 4 elements" or 1 by 4. I tried to fix this by implementing a Function Block which takes the mux output and transform it into a 4 by 4 matrix. This script is fairly simple as it just produce an identity matrix. However, I am getting some issues and the errors say there's some "Matrix Multiply Propagation error". I was wondering how can this be since a 6 by 4 multiply to a 4 by 4 and that should give a 6 by 4.

I then tested the Matrix Multiply block by getting two constant blocks (4 by 2 and 2 by 4) and connected it to the block. I attached a display to the output of the block and it gave me a 4 by 4 as expected. I then removed the Matrix Multiply block from the setup and attached two displays to show the output. The output was correct. I was getting 6 by 4 from the Jacobian and 4 by 4 from the robot subsystem. To further see what the error was, I removed the connection of the robot subsystem and attached a 4 by 2 from a constant block. That also gave me an error to do with the Jacobian part. I then removed the Jacobian connection, connected a 6 by 4 constant block to the input 2 of the Matrix Multiply and connect the connection from the robot subsystem to input 1. This gave me an error to do with the robot subsystem. Therefore, I think the error is to do with both of the values from the extracted Jacobian and the robot subsytem. Please let me know what you think I should do in this scenario.

I have followed an example online where the Jacobian was connected to a matrix multiply and the robot system velocity output was connected to a mux which was connected the matrix multiply, and there was no issue. I'm wondering how his doesn't give an error. I am looking to also get the output of the robot subsystem velocity to connect to a MUX and then directly to the Matrix Multiply without having to do additional scripts. This is because I believe that the values for the velocities will change and I do not want to hard code what I think the output would be and then feed that to the Matrix Multiply. If that makes sense. I have attached a link to that video here: https://www.youtube.com/watch?v=b5XeXsXoApI&list=PLWF9TXck7O_ymYWT8Q33omPb5K-A5v4Ae&index=5

I have attached the appropriate figures and my overall setup, and my apologies if it looks messy, I am just trying to show the full picture. Please help me where you can! I'd greatly appreciate it and thank you so much for your time!

[Cuong]

I have also used a reshape block to try and fix the error but that did not work. I have set it as a column vector and multiple other options and still got similar results.

I have also followed your advice and created a simpler system where there's only four joints based on the movement and workspace of the robot. Everything works great (including the forward and inverse kinematic) except for the Jacobian block which has the same errors as with my full model. The intriguing part is that I am not using any additional scripts to slice or transform the Jacobian since I already have the four joints I am focusing on, yet I still get the same error. Please help me where you can, I'd truly appreciate it! And thank you so much for your time once again!

[Umar]

Hi @Cuong Huynh Nguyen,

It appears you are making significant progress in your robotics project, but the issues you're encountering with the Matrix Multiply block stem from the mismatch in dimensions between the inputs. After reviewing your robotics Simulink diagrams and the following documentations listed below

<https://www.mathworks.com/help/simulink/slref/mux.html?searchHighlight=mux%252520block&s_tid=srchtitle_support_results_1_mux%25252520block%20 https://www.mathworks.com/help/simulink/slref/mux.html?searchHighlight=mux%2520block&s_tid=srchtitle_support_results_1_mux%252520block >

https://www.mathworks.com/help/simulink/slref/buscreator.html

https://www.mathworks.com/help/simulink/slref/product.html?searchHighlight=matrix%20multiply%20block&s_tid=srchtitle_support_results_1_matrix%2520multiply%2520block

Let me break down the situation and address your concerns systematically.

Understanding the Matrix Multiply Block

The Matrix Multiply block performs matrix multiplication based on the defined dimensions of its inputs. For your case:

Input A (from the MUX): You have a 4x1 vector (1D array with 4 elements). Input B (from the Jacobian): You have a 6x4 matrix.

For matrix multiplication to be valid, the number of columns in Input A must match the number of rows in Input B. Since Input A is 4x1, it cannot multiply with Input B directly because the required configuration for multiplication is that Input A should ideally be 4xn where n can be any integer.

So, based on the errors you encountered, I would suggest the following suggestions.

Dimension Adjustment: Since you're extracting joint velocities and want to use them effectively, ensure that Input A is reshaped correctly. Instead of using a MUX to create a 1D output, consider creating a 4x1 or 4xN output directly so that it aligns with your intended operations.

Using Identity Matrices: Your approach to using an identity matrix as a transformation for joint velocities could work if set up correctly. However, ensure that this transformation results in a compatible size. For example, if you want to transform your 4-element velocity vector into a 4xN format for multiplication, consider explicitly defining this transformation rather than relying solely on an identity matrix.

Validate Slicing Logic: Double-check your slicing logic in extractJacobian function. Make sure that it indeed returns a 6x4 matrix. If your original Jacobian is indeed 6x11, your slicing command should only extract the columns corresponding to the joints of interest.

   reducedJacobian = Jacobian(:, [1, 2, 3, 4]); % Ensure these    indices match your desired joints

Matrix Multiply Configuration: Make sure that you configure the Matrix Multiply block properly: Set Number of Inputs to 2 and set Multiplication to Matrix(*).

Testing Outputs: Before connecting everything together, test each output separately:

• Connect constant blocks with known dimensions to see if they yield expected results.

• If you get errors when connecting outputs from different subsystems, investigate each subsystem's output dimension closely.

In my opinion, errors often propagate through interconnected systems due to mismatched expectations in dimensions. Pay careful attention to how each block's output is defined and used downstream.

Since you are interested in avoiding hardcoding values for velocity outputs, consider utilizing Simulink's capabilities for dynamic signal routing or use Bus Creator blocks for more flexible connections instead of MUX blocks.

If issues persist, consider isolating sections of your model to identify specific points of failure more easily. Feel free to reach out again for further clarification or assistance!

[Cuong]

Thank you for your help so far!