Forward and Inverse Kinematics

Using TinuIK library

This is a small and very simple library that, nevertheless, does the job well. Installation is rather simple:

                        $ pip install tinyik
                    
                        # With the visualization feature:
                        $ pip install tinyik[viz]
                    

Here is the example of using it (basic_trajectory_action_service.py):

Here is an image that I used to fill the arm's dimensions array.

Writing our own code

Same objective can be achieved by our own code. The code is located in inverse_kinematics.py.

We consider the robotic arm made of "links", each link has two joints on its opposite ends.

Every joint has an associate axe, one around which it rotates.

Here is an example description of an arm:

                            arrJointCoords = [
                            'z', (0, 0, 0.114), 
                            'y', (0, 0.082, 0.082), 
                            'y', (0, -0.082, 0.5),
                            'z', (0, 0, 0.26),
                            'y', (0, 0.082, 0.240),
                            'z', (0, 0.082, 0.082)
                            ]
                        

These are coordinates relative to previous joint. joint0 is assumed to have (0,0,0) coordinates.

The objective is to calculate absolute joints' coordinates ("absolute" means - relative to joint0)

To do it, starting from the first joint, in cycle
Using simple trigonometry, calculate next joint's position relative to current joint, after current joint rotates at the specified angle. Angles are provided when we call the function: def forward_kinematics(self, arrJointCoords, arrTargetAngles)
Add relative coordinates of the next joint we got in (a) to absolute coordinates of current joint. This way we get absolute coordinates of the next joint.
We have an array of target angles, that looks like [angle1, angle2, ...] They are single numbers because we also know the axe around which the joint rotates. Using this information, array of target angles can be represented as array of triplets: (angle, 0,0) for X axe, (0,angle,0) for Y axe and (0,0,angle) for Z axe.
In the same cycle, after the absolute coordinates of the next angle are calculated, we need to calculate absolute angles. To do it, add angles (as triplet) of the current joint to the target angles of the next joint.
Example: joint1: axe 'z', angle math.pi/2, which is the same as (0, 0, math.pi/2). If initial target angle (passed to FK function) for joint2 is 'x', math.pi/2, then cumulative absolute angle for joint2 will become (math.pi/2, 0, math.pi/2) - we added corresponding elements of a triplet.
The coordinates of a joint after the next will be calculated using these cumulative angles.

We want to create a Python function with the following signature:

                        def forward_kinematics(self, arrJointCoords, arrTargetAngles)

                        # Here is the input:
                        arrJointCoords = [
                            'z', (0, 0, 0.114), 
                            'y', (0, 0.082, 0.082), 
                            'y', (0, -0.082, 0.5),
                            'z', (0, 0, 0.26),
                            'y', (0, 0.082, 0.240),
                            'z', (0, 0.082, 0.082)
                        ]

                        arrTargerAngles = [math.pi/2, math.pi/2, -math.pi/2,math.pi/2,math.pi/2,math.pi/2]
                    

Now, in basic_trajectory_action_servive.py, there is a commented out code for this test. It produces results, similar to what using a 3rd party library produces: