Control hybrid force-position for a end effector ofthree fingers

Vladimir Prada Jiménez1,a, Oscar Fernando Avilés Sánchez2,b,Mauricio Felipe Mauledoux Monroy3,c.

March 1, 2013

1Mechatronics Engineering Program. Militar Nueva Granada University. Bogotá. Colombia.2Mechatronics Engineering Program. Militar Nueva Granada University. Bogotá. Colombia.3Mechatronics Engineering Program. Militar Nueva Granada University. Bogotá. Colombia.

avladmont@gmail.com, boscar.aviles@unimilitar.edu.co,cmauricio.mauledoux@unimilitar.edu.co

Keywords: Control, Dynamics, GDL, Hybrid, Kinematics, Modeling, Position, Robotics, Simula-tion, Strength.

Abstract. This paper describes the implementation of a hybrid controller to an end effector of threefingers, each finger with two degrees of freedom (2 DOF). Since the mobility of each phalanx showsthe workspace by finger. Modeling is presented which includes the kinematics and dynamics ofeffector and implementation of force-position hybrid controller. Simulations are presented to validatethe behavior of the robot and the controller in Matlab Simulink.

Introduction.

The man has always had interest in research and implementation of machines that will provide solu-tions to their main need in highlighting the high accuracy tasks, requiring repetitive or force amongothers. For this is that in the first century b.C. Autonomous machine appears it was an artifact with fireand wind created by Ctesibius of Alexandria, Philo of Byzantium, Heron of Alexandria and others.Then in 1206 shows a boat with four robotic musicians that is the first programmable humanoid robotcreated by Al Jazari. Designs appear humanoid robot Leonardo da Vinci and is crossed by great con-tributions throughout history until the outstanding achievement of current ASIMO humanoid robotfrom Honda Motor Co. Ltd which is able to move from biped form and interact with people. On theline of industrial robotics includes the installation of the first industrial robot in 1961 Unimate createdby George Devol, the first six-axis robot with electromechanical Famulus in 1973 created by KUKARobot Group and Universal programmable manipulator arm PUMA in 1975 created by Victo Schein-man and the Da Vinci robot in 1999 that allows surgeons to perform more complex interventionsusing manipulators [1–3].

1

This research project grew out of a search of the needs found in applications to industrial and medical.Consulting found that today’s engineers and doctors have seen that robots are a tangible solution interms of execution time, efficiency, quality, accuracy, object manipulation, etc. [4, 5]. But in somecases these benefits are not sufficient for handling objects of different size or weight, or cyclic tasksrequiring speed, avoiding obstacles, with precise movements and with different levels of force. Withso many needs present in our country and with different characteristics was decided to work on thecontrol both in position and force covering both medical and industrial applications.

These specific problems involve the design and construction of new end effectors and control methods.Therefore it is desired to control the movement of an effector of three fingers with 2 GDL hybridcontrol by force-position. The validation of this work is done through systems of computer-aideddesign (CAD), computer aided engineering (CAE), which integrates finite element analysis (FEA),and Matlab [3, 6].

Modeling with 2 DOF finger-type Rotational Rotational.

The modeling is called an abstract representation, conceptual, graphic, physics, mathematics, systemsto analyze, explain, simulate or control. A model to determine a final result from a data input. Themodel is then performed mathematical and physical type. This model includes a numeric representa-tion for structured logical aspects and aspects of mathematical science and physical reality played ina simplified system, a model or prototype that has some bearing on the likely reality modeling. [7–9].

Figure 1: Articulation RR interconnected with two links.

Morphology of the finger. The morphology of the robot refers to the description of components, partsand mechanical structure. An effector is composed of a consecutive series of links and joints to forman open kinematic chain [8]. Each joint or seal is an interconnection between two consecutive links.In Fig. 1 illustrates an open kinematic chain of each finger. Said kinematic chain is constituted asfollows: the first articulation serves to form the base, followed by an articulation between connected

2

links. Each joint contributes a degree of freedom and this may produce a rotational movement. Thejoints which produce a rotational movement or rotational rotational joints are called R [10].

A link is formed by a rigid bar in this case metal rotor mechanically coupled to the next joint andcoupled to the motor stator. The number of degrees of freedom that the finger has depends of thenumber of independent variables position that have to be specified in order to locate all parts of themechanism in a workspace.

In the next section shows in detail the modeling workspace and GDL finger 2, which lead to mechan-ical design and torque values for the motors.

Kinematics. Kinematics is the science that studies the movement of an object without consideringthe forces that cause it. This area of science is responsible for studying the position, velocity andacceleration among others. In direct and inverse kinematics is considered the position and orientationof the joints of the manipulators in static conditions [8].

To understand the geometry must allocate effector coordinate system to the various parts of the mech-anism, in particular joints [8, 9]. Any robot kinematics can be described as providing four quantitiesvalues for each link. Two describes the board itself, and two describe the connection of the joint withan adjacent set. In the case of a joint angle, θi variable called articulation and the other quantities arefixed parameters of board. For prismatic joints, di articulation is variable and the other quantities arefixed parameters together. The definition of mechanisms through these quantities is a convention thatis generally known as Denavit-Hartenberg notation [9].

Below is a summary of the parameters in terms of the joint coordinate system and a summary ofthe procedure for assigning coordinate systems presented together Denavit-Hartenberg notation, it isnecessary to describe the kinematics of the robot:

Summary joints parameters in terms of the coordinate systems in general.

ai=the distance of Zi to Z(i+1) measured on Xi

αi=the angle of Zi to Z(i+1) measured onXi

di=the distance of X(i−1) to Xi measured on Zi

θi=the angle of X(i−1) to Xi measured on Zi

For the modeling of the manipulator is necessary coordinate systems to assign each of the joints andare numbered starting from a stationary base which is called {0} or system reference frame. Describesthe position of all other boards coordinate system in terms of the system. The first system is movablebody 1 and so on until the free end of the arm [9].

For 2 GDL finger coordinate systems are assigned as follows:

3

Figure 2: Assignments to coordinate axes rotational pairs.

Table 1: Parameters link manipulator

The coordinate system {0} is arbitrary, so that X0 is matched to the coordinate system by which therotational joint variable is 0. The direction of X1 is selected so that it always aligns with X0 whenθ1 = 0 and the origin of the frame 1 is selected so that d1 = 0. Since the axes of articulation areparallel to the axes Z considered pointing out the role, making all αi are zero. For frame 2, X2 alignedwith the rotation axis of the joint 2 and the axis X2 with the arm 2 as shown in Fig 2.

D-H parameters are shown in Table 1, and describe the major features of the effector as are the anglesof rotation or rotóides (θi,αi), the lengths of the arms (ai) and the distances between the joints (di).

Forward Kinematics. Direct geometric pattern (DGP) or Forward Kinematics (FK) to determine thedependency between joints or generalized coordinates, their geometric parameters and the cartesiancoordinates [x, y, z] and orientation [θ,φ,ψ] of the last coordinate system or final end effector withrespect to the two joints. To determine the position of the last frame with respect to the base of thefinger, use the transformation matrix which in general is given in Eq. 1 [8].

i−1i T =

[n(q) s(q) a(q) | p(q)

0 0 0 | 1

](1)

In mathematical form, wherei−1i T is the transformation matrix of joint i with respect to the joint i−1.

For RR finger is necessary to know the position of the weft end by finding the transformation matrix03T using matrices 0

1T , 12T , 2

3T .

The transformation matrix 03T shown in Eq. 2:

03T = 0

1T 12T 2

3T

03T =

Cos(θ1 +θ2) −Sen(θ1 +θ2) 0 Cos(θ1 +θ2)L2 +Cos(θ1)L1Sen(θ1 +θ2) Cos(θ1 +θ2) 0 Sen(θ1 +θ2)L2 +Sen(θ1)L1

0 0 1 d20 0 0 1

(2)

4

With the transformation matrix and in particular the translation vector p(q), Determines the robotwork space as they have both the position x and as y the end effector.

Workspace. The workspace is the physical layout of work that covers the end point of each finger ac-cording to the degree of mobility of each joint. This was calculated from the kinematic characteristicof each finger. Therefore recursive algorithm is implemented using the Eq. 3 and 4. In order to knowthe workspace finger sweeps in the range of each joint with the lengths of the links specified above inTable 1.

px = cos(θ1 +θ2)L2 + cos(θ1)L1 (3)

py = sin(θ1 +θ2)L2 + sin(θ1)L1 (4)

The Fig. 3 shows blue color points of the trajectory that makes the end of the link 1 and the greenarea covering the end of the link 2.

Figure 3: Representation workspace finger under kinematic conditions.

Inverse Kinematics. The inverse geometric model (IGM) or Inverse Kinematics (IK) in an area ofrobotics is more complex than the forward kinematics. It is always possible to find direct kinematicswhile the inverse kinematics can be various scenarios, for example may have several solutions or haveno analytical solution, if this is the case then it may be proposed numerical methods, iterative, geo-metric, etc.., As potential forms of solution. Inverse kinematics is a nonlinear problem then relatingthe joint coordinates in cartesian coordinates function. For common kinematic configurations, includ-ing three fingers effector with 2 DOF each one, you can use a geometric method to find the variablesθ1 and θ2 [7]. In the case of 2 DOF effector determines the value of the joint angle θi geometrically,projecting the configuration of the manipulator on plane x, y as shown in Fig. 4 subtracting Eq. 5 and6. For θ2:

Cosθ2 =P2

x +P2y −L2

1−L22

2L1L2= r

Using the trigonometric identity Cos2θ2 +Sen2θ2 = 1, can see that:

Senθ2 =±√

1− r2

Then:θ2 = Atan(±

√1− r2,r) (5)

5

Figure 4: Geometric method in the 2-DOF robot.

It is observed from the equation forθ2 have two possible solutions depending on the sign of the radical.

For θ1 :θ1 = Atan(Py,Px)−Atan(L2Senθ2,L1 +L2Cosθ2) (6)

Figure 5: Possible solutions for a position.

One problem that is presented to solve is the kinematic equations of multiple solutions. A two planarfinger joints can achieve angular position in a workspace elbow calls up or down according to elbowsign is taken of the equation (5), as shown in Fig. 5.

6

The fact that a finger has multiple solutions may cause problems, since the system has to be ableto choose one. The criteria on which to base a decision vary, but a reasonable choice would be theclosest solution to the endpoint of movement or to dodge an obstacle. We assume that there are noobstacles in the robot’s workspace taken if the positive sign indicating that work side down.

Direct Differential Kinematics. The kinematics is directly differential derivative with respect to timeof the direct kinematics.

ddt[xyzθφψ]T =

[vw

]=

ddt

fR(q)

where q represents the vector of generalized joint positions effector, θφψ are the ultimate orientationof the robot tool, fR is a continuous and differentiable function in the state variable q and [xyz]T arethe cartesian coordinates.

ddt[xyzθφψ]T =

∂ fR(q)∂q

q = J(q)q

As seen, this relates the joint velocity q belongs to Rn with linear velocity v = ddt [xyz]T = d

dt [x y z]T

belongs to R3 and the angular velocity w = ddt [θφψ]T = d

dt [θ φ ψ]T belongs to R3, Also the mappingis described in terms of a matrix J(θ) = ∂ fR(q)

∂q belongs to R6xn robot called Jacobian or Jacobiananalytical:

J(q) =[

Jv(q)Jw(q)

]

Jv(q) relates the joint velocity q with the linear velocity v, while Jw(q) relates the angular speed wwith joint velocity q, namely:

[vw

]= J(q)q =

[Jv(q)qJw(q)q

]

The effector Jacobian represents an important tool in robotics as it serves to characterize a robot, findsingular configurations, redundancy analysis, differential inverse kinematics determine and describethe relationship between the applied force and the resulting torques pairs or end end . It is essentialfor the analysis and design of control algorithms such as cartesian control hybrid [8]. DifferentialInverse Kinematics. The inverse kinematics differential represents the relationship between the jointvelocity q with the linear speed of movement v and the angular speed w, expressed in terms of theinverse of the Jacobian of the robot:

q = J−1(q)[

vw

]7

where J−1(q) is the inverse of the Jacobian matrix of the robot, which if there is a square matrix andits determinant is nonzero.

If the robot Jacobian determinant J(q) is zero, then we say that is not full rank and singularity prob-lems occur. Uniqueness means that it is possible to indicate an arbitrary motion of the robot at theextreme end, i.e. for a linear velocity v and angular velocity w finite speed may correspond articularq infinite.

Mass Properties. One of the studies necessary to analyze the static behavior of robot kinematics,and also calculate the center of mass, inertia matrix, principal moments of inertia, products of inertia,parallel axis theorem of a rigid body as necessary to develop robot dynamics [9].

In the case of 2 DOF effector coordinate systems are designed on the rotation axes of the joints. Asthe effector comprises various elements, it is necessary to find the moment of inertia in x, y and z ofeach element and using the parallel axis theorem to find inertia with respect to the rotation axis orcoordinate systems and well known turn inertia tensor. As this is a consuming task and the inertiatensor is needed to understand the dynamics of the robot is used SolidWorks to find out, this being agood approximation. The inertia tensor measured for the first link from the coordinate system {1}(See Fig. 2) in [kg.mm2] is:

1I =

422.107 −0.936 228.284−0.936 23090.834 0.001228.284 0.001 23134.654

With the center of mass located in (See Fig. 6 a.), [mm] units:

X = 120.389

Y = 0

Z = 1.22

the inertia tensor for the first link measured at the center of mass in [kg.mm2] is:

c.m.1I =

420.899 −0.532 109.234−0.532 11357.538 0.005109.234 0.005 11402.566

The inertia tensor measured for the second link from the coordinate system {2} (See Fig. 2) in[kg.mm2] is:

2I =

386.08 0 −2.080 17852.73 0

−2.08 0 17899.82

With the center of mass located in (See Fig. 6 b.), [mm] units:

X = 114.37

Y = 0

Z = 0.31

8

the inertia tensor of the second link measured at the center of mass in [kg.mm2] is:

c.m.2I =

386.01 0 −26.960 8735.63 0

−26.96 0 8782.79

Figure 6: a) Mass center of the link 1. b) Mass center of the link 2.

Dynamic. The dynamic is the part of physics that describes the time evolution of a physical system,linking changes in motion with respect to the causes that generate it. The objective is to describethe dynamics of the factors that may cause alterations (gravity, friction, inertia) of a physical system,quantify and raise equations of motion or evolution equations for the operating system [11]. Thestudy of the dynamics are of great importance in mechanical systems.

There are several ways to raise equations of motion to predict the time evolution of a mechanicalsystem depending on the initial conditions and forces acting. Lagrangian mechanics, this method alsouses ordinary differential equations of second order, but allows the use of fully general coordinates,called generalized coordinates, which are better adapted to the geometry of the problem, ie, that theequations are valid in any system inertial reference or it is not. [9, 12, 13]

Dynamic Reverse. The inverse dynamic model (IDM) to express the behavior of the manipulatorby means of a torque value Fig. 4, having as parameter values for position, speed and acceleration,using the equations of motion. For two effector DOF modeling was performed using the Lagrangianformulation.

With the Lagrangian formulation, the equations of motion can be derived systematically coordinatesystem independent. Once the set of coordinates qi, i = 1, ...,n called generalized coordinates is cho-sen which describes the actual position of the manipulator link n degrees of freedom, the Lagrangianof the mechanical system can be defined as a function of generalized coordinates as follows (See Eq.7) as:

L = K−P (7)

9

where K is the kinetic energy and P the potential energy of the system [8].

The equation of motion for a conservative system is given by the Eq. 8:

τi =ddt

∂L∂qi− ∂L

∂qi(8)

Whereq is a vector n generalized coordinates qi, τ is a vector n generalized forces τi and the La-grangian is the difference between kinetic energy and potential.

To understand the dynamics of 2 DOF robot must work the following assignments as shown in Fig.7, the gravity vector is entering the sheet.

Figure 7: Location centers of mass of the links.

where:

mi=Mass of link i

Ii=Inertia of link i

Li=Distance of the link i

Lcmi=Distance from the frame i the center of mass of link i

10

Ixxi=Moment of inertia of link i with respect to the axis x

Iyyi=Moment of inertia of link i with respect to the axis y

Izzi=Moment of inertia of link i with respect to the axis z

Returning to the Lagrangian formulation [14] for 2-DOF finger, you have to first link the kinetic andpotential energy with respect to the coordinate system {1}, Eq. 9 and 10:

K1 =12

m1L2cm1θ

21 +

12

Izz1 θ21 (9)

P1 = 0 (10)

For the second link kinetic and potential energy with respect to the coordinate system {2}, Eq 11 y12:

K2 =12m2v2

2 +12 Izz2(θ1 + θ2)

2

Where:

v22 = x2

2 + y22

x2 = L1Cos(θ1)+L2Cos(θ1 +θ2)

y2 = L1Sen(θ1)+L2Sen(θ1 +θ2)

x2 =−L1θ1Sen(θ1)−L2(θ1 + θ2)Sen(θ1 +θ2)

y2 = L1θ1Cos(θ1)+L2(θ1 + θ2)Cos(θ1 +θ2)

v22 = L2

1θ21 +L2

2(θ1 + θ2)2 +2L1L2(θ

21 + θ1θ2)Cos(θ2)

Therefore:K2 =

12

m2[L21θ

21 +L2

2(θ1 + θ2)2 +2L1Lcm2(θ

21 + θ1θ2)Cos(θ2)]+

12

Izz2(θ1 + θ2)2 (11)

P2 = 0 (12)

Then are added the potential and kinetic energies of each arm, Eq. 9 with Eq. 11 and Eq. 10 withEq. 12. After performing the subtraction of the kinetic energy less the potential according to theLagrangian formulation as shown in Eq. 13:

L =12

m1L2cm1θ

21 +

12

Izz1 θ21 +

12

m2[L21θ

21 +L2

cm2(θ1 + θ2)2+

2L1Lcm2(θ21 + θ1θ2)Cos(θ2)]+

12

Izz2(θ1 + θ2)2 (13)

11

From Lagrange equation derived are performed, and partial derivatives to obtain the equation of mo-tion τ:

∂L∂θ1

=

m1L2cm1θ1 + Izz1 θ1 +m2L2

1θ1 +m2L2cm2θ1 +m2L2

cm2θ2 +m2L1Lcm2(2θ1 + θ2)Cos(θ2)+ Izz2 θ1 + Izz2 θ2

ddt

∂L∂θ1

= m1L2cm1θ1 + Izz1 θ1 +m2L2

1θ1 +m2L2cm2θ1 +m2L2

cm2θ2 +m2L1Lcm2(2θ1 + θ2)Cos(θ2)−m2L1Lcm2(2θ1 + θ2)Sen(θ2)+ Izz2 θ1 + Izz2 θ2

∂L∂θ1

= 0

∂L∂θ2

= m2L2cm2(θ1 + θ2)+m2L1Lcm2θ1Cos(θ2)+ Izz2 θ1 + Izz2 θ2

ddt

∂L∂θ2

= m2L2cm2(θ1 + θ2)+m2L1Lcm2θ1Cos(θ2)−m2L1Lcm2θ1θ2Sen(θ2)+ Izz2 θ1 + Izz2 θ2

∂L∂θ2

=−m2L1Lcm2(θ21 + θ1θ2)Sen(θ2)

Finally obtaining the Eq. 14 and 15 expressing the torque value for each of the gaskets:

τ1 = m1L2cm1θ1 +m2L2

1θ1 +(Izz1 + Izz2)θ1 +m2L2cm2(θ1 + θ2)+

m2L1Lcm2(2θ1 + θ2)Cos(θ2)−m2L1Lcm2(2θ1θ2 + θ22)Sen(θ2)+

Izz2 θ2 (14)

τ2 = m2L2cm2(θ1 + θ2)+m2L1Lcm2θ1Cos(θ2)−

m2L1Lcm2θ1θ2Sen(θ2)+m2L1Lcm2(θ21 + θ1θ2)Sen(θ2)+

Izz2(θ1 + θ2) (15)

In matrix form would be:

τ = M(θ)θ+V (θ, θ)+G(θ)

M(θ) =

[m11 m12m21 m22

]

m11 = L2cm1m1 +L2

1m2 +L2cm2m2 +2L1Lcm2m2c2 + Izz1 + Izz2

m12 = L2cm2m2 +L1Lcm2m2c2 + Izz2

m21 = L2cm2m2 +L1Lcm2m2c2 + Izz2

m22 = L2cm2m2 + Izz2

V (θ, θ) =

[−2m2L1Lcm2s2θ1θ2−m2L1Lcm2s2θ2

2m2L1Lcm2s2θ2

1

]

G(θ) =

[00

]

12

Direct Dynamics. Finally the direct dynamic model (DDM) calculates the acceleration in each axisof rotation at any given instant of time the position, speed and torque. It is expressed by the Eq. 16and to obtain it is necessary to know the inverse dynamics [9, 13].

θ = M(θ)−1(τ−V (θ, θ)−G(θ)) (16)

Control hybrid force/position

This chapter presents the architecture for the control system that implements the controller hybridposition / force. The controller hybrid force / position must solve three problems:

1. The control of the position of a robot on the directions in which there is a natural restrictionforce.

2. The force control of a robot on the directions in which there is a natural restriction position.3. One scheme for implementing these modes arbitrary mixture of the degrees of freedom of a

frame arbitrary orthogonal.The position control tasks considered planning, trajectory generation, measurement through sen-

sors and position feedback. Moreover, the force control requires additional elements as environmentmodels (media), force sensors and force and position feedback. There are several tasks that involve amovement restricted to the handler as polishing, cutting and some assembly tasks. Different force al-gorithms have been proposed by different authors. Whitney presents a summary of the early research.Some important contributions are explicit control force [15], the hybrid control [16, 17], impedancecontrol [18], stiffness control [19] and proposed parallel control [20]. Because of the difficulty pre-sented by the interaction of the manipulators with their environment and high precision tasks to beperformed are necessary advanced control strategies. One strategy that can cover these difficulties isthe control of two degrees of freedom [21]. The drivers of two degrees of freedom, are considered asan advanced method of control by the ease with which to implement the kinematics and dynamics andlikewise successful in the position and force control [22, 23]. In this work, the driver for two degreesof freedom, is used for the control of position and force.

Modeling Environment. A robot by definition is a machine capable of interacting with its environ-ment, that is, must be able to adapt their movements and actions of interaction based on the physicalcharacteristics of the environments in which you use it and the objects in them. To achieve thisadaptability, the first thing you need is to have robots situational awareness. Environment modelsdescribed in the literature can be classified into rigid environment (restricted) and docile environment(compliant) [8].

The controller hybrid force / position must solve three problems:

fe = Ke(x− xe) (17)

wherein the stiffness of the surface is Ke ∈ Rm×m, x is the position of the extreme end of the link 2 andxe is the position x the object in the world. This linear relationship between force and deformationrepresents a first approach to the modeling of robot-environment interaction. Investigating the dy-namics of interaction, [18] concluded that the environment can be modeled as a mass-spring-damperwidespread.

13

Figure 8: Hybrid Control - environment.

The hybrid control of force / position control is a strategy in which forces are specified in those direc-tions desired task space in which the movement of the end effector is constrained by the environment,and a desired path in the other directions, see Fig. 8.

Therefore, the task space is divided into a position control subspace and a subspace force control.The articular level hybrid control is easy to implement, but it hardly corresponds to the nature of thetask because the selected joints that provide strength and position do not correspond one-to-one withthe restricted and unrestricted address space task of robot manipulator [8]. Khatib variant proposed ahybrid control based on a description of the dynamics of the manipulator robot end effector seen fromthe operational space formulation call or task. This representation is valid configurations excludingsingular task space [24].

Because the position controller must control some directions or degrees of freedom and control to reg-ulate the remaining force, this control strategy is suitable for tasks where the behavior is predictable.The hybrid control schemes usually have two parallel control loops, one for position and one for theforce as shown in Fig. 9, but in fact each joint contributes to both performing position control andforce control, the outputs of these loops are summed before being sent to the robot in the form of aglobal control vector. The simultaneous action of the two controls can generate conflict, as two differ-ent types of action are ordering the same actuators. To avoid antagonism between controls usually usea diagonal selection matrix often called S, which lets you choose the type of control either position orforce [8, 13].

The structure of a hybrid controller as follows, Eq. 18:

τ = JT (q)[K f S( fd− fe)+Kp(I−S)(xd− x)−Kvx] (18)

14

Figure 9: Block diagram of the hybrid control force / position.

where S∈ Rm×m is a diagonal matrix consisting of 1’s and 0’s, which serves to specify addresses to becontrolled by force and to be controlled by position, the directions corresponding to the elements ofS that are 1’s are controlled by force and thus the rest are controlled by position. I ∈ Rm×m representsan identity matrix, the controller parameters K f ∈ Rm×m and Kp ∈ Rm×m represent proportional gainsof position and force, on the other hand Kv ∈ Rm×m corresponds to the derivative gain related to thespeed of the robot end effector. This is shown in the following Fig. 9 block diagram:

The robot’s behavior is numerically simulated using MATLAB, for this purpose the dynamic modelwas rewritten in generalized coordinates as:

q = M−1(q)[τ− JT (q) fe−C(q, q)q]

Figure 10: Hybrid Control with the two control laws in MATLAB.

15

In order to simulate the interaction between the robot with a docile environment is considered a wallsurface or XEY = 1mm from the origin, on which a force is exerted along the axis. Elastic wassimulated environment represented by the Eq. 17 of section with stiffness Ke = diag1500[N/m] togenerate the interaction force. To perform this task control interaction was used hybrid force / positionas shown in Fig. 10.

Forward Kinematics block takes the values of the angular position of each joint, and thus obtaining

the vector positions POS, being POS =

[POSXPOSY

], the inverse kinematics vector takes POSD =[

POSXDPOSY D

]with the desired positions of each joint and calculates the angular position of each joint.

The equations were implemented in these blocks were obtained from section and .

The blocks and ISE are implemented internally selection matrix with the difference that ISE is mul-tiplied by the identity matrix. By multiplying the matrix by the identity matrix selection the result iscontrary to the matrix selection which means that while working on a power control works ISE axisposition control on the other axis. The equations were implemented in these blocks were obtainedfrom section .

Position Control, Force Control and Cruise Control blocks containing the matrices described in Eq.19, 20 and 21, these blocks are implemented profits matrices:

MatrixK p =

[ConstantK p 0

0 ConstantK p

](19)

MatrixK f =[

ConstantK f 00 ConstantK f

](20)

MatrixKv =[

ConstantKv 00 ConstantKv

](21)

Control Position blocks and force control taking previously multiplied by the signal matrix gain se-lection and gives to the axis about which it must interact. The speed control block takes the values ofcurrent and desired angular position of the robot and gives profit to control robot speed.

The block modeling environment that aims to clarify the fundamental aspects of the control of in-teraction with the environment. This block takes the current position of the robot and the positionof the wall that interacts with the effector. If the robot comes into contact with the wall strength isevaluated with this interaction is performed by f e = Ke(x− xe). This and other equations that wereimplemented in this block were obtained in section .

JacobianoT block is responsible for obtaining the torque through the transpose of the Jacobian andfrom the force values and current angular positions of the robot.

Results

16

Figure 11: Position in X and Y control.

The following shows the results for the control of 2 DOF for finger position and force hybrid. In Fig.11 shows the robot position control in both in X and Y , for which selective matrix type as follows

S =

[0 00 0

].

Figure 12: Position in X and force in Y control.

For controlling the position on X axis and strength in Y selective matrix is entered with the following

values S =

[0 00 1

], obtaining the Fig. 12.

17

Figure 13: Position in Y and force in X control.

For the position control in the Y and strength in X was obtained Fig. 13 entering the selection matrix

as follows S =

[1 00 0

].

It is noted in Fig. 12, 13 that the desired trajectory is followed properly by the extreme end of therobot axis to be controlled designated position. Likewise it can be seen that the axes to be controlledby force does not follow the path but maintains the value of force between the trailing end of the robotand the wall surface or located in XEX = 501[mm] and XEY = 1[mm] for each of the cases shownabove.

Figure 14: Control force in X and Y [mm].

Finally the Fig. 14 sample manipulator interaction with the environment by the action of force control,without any action on position control, which is why the handle is held in a single point. The selection

matrix for control by force is S =

[1 00 1

].

18

The proceeds were used for the hybrid controller were K p = diag500000[N/m], K f = diag500000 yKv = diag500[N · s/m]. based on those used by the [8].

Figure 15: MATLAB Simulation Validation

It is clarified that despite having used for tuning PID gainsK p the controller position in X and in Yand then by force in X and in Y separate responses were satisfactory, but when deploying the hybridcontrol system response was not as expected, showing an error of + / - 5% as shown in Fig. 15 as wasagain necessary in hybrid tuning, namely whether control is desired position or force or hybrid mustperform controller tuning.

Conclutions

• The implementation of the hybrid controller is simple numerical programs, but this driverpresents difficulties when implementing it as to make changes in the environment or effec-tor task, it is necessary to make changes to the controller and operating parameters. It is veryversatile.

• There were great difficulties in tuning and implementation of hybrid controller because theresponse of the controller in terms of position and force were the desired torque values ranged.

• Implemented a PID controller (2-DOF) MATLAB self-tuning the position variable to yieldgood results compared with only a proportional gain.

• Simulations and experiments allow to observe an adequate ability to reorient the end of thefinger continuously in function of the contact pairs, maintaining at all times the force appliedon the contact surface of the sensor or tracer element.

• This paper presented a control scheme in two dimensions and future could add a degree offreedom, which is replaced by a three-dimensional control of forces. Thus accommodating

19

movement is generated that reorients the tracer element on the contact surface. Once the nec-essary conditions are hybrid control is performed in which the axes of the plane tangent to thecontact surface are controlled in position while on the axis perpendicular to the contact surfaceis made of a control force.

• As future work can be seen the implementation of force sensors that are located in each joint,this would facilitate the control technique.

References[1] S. Bermejo, Desarrollo de robots basados en el comportamiento. ISBN 84-8301-712-1., UPC,

2003.

[2] lanacion.com, “Da vinci, el primer robot cirujano, ya opera en la argentina,” Julio 2008.

[3] M. Salazar, “Cim-manufactura integrada por computadora,” 07 2009.

[4] BBC, “Manos robóticas que sienten lo que tocan,” Junio 2012.

[5] Robotnik, “Manos robóticas.”

[6] A. O. Baturone, Robótica. Manipuladores y robots móviles. 2001.

[7] I. A. O. Caparroso, “Análisis cinemático de un robot industrial tipo scara,” Revista de la facultadde ingeniería, pp. 17–27, Julio 1999.

[8] F. R. Cortés, Robótica. Control de Robots Manipuladores. Alfaomega, Marzo 2011.

[9] J. J. Craig, Robótica. Prentice Hall, 2006.

[10] J. A. C. G. Roque Calero Pérez, Fundamentos de mecanismos y máquinas para ingenieros.1999.

[11] E. Prisma.com, “Dinámica del cuerpo rigido-fundamentos,” Diciembre 2009.

[12] D. J. M. . W. King, Dinamica II: Mecánica para ingeniería y sus aplicaciones. 2004.

[13] B. S. Lorenzo Sciavicco, Modeling and control of robot manipulators. Series in Electrical andComputer Engineering. Lorenzo Sciavicco, Bruno Siciliano., 1996.

[14] C. T. A. Frank L. Lewis, Darren M. Dawson, Robot Manipulator Control. Theory and Practice.2004.

[15] D. Whitney, “Force feedback control of manipulator fine motions,” Proceedings Joint AutomaticControl Conference, 1976, San Francisco.

[16] M. Mason, “Compliance and force control for computer controlled manipulators,” tech. rep.,Laboratorio de Inteligencia Artificila (IA) del MIT, Mayo de 1978.

[17] M. Raibert, “Hybrid position/force control of manipulators,” ASME Journal of Dynamic Sys-tems, Measurement, and Control, Junio de 1981.

[18] N. Hogan, “Stable execution of contact tasks using impedance control,” Proceedings of IEEEConference on Robotics and Automation., 1987. Raleigh, NC.

20

[19] J. K. Salisbury, “Active stiffness control of manipulator in cartesian coordinates,” 19th IEEEConference on Decision and Control, diciembre de 1980.

[20] S. Chiaverini, Controllo in forza di Manipolatori (in Italian). PhD thesis, Tesi di Dottorato diRicerca, Universita degli Studi di Napoli Federico II, 1990.

[21] W. A. Wolovich, “Automatic control systems. basic analysis and desing,” Saudewrs CollegePublishing, 1995.

[22] C. J. V. Bisso, “Controle de posição de robôs manipuladores rígidos e com transmisões flexiveis,dissertação de mestrado,” tech. rep., Univesidade Federal de Santa Catarina, Brasil, 1999.

[23] T. P. V. Fernando, “Independient joint control using two degree of fredom control structures,”(Italy), pp. 552–556, Proccding of IEEE International Conference on Control Applications,1998.

[24] O. Khatib, “A unified approach to motion and force control of cobot manipulators: the opera-tional space formulation,” IEEE. of Robot. Autom., pp. 43–53, 1987.

21