Inverse Kinematics Error using Robotic Toolbox on Articulated Manipulator

I'm trying to implement a force control architure on 3 DoF articulated manipulator. The robot's model is imported from SolidWorks.

When I give the joint angles (q's), computed on specfied end-effector coordinates (xyz) from Inverse Kinematic Block to Get Transform (Forward Kinematics Block), I'm not getting the same (y and z) end-effector coordinates. The x-coordinates are the same.

As you can see from the attached image, the given xyz coordinates are [0.33 0.33 0] and Inverse Kinematics block gives joint angles [-1.127 1.579 0]. When computed joint angles are applied to forward kinematics / Get Transform block, the computed end effector coordinates are [0.3312 -0.02108 0.2981] ('Red Encircled'). Shoudn't they be [0.33 0.33 0].

PS: 1) I have also changed the imported robot model with the one given in the example named ("Trace An End-Effector Trajectory with Inverse Kinematics in Simulink") and faced the same issue.

2) I have made sure that the given end effector coordinates are within Task-space of the robot.

Answer 1

[Hannes Daepp]

From the screenshot you provided, it seems that your get transform block is configured backwards. If you want the position of Body3 in the base frame, you have to set "Body3" to be the Source body, and "base" to be the target body, as detailed in the block documentation. Currently you are returning the position of the base in the reference frame of Body3. For more details, refer to the documentation and the associated examples:

• https://www.mathworks.com/help/robotics/ref/gettransformblock.html?s_tid=doc_ta#d123e66194
• https://www.mathworks.com/help/robotics/ug/get-transformations-for-manipulator-bodies.html

Of course, after correcting this fix, it's also possible that the provided target pose is not feasible or that the numerical solver didn't get there and the settings need to be adjusted. To check this, take a look at the info output of the inverse kinematics block (your out.info data). If the solver has a Status value other than 1, the block cannot find a solution. In that case, verify that the solution is feasible, or adjust the block settings. See the documentation for more information.

https://www.mathworks.com/help/robotics/ref/inversekinematics.html#mw_8f6480d5-ccf4-4161-be1b-4647bb763cd8