A Dynamic Control Scheme with Differential Foot Placement for a Hexapod Robot

Sander Idelson, Andy Anthony and Brian Stevenson
point and end point were defined and the robot calculates the best way to reach these points using inverse kinematics. This robot also used dynamics to determine the loads necessary to perform specific movements. The robot used force and torque feedback to accomplish its task. A very basic way to move the legs was to use forward kinematics based on a position vector that is input into the system [3]. The robot moves according the specified path and determined by the vector. This method is typical of many current hexapods and has many applications. B. Force Sensing The challenge of using force sensing for hexapod movement over uneven surfaces has been studied many others. Information on previous uses of force feedback were analyzed by the team. One solution for hexapod motion over uneven terrain uses the combination of designed force threshold-based foot motion and center-of-body (CoB) based omnidirectional movement. Using these processes the movement of the hexapods omnidirectional movement could be stabilized with a dynamic gait application. Applying the dynamic solutions to the robot allowed for enhanced stability when traveling over uneven terrain. Using the CoB-based omnidirectional movement provided better results when implemented with a process called environment trailed trajectory (EET). This allows the robot to intelligently solve foot placement solutions when presented with uneven terrain while keeping the body stable [4]. Another paper talks about many of the ways that force sensing can be performed in hexapod locomotion as compared to real six-legged insects [5]. It was found from this paper that force sensing can be used to detect force and its rate of change during locomotion, provide information about the direction of force while standing still, detect sudden force decreases (such as slipping), and enhance muscle feedback while moving [5]. The paper goes on to analyze how strain gauges can be used to perform force sensing in a similar way to real insect sensory systems. This paper gives a biological viewpoint of hexapod movement and force sensing. One force sensing technique was to use current measurement through shunt resistors. The voltage drop across the resistor is amplified and then passed through an analog to digital converter before being processed by a digital control loop. It provides insight in how to implement trace resistance and temperature sensors to correct for thermal drift errors, in summary the shunt resistors are replaced by resistive traces and must be amplified and
Abstract – In this paper, a new dynamic walking scheme is developed for a 3 degree of freedom hexapod robot. The method uses dynamic differential motion, inverse kinematics, a special zsub-stepping function and current sensing to walk dynamically.

I.

INTRODUCTION

Currently, the field of bio-robotics is an area of growing interest to researchers and industry alike. Often, hexapods, or six legged robots are used to simulate the movement of spiders, grasshoppers and other insects. The basis of this project is to create a means of ambulation for a hexapod that can dynamically change foot placement based on an input vector. The motivation for this project is to create a more advanced leg placement technique that will allow robots to better replicate insect behavior. Creating a walking method that allows the robot to change its leg direction mid-step, as well as react to uneven surfaces in real time, was done with inverse kinematics, differential motion. Just as a spider can lift its leg, move it in the x and y plane and place it at another point (unrelated to the position of the other legs), the hexapod should be able to move each of its legs independently and based on instantaneous vectors. It should also be able to rotate its body with respect to the central x, y and z axes. The allowance of this much freedom enhances the ability of a walking gait to steer in the exact direction that is given, as well as change its direction instantaneously. The method that was chosen uses sub-stepping to increment the differential vectors of the leg position and move the leg accordingly. II. LITERATURE SURVEY

A. Dynamic Leg Movement Creating a walking scheme with dynamic movement is a problem that has been solved with a number of methods. Each technique described was used as a basis for the team’s technique. One design used a similar z-stepping technique in order to move the leg from one point to another. A single direction vector was input into the system and a final position of each leg was defined. Using inverse kinematics, and the final position, the leg was moved towards its goal. The quadratic error from the final position was calculated after each movement and fed back in order to make its next movement. The total movement was meant to look like a parabolic motion [1]. Another method used a predefined trajectory to accomplish a walking movement [2]. A starting point, midDr. Ferat Sahin, Rochester Institute of Technology

digitized in a similar procedure. The other solutions discussed in the paper include induction measurement for current estimation and the used of magnetic field sensors. A comparison of each method, its accuracy, limitations and efficiency is presented at the end of the article allowing the used to pick the most applicable approach to their project [6]. III. HEXAPOD STRUCTURE

A new hexapod, nicknamed the “Tigerbug”, was constructed for this project. The design is based on a similar Hexapod built at the RIT Multi-Agent BioRobotics Lab (MABL). The team’s hexapod was built from the ground up, in an attempt to improve the strength, flexibility and capability of the previous model. Some of the improvements that were made include more powerful servo motors, a more capable on-board computer, a larger size, and a better size to weight ratio. A. Leg Structure The hexapod robot has six legs, each spaced by 60 degrees around the central axis. They are numbered 0 to 5, counter-clockwise around the robot, for ease of programming. Each leg has 3 degrees of freedom allowing for a greater range of motion than other hexapods available. The leg has three servos, each controlling a movement of the leg. The hip servo, connected to the base of the robot, moves the leg backward and forward. The knee servo is attached to the hip and controls a portion of the vertical and horizontal motion of the leg. Finally, the wrist servo mostly controls the horizontal motion of the leg. The foot is the point of contact with the ground, at the end of the wrist joint. Figure 1 shows the leg lengths of the robot.

Figure 2: This is a diagram of the frames on a single leg.

The D-H Table has more steps than joints due to the rotation of the foot frame to match the center frame orientation. Table 1 is the complete D-H Table for the hexapod.
TABLE I. DENAVIT-HARTENBERG TABLE

Joint c T0 1 (Hip) 2 (Knee) 3 (Wrist) F1 F2 F3 F

θ (Leg #)*60 0 θ1 θ2 θ3-90 -90 180- θ2- θ3 θ1- θL

d 0 -L2 0 0 0 0 0 0

a L1 0 L3 L4 L5 L6 0 0

α 0 0 90 0 0 0 -90 0

Figure 1: This image shows the leg lengths for one leg of the robot.

The first joint that is shown in the table is a preliminary joint to move the center frame to just above the first frame for any leg. Each leg has a number that is defined sequentially around the robot. The angle for each leg is given by 60 degrees times the leg number. The equation for the transformation from the center to the foot frame, F, is the multiplication of all of the frame transformations of the joints. The D-H Table allowed the team to calculate the transformation matrices and the Jacobian matrix (Appendix A) needed for the inverse kinematics. C. Construction of the Robot The hexapod was designed and built entirely by the team. The actuator at each leg joint is a RoBoard-1270 digital servo motor. These servos were chosen specifically because they are capable of high torque (35 kg-cm). Eighteen of these servos are used on the robot. The RoBoard RB-100 Single Board Computer with onboard Linux was used as the brain and controller. Using a computer to perform the calculations and control the robot was necessary because real-time processing was needed for the movement of the robot. This particular computer runs at 1000 MHz, which is fast enough to handle the real-time processing without

B. Mapping of the Robot The robot was mapped using the Denavit-Hartenberg (D-H) approach. The goal was to create a transformation from the center of the robot to the foot of every leg. It is important to note that the orientation of the z-axis of the frame at each foot must be parallel to the frame at the center of the robot in order for the team’s control method to work. Figure 2 shows the frames that are used on a single leg of the robot.

stuttering. For ease and to free up pins on the Roboard, a Lynxmotion SSC-32 Servo Controller was used to interface with the servos. Current sensing was performed with an analog sensor board built designed at RIT. A Xbox 360 controller, with wireless USB interfacing, was used to control the robot. The hexapod body is built with carbon fiber with foam core sheets, and Lexan. Creating a very light body was essential to the hexapod development because extra weight can strain the motors.

position is denoted dz and is input into the vector. The angle, θ, of the joystick from its straight forward position will be used to determine the x-y portion of the vector of the robot. The input vector, V is shown below. 𝑣 ∗ cos 𝜃 𝑉 = � 𝑣 ∗ sin 𝜃 � 𝑑𝑧 (1)

In order to convert the input vector to the leg frame, a frame transformation must be performed. The input vector, V, is put into to the position column of the center differential matrix, cΔ (Appendix A, Eq. 3). Using the D-H Table of the robot, the transformation from the center of the robot to the leg frame, cTF, was found (Appendix A, Eq. 1). After finding cTF, the differential matrix is needed in order to get the change in position of the foot. The transformation below produces the foot frame differential matrix, ΔF. ∆𝐹 = 𝑐𝑇 𝐹 −1 ∗ cΔ ∗ 𝑐𝑇 𝐹 (2)

Figure 3: The final construction of the robot

IV.

PROPOSED APPROACH

A. Main Loop The robot operates in a while loop that is dependent on the input from the Xbox 360 controller. The user inputs a direction vector with the joystick on the controller. The vector is interpreted as the direction that the center of the robot should travel in the x-y plane. The method is to use inverse kinematics and differential motion to convert the vector into differential angles for each to servo to move. These differential values are added to the current servo angles, converted to pulse width modulation (PWM) signals and sent to the motors. The movement of the legs is an iterative procedure that continuously updates with the current vector from the joystick. Each leg has a finite number of differential movements, k, before it reaches the ground (at value k = kmax) and the next leg in the gait sequence starts moving starting once again with k = 0. While the x and y of the leg positions are determined by the input vector, the z position is defined with a function that is based on the incremented k value. The change in the z-position of the leg is fed directly into the differential motion equations that are shown later and carried through with the input vector from the joystick. The z-position of the leg is controlled by a function that utilizes both the iteration of k and the current sensing. B. Kinematics Inverse kinematics are used to determine the differential angles that the leg servos should move from one step to the next. First, a direction from the joystick is interpreted as a vector with step size, v. The height of the center of the robot should not change, so its value here is always zero. If the height of the center of the robot changes, that change in

Next, the differential change in the position of the foot frame, D, is extracted from ΔL. The Jacobian (J) (Appendix A, Eq. 2) of the cTF matrix must be calculated. To find the differential change in the motor angles, Dθ, the inverse of the Jacobian is pre-multiplied by D. The vector Dθ is the angular change that will be made in the current step. 𝜃1 𝐷𝜃 = �𝜃2 � = 𝐽−1 ∗ 𝐷 𝜃3

(3)

Finally, the differential change in motor position is added to the current servo position, converted to a pulse width modulation signal and sent via serial to the servo controller board. C. Z-Position Function The z-position of the foot frame uses a separate function that depends on the increment, k, and the current sensing to place the foot at the correct height from the ground. The basic idea is to move the leg up and down independently of the x and y positions of the leg from the input vector. For each increment, the leg moves a small amount along a path that is defined by the user. The more increments that are used, the smoother the motion will be. The change in the zposition that is defined by the function is input directly into the D vector before it is multiplied by the inverse Jacobian matrix to obtain the servo angles. The z-function comprises of a simple if-then statement that determines whether the leg is interacting with an uneven surface. The structure of the statement will be to check that the current that was sensed from the last movement is within an expected range for that movement. If the detected current differs from the expected current by a

substantial amount, the leg will stop moving. During the next movement, the body height will raise or lower to compensate for that obstacle. This will be done by adding the current height of the leg that has stopped moving to the z value of the vector, V and carrying through with the normal kinematics on all of the legs at once. This will raise the height of the body in one step. If no obstacle is detected with the current sensing, the zfunction will have a predefined motion. The function must start at k = 0 and end at k = kmax. There are very few other requirements for this function, but it must start and end at a height of zero from the ground. The tea experimented with a few different functions to compare their results. D. Incrementation Process Incrementing of a sub-step value is done to allow the robot to act based on real-time vectors from the controller. Every time an input from the Playstation 2 controller joystick is received by the Roboard, the processor will record the vector direction and will increment the sub-step, n. The speed that the robot can move its leg is dependent only on how fast the leg can increment through its cycle. A single sub-step cycle consists of a joystick press, the kinematic calculations, the determination of the z-function and the movement of the servos. This is a lot of processing to do in real time, so the algorithm is truly limited by how fast the computer can perform a single cycle. As k increases, the leg will move up, then down and then reach the ground at k = kmax. Every time k is increased, it must pass through function that determines which leg is moving. The function simply states that if k is greater than kmax, then k will be set back to zero and the next leg in the gait will be chosen, otherwise the leg will stay the same. This is done by checking if k equals zero and then determining the current leg. To choose the new leg, an ‘ifelse if’ statement determines the current leg and then changes the leg based on the pattern of the gait. The leg number variable is used in the kinematics to determine what angle the leg is at with respect to the center of the robot. It is also used to determine the leg to send the PWM signal to. V. EXPECTED RESULTS

VI.

SIMULATION

In order to test the hexapod’s movement and to develop a better understanding of how it will walk, a model was built in Webots™ robotic simulation software. This software allows the robot to be constructed in a similar manner to the physical robot. The physics engine that is incorporated in Webots™ allows an accurate recreation of the real hexapod. A terrain generator can also be used to test various environments. The servos in the simulation were initialized such that they could generate feedback for their position and supplied torque. Additionally, an accelerometer, a gyroscope, and a compass were added to the hexapods body frame. Using a MATLAB™ controller file, all of the sensors, servo feedback values, and time data can be collected during any simulation then plotted or stored in comma-separated values (CSV) files. These sensors are not on the hardware version of the hexapod but are included for data analysis and expandability purposes. This controller file can also implement motion files generated by a user. These motion files can control all of the joints of the hexapod to show gaits and other movements the hexapod can do. The servos can also be set from the main code as well. This allows joint values to be set with kinematic solutions and/or external controller such as an Xbox 360 or PlayStation 2 controller. The robot successfully demonstrated in simulation that it is fully cable of maneuvering itself using the novel control scheme. The simulation can also confirm that the inverse kinematics and motion files used are correct. An image of the Webots simulation model is shown below.

Figure 4: Webots simulation model

The robot is expected to be able to walk more fluidly and dynamically with the implemented structure. The main goal is for the hexapod’s legs to be able to change their direction mid-step based on the user’s input direction. The user will hold the joystick on the PlayStation 2 controller in a certain direction and one of the legs of the robot will begin to move in that direction. Once that leg comes back the ground, the next leg in the gait will pick up and move. If the joystick vector is changed at any point, the leg of the robot should continue moving, but its direction will change based on the new vector. If any leg encounters an obstacle or an uneven surface, the robot will automatically account for that surface based on how its current sensor readings. The robot body will move up or down to compensate for the obstacle.

VII. IMPLEMENTATION OF CODE A. Code Structure The robot was programmed entirely in C++ in Microsoft Visual Studio. The ‘main’ function initializes the robot and then runs a while loop that contains all of the robots functionality. Inside the loop, certain functions are initiated by button presses and joystick movements. The ‘A’ button reinitializes the robot to its main position. Up and down on the D-pad move the robot’s center up and down in the z-axis. Left and right on the D-pad rotate the robot about its z-axis, creating a twisting motion. The right analog joystick applies a rotational vector about the x and y axes. This allows the robot to tilt its body in any direction

specified. Lastly, the left analog joystick gives the robot a direction vector for it walk toward. Every time the loop repeats, it gathers all of the rotation and direction vector information and applies it to a single sub-step. The robot is programmed so that each leg’s movement is stored in a vector and then the inverse kinematics are applied to that vector. This way, each leg can move independently, yet also obeys the vector information given to them. Once the kinematics are performed and the leg servo angles are determined, those angles are converted to PWM and sent to the motors. A flow chart of the code structure is shown in Appendix B. B. Gait A simple three part gait was used to demonstrate the functionality of the walking scheme. The ‘Leg Movement’ function defines the gait movement. The gait works by moving two legs forward in the direction of the vector, V, and moving the rest of the legs in the opposite direction. Moving the other legs in the opposite the specified direction, on the ground, pushes the robot toward the direction that is desired. The two z-stepping legs move forward in the direction of the vector and follow the z-path that is specified. If the direction is changed mid-step, the legs will all change direction instantaneously, and the robot will begin to walking in the new direction. The gait simply sends two legs at a time moving forward and the other four legs moving backwards. When k, the sub-stepping counter, reaches kmax, the legs switch to the next pair of legs. The pattern moves counter-clockwise around the robot, with opposite pairs of legs moving at the same time. First, leg’s 0 and 3 move forward, then leg’s 1 and 4, and finally leg’s 2 and 5. Once, all of the legs have moved forward, the pattern resets and leg’s 0 and 3 begin again. The leg numbers are shown below in Figure 5.

input device, and data was sent to the SSC-32 motor controller via serial cable. This allowed the team to easily experiment and control the robot directly from Microsoft Visual Studio and be able avoid the hassle of programming and using Linux. The robot was able to walk in exactly the way that it was supposed to. It was also able to perform rotations around all three of its central axes. The control method worked as predicted and allowed the used to switch the direction of the body mid-step. Instantaneous changes in direction allowed the robot move freely and dynamically, unlike other hexapod robot walking methods. A. Calibration Each motor was calibrated with a linear fit to convert from the desired angle to a PWM signal to send to the motor. The curves were found by measuring the angle of the leg joints at different PWM inputs for each servo. Every calibration was distinct because every servo is slightly different. Limits were also determined so that the robot could not injure itself by moving to an unsafe position. Each servo was given angular limits that were calibrated for the walking gait. The robot was also given a maximum height that it can reach, so that it does not exert itself too high. The step size of the movement backwards and forwards was also calibrated so that the robot would move a certain distance. This was a bit tougher to do because as the number of sub-steps in the z-function changed, the distance that they needed to travel also changed. More calibration is needed to perfect stepping of the robot. B. Limitations During the project a few limitations were encountered that could be fixed in the future. One physical constraint was the current draw of the motors. The 18 Roboard 1270 servos drew about 2.5 Amps just standing still on the ground. During movement, when not all of the legs were on the ground, the total current draw was around 5 Amps. This caused the batteries to drain fairly quickly, so most experimentation had to be done without the legs on the ground. Another weakness was the fact that the robot cannot walk in place, or around in a circle, in place. This is because the gait always specifies vector of small displacements, meaning that it always has to go forward in one direction. The robot can come close to walking in a circle, in place, but it is different than most hexapod’s with standard kinematics, and it cannot perform this task. The team encountered a problem with imperfect calibrations. While the linear fits worked most of the time, there was an occasional instance when one of the legs would get stuck going in a certain direction or move too high or too low for the rest of the body. One issue with this control method is that it causes the robot to move slower. This is due to a variety of factors. One factor could be the large amount of calculations being performed or that the code is moving slowly. Another

Figure 5: This image shows the leg spacing and numbering

VIII. EXPERIMENTAL RESULTS The hexapod robot control scheme was implemented very successfully on the Tigerbug hexapod robot. The first usage of the control method was run directly from a Windows PC, due to problems with the Roboard 100 Single Board Computer. An XBOX 360 controller was used as the

reason could be that the motors are set at a relatively low speed, so they move slowly. If a shorter step size was used with more sub-steps, it is possible that the movement would further slowdown. An interesting side effect of speeding up the movement is that it makes the robot jerkier and the movements seem more granular. The last concern that was encountered was that the motors were very loud. While the robot was under strain, the motors produced a high pitched humming sound. This is probably just the normal sound that the motors make when in use. However, it is possible that the PWM signal that is being sent to the motors is the wrong format signal and is causing them to hum. IX. CONCLUSION

[3]

[4]

[5]

The Tigerbug hexapod robot’s movement control scheme was very successful. It allowed the robot to walk, move and orient itself correctly in almost all situations and positions. The walking scheme also let the hexapod change direction instantaneously and mid-step. This novel method of movement was versatile and lends itself well to future work. Overall, the hexapod robot method of walking was extremely effective. X. FUTURE WORK

[6]

and Computing (ICSTCC), 2012 16th International Conference on , vol., no., pp.1,5, 12-14 Oct. 2012 Graca, P.; Zimon, J., "Mechanical construction and kinematic calculations of the six-legged walking machine ARTHRON," Electrodynamic and Mechatronics, 2009. SCE 11 '09. 2nd International Students Conference on , vol., no., pp.23,24, 19-21 May 2009 Irawan, A.; Nonami, K., "Force Threshold-Based Omni-directional Movement for Hexapod Robot Walking on Uneven Terrain," Computational Intelligence, Modelling and Simulation (CIMSiM), 2012 Fourth International Conference on , vol., no., pp.127,132, 25-27 Sept. 2012 Sathya Kaliyamoorthy, Roger D. Quinn, and Sasha N. Zill. ”Force Sensors in Hexapod Locomotion.” The International Journal of Robotics Research July 2005 24: 563-574 Ziegler, S.; Woodward, R.C.; Iu, H.H.-C.; Borle, L.J., "Current Sensing Techniques: A Review," Sensors Journal, IEEE , vol.9, no.4, pp.354,376, April 2009

There are many capabilities that can be enhanced or added to this robot. One of the most significant improvements that can be made and will be made is the integration of an on-board computer system such as the RoBoard RB-100. This controller allows an install of a Windows or Linux Operating System onto it allowing a complete interface to the servos, sensors, and other devices. Additional sensors would also expand the hexapods maneuverability. Proximity sensors added to the outside of the hexapod, such as infrared (IR) or ultrasonic sensors, would allow it avoid obstacles. Other sensors such as an Inertial Measurement Unit (IMU) would also help improve the robots maneuverability and navigation. The data collected from the IMU could be used for balancing on uneven terrain and dead reckoning navigation. Experimentation of different walking gaits is also planned for this omni-directional hexapod. Furthermore, implementation of current sensing of the servos will be added as well. This would greatly increase the robot foot placement solutions and load detection. Additional hardware is currently being made for such a purpose using Allegro® ACS712 hall-effect current sense IC. REFERENCES [1] Seljanko, F., "Towards omnidirectional locomotion strategy for hexapod walking robot," Safety, Security, and Rescue Robotics (SSRR), 2011 IEEE International Symposium on , vol., no., pp.143,148, 1-5 Nov. 2011 [2] Sorin, M.-O.; Mircea, N., "The modeling of the hexapod mobile robot leg and associated interpolated movements while stepping," System Theory, Control

APPENDIX A: KINEMATICS Kinematics Equations 𝑪𝑻
𝑭 𝟎 𝑳𝟔 𝑪𝜽𝒍𝜽𝟏 + 𝑳𝟏 𝑪𝜽𝒍 + 𝑳𝟒 𝑪𝜽𝒍𝜽𝟏 𝑪𝜽𝟐 − 𝑪𝜽𝒍𝜽𝟏 𝑪𝜽𝟐 �𝑳𝟔 𝑪𝜽𝟑 − 𝑳𝟓 𝑺𝜽𝟑 � + 𝑪𝜽𝒍𝜽𝟏 𝑺𝜽𝟐 �𝑳𝟓 𝑪𝜽𝟑 − 𝑳𝟔 𝑺𝜽𝟑 � ⎡𝟏 𝟎 ⎤ ⎢𝟎 𝟏 𝟎 𝑳𝟔 𝑺𝜽𝒍𝜽𝟏 + 𝑳𝟏 𝑺𝜽𝒍 + 𝑳𝟒 𝑺𝜽𝒍𝜽𝟏 𝑪𝜽𝟐 − 𝑺𝜽𝒍𝜽𝟏 𝑪𝜽𝟐 �𝑳𝟔 𝑪𝜽𝟑 − 𝑳𝟓 𝑺𝜽𝟑 � + 𝑺𝜽𝒍𝜽𝟏 𝑺𝜽𝟐 �𝑳𝟓 𝑪𝜽𝟑 − 𝑳𝟔 𝑺𝜽𝟑 � ⎥ =⎢ ⎥ 𝑳𝟒 𝑺𝜽𝟐 − 𝑳𝟓 𝑪𝜽𝟐𝜽𝟑 − 𝑳𝟔 𝑺𝜽𝟐𝜽𝟑 − 𝑳𝟐 ⎥ ⎢ 𝟎 𝟎 𝟏 ⎣ 𝟎 𝟎 𝟎 ⎦ 𝟏
Equation 1: Transformation from center of robot to foot frame

−𝑺𝜽𝒍𝜽𝟏 (𝑳𝟑 − 𝑳𝟔 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟓 𝑺𝜽𝟐𝜽𝟑 + 𝑳𝟒 𝑪𝜽𝟐 ) 𝑱 = � 𝑪𝜽𝒍𝜽𝟏 (𝑳𝟑 − 𝑳𝟔 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟓 𝑺𝜽𝟐𝜽𝟑 + 𝑳𝟒 𝑪𝜽𝟐 ) 𝟎

Equation 2: Jacobian Matrix 𝑪𝜽𝒍𝜽𝟏

(𝑳𝟓 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟔 𝑺𝜽𝟐𝜽𝟑 − 𝑳𝟒 𝑺𝜽𝟐 ) 𝑪𝜽𝒍𝜽𝟏 (𝑳𝟓 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟔 𝑺𝜽𝟐𝜽𝟑 ) 𝑺𝜽𝒍𝜽𝟏 (𝑳𝟓 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟔 𝑺𝜽𝟐𝜽𝟑 − 𝑳𝟒 𝑺𝜽𝟐 ) 𝑺𝜽𝒍𝜽𝟏 (𝑳𝟓 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟔 𝑺𝜽𝟐𝜽𝟑 )� 𝑳𝟓 𝑺𝜽𝟐𝜽𝟑 − 𝑳𝟔 𝑪𝜽𝟐𝜽𝟑 + 𝑳𝟒 𝑪𝜽𝟐 𝑳𝟓 𝑺𝜽𝟐𝜽𝟑 − 𝑳𝟔 𝑪𝜽𝟐𝜽𝟑

Identities: 𝐶𝑎𝑏 = cos(𝑎 + 𝑏) 𝑆𝑎𝑏 = sin(𝑎 + 𝑏) Differential Motion Matrices
0 𝛿𝑧 −𝛿𝑦 𝑑𝑥 𝛿𝑥 𝑑𝑦 −𝛿𝑧 0 � cΔ = � 𝛿𝑦 −𝛿𝑥 0 𝑑𝑧 0 0 0 0

Equation 3: Differential Motion matrix for the Center frame of the robot

Equation 4: Input vectors for the robot

Where: 𝑣 ∗ cos 𝜃 𝑑𝑥 ⎡ 𝑣 ∗ sin 𝜃 ⎤ ⎡𝑑𝑦⎤ ⎢ ⎥ ⎢ ⎥ 𝑑𝑧 𝑉 � � = ⎢ 𝑑𝑧 ⎥ = ⎢ 𝑟𝑜𝑡(𝑥) ⎥ 𝑅𝑜𝑡 ⎢ 𝛿𝑥 ⎥ ⎢ ⎥ ⎢ 𝛿𝑦 ⎥ ⎢ 𝑟𝑜𝑡(𝑦) ⎥ ⎣ 𝛿𝑧 ⎦ ⎣ 𝑟𝑜𝑡(𝑧) ⎦

APPENDIX B: CODE STRUCTURE