A method for the automatic generation of inverse ... - SAGE Journals

Download PDF
2 downloads 0 Views 729KB Size Report
Comment
This article is organized as follows: we present the mate- rials and methods in the ...... machinists to build manufacturing platforms with prior knowledge of the ...
Research Article

A method for the automatic generation of inverse kinematic maps in modular robotic systems

International Journal of Advanced Robotic Systems September-October 2016: 1–15 ª The Author(s) 2016 DOI: 10.1177/1729881416662790 arx.sagepub.com

Mohammad Mayyas1 and Rochelle Mellish2

Abstract Flexible manufacturing based on rapidly reconfigurable robotic systems will enable factories to meet time-sensitive and fast-changing industrial demands. However, with the rise of modular systems there is also the need to quickly and easily determine which configuration is optimal for performing a certain task. In this article, we present a path-based ad hoc technique for determining the inverse and forward kinematics map based on relative joint space variable to reduce the computational complexity. The proposed technique is nonsingular and suits kinematic analysis and optimization of robotic systems with undetermined configuration, and it can be extended to solve generalized inverse kinematic of robotics system involving large number of joint variable. Keywords Flexible manufacturing, modular robot, inverse kinematics, automation Date received: 18 February 2016; accepted: 14 June 2016 Topic: Robot Manipulation and Control Topic Editor: Andrey V Savkin

Introduction In the ever-changing world of technology, electronic products are continually becoming smaller and smaller, implying the need for the micromanufacturing of system components. The microfactory is an industrial robotic setup that allows for the quick changing of a factory’s layout to meet the changing needs of industry.1–3 It is popular to use modular components for the construction of robots, as modular components tend to be independent agents, making them amenable to relocation within the robotic infrastructure.4 While downsizing for the purpose of micromanufacturing and modularizing joints for quick reconfigurability, designers must keep in mind the main concerns of the micromanufacturing industry, that is, the research of minimal actuations factory parts and fabrication accuracy. The fabrication process in an automated manufacturing setting is normally a set of commands given by a central processing unit to a robotic manipulator to follow a certain trajectory. Therefore, the path that a manipulator must take in Cartesian space is often known before a specific

robotic manipulator is designed. At this stage, the problem of fabrication comes down to selecting a configuration of prismatic and rotary robotic links that will minimize path error and also minimize the amount of space and time needed to complete the task. The most common way to obtain the joint angles needed for a robotic system is to solve kinematic problem. The approach can be applied when both the path and the robotic manipulator are known. The determination of configuration using inverse kinematic is a method widely used in biomechanics for the analysis of human motion. For example, inverse kinematic

1

Department of Engineering Technologies, Bowling Green State University, Bowling Green, OH, USA 2 Department of Aeronautical and Astronautical Engineering, Purdue University, West Lafayette, IN, USA Corresponding author: Mohammad Mayyas, Department of Engineering Technologies, Bowling Green State University, 219 Tech. Building, Bowling Green, OH 43403, USA. Email: [email protected]

Creative Commons CC-BY: This article is distributed under the terms of the Creative Commons Attribution 3.0 License (http://www.creativecommons.org/licenses/by/3.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (https://us.sagepub.com/en-us/nam/ open-access-at-sage).

2 analysis may be used to reconstruct anatomical parameters during some motor tasks such as handwriting or upper limb motor strategies. Vimercati et al.5 observed that participants with Down syndrome tend to draw faster but with less accuracy, or that the motor sequence adopted in a pointing and tapping task may be also affected by the pathology.6 Sale et al.7 adopted an end effector-based robot to control the kinematics of the lower limb in patients with Parkinson’s disease in order to improve their mobility. Another example is the work of Ancillao et al.8 who developed a protocol to reconstruct the kinematics of the upper limb and upper body during a drawing/handwriting task. At the qualitative level, previous work in determining configurations for robotic and kinematic tasks has taken an agent-based approach, where each link is an agent and the objective is to coordinate the behaviors of all agents. For example, in 1996, Carnegie Mellon University’s Robotics Institute began a program called the Agile Assembly Architecture, whose purpose was to create an Internet interface through which users could input which robotic components they desired to use; a decentralized cooperative framework enabled each component to be modular, and thus allowed the system to be easily reconfigurable, while maintaining high operability.9–12 In similar work by Chen,13 the tools for developing a reconfigurable robotic workcell were considered (with a workcell being a specific system component or robot). A model for automatic robot generation was presented, which was modular and geometry independent. Optimization of the robotic workcell was specified, and a communication and control framework presented for coordination among the system workcells, with the end goal being a CAD-like program that would enable the appropriate robotic configuration to be determined for a specific task.13 At the quantitative level, Barraquand et al.14 presented a robot path-planning method to handle high degrees of freedom (DOFs). In their method, a graph of the local minima of a potential function was constructed incrementally during the search for the goal configuration. Their method relied on numerical methods for a solution. Optimization from a slightly different approach was carried out by Snyman and Berner15 who optimized over robotic parameters (link lengths, masses, etc.) rather than positions. Other work includes the development of inverse kinematic mappings for ‘‘hyper-redundant’’ robots. In the study by Chirikjian,16 the author presents a ‘‘backbone curve’’ technique to compare manipulator configurations. Goldenberg et al.17 present a technique based on the Newton–Raphson numerical method to iteratively find an inverse kinematic mapping of a kinematically redundant robot. On the other hand, one work that closely addresses accuracy concerns is that by Park and Bobrow,18 in which the authors develop methods for optimizing robotic configurations via the ‘‘rigid body guidance problem.’’ Fault tolerance, that is, trajectory planning that takes into account the failure of a joint, was proposed by Paredis and Khosla.19 The authors discussed that added redundancy in the form of multiple actuators and joints can aid the robot in

International Journal of Advanced Robotic Systems completing a task. Their most important point was that certain joint configurations make it difficult to use the manipulator. In recent years, neural network-based technique has found various applications for inverse kinematics.20 The work by Li et al.21 proposes a strategy with hierarchical tree generation on topology and nonlinear neural dynamics to control multiple manipulator with only partial command. The method is globally stable and the optimality is guaranteed, which makes it scalable to large number and types of manipulators including redundant DOF system. To solve redundant manipulator under physical constraints, Zhang et al.22 proposed an online approach based on quadratic programming problem formulation for manipulator torque optimization using various levels of acceleration and velocity schemes. Another approach to solving the inverse kinematics problem for redundant robots was given by Tabandeh et al.,23 who used a genetic algorithm to solve the inverse kinematics problem for a redundant robot by way of a niching method. They also used a numerical inverse kinematics solver to attain arbitrary accuracies of joint angles. Work by Chen2 and Chen and Yang,24 addresses and solves the inverse kinematics problem for redundant serial and tree-type modular robots. Assuming that each link has only 1-DOF, is symmetric to all other links, the Jacobian is used to solve differential kinematics equations for the joint angles using the Newton–Raphson method. The work by Pac et al.25 uses interval analysis in inverse kinematics problem to provide a range of joint space variables for a desired end effector position. Wu et al.26 describe a rooted tree in module-level approach to generate kinematics of modular robots based on task-oriented concepts. Meredith and Maddock27 describe four different types of inverse kinematic mappings, one of which is called the analytical method; analytical methods are described as those that ‘‘reduce the inverse kinematics problem to a mathematical equation that need only be evaluated in a single step to produce a result.’’27 One of the drawbacks of analytical methods is that they are often not scalable enough to handle large chains of integrators.27 In many robotics applications, particularly in the case of a modular robot, it is economically efficient to determine the robotic configuration that will be the best for a certain task from an existing pool of components. For a system with many links, determining an inverse kinematic mapping for each possible configuration is time consuming, and limiting to the time-sensitive efficiency of many factories. Thus, an inverse kinematic mapping that can be applied to many different configurations of robotic components is needed. Given a path, a global definition of the translation and rotation necessary to move from one point to another must be reiterated in terms of the joint variables and joint limits. Many of the concerns for modular robotic systems have been addressed in the literature, particularly in developing globalized inverse kinematic maps whose coordinates can be mapped to various robotic structures. In addition, many continuous inverse kinematic problems based on linearization method have been addressed in literature to compute a

Mayyas and Mellish

3

Figure 1. Projections of the center of mass on a hemisphere, with t ¼ 0:1 The black squares represent the paths points p and q , and the lines between points p and q represent the line segments li . The dotted lines are the end effector vectors ei , and the black circles are the center of mass.

configuration of a manipulator capable of producing a prescribed trajectory in the task space.28–30 However, much of this work relies on the Homotopy technique or Newton-Raphson method, whose success depends on how close the initial guess of the solution is to the actual solution.31 For initial guesses that are close to the actual solution, the run time of the algorithm is low, and Newton–Raphson techniques are ideal. For large joint displacements over small periods of time, in which smaller sampling intervals must be considered to maintain reasonable initial guesses for Newton–Raphson, computational complexity will increase.

Research aim The first goal of this work is to present an algorithm that estimates the amount of rotation and translation needed to traverse a planar path using the path’s geometry. The second goal of this work is to outline a mapping scheme from a simplified robot to an actual 6-DOF manipulator, and then compare the manipulator’s performance for different configurations. The contribution of this article is to present a framework in which the joint angles may be retrieved for motion along planar paths in Cartesian coordinates. This method is different from previously derived inverse kinematics methods in that no knowledge of the robot’s structure is needed to determine the joint angles. The method used is a non-gradientbased optimization problem that focuses on changes in the joint angles rather than the actual joint angles of the robots at each point during the task. Since these coordinates may be mapped to the workspace of any robotic configuration, this framework is ideal for reconfigurable robots. The contribution is also to compare the performance of a more

complicated robotic arm to a simplified system, enabling the fast determination of manipulator efficiency along a path. This article is organized as follows: we present the materials and methods in the ‘‘Materials and methods’’ section. We introduce inverse kinematic mapping to find joint angles along a path. Then, we introduce a simple robot with four links, which is used to implement an existing inverse kinematics algorithm. Then, we map the joint angles from the inverse kinematics map to a physical robot by introducing constraint equations. We present the results and discussion in the ‘‘Results and discussion’’ section. In this section, we discuss simulation results highlighting the use of the inverse kinematic map and the results of mapping joint angles to a physical robot. Finally, in the ‘‘Conclusion’’ section, we offer concluding remarks and discuss future directions for research.

Materials and methods Path-based determination of the inverse kinematic map The inverse kinematic mapping of a robot—serial, parallel, and mixed—is based on a method that involves approximating the curve by sampling points along the curve, and using the joint center of gravity and the vectors between two consecutive path points to obtain translation and rotation of the joint. For consecutive movement increments, the amount of rotation and translation is divided among the joints in proportion with the total amount of movement the joint is capable of. Suppose we have a path P 2 R 3 parameterized by time, so that P ¼ PðtÞ. At each time step t, the path is specified by a point pðtÞ 2 R 3 , where t ¼ 1; 2; ::: . . . ; n and n is the total number of timeparameterized data points that comprise path P as illustrated in Figure 1. Let two consecutive points be given by p and q, where

4

International Journal of Advanced Robotic Systems

p ¼ pðtÞ and q ¼ pðt þ 1Þ. The homogeneous transformation of the end effector from point p to point q is given by T p ¼ q

(1)

where p ¼ ½p; 1T , q ¼ ½q; 1T , and where T 2 SEð3Þ is the homogeneous transformation   R r (2) T¼ 0 13 1 R ¼ RðÞ 2 SOð3Þ is a rotational transformation and r ¼ ½rx ; ry ; rz  is the translation of the center of mass. Notice that to solve this equation, two quantities must be specified: the translational joint angles, given by r, and the rotational joint angles, given by . With only knowledge of the path, finding this information reduces to a geometric problem, detailed below.

Theorem 1. Recall that the translation of the mth link’s center of mass is given by r, and let us denote the change in the center of mass’s location as dgðtÞ ¼ gðt þ dtÞ  gðtÞ. Then lim r  dgðtÞ ¼ 0

dt!0

Proof. In appendix 1. The results of this proof indicate that using a large number of data points enhances the accuracy of the inverse kinematic mapping. On the other hand, the number of data points is proportional to the amount of time taken for the robot to complete the path. If we let the time needed to compute a single data point be tc , the desired time limit be T , and the time increment be dt, then the proportionality relationship is given by tpath ¼

Translation of the end effector Suppose that between every consecutive pair of points p and q there is a straight line lt connecting point p to point q. This line segment has a length jlt j, and an orientation in the base frame O ¼ Oðt ; t ; t Þ, where  is the angle of rotation about x,  is the angle of rotation about y, and  is the rotation about z. We assume that the end effector of the manipulator is defined in coordinate frame with a vector in R 3 denoted by et , is always orthogonal to the line segment lt and axis of rotation for the path u^, that is, it lies in the plane of rotation. Therefore, the end effector et originates at the center of mass of the final link and ends at the path point q. To find the line segment et , we assume that the axis of rotation for the path, is known; in fact, it is the axis orthogonal to the plane of the path. With knowledge of the path’s axis of rotation (assumed to be parallel to one of the global axes x^, y^, or z^), we can form a 3-axis body coordinate system in which the axis of rotation is the first axis, and lt is the second axis. We would like for the final axis to point inward toward the center of rotation, thus serving as the end effector et and completing the 3-axis coordinate system. Therefore, the body frame’s coordinate system is given by b ¼ bð^ u; lt ; et Þ. We have that et ¼ u^  lt

(3)

(6)

T tc dt

(7)

Thus, although smaller time increments increase accuracy, they also increase the run time of the robot along the path. The engineer must balance throughput with accuracy by tweaking such parameters. c

Rotation of the end effector For any given vector in R3 , there are 3 rotational DOFs that may be used to characterize its orientation; they are rotation about the x-, y-, and z-axes. The overall pose of the end effector is often a coupling of all 3 rotational DOFs, making the individual determination of all 3 rotational DOFs elusive at best. In this section, we present a procedural and algorithmic approach to determine the rotation of the end effector in the plane. Since we have approximated the path with line segments between path-points, we may estimate the amount of rotation that the manipulator has undergone using the dot product between two consecutive end effector orientations etþ1 and et . Letting  represent the rotational angle needed to define the orientation of the end effector, we have   hetþ1 ; et i  ¼ arccos (8) jetþ1 jjet j

We assume that et acts through point q; thus, defining the center of mass to be the variable g, the location of the center of mass in R3 at point p is given by gt ¼ lt þ et . The change in location of the center of mass is what gives us the incremental translation of the manipulator. Hence, we have the translation as

This gives us the amount of rotation in the plane formed by the two vectors etþ1 and et . We measure the accuracy of the robot along the path by comparing the ‘‘fit’’ of the path generated from the inverse kinematics mapping—which is defined by a polygon that is passing through a set of vertices—to the original path, that is, we measure the Euclidean distance between points on the generated path and the corresponding points on the original path. Since error is cumulative, we compare performance at different path parcings by examining the average error per time increment. The error at time t is given by

dgt ¼ gtþ1  gt

"ðtÞ ¼ p  T ð0Þ pð0Þ  T ðtÞ pðtÞ

If we let d to be the distance from the path to the center of mass of the final link, then et is given by et ¼ d

et jet j

(4)

(5)

(9)

Mayyas and Mellish

5 z . The second rotation in sequence, which we can denote x , is about the local x-axis a^2 ¼ ½1; 0; 0T . Subproblem 2 requires finding an intermediate point h between p and q that can be reached by either of the transformations32 T2 ðx Þ p ¼ h T1 ðz Þ q ¼ h

Figure 2. The simple robot in the (1234) configuration shown in the top view and side view. Links 1, 2, and 3 are the x-, y-, and z-directional linear joints, respectively. Link 4 is the 2-DOF rotary joint. The white dot on link 4 is representative of an end effector.

where T ð0Þ pð0Þ is the initial location of the end effector. Therefore, for a total of n data points, the average error per unit time along the path is n 1X "avg ðtÞ ¼ j"ðtÞj n t¼1

We find the vector, z, that connects h to a predetermined point on the axis of rotation of the first rotation; here, we’ve chosen that point to be r ¼ ½0; 0; 0; 1T . Thus, z ¼ h  r. We also can define vectors between point p and r and point q and  0 ¼ p  r, and v0 ¼ q  r. Letting a^3 ¼ a^1  a^2 , and r as w then using a linear combination of the triad a^1 , a^2 , and a^3 gives z as z ¼ ^ a1 þ ^ a2 þ g^ a3

¼

¼

 0  a^1 v0 h^ a1 ; a^2 i^ aT2 w ðh^ a1 ; a^2 iÞ2  1 0 h^ a1 ; a^2 i^ aT1 v0  a^2 w ðh^ a1 ; a^2 iÞ 2  1

Global joint angles from a simplified manipulator

n X k¼1

n! ðn  kÞ!

(14)

(15)

and g2 ¼

 0 j 2   2   2  2h^ jw a1 ; a^2 i j^ a1  a^2 j 2

(16)

 To solve for Using z ¼ h  r, we can solve for the point h. the joint angles, we follow the steps outlined in Murray32 by solving subproblem 1 for x and z . To find z , if we define 0 ¼w  0  a^1 a^T1 w w

(17)

v ¼ v0  a^1 a^T1 v0

(18)

and

(11)

configurations possible from the four links, and we compare them based on average error and computational cost, where n is the original set of joints. For a given path we measure the error in the manipulator’s performance under the inverse kinematic map based on the equation (10). We obtain the joint angles from the paths using the wellknown Paden–Kahan subproblems produced by Murray et al.32 Since there are essentially only two revolute joints, we determine joint angles using Paden–Kahan subproblem 2, featured. The joint angle trajectories are determined from the path by applying subproblem 2 repeatedly to consecutive points along the path. Again, let the first point sampled be denoted by p, and the second point by q. We wish to find the amount of rotation and translation that has occurred between both points during the time increment dt. Starting with rotation, we determine the axis of revolution for the first rotation in sequence to be about the z-axis, whose unit vector is given by a^1 ¼ ½0; 0; 1T ; we denote this angle by

(13)

The coefficients , , and g are found by

(10)

In this section, we demonstrate the use of joint angles from an inverse kinematic map using a simple robot. We consider a robotic manipulator that consists of three linear joints: one that traverses the x direction, one that traverses the y direction, and one that traverses the z direction. It also consists of one rotary joint that is assumed to rotate about the local x- or z-axis. Such kind of robots essentially boil down to the inverse kinematics of a spherical dyad. The four links are shown in Figure 2. There are 64 configurations possible from these components, that is

(12)

then the rotation may be found as  T    vÞ a^ ðw dz ¼ tan1 1 T  v w

(19)

An identical procedure may be used to find x . To find the amount of translation that has occurred between two points, we first use homogeneous transformations using the two rotational angles on the point p, and then subtract the result from the point q. Thus, 2 3 cosðz Þ  sinðz Þ 0 R1 ¼ 4 sinðz Þ cosðz Þ 05 (20) 0 0 1 and 2

1 R2 ¼ 4 0 0

3 0 0 cosðx Þ  sinðx Þ 5 sinðx Þ cosðx Þ

(21)

6

International Journal of Advanced Robotic Systems

so that  T1 ¼

R1 013

031 1



 and

T2 ¼

R2 013

031 1

 (22)

we have that the translation is given by d ¼ q  T 1 T2 p. An example using this method is given in the ‘‘Results’’ section.

Mapping joint angles to a physical system The variables that we wish to find at each time step are the changes in the robot’s joint angles. The robot’s movement in 3-space can then be defined as fR ¼ fR ðd x; d y; d z; dÞ,  where d x, d y, d z, and d are the joint variables associated with each of the manipulator’s links; each variable can be a multidimensional vector depending on the redundancy of links exhibiting such motion. We define the global position variables obtained from the inverse kinematic mapping to be dx global , dy global , and dz global , while the global rotational displacement is given by dg global . Constraints between the physical robot and the three components of Cartesian space are given by equating the global position variables to the projections of the robot’s motion onto the three ordinal directions. That is, for the inertial unit vectors x^, y^, and z^ dx global dy global dz global

¼ fR  x^ ¼ fR  y^ ¼ fR  z^

(23)

We also represent constraints on the revolute motion of the robot as g global ¼

m X

 i di ;

(24)

i¼1

where m is the number of revolute variables affected by the constraint, g global is the constraint condition, di is the i th revolute variable, and  i is a weight. To determine the optimal solution angles based on minimum path error between time t and t þ 1, we propose constrained optimization, with the Lagrangian33 L ¼ fR ðd x; d y; d z; dÞ þ  1 ðdx global  fR  x^Þ þ  2 ðdy global  fR  y^Þ þ  3 ðdz global  fR  ^ zÞ m X  i di Þ þ  4 ðdg global 

(25)

i¼1

By taking partial derivatives of the Lagrangian with respect to each variable and setting the resulting equations equal to zero, we have @L ¼ 0; @d x

@L ¼ 0; @d y

@L ¼ 0; @d z

@L ¼0 @d

@L ¼ 0; @ 1

@L ¼ 0; @ 2

@L ¼ 0; @ 3

@L ¼0 @ 4

(26)

With this framework, we are able to solve for each of the desired joint angles of the target robot.

Figure 3. 6-DOF robot. Joints that enable rotation about the local z-axis are denoted by the cylindrical prisms, while joints that enable rotation about the local x-axis are denoted by circles. 6-DOF: 6 degrees of freedom.

Example using a 6-DOF manipulator Inverse kinematics using the simplified robotic structure gives us global definitions of the amount of rotation about the x- and y-axes, and translation in all three directions. The next step is to map these globally referenced joint variables to the 6-DOF robotic manipulator. Unlike the simplified representation, only 6-DOF robot has rotary links. To accomplish the mapping, we note that we have two rotational DOFs centered at the same center of rotation; during our comparison process, we sampled different configurations of the simplified robot, and moved the center of rotation to various positions within the robot’s structure. We were able to compare which rotation center produced the most accurate following of the three-dimensional (3-D) path. As can be seen from Figure 3, the manipulator has six joints like the center of rotation for the simplified robot, each successive pair of joints can rotate in about two directions. Thus, in the mapping process, we can choose which two joints we would like to be the center of rotation and assign the two rotational joint angles to that pair. For the remaining joint angles, a combination of the unused rotational DOFs would need to be used to produce the needed translational motion. Let di (i ¼ 1; . . . . . . 6) represent the i th joint angle of the 6-DOF manipulator, and let the optimal joint  angles be given by d ¼ ½d 1 ; d 2 ; d 3 ; d 4 ; d5 ; d6 T . Choosing d1 to be fixed and the manipulator’s 5th and 6th rotational joints, d 5 and d6 , respectively, to be the center of rotation, we may remove these angles from the vector d. For the remaining rotational DOFs, we have the following constraint relationships dx global ¼ l2 cosðd2 Þ þ l 3 cosðd 3 Þ þ l 5 cosðd5 Þ dy global ¼ l2 sinðd 2 Þ þ l3 sinðd3 Þ þ l5 sinðd5 Þ

(27)

Mayyas and Mellish

7

We now use constrained optimization to determine the remaining joint. The Lagrangian is given by   L ¼ d þ 1 dx global  l2 cosðd2 Þ  l3 cosðd3 Þ  l5 cosðd 5 Þ   þ 2 dy global  l2 sinðd 2 Þ  l3 sinðd 3 Þ  l 5 sinðd 5 Þ þ 3 ðB  d 6  d 4  d 1 Þ þ  4 ðA  d 2  d3  d 5 Þ

(28) where A and B are the two rotational DOFs retrieved from the all DOF robot, and  1 ,  2 ,  3 , and  4 are Lagrange multipliers. The partial derivatives of the Lagrangian are given by @L @L @L ¼ ¼ ¼ 1  3 ¼ 0 @d1 @d 6 @d4 @L ¼ 1 þ  1 l2 sinðd 2 Þ   2 l 2 cosðd 2 Þ   4 ¼ 0 @d2 @L ¼ 1 þ  1 l3 sinðd 3 Þ   2 l 3 cosðd 3 Þ   4 ¼ 0 @d3 @L ¼ 1 þ  1 l5 sinðd 5 Þ   2 ł 5 cosðd 5 Þ   4 ¼ 0 @d5 @L ¼ dx  l 2 cosðd 2 Þ  l 3 cosðd3 Þ  l 5 cosðd 5 Þ ¼ 0 @d 1 @L ¼ dy  l 2 sinðd 2 Þ  l 3 sinðd 3 Þ  l 5 sinðd 5 Þ ¼ 0 @d 2 @L ¼ B  d 6  d 4  d 1 ¼ 0 @d 3 @L ¼ A  d 2  d 3  d 5 ¼ 0 @d 4 (29) 2

  Kðd 2 Þ l2  6 0 6 6 0 6 J ¼6 l sinðd 2 2Þ 6 6 l 2 cosðd 2 Þ 6 4 0 1

0   Kðd 3 Þ l3  0 l 3 sinðd 3 Þ l 3 cosðd3 Þ 0 1

Suppose we choose joints 5 and 6 to be the center of rotation for the manipulator, and since joint 1 is at the base of the manipulator, we fix it so that it has zero rotation at all times. We can then reduce the size of the problem to one in which we have only three unknown joint angles to solve for. Thus, along with the Lagrange multipliers, the optimization variables we want to find are given by d ¼ ½d2 ; d 3 ; d 4 ;  1 ;  2 ;  3 ;  4 T , and the new reduced system to be optimized is f1 f2 f3 f4 f5 f6 f7

¼ 1 þ  1 l 2 sinðd 2 Þ   2 l 2 cosðd2 Þ   4 ¼ 1 þ  1 l 3 sinðd 3 Þ   2 l 3 cosðd3 Þ   4 ¼ 1  3 ¼ dx global  l 2 cosðd 2 Þ  l3 cosðd3 Þ  l 5 cosðd 5 Þ ¼ dy global  l 2 sinðd 2 Þ  l 3 sinðd 3 Þ  l 5 sinðd 5 Þ ¼ B  d 6  d4  d 1 ¼ A  d 2  d3  d 5 (30)

The procedure for finding the core set of partial derivatives when the center of rotation is either links 3 and 4 or 1 and 2 is a similar process of removing those equations for the joint variables that are already known. Equation (29) must be solved at every time step, and they have no closed form solution. Performing a Newton–Raphson technique at every time step could be computationally time consuming when path is discretized into very small step size. Thus, since we are taking small increments of movement and time, we linearize the equation (29) about the manipulator’s current pose, and solve the linearized system. Let ‘‘’’ denote the dot  ¼ ½ 1 ;  2 , and Kðdi Þ ¼ ½ cosðdi Þ; sinðdi Þ. product,  With the Jacobian matrix

0 l2 sinðd 2 Þ l2 cosðd 2 Þ 0 0 l3 sinðd 3 Þ l3 cosðd 3 Þ 0 0 0 0 1 0 l5 sinðd 5 Þ 0 0 0 l5 cosðd5 Þ 0 0 1 0 0 0 0 0 0 0

 The linearized system is given by J d ¼ 0. Thus, we survey the null space of equation (30) to find our optimal  solution d ¼ ½d2 ; d3 ; d 4 ; 1 ;  2 ;  3 ;  4 T at each time step. To show that the joint angles obtained from the null space of the linearized system are suitable for equation (31), we have the following theorem.  Theorem 2. As the size of the state d ! 0, the linear approximation of the partial derivatives given in equation (31) approaches the nonlinear partial derivatives of equation (30).

3 1 1 7 7 0 7 7 0 7 7 0 7 7 0 5 0

(31)

Proof. In Appendix 2. For sufficiently small d, we can use the null space of the Jacobian matrix J to find the joint angles d  . This raises the question of what is ‘‘sufficiently small’’ for the constructed framework, and how do we measure whether or not the joint angles commanded by the optimization fall within limits for which the linearization is valid? These questions lead to the next theorem. c Theorem 3. The Jacobian matrix J given by equation (31) is evaluated at the current value of the state d. That is

8

International Journal of Advanced Robotic Systems

J¼

@fi;L   j    ! J d ¼ J ðd Þd @d d¼d

(32)

Therefore, the linearization is valid for arbitrarily large d  . Proof. In appendix 3. Upon finding the optimal joint angles, we substitute them into the forward kinematic map for the 6-DOF robot. If we let the matrix describing local rotations about the y-axis be given by 2 3 0 sinðdi Þ cosðdi Þ 5 Ry ðdi Þ ¼ 4 0 1 0 (33)  sinðdi Þ 0 cosðdi Þ and the matrix describing local rotations about the z-axis be given by 2 3 cosðdi Þ  sinðdi Þ 0 Rz ðdi Þ ¼ 4 sinðdi Þ cosðdi Þ 05 (34) 0 0 1 and furthermore, let the center of mass of the i th link be given by di , the respective homogeneous transformations for rotary links about the y-axis and about the z-axis are given by     Ry di Rz di and Tz ðdi Þ ¼ (35) Ty ðdi Þ ¼ 0 1 0 1 Thus, if p ¼ ½x; y; z; 1T is the current location of the robotic manipulator, the transformation from the manipulator’s frame (m) to the robot’s base frame (b) is given by Tmb ðÞ ¼ Ty ðd 1 ÞTz ðd2 ÞTz ðd 3 ÞTy ðd4 ÞTz ðd5 ÞTy ðd 6 Þ p (36) Since the system is linearized, using small time increments would be the most advantageous for increased accuracy along the path. c Theorem 4. An optimal solution always exists for the system J di ¼ 0. Proof. Since any system Ax ¼ 0 has at least the solution x ¼ 0, the system (35) always has a solution. Therefore, the inverse kinematic mapping based on the relative joint space method has no singularities.

" 6 DOF ðtÞ ¼ qðtÞ  T6 DOF ð0Þ pð0Þ  T 6 DOF ðtÞ pðtÞ

(38)

pð0Þ and 6 DOF ¼T6 DOF ð0Þ pð0Þ. Let simple ¼ T simple ð0Þ With these substitutions, and dropping the ðtÞ from the defined variables for simplicity, we may solve equation (37) for the position p as p ¼ T 1 q  simple  " simple Þ simple ð

(39)

Substituting equation (39) into equation (38) for pðtÞ gives " 6 DOF ¼ q  T6 DOF T 1 q  simple  " simple Þ  6 DOF simple ð (40) This transformation gives a mapping at time t from the error measured by the simple robot to the error measured by the 6-DOF manipulator. Since the transformations T simple and T6 DOF are in the group SEð3Þ, they are guaranteed to be invertible for all time t.

Cost function Different configurations have dissimilar path-following attributes that may make certain configurations more desirable than others. For instance, the more a linear joint moves along its DOFs, the more actuation is needed to produce the motion. Also, the heavier a joint, the more stress on the actuators to overcome the joint’s inertia. Finally, the most important factor—how well the robot, using the inverse kinematic map, follows the path, or the accuracy of the robot along the path— must be considered in a cost function. In this section, we focus on formulating a cost function that encapsulates all of these concepts in order to aid us in identifying which configuration has the most efficient behavior along a given path. The cost function is composed of the joint variables, the inertia penalty, and the path error. We choose an energylike cost function so that it will be amenable to control design. To obtain the cost, we use 1 T C ¼ ð~ x Mx~ z þ Ip þ Ex2 þ Ey2 þ Ez2 Þ x þ~ y T My~ y þ~ z T Mz~ 2 (41)

Transforming error of the simple robot to the 6-DOF manipulator What if, given the average error per unit time of the simplified robot, we would like to predict the average error of the 6-DOF manipulator? Based on equation (13), we define the error measured by the simple robot to be " simple ðtÞ ¼ qðtÞ  T simple ð0Þ pð0Þ  T simple ðtÞ pðtÞ

the simple robot. In particular, we have included the initial pð0Þ location of the simple robot’s end effector as T simple ð0Þ and subtracted it from qðtÞ to ‘‘realign’’ the path traced by the physical robot with the commanded path given by the equation. Likewise, the error measured by the 6-DOF manipulator is given by

(37)

where pðtÞ and qðtÞ are the beginning and end points of the travel segment, T simple ðtÞ is the homogeneous transformation from the end effector’s frame to the base frame of

where ~ x, ~ y , and ~ z are the n  1 vectors containing the x–y– z variables for each joint of the robot, Mx , My , Mz are positive semi-definite weighting matrices, where we have chosen to set each equal to 2 3 m1 0 0 0 60 m2 0 0 7 6 7 Mx ¼ My ¼ Mz ¼ 6 (42) 7 .. 40 0 . 0 5 0 0 0 mn

Mayyas and Mellish

9

~ be the n  1 vector and Ip is the inertia penalty. Let  containing the angular displacement for each of the n joints, let I 1 , I 2 , . . . , In be the inertia of each joint about its own center of mass, and let Ir;1 ðtÞ

¼ m1

K X

d1;k ðtÞ 2

k¼1

Ir;2 ðtÞ

¼ m2

K X

d2;k ðtÞ 2

(43)

k¼1

Ir;n ðtÞ

¼

mn

K X

.. . dn;k ðtÞ 2

k¼1

where d1;k , d2;k , . . . , dn;k are the time-varying distances of the joints from each of the K centers of rotation. Note that only joints between the rotary joint and the path will be affected by the rotary inertia. Placing 2

I1 60 6  T6 Ip ¼ ðtÞ 6 40

0 I2 0

0 0 ..

0

0

0

.

Ey ¼ hE; y^i

(46)

zi Ez ¼ hE; ^

(47)

where E is the difference between the actual path p actual and the path generated from the inverse kinematics joint variables p IK , given by

0

0

0 ..

0

0

.

0

3 7 7  7ðtÞ 7 5

(44)

Ir;n ðtÞ

where 0  t  2 in steps of size dt. The simplified robot is in the (1234) configuration in Figure (3). Following the analysis in section ‘‘Materials and methods,’’ we assumed that the lengths of the three linear links are Lx ¼ Ly ¼ Lz ¼ 0:1 m. The results are shown in Figure 4, and confirm that smaller time increments—which correspond to small d increments in Theorem 3—produce higher accuracy between the generated curve and the curve produced using the output of the inverse kinematic map.

(48)

2-D path generation along a planar sinusoid— Generalized cost function

Results and discussion In this section, we present the results of simulations using the proposed methodology for determining two-dimensional (2D) path inverse kinematic maps, and for the case study of multiple configurations of a simplified robot whose joint angles have been mapped to a 6-DOF robotic arm. We implemented the algorithms in MATLAB (T. M. MATLAB 8.5, Inc., Natick, Massachusetts, United States).6

2-D path generation along a planar sinusoid—error cost function The planar path that we consider is given by xðtÞ ¼ yðtÞ ¼ zðtÞ ¼

all of these components together, we obtain the inertia penalty as

3 2 Ir;1 ðtÞ 0 0 7 6 Ir;2 ðtÞ 07 60  þ ðtÞ  T6 7ðtÞ 7 6 40 05 0 In 0 0

Ex , Ey , and Ez are the x-, y-, and z-projections of the path error, given by Ex ¼ hE; x^i (45)

E ¼ p actual  p IK

Figure 4. Tracing of a curved planar sinusoidal trajectory using the proposed inverse kinematic mapping. The time increments of dt ¼ 0:1 and dt ¼ 0:001 give average path errors of e ¼ 0:1609 and e ¼ 0:0016, respectively. The error cost function is linearly proportional to the time increment, and both have the same order of magnitude.

cosðtÞ sinðtÞ 0

(49)

To test the inverse kinematic mapping, we use a three stages ‘‘joints’’ robot. Each slot is stacked vertically (along the y-direction). We have two linear joints, the first with one redundant DOF (x) and the other with two DOFs (x–z), and a rotary joint. We make the simplifying assumption that the rotary joint is a disk, and the two linear joints are rectangular plates as shown in Figure 5. We will use this system to test the inverse kinematic mapping along a sinusoid. The joint dimensions, movement capabilities, and slot information are shown in Table 1. For this exercise, we study the robot in the 1-2-3 configuration, whose forward kinematic mapping is given by

10

International Journal of Advanced Robotic Systems 2 6 cosð3 Þ 6 6 60 TeO ðÞ ¼ 6 6 sinð Þ 6 3 4 0

0

 sinð 3 Þ

1

0

0

cosð 3 Þ

0

0

3 19 21 cosð3 Þ þ 7 400 80 7 7 7 S12 þ S22 þ S32 7 19 7 sinð 3 Þ S13 þ S23 þ S33 þ d2 þ 7 400 5 1

S11 þ S21 þ S31 þ a1 þ a2 þ

Figure 5. Physical representation of each stage ‘‘joint’’.

We can already see that for the selection of joint types and their slot orientations, the path of the end effector will always be planar, regardless of configuration. The general equation for the sinusoid that we use is    zt ¼ p start þ A sin xt (51) B where p start is the insertion point in the x–z plane of the sinusoid function, A is the amplitude of the sinusoidal wave,  is the frequency, and B is the wavelength. Using the position in TeO ð0Þ, we were able to identify values of xt and zt at t ¼ 0 (given by p1;x and p1;z , respectively) and solve for p start . With the parameters A ¼ 0:01, B ¼ 0:5, and  ¼ 2 , we determined p start to be given by the equation    p1;x p start ¼ p 1;z  A sin (52) B where p1;z is the z-component of the insertion point, and p1;x is the x-component. Table 2 and Figure 6 compare the accuracy of the inverse kinematic map along the sinusoidal curve for increasing numbers of data points. We use the 1-norm for comparison because using a single number allows us to quickly see the net effect of error over time. Taking the 1-norm of the difference between the desired and actual paths confirms that increasing the number of data points increases accuracy of the estimated path. Using the automatic-inverse kinematic map generation algorithm, we were able to compare n! configurations for the three-joint robot. The results are shown in Table 3. Under the chosen cost function, all of the six configurations that use all three joints have similar costs, while marked

(50)

differences in cost can be seen for the six configurations that use only two of the joints and the three configurations that use only one of the joints. The reason for this pattern in the six configurations that use all three joints may be that regardless of the joint sequence, the same net movement is undertaken by the robot. This seems to indicate that the order in which parts are assembled does not matter, and could be due to the commutative nature of many of the joints involved and the fact that the approximate inverses always have a ‘‘trivial’’ unique solution. Increases in the inertia penalty due to configurations that place the rotary joint in either the first slot or the second slot are leveraged by smaller translations made by the joints. The ‘‘sweeping motion’’ of the rotary joint, particularly when the rotary joint is further in position from the path, eliminates much of the needed translation. The costs for two-joint configurations is largely dependent on the joints being used. For instance, configuration 1-3 has a much lower cost than configuration 2-3 because 2-3 cannot traverse the entire sinusoid, and therefore accrues much more error than configuration 1-3. Configurations 1-2 and 2-1 actually have fairly low costs, even without the help of the rotary joint, because they are independently able to traverse the entire sinusoidal curve.

3-D path generation along a curved sinusoid To test the efficiency of the various configurations of the simple robot, we consider using a curved sinusoid, given by the parametric representation xðtÞ ¼ cosðtÞ yðtÞ ¼ sinðtÞ zðtÞ ¼ sinð15tÞ

(53)

where =3  t  2 =3, with increments of dt ¼ 0:001. We tested both the simple robot and the 6-DOF manipulator along the path. Starting with the simple robot of four groups of configurations shown in Figure 7, we chose to make it both massless and dimensionless, that is, all motion is concentrated at a single moving point. All 64 configurations were tested. Configuration sets that have the same cardinality (4, 3, 2, or 1 link(s)) also have the same average error, because of the small increments in movement taken. When infinitesimally small changes in the joint angles are used in majority of simplified manipulator, the end effector’s motion is independent of the sequence of links. The findings in Table 4 indicate that differences in average error between configurations depends on

Mayyas and Mellish

11

Table 1. Part and slot information. Part no. 1 2 3

Range

Symbol

End effector

Dimensions

Mass

x ¼ 0–1.0 m x ¼ 0–0.15 m; z ¼ 0–0.15 m  ¼ 0–2 rad

a1 a2; d2 3

0.125 m 0.1375 m 0.0475 m

1:1.395 m; w:0 .225 n1; h:0.100 m 1:0.275 m; w:0.130 m; h:0.04 m h:0.055 m; r:0.0475 m

44.1 kg 4.6 kg 2.0 kg

Slot no.

Rel. Loc. (m)

Symbol

Rel. Rot.

Relative to:

1 2 3

[0, 0, 1.950] [0, 0.15, 0] [0, 0.15, 0]

[S11, S12, S13] [S21, S22, S23] [S31, S32, S33]

I3  3 I3  3 I3  3

Origin 1 2

Table 2. Comparison of accuracy with 10, 20, 40 and 100 data points. Number of data points 10 20 40 100

1-Norm of position error 0.8609 0.3687 0.1268 0.0927

the number of links used. The same idea holds true for computational efficiency. The number of computational operations depends primarily on the implementation of the algorithm, and this number is independent of permutation when configurations of the same cardinality are used. Between cardinalities, the difference lies in the length of the chain of exponentials used to compute the forward kinematic map. For the multiplication of two matrices A and B of size b  n and n  p, matrix multiplication alone has bp ð2n  1Þ operations (often referred to as ‘‘flops’’). Depending on how the forward kinematics algorithms are implemented, matrix computations can be the most costly. Thus, the programmer must weigh the cost of production, judging between accuracy and computational efficiency. The patterns in accuracy and computational efficiency of the simple robot can be extended to the 6-DOF manipulator when considered as separate cases. However, generating a correspondence relationship between the simple robot’s four links and the links of the 6-DOF manipulator is elusive at best, since several of the 6-DOF robot’s joint angles contribute to multiple global DOFs. To see an example of this, suppose that we set dx global to zero to represent the removal of the simple robot’s x-directional link. The unevaluated Jacobian matrix equation (31) will still be the same as if we had not. The only way to alter the Jacobian would be to set dx global and dy global to zero (removing the xand y-directional links of the simple robot). Doing this would leave the equation B  d6  d 4  d 1 ¼ 0, and there are an infinite number of solutions to this equation. Mapping error from the simple robot to the 6-DOF manipulator is more reliable. Using equation (40) and only considering the case when all four links of the simple robot and all six links of the 6-DOF robot are used, the results of the error mapping are shown in Figure 7. The inputs were the forward kinematic maps of the simple and 6-DOF robots, the

error of the simple robot at time t, the end point q of a path segment at time t, and the initial positions simple and 6 DOF of each robot’s end effector. To generate Figure 8, the result of equation (40) was subtracted from the original path’s coordinates to obtain the actual path followed by the 6-DOF robot. As expected, since we use all four links of the simple robot, the error is relatively small, where there is no difference observed between the plotted curves in the graph.

Conclusion In this work, we have described a computationally efficient joint space algorithm that determines the configuration of a modular robotic system for a given path. We presented a framework in which the joint angles may be retrieved for motion along planar paths in Cartesian coordinates, where we reconstruct robot configurations using the change in the joint angles rather than the absolute values. To accomplish this, we first determine the inverse kinematic mapping based solely on the path traveled; then, applying the inverse kinematic map to all configurations of the robot, we compare each configuration based on accuracy in following the path, and minimal computation required. The developed algorithm allows users to input information about the joints of a robotic platform and receive an automatically generated forward kinematic mapping for any configuration of the joints, as well as the time-dependent joint variables along a given path. The generalized forward kinematic mapping can handle a set of distinct robotic joints as well as duplicate joints and still produce the appropriate mapping by the utilization of numerical part markers that the algorithm uses to query information about the joints considered. Using this inverse kinematic mapping has proven to render accurate reproductions of a path with bounded curvature and without singularity when the measured joint variables were placed through the forward kinematic mapping. We also successfully map joint angles obtained from an existing 3-D inverse kinematics method to a physical 6DOF manipulator and set up the framework for comparing multiple configurations of this robot. Although such simple manipulator may have closed form solution, but our method is suitable for solving inverse kinematic of robotics system involving large number of joint variable, example may include self-assembly of molecular scale robots. This

12

International Journal of Advanced Robotic Systems

Figure 6. (a) 40 data points and (b) 100 data points. Table 3. Comparison of 1-norm of costs for all configurations when n ¼ 3. Permutation

1-Norm of cost

(3-1-2),(3-2-1),(1-3-2),(1-2-3),(2-1-3),(2-3-1) (1-3), (3-1) (2-3), (3-2) (2-1), (1-2) (3) (1) (2)

546.3976 550.9591 12.3874 556.1739 16.4216 537.3598 12.4337

new comparison metric will enable designers and micromachinists to build manufacturing platforms with prior knowledge of the configurations that produce the optimal output for the task, hence speeding up the rapid prototyping process significantly. In future work, we will extend the two dimensional inverse kinematics scheme to three dimensions in order to completely specify a mechanism for the fast determination of optimal configurations in modular robotic platforms. We will also develop a cost function to more fully characterize the tradeoff between computational efficiency and accuracy.

Figure 7. Schematic of simple robot with four groups of configurations.(a) Group of one rotational joint, (b) group of two rotational joints, (c) group of three rotational joints, and (d) group of four rotational joints. Table 4. Four groups of configurations for the simple robot based on the number of links. The time increment is dt ¼ 0:001: Assigned number

Configurations (in order)

1,2,3,4,5,6,7,8,9,10,11,12,13; 14,15,16,17,18,19,20,21, 22,23 ;24 25,26,27,28,29,30,31,32, 33,34,35,36,35,37,38, 39,40,41,42,43,44,45,46,47,48 49,50,51,52,53,54,55,56,57,58,59,60 61,62,63,64

(1432),(1423),(1342),(1324), (1234),(1243), (4132),(4123), (4312),(4321),(4231),(4213), (3412),(3421),(3142),(3124), (3214),(3241), (2431),(2413), (2341),(2314),(2134),(2143) (341),(314),(431),(413),(143), (134),(241),(214),(421),(412), (142),(124),(231),(213),(321), (312),(132),(123),(234),(243), (324),(342),(432),(423) (41),(14),(31),(13),(21),(12), (34),(43),(24),(42),(23),(32) (1),(4),(3),(2)

Average error (mm), computational operations 0.000014344, 471

0.002154348, 407

0.009943061, 343 0.009929444, 279

Mayyas and Mellish

Figure 8. Comparison of path following given the original path pðtÞ, the path generated by the 6-DOF manipulator’s inverse kinematics map, the path generated by pðtÞ  " 6 DOF ðtÞ, and the path generated by pðtÞ  " simple ðtÞ. 6-DOF: 6 degrees of freedom.

Acknowledgement The authors would like to thank Dr Woo Ho Lee and Dr John Shin from the Robotics Division of the University of Texas at Arlington Research Institute for the valuable feedback. The authors declare that there is not conflict of interest that may have affected the outcomes of this work.

Declaration of conflicting interests The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was partially funded under a grant from Office of Naval Research.

References 1. Mishima N, Nakano S, Ashida K, et al. Development of a downsized modular manufacturing system and its efficiency analysis. In: 42nd CIRP Conference on Manufacturing Systems Sustainable Development of Manufacturing Systems, Grenoble, France, 24–26 June 2009. 2. Chen I-M, Yang G and Kang I-G. Numerical inverse kinematics for modular reconfigurable robots. Journal of Robotic Systems 1999; 16: 213–225. 3. Chen I-M. Modular robots. In: Nee AYC (ed) Handbook of manufacturing engineering and technology. London: Springer, 2015, pp. 2129–2168. 4. Horbach S, Ackermann J, Mu¨ller E, et al. Building blocks for adaptable factory systems. Robot Comput Integr Manuf 2011; 27: 735–740. 5. Vimercati S, Galli M, Stella G, et al. Clumsiness in fine motor tasks: evidence from the quantitative drawing evaluation of children with Down Syndrome. J Intell Disabil Res 2015; 59: 248–256.

13 6. Vimercati SL, Galli M, Rigoldi C, et al. Motor strategies and motor programs during an arm tapping task in adults with Down syndrome. Exp Brain Res 2013; 225: 333–338. 7. Sale P, De Pandis MF, Sova I, et al. Robot-assisted walking training for individuals with Parkinson’s disease: a pilot randomized controlled trial. BMC Neurol 2013; 13: 1. 8. Ancillao A, Galli M, Vimercati SL, et al. An optoelectronic based approach for handwriting capture. Comput Meth Prog Biomed 2013; 111: 357–365. 9. Gowdy J and Rizzi A. Programming in the architecture for agile assembly. In: Robotics and Automation, 1999. Proceedings. 1999 IEEE International Conference on, Detroit, MI, 1999, vol. 4. pp. 3103–3108. doi: 10.1109/ROBOT.1999.774070. 10. Rizzi AA and Hollis RL. Opportunities for increased intelligence and autonomy in robotic systems for manufacturing. In: Shirai Y and Hirosw S (eds) Robotics research. London: Springer, 1998, pp. 141–151. 11. Kume S and Rizzi A. A high-performance network infrastructure and protocols for distributed automation. In: Robotics and automation, 2001. Proceedings 2001 ICRA. IEEE international conference on, 2001, pp. 3121–3126. 12. Hollis R, Rizzi A, Brown H, et al. Toward a second-generation minifactory for precision assembly. In: International advances robotics program workshop on microrobots, micromachines and microsystems, Moscow, Russia, 2003. 13. Chen I-M. Rapid response manufacturing through a rapidly reconfigurable robotic workcell. Robot Comput Integr Manuf 2001; 17: 199–213. 14. Barraquand J, Langlois B and Latombe J-C. Numerical potential field techniques for robot path planning. IEEE Trans Syst Man Cybernet 1992; 22: 224–241. 15. Snyman J and Berner D. The design of a planar robotic manipulator for optimum performance of prescribed tasks. Struct Optim 1999; 18: 95–106. 16. Chirikjian GS. Hyper-redundant manipulator dynamics: a continuum approximation. Adv Robot 1994; 9: 217–243. 17. Goldenberg A, Benhabib B and Fenton RG. A complete generalized solution to the inverse kinematics of robots. IEEE J Robot Autom 1985; 1: 14–20. 18. Park FC and Bobrow JE. Geometric optimization algorithims for robot kinematic design. J Robot Syst 1995; 12: 453–463. 19. Paredis CJ and Khosla PK. Fault tolerant task execution through global trajectory planning. Reliab Eng Syst Saf 1996; 53: 225–235. 20. Li S, He J, Li Y, et al. Distributed Recurrent Neural Networks for Cooperative Control of Manipulators: A Game-Theoretic Perspective. IEEE Trans Neural Netw Learn Syst 2016; PP(99): 1–12. 21. Li S, Cui H, Li Y, et al. Decentralized control of collaborative redundant manipulators with partial command coverage via locally connected recurrent neural networks. Neural Comput Appl 2013; 23: 1051–1060. 22. Zhang Y, Ge SS and Lee TH. A unified quadraticprogramming-based dynamical system approach to joint torque optimization of physically constrained redundant manipulators. IEEE Trans Syst Man Cybernet B Cybernet 2004; 34: 2126–2132.

14

International Journal of Advanced Robotic Systems

23. Tabandeh S, Melek WW and Clark CM. An adaptive niching genetic algorithm approach for generating multiple solutions of serial manipulator inverse kinematics with applications to modular robots. Robotica 2010; 28: 493–507. 24. Chen I-Ming and Yang Guilin. Inverse kinematics for modular reconfigurable robots. Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on, Leuven, 1998. vol. 2, pp. 1647–1652. doi: 10.1109/ROBOT.1998.677390. 25. Pac MR, Rakotondrabe M, Khadraoui S, et al. Guaranteed Manipulator Precision via Interval Analysis of Inverse Kinematics. In: Proc. of ASME 2013 International Design Engineering Technical Conferences, Portland, Oregon, August 2013. 26. Wu Wenqiang, Guan Yisheng, Li Huaizhu, et al. Taskoriented inverse kinematics of modular reconfigurable robots. In: 2013 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Wollongong, Australia, 9–2 July 2013, pp. 1187–1192. IEEE. 27. Meredith M and Maddock S. Real-time inverse kinematics: the return of the Jacobian. Technical Report No. CS-04-06, Sheffiel: Department of Computer Science, University of Sheffield, 2004. 28. Sentis L. Compliant control of whole-body multi-contact behaviors in humanoid robots. In: Harada Kensuke, Yoshida Eiichi and Yukoi Kazuhito (eds) Motion planning for humanoid robots. London: Springer, 2010, pp. 29–66. ISBN 978-184996-220-9. 29. Chang P-H. A closed-form solution for inverse kinematics of robot manipulators with redundancy. IEEE J Robot Autom 1987; 3(5): 393–403. 30. Siciliano B. A closed-loop inverse kinematic scheme for online joint-based robot control. 1990; Robotica 8: 231–243. 31. Deuflhard P. Newton methods for nonlinear problems: affine invariance and adaptive algorithms. Vol. 35. Berlin: Springer Science & Business Media, 2011. 32. Murray RM, Li Z, Sastry SS, et al. A mathematical introduction to robotic manipulation. Boca Raton: CRC press, 1994. 33. Rockafellar RT. Lagrange multipliers and optimality. SIAM Rev 1993; 35(2): 183–238.

Appendix 1

We know that as dt ! 0, pðt þ dtÞ ! pðtÞ, which implies   that pðt þ dtÞ  pðtÞ ! 0. To show that pðtÞ I  T ðÞ vanishes, recall that   het ; etþt i (1.2) d ¼ arccos jet jjetþt j As dt ! 0 het ; etþt i het ; et i jet j 2 ! ! ¼1 jet jjetþt j jet jjet j jet j 2 !

(1.3)

d ¼ arccosð1Þ ¼ 0

!

T ðdÞ ¼ T ð0Þ ¼ I   ! pðtÞ I  T ðdÞ ¼ pðtÞðI  IÞ ¼ 0

(1.4)

This completes the proof.

A2. Proof of theorem 2 Proof. Let @fi for i ¼ 1; . . . . . . ; 7 represent the Taylor series expansion of the system (34), where we expand each equation 1; . . . . . . ; 7 separately. If we consider equilibrium to be  ¼ 0 (that is, when the linearization is theoretically equivalent to the nonlinear representation), then performing the Taylor series expansion on equation (30), and choosing the scenario in which i ¼ 0 to be the reference condition, we have @f 1 ¼ l 2 sin 2 þ d2 l2 1 cos 2  d 2 l 2 cos2 þ d 2  2 l 2 sin2  d 4 ¼ 0 @f 2 ¼ d 1 l 3 sin 3 þ  1 l 3 d 3 cos 3  d 2 l 3 cos3 þ  2 l3 d3 sin3  d 4 ¼ 0 @f 3 ¼ d 3 ¼ 0 @f 4 ¼ l 2 d2 sin 2 þ l3 d 3 sin3 þ l 5 d 5 sin 5 ¼ 0 @f 5 ¼ l 2 d 2 cos2  l 3 d 3 cos3  l 5 d 5 cos5 ¼ 0 @f 6 ¼ d 4 ¼ 0 @f 7 ¼ d 2  d3 ¼ 0 (2.1) Plugging in i ¼ 0 for the appropriate i gives

A1. Proof of theorem 1 Proof. Let et ¼ ½et 1T . Then we have     r  dgðtÞ ¼ qðtÞ  TðÞ pðtÞ  qðtÞ þ etþt  pðtÞ  et ¼ T ðÞ pðtÞ þ pðtÞ  etþt þ et   ¼ pðtÞ I  T ðÞ þ et  etþt   ¼ pðtÞ I  T ðÞ  lðtÞ     ¼ pðtÞ I  TðÞ  qðtÞ  pðtÞ     ¼ pðtÞ I  TðÞ  pðt þ dtÞ  pðtÞ

@f 1

¼

d2 l2  1  d 4 ¼ 0

@f 2 @f 3

¼ ¼

1 l3 d3  d 2 l 3  d 4 ¼ 0 d 3 ¼ 0

@f 4 @f 5

¼ l5 d5 sin5 ¼ 0 ¼ d 2 l 2  l3 d3  l 5 d 5 cos5 ¼ 0

@f 6

¼

d 4 ¼ 0

@f 7

¼

d 2  d3 ¼ 0

(2.2)

For the linear system, we may use the Jacobian matrix given in equation (31), to form the differential equation (1.1)

dd ¼ J d ¼ 0 dt

(2.3)

Mayyas and Mellish

15

Multiplying out the terms of this equation gives df1;L ¼ d 2 l 2  1 cos2 þ d 2 l 2  2 sin2 þ d 1 l 2 sin 2 dt d 2 l2 cos 2  d 4 ¼ 0 df2;L ¼ d 3 l 3  1 cos3 þ d 3 l 3  2 sin3 þ d 1 l 3 cos3 dt df3;L dt df4;L dt df5;L dt df6;L dt df7;L dt

Since  1 and 5 are known, any term pre-multiplied by them goes to zero. If we let @fi represent the i th partial derivative of equation (30), where i ¼ 1; . . . . . . ; 7, and dfi;L =dt represent the i th differential equation of equation (38), then we have that @fi;L @fi ðdÞ   ! 0asdi ! 0 (2.6) @d  Assuming that d ¼ 0 is our reference condition, this is

d 2 l2 cos 2  d 4 ¼ 0 ¼ d 3 ¼ 0 ¼ d 2 l 2 sin 2 þ d3 l 3 sin3 þ d 1 l 5 sin5 ¼ 0

dfi;L j  d ¼ 0 @d d¼0

¼ d 2 l 2 cos 2  d 3 l 3 cos3  d 1 l 5 cos 5 ¼ 0

(2.7)

However, this is precisely equation (2.7). This completes the proof.

¼ d 4 ¼ 0 ¼ d 2  d3 ¼ 0 (2.4)

Again, plugging in i ¼ 0 for the appropriate i gives df 1;L dt df 2;L dt df 3;L dt df 4;L dt df 5;L dt df 6;L dt df 7;L dt

3 2 3 f 1 ðdÞ f 1 ðd  Þ dfi;L 6 .. 7 6. 7   4. 5 ¼ 4 .. 5þ  jd ¼d @  @d  f 7 ðdÞ f 7 ðd Þ

¼  1 l 3 d 3  d 2 l 3  d 4 ¼ 0 ¼ d3 ¼ 0

¼ d2 l 2  l 3 d 3  l 5 d 1 cos 5 ¼ 0 ¼ d4 ¼ 0 ¼ d2  d 3 ¼ 0

Proof. Considering that the first-order linear approximation of a system near the reference state d  is given by 2

¼ d 2 l 2  1  d 4 ¼ 0

¼ l 5 d 5 sin 5 ¼ 0

A3. Proof of theorem 3

(2.5)

(3.1)

We can readily see that for our framework, d ¼ d  . Hence we are left with the zeroith-order term for ½f1 ; ::: . . . ; f 7 T . That is 2

3 2 3 f 1 ðd  Þ f 1 ðdÞ 6 .. 7 6. 7 4. 5 ¼ 4 .. 5 f 7 ðdÞ f 7 ðd  Þ This completes the proof.

(3.2)