Kinematics modeling of six degrees of freedom humanoid robot arm using improved damped least squares for visual grasping

The robotic arm has functioned as an arm in the humanoid robot and is generally used to perform grasping tasks. Accordingly, kinematics modeling both forward and inverse kinematics is required to calculate the end-effector position in the cartesian space before performing grasping activities. This research presents the kinematics modeling of six degrees of freedom (6-DOF) robotic arm of the T-FLoW humanoid robot for the grasping mechanism of visual grasping systems on the robot operating system (ROS) platform and CoppeliaSim. Kinematic singularity is a common problem in the inverse kinematics model of robots, but. However, other problems are mechanical limitations and computational time. The work uses the homogeneous transformation matrix (HTM) based on the Euler system of the robot for the forward kinematics and demonstrates the capability of an improved damped least squares (I-DLS) method for the inverse kinematics. The I-DLS method was obtained by improving the original DLS method with the joint limits and clamping techniques. The I-DLS performs better than the original DLS during the experiments yet increases the calculation iteration by 10.95%, with a maximum error position between the end-effector and target positions in path planning of 0.1 cm.


INTRODUCTION
In the last few decades, robot manipulators have been increasingly used in the industry.Many approaches to building a robot manipulator include finite element analysis [1].In a humanoid robot, the manipulator is an arm and usually performs like the human arm to grasp an object.An example of implementing a grasping task using a robotic manipulator or arm is pick-and-place operations of the assembly process in the manufacturing industry [2].With the rapid development of robotics, the robotics and intelligent systems center (RoISC) is also participating and developing humanoid robots to be helpful to humans, FLoW humanoid robot [3], [4].The current version of the humanoid robot T-FLoW is installed with a six degrees of freedom (6-DOF) robotic arm that functions as an arm to perform tasks like the grasping task.
The kinematics vectors of the arm robot model are calculated using forward and inverse kinematics.The forward kinematics transform the joint space vectors to the cartesian space vectors whilst the inverse ISSN: 2088-8708  Kinematics modeling of six degrees of freedom humanoid robot … (Muhammad Ramadhan Hadi Setyawan) 289 kinematics do the opposite way [5].The most used method to solve forward kinematics is the homogeneous transformation matrix (HTM) based on the Denavit-Hartenberg (D-H) parameters [6].The HTM can be defined from the rotation and the translation elements in the D-H parameters or directly from the Euler system of the robot.There are numerous methods to solve the inverse kinematics problem.The numerical method solves the inverse kinematics of the robot manipulator that cannot be solved in closed form using the new inverse kinematics algorithm (NIKA) [7].The geometric method implements the mathematical solution based on geometric algebra to find the reverse of the forward kinematics [8], [9].The pseudoinverse of the Jacobian matrix solves the problem of computing the inverse matrix when the matrix is a non-square matrix [10].The inverse kinematics is based on the weighted least-norm solution that avoids the joint angular position and velocity limits [11].The kinematics solutions of the robotic arm are immense.They can be obtained from either position-based or velocity-based methods.The kinematics model is not only the study used to build a robot manipulator.The dynamics model is also an aspect that must be considered to developing a robot manipulator.The Euler-Lagrange formulation is the conventional method for building the dynamic model.An Euler-Lagrange formulation is an approach based on the kinetic and potential energy of the robot [12].The dynamic model based on the assumed mode method (AMM), a discrete method for continuous systems, uses the linear sum of assumed mode to describe the vibration of the robot body [13].The other approach to defining the dynamic model is the system identification method.The system identification method creates a mathematical model using input and output data from a real dynamic system [14].However, there are many approaches to defining the dynamics model of the robot.Therefore, this research will not discuss the dynamics model but only focus on the kinematics model of the robot manipulator.
The kinematics singularities are the common problem in the kinematics model of the robot manipulators or the robotic arm.The kinematics singularities occur when the robot is in the neighborhood of a singular configuration, which causes limitations in the motion [15].Many research discusses the methods to avoid such singularity, for example, the singular value decomposition (SVD) of the Jacobian matrix [16], the damped least-squares method [17]- [19], the singular robustness algorithm [20], the virtual spring model, and joint-based damping control [21].All research aims to find the best algorithm for computation, efficiency, and robustness kinematics.
The kinematics singularity is not the only thing to consider in controlling the robotic arm.Such problems that must be considered are a mechanical restriction, actuator safety, and natural motion configuration.To overcome those problems, the joint limits technique is used to limit the angle of each joint to accommodate the mechanical restriction and provide the natural motion configuration [22].Whilst the clamping technique creates the reachable target when the real target is unreachable or outside the workspace area.This clamping technique will reduce the amount of iteration when calculating the solution since the target will never exceed the workspace area of the robot [23].The original damped least-squares (DLS) method can solve kinematics singularity yet does not have the safety factor of the actuator and does not consider the unreachable target.
We developed a kinematics model for the 6-DOF robotic arm of the T-FLoW humanoid robot with a visual grasping system.The forward kinematics model using the HTM was obtained from the Euler system of the arm robot.This research improves the original DLS method with a simple joint limits technique using the scale back law and the clamping technique, the improved damped least-squares (I-DLS) method for the inverse kinematics model.
The main contributions of this research can be summarized as: i) the system of the kinematics model on the robot operating system (ROS) platform and CoppeliaSim simulator; ii) the improvement of the original DLS by combining with the joint limits technique to create a safety factor for the joint actuator and the clamping technique to reduce the amount of computation iteration; iii) the improvement of the original DLS by combining with the clamping technique reduces the amount of iteration of calculation because of the unreachable target; and iv) the movement mechanism of the grasping activities.

FORWARD KINEMATICS
The forward kinematics model transforms the joint space into the cartesian space.This model provides the position of the end-effector of the robot in the world coordinate.As shown in Figure 1, The robot in this research consists of six revolute joints built by a double spherical wrist configuration.The mechanical structure and the joint coordinate system of the robot, as shown in Figure 1(a), with the subscript number  ( = 1, … , 7),   values represent the angle values of joint-1 to joint-6 and   represents the length of link-1 to link-6 with 0 offsets of all links, and will be used to create the kinematics model and describe the kinematic relationship between the position of the end-effector and the joint angle of the robot.Figure 1(b) shows the position of each joint in the robot model.Forward kinematics solution can be solved using the HTM [24].Combining the 3 × 3 rotation matrix (  0 ) and the 3 × 1 translation or displacement matrix (  0 ), the homogeneous transformation matrix (  0 ) is obtained as (1): where  is defined as the number of joints.Using the Euler system of the robot based on the robot coordinate system in Figure 1(a), the homogeneous transformation matrices of all joints from the first joint to the sixth joint are: where  is indicated sine function and  for cosine function.
The end-effector robot position can be obtained from the total HTM by multiplying each HTM of the joints, as shown in (3).
The (3) can be simplified to get the vector position  ∈ ℝ 3 of the end-effector in the cartesian space, as explained in (4).

INVERSE KINEMATICS
Since the solution of inverse kinematics is infinite, many approach algorithms are used to solve inverse kinematics problems, such as the geometric and numeric methods based on the structure and coordinate system of the robot.This research uses the DLS method combined with joint limits and a clamping technique called the I-DLS.This method is used to find the joint angles of the 6-DOF robotics arm of the T-FLoW humanoid robot.

Damped least-squares
The general method to solve the singularity is the DLS, which has not been able to be solved by the pseudoinverse method.For the robot manipulator or arm that has joints connected by links, the angle values of the joint can be written as vector column  = ( 1 ,  2 , … ,   )  ,  is the number of joints.The end-effector position in the cartesian space can be written as a vector column  ⃗ = ( 1 ,  2 , … ,   )  and the target positions as a vector column  ⃗ = ( 1 ,  2 , … ,   )  , with  is the number of values from ℝ 3 .We use forward kinematics to find the  ⃗ in (5),  ⃗ = (  ,   ,   )  .
The basic algorithm of inverse kinematics is to calculate the vector joint to reach the target position and can be expressed as (6).
The ( 6) was solved using the iterated local search (ILS) algorithm based on the  ×  Jacobian matrix  which is defined as (7): and computes the value of ∆ by using the current values of ,  ⃗ and  ⃗ to obtain the change of end-effector position  ⃗, as explained in (8).
Furthermore, the algorithm updates the values of joint angle  by adding up the ∆ value of each iteration, which can be expressed as ( 8) The pseudoinverse method [25] calculates the pseudoinverse of the Jacobian matrix  to compute the addition of joint angles, as explained in (10).
However, if the size of Jacobian matrix  is not full-row rank or () < , the pseudoinverse method is unstable near the singularity and becomes unconditioned.
In [18], [19] propose an inverse kinematics solution using the damped least-squares method to overcome the singularity problem.The damped least-squares computing the value of ∆ by minimizes the quantity of (11): where  ∈ ℝ > 0 is a non-zero damping factor.The DLS computes the ∆ values by minimizing tracking errors and joint velocities but compromising the feasibility and the accuracy of the solution.This method accords to the solution of (12).
Thus, the DLS solution gives numerically stable to select ∆ values, as explained in (13).
According to the laws identified in [17]- [19], [26], selecting the damping factor  can be dynamically using different methods.In this research, the damping factor  dynamically selected as (14): where the  0 value is the value defined by the user to obtain the solution around the singularity.The  0 value is the definition of the size of the singular region, and the  value is the scalar objective function of the joint variable that can be calculated by (15).

Improved damped least-squares
The I-DLS affects the original DLS in (13).The joint limits technique limits the ∆ values to a certain range based on the user determination.Also, the I-DLS creates the alternative target position as input of inverse kinematics to obtain the change of end effector position  ⃗ with the clamping technique.

Joint limits
The joint limits technique is used to limit the angle of each joint, which is useful so that the actuator does not reach an excessive angle to avoid damaging the actuator or the link and can be used to achieve a natural motion configuration.The actuator of each joint has the minimum value  min and the maximum value  max , thus the angle from the calculation result of inverse kinematics   should not exceed those values.We are using the scale back law to limit the value of joint angle   as shown in (16): where  is the joint number.The values of  min and  max can be specified according to the requirement of the user or can refer to the human arm mechanism to limit the angle.The minimum and maximum values of ∆ are used to limit the inverse kinematics angle   based on the  min and  max .

Clamping the distance of the target position
When the target position is unreachable or beyond the workspace of the robot, the original DLS fails to calculate the joint angle, and the error position will be enormous.Clamping the target position reduces the position error when the target is unreachable by moving the target closer to the end-effector position and does not exceed the workspace area.Using the technique that refers to [23], this method works by clamping the length of  ⃗ in the (8), can be written as (17): where the ( − ,   ) is the function to find the new target position  based on the input target , as explained in (18).
The ‖‖ is representing the Euclidean norm of  and the   value is an upper bound on how far the end-effector moves in a single update step.

. Experimental platform
The experimental platform of this research, as shown in Figure 2, aims to study the kinematics model of the 6-DOF robotic arm T-FLoW humanoid robot.The simulation environment, including the full body of the robot and the object target, was built in the CoppeliaSim simulator V4.2.0 [27] using Vortex physical engine [28].The kinematics model was built using C++ based on the robot operating system Foxy Fitzroy (ROS 2) [29].The motion path was generated using the path planner pen motion planning library (OMPL) [30] in the CoppeliaSim.Two ROS nodes regarding the kinematics model and the CoppeliaSim communicate through the two ROS topics.The stereo camera model in the CoppeliaSim node is used to obtain the position of the object in the world coordinate.The blob detection method gets the position of the object relative to the stereo camera perspective and transforms it into the world coordinate perspective.The path planner will create the motion path of the robot between the initial end-effector position and the target position.The target position of each step in the motion path will be sequentially sent to the kinematics node through the target pos topic.The kinematics node will calculate the angle of all joints using the kinematics model and send it to the set angle topic, then will be forwarded to the CoppeliaSim node.Each joint of the arm robot in the simulation will move based on the set angle topic.All joints in the simulation are used as the actuator that rotates like servo motors, using an internal proportional-integral-derivative (PID) controller in all joints to maintain the angle position.The experiment was conducted using a computer with Ubuntu 20.04 LTS, AMD Ryzen 5 3550H, and 16 GB DDR4 to run the simulation.

Analysis of kinematics model
The kinematics model analysis will be divided into two sections, the forward kinematics analysis and the inverse kinematics analysis.The forward kinematics analysis provides the comparison result of the proposed forward kinematics with the general robotic toolbox.Furthermore, the inverse kinematics analysis provides the results of the proposed inverse kinematics in several experiments.

Forward kinematics analysis
Since the inverse kinematics are based on the DLS method that uses the iterated local search algorithm, the forward kinematics is needed to calculate the current end-effector position of the robot  ⃗ that is used feedback for inverse kinematics.The input of forward kinematics is the joint angle from the inverse kinematics calculation.The result of forward kinematics  ⃗ is used to obtain the error position  ⃗ as explained in (8) by subtracting that with the target position of the end-effector.As shown in Table 1, several experiments were performed to validate the success of the forward kinematics model by comparing the results with the robotic toolbox by Peter Corke in MATLAB [31].With several randomized set angles as input, the proposed forward kinematics model successfully calculates the end-effector position in the cartesian space (x, y, and z) without error.

Inverse kinematics analysis
To analyze the inverse kinematics model, we compare the inverse kinematics based on the original DLS with the proposed inverse kinematics using the I-DLS.The simulations were conducted to get a comparative analysis by performing a motion to reach a target position.The first experiment examines the capability of the joint limits method.As shown in Figure 3, an example experiment aims to test the joint 294 limits technique, performed by limiting the joint-4, the minimum angle limit   to −90 ° and the maximum angle limit   to 90 °. Figure 3(a) shows the angle values without joint limits, which after the 45 th step, the joint-4 angle value exceeds 90 °.Whereas in Figure 3(b), after the 45 th step, the joint-4 angle values with joint limits do not exceed 90 ° and decrease.However, the robot cannot reach the target position because of the workspace area reduction.
Further experiments are performed to validate the capability of the joint limits technique.As shown in Table 2, The experiments to test the joint limits by setting the minimum angle   and maximum angle   based on the mechanical design of the robot used in joint 2, joint 4, and joint 6.The red-colored value is the joint angle without the joint limits using the original DLS.The blue-colored value is the joint angle with the joint limits using the proposed DLS or improved DLS.The proposed DLS success performs joint limitation based on the minimum   and the maximum   angle.
The following experiment was performed to analyze the clamps method to reduce the calculation iteration of the DLS solution.The clamps method works by creating an alternative target position from the actual target position.The experiment works by setting the target position out of the workspace area, as shown in Figure 4(a).

Reach movement in the simulation
The last experiment is to perform a motion to reach the target object in the simulation.We use a cup as a target object and obtain the coordinate position using a stereo camera attached to the head of the robot.The OMPL path planner creates a motion path with obstacle avoidance between the initial end-effector and target positions detected by a stereo camera, as shown in Figure 5.The OMPL path planning makes 98 movement steps where the kinematics node in ROS2 will calculate the value of all joints based on the target position for each step.The comparative data between the end-effector position and the target position can be seen in Figure 6.6(b) is the error between the end-effector and the target positions on each axis.The obtained maximum error is 0.1 cm, and the average error is 0.0081, 0.0013, and 0.0979 cm on x, y, and z, respectively.The additional data to validate the experiment of reaching the unreachable target by comparing the original DLS and proposed I-DLS can be shown in Table 3.Even though both methods could not reach the target because it was unreachable, the proposed method could

CONCLUSION
The kinematics model has been successfully built in the ROS2 platform for a 6-DOF robotic arm T-FLoW humanoid robot to perform a reach objects motion for grasping mechanism.We test the forward kinematics to validate its success by comparing it with the robotic toolbox by Peter Corke.We also test the inverse kinematics by comparing the proposed I-DLS combined with joint limit and clamps with the original DLS.The result of the forward kinematics successfully calculated the end-effector position with 0 error.The result of inverse kinematics using the improved DLS is that the joint limits technique can successfully limit the joint angle based on the defined value of the minimum and the maximum angle to avoid mechanical limitations.When the target is unreachable, the original DLS fails to find the solution and obtain infinite iteration.However, the proposed I-DLS with clamps successfully finds the solution despite increasing the iteration by 10.95%.The experiment results to reach an object in the simulation show that the proposed kinematics model successfully follows a motion path with a maximum error is 0.1 cm.The average error of each axis (x, y, and z) is 0.0081, 0.0013, and 0.0979 cm.However, the disadvantage of using the joint limit technique is that it can reduce the workspace area of the robot due to the limited movement because of the angle restrictions.Thus, using the joint limits technique requires additional calculation iterations.Further research is to develop the real robot arms, the dynamics model of the robot, obtain the pose of the hand, and develop the grasping movement algorithms.

Figure 1 . 6 -
Figure 1.6-DOF robot arm of T-FLoW humanoid robot (a) structure of the robot and (b) joint position

Int
Kinematics modeling of six degrees of freedom humanoid robot … (Muhammad Ramadhan Hadi Setyawan)

Figure 3 .
Figure 3. Joint angle values comparative (a) without joint limits and (b) with joint limits

Figure 4 .
Figure 4. Path planning: (a) clamping experiment by setting target position unreachable and (b) iteration comparison

Figure 5 .
Figure 5. Reach target experiment in simulation Figure 6(a)  shows the comparative data between the end-effector position calculated from the kinematics node and the target position.Figure6(b) is the error between the end-effector and the target positions on each axis.The obtained maximum error is 0.1 cm, and the average error is 0.0081, 0.0013, and 0.0979 cm on x, y, and z, respectively.The additional data to validate the experiment of reaching the unreachable target by comparing the original DLS and proposed I-DLS can be shown in Table3.Even though both methods could not reach the target because it was unreachable, the proposed method could

Figure 6 .
Figure 6.Reach target experiment data (a) comparative data position and (b) error data position

Table 1 .
The experiment of the forward kinematics model

Table 2 .
Joint limits experimental table