and robot dynamics - IIT - Universidad Pontificia Comillas

Transcription

Autorizada la entrega del proyecto a la alumna:
Irene Ruano de Pablo
EL DIRECTOR DEL PROYECTO
Timothy Bretl / Álvaro Sánchez Miralles
Fdo.:
Fecha:
VºBº del coordinador de proyectos
Álvaro Sánchez Miralles
Fdo.:
Fecha:
UNIVERSIDAD PONTIFICIA COMILLAS
ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA (ICAI)
INGENIERA EN AUTOMÁTICA Y ELECTRÓNICA INDUSTRIAL
PROYECTO FIN DE CARRERA
AN OPTIMAL
SOLUTION
TO THE
SEARCH
AN OPTIMAL
SOLUTION
TO LINEAR
THE LINEAR
PROBLEM
FOR A ROBOT
DYNAMICS
SEARCH PROBLEM
FORWITH
A ROBOT
WITH
DYNAMICS
AUTORA: Irene Ruano de Pablo
MADRID, junio de 2010
AN OPTIMAL SOLUTION
TO THE LINEAR SEARCH PROBLEM
FOR A ROBOT WITH DYNAMICS
RESUMEN
Día a día los robots se introducen más en la vida de las personas y llevan a
cabo tareas con una destreza y precisión que cada vez distan más de las capacidades
humanas. Constantemente surgen nuevas necesidades que requieren la ayuda de la
tecnología artificial, y ello obliga a un estudio y desarrollo constante de su potencial. El
mundo de la robótica puede descomponerse en infinidad de campos; es de especial
relevancia para multitud de aplicaciones la faceta de los robots de interactuar con el
medio físico en el que se encuentran y obtener información del mismo de una manera
suficientemente precisa. Es en esta línea en la que se encuadra el proyecto que se
presenta a continuación.
La capacidad de encontrar un objeto de la manera más rápida posible puede
constituir un gran avance para muchas y diversas aplicaciones: actividades de rescate
(helicóptero que sobrevuela un incendio buscando supervivientes, o una tormenta en el
océano), máquinas interfaz del cerebro (para realizar tareas de coger y posicionar
objetos localizándolos rápidamente), actividades de carga y descarga llevadas a cabo
por un robot, o incluso visión artificial (emulando el movimiento del ojo humano). Todo
esto justifica el volumen de trabajos existentes relacionados con esta materia a lo largo
de los años, así como el interés por desarrollar un algoritmo óptimo de búsqueda.
Este proyecto implementa una nueva estrategia de control que optimiza el
proceso de búsqueda de un objeto que está distribuido uniformemente en un intervalo
unitario. Su localización es desconocida por el buscador hasta que éste sobrepasa el
objeto. Este algoritmo está basado en el problema de búsqueda lineal introducido
originalmente por Richard E. Bellman y constituye una variante del mismo en la que el
I
AN OPTIMAL SOLUTION
RESUMEN
buscador puede cambiar su velocidad pero está sujeto a límites de aceleración. Una
vez encontrado y pasado el objeto, es necesario que dicho buscador retorne a la
posición en la que fue hallado. Este algoritmo minimiza el tiempo total que le lleva a un
dispositivo sujeto a las condiciones previamente mencionadas encontrar un objeto,
contando desde que empieza en el origen parado hasta que se posiciona sobre el
mismo tras la búsqueda.
La implementación ha sido realizada en un robot plano de dos grados de
libertad que buscaba monedas de metal utilizando para la detección un sensor
inductivo de proximidad insensible a objetos no metálicos.
Figura 1: Brazo robótico utilizado en el proyecto
II
AN OPTIMAL SOLUTION
RESUMEN
Las no linealidades del brazo robótico (fricción, inercia, etc.) han sido
compensadas para así poder ejecutar un movimiento más suave y tener un control más
preciso del artefacto.
Como extensión de este trabajo, se demuestra en este proyecto que la
solución óptima del algoritmo puede también extenderse a una búsqueda óptima a lo
largo de curvas, como barridos ascendentes sucesivos de lado a lado de una
superficie.
Figura 2: Ejemplo de búsqueda con curvas mediante un barrido ascendente
Esto significa que el problema de búsqueda lineal no está confinado sólo a
líneas rectas, lo cual abre enormemente el abanico de posibilidades y aplicaciones a
las que este algoritmo puede ser aplicado.
III
AN OPTIMAL SOLUTION
SUMMARY
Currently, robots are becoming more and more present in people’s lives every
day and they can perform tasks that are beyond human’s capacities. New needs that
require artificial technology continue to arise and this entails a constant study and
development of its potential. The world of robotics has a wide variety of different fields;
robots’ interaction with their environment and obtaining accurate information from it is of
special interest for many applications. The project that is presented here has focused
on this area of study.
The ability to find a target quickly can involve great improvements and be a
step forward for many different real applications: life-rescue activities (helicopter flying
over a fire, or a storm in the sea), brain machine interfaces (pick and place tasks), load
and unload activities (performed by a robot) or even artificial vision (emulate human
eyes’ movement). Therefore, this encourages finding an optimal search algorithm and
there has been a long string of papers on this over the past years.
This project implements a new control policy that optimizes the process of
searching a target that is distributed uniformly over the unit interval and its location is
unknown by the searcher until it passes over it. This algorithm minimizes the total time it
takes a point mass with bounded acceleration to find an object, starting from the origin
at rest, and then return to its position.
The implementation was made on proof-of-concept hardware with a two-link
planar robot arm that searched for metal coins using an inductive proximity sensor that
did not respond to nonmetallic objects. The dynamics of the manipulator were taken
IV
AN OPTIMAL SOLUTION
SUMMARY
into account in order to compensate for them and therefore be able to perform a
smoother movement and have a more accurate control.
Figure 1: Robot arm used to work on the implementation
Particularly, as an extension of this work, it is demonstrated on this project that
the optimal solution approach can also be extended to optimal search along arbitrary
curves such as raster-scan patterns, meaning that the linear search problem is not only
confined to straight lines. This can be of high interest to many other different
applications.
Figure 2: Raster- scan path performed in a 2-D search
V
AN OPTIMAL SOLUTION TO THE LINEAR
SEARCH PROBLEM FOR A ROBOT WITH
DYNAMICS
by
Irene Ruano de Pablo
Advisor: Timothy Bretl
ECE 497/499
May/8/2010
University of Illinois at Urbana-Champaign
AN OPTIMAL SOLUTION
To my parents, for their unconditional love and support.
To my sister Laura, because I could not think of a better one.
To Alberto, for being by my side everyday during this year. So far and yet so close.
AN OPTIMAL SOLUTION
ACKNOWLEDGEMENTS
I would like to express my gratitude to my advisor, Timothy Bretl. Thank you for
giving me the opportunity to be part of this project and for your guidance throughout it.
I want to give very special thanks to Aaron Becker for his constant mentoring
and teaching. It has been a pleasure to work with you and I have learned a lot. I would
not have been able to do this project without your help.
I would also like to mention the people in the 18C Lab of Talbot for helping me
when I needed it.
Also thanks to all my friends in Urbana-Champaign, and especially my friends
Lindsey, for her long lasting friendship, and Karen, for her company every day. They
have all made this year of exchange so enjoyable.
AN OPTIMAL SOLUTION
TABLE OF CONTENTS
1. INTRODUCTION...........................................................................................................1
2. LITERATURE REVIEW: PRIOR WORK............................................................................4
3. RESEARCH RESULTS ....................................................................................................6
3.1. Problem statement........................................................................................7
3.2. Robot implementation ..................................................................................8
3.3. Acceleration and velocity profiles ...............................................................10
3.4. Raster-scan search path ..............................................................................14
3.5. Second order linear controller: Task Inverse Dynamics ..............................16
3.6. Inner-loop (computed torque) and robot dynamics ...................................18
3.7. Trajectory planning: path motion................................................................21
3.8. Kinematics ...................................................................................................22
3.9. Simulation results ........................................................................................25
3.9.1. Straight line path .........................................................................25
3.9.2. Summary table of straight line path results ................................30
3.9.3. Raster scan path ..........................................................................30
3.10. Experimental results ..................................................................................36
3.10.1. Overview ....................................................................................36
3.10.2. Block diagrams of experimental implementation .....................39
3.11. Further improvements ..............................................................................40
3.11.1. New inner-controller .................................................................40
3.11.2. Friction compensation ...............................................................46
4. CONCLUSIONS ...........................................................................................................47
APPENDIX A. EXPERIMENTAL HARDWARE ...................................................................50
APPENDIX B. PARAMETER iDENTIFICATION .................................................................55
APPENDIX C. BLOCK DIAGRAMS ...................................................................................56
APPENDIX D. CONTROL OF THE ROBOT: C++ CODE ...................................................61
REFERENCES ..................................................................................................................98
AN OPTIMAL SOLUTION
1
INTRODUCTION
~1~
AN OPTIMAL SOLUTION
1.
INTRODUCTION
The classical linear search problem concerns an object which is the subject of
a search and was originally posed by Bellman [1] and Beck [2]. It is stated as follows:
A target
is placed somewhere on the real line
probability distribution
according to a known
. A searcher, which is assumed to be able to change the
direction of his motion without any loss of time and whose maximal velocity is one,
starts from the origin and wishes to discover the target in minimal expected time. It is
also assumed that the searcher cannot see the target until it reaches the point where
the target is at. The elapsed time until that moment is the duration of the search. This
brings up the problem: what policy minimizes this expected time?
This project focuses on a variant of this original problem in which the searcher
is a point mass that can change its velocity and it is subject to bounded acceleration.
Now the objective is to, after finding and passing the target, return to its location in
order to be able to perform, for example, a pick and place task afterwards. The goal is
to do this in minimum expected time, and this presents a tradeoff: to find the target as
fast as possible it seems intuitive that the faster the searcher goes the better. But going
fast implies that it will take the point mass longer to be able to stop and then return to
the target, so thinking about the problem this way, one would say that it is a better
option to go slow. So, what should be done?
A solution to this problem has been derived [3], and the main purpose of this
thesis is to demonstrate that said solution approach works – it is able to find a target
uniformly distributed and return to its position in minimal expected time. In section 2
there is an overview of prior published work related to the problem addressed here, and
section 3 constitutes the main body of this thesis, where after posing the structure of the
algorithm, we present its implementation and testing in hardware. Section 3 includes
experimental data collected to support this work. Section 4 offers the conclusions drawn
from the results and gives some ideas for future work.
~2~
AN OPTIMAL SOLUTION
This variant of the linear search problem is of particular relevance to robotics,
where the point mass could be any kind of unmanned manipulator performing pick and
place tasks of all sorts, or looking for objects on a surface.
~3~
AN OPTIMAL SOLUTION
2
LITERATURE
REVIEW:
PRIOR WORK
~4~
AN OPTIMAL SOLUTION
2.
LITERATURE REVIEW: PRIOR WORK
The linear search problem has encouraged a long string of papers over the
past years ([4] - [8]) to try to find a solution for it, and some of them have considered
extensions like achieving rendezvous with two searchers [9] or doing linear search on a
network [10]. Linear search falls within a broader class of search problems that [11] and
[12] go over.
Nevertheless, for this particular project the variant of this problem that has
been mentioned before has been considered. This variant has also received some
previous attention. The work of Demaine et al. [13] and earlier of Lopez-Ortiz [14]
considered the linear search problem with an additional cost for turning, but still
assumed unit speed between turns. Much more general search problems than the one
addressed in this thesis have been considered over the past decade by the robotics
community. This work tends to focus on pursuit-evasion games [15], where both the
searcher and the target are moving. It addresses complications like probabilistic motion
models, probabilistic sensor models, multiple searchers or targets, and visibility
constraints ([16] - [18]). [19] is closely related both to the problem of coverage and to
the problem of exploration. For work classified more broadly as planning under
uncertainty, there exist solution approaches (e.g., POMDP solvers [20] - [22]). However,
these approaches rarely lead to exact solutions, and are often computationally
prohibitive. Common heuristics include finite horizon policies and policies based on the
certainty equivalence principle (i.e., on an assumed decoupling between optimal
estimation and optimal control).
~5~
AN OPTIMAL SOLUTION
3
RESEARCH
RESULTS
~6~
AN OPTIMAL SOLUTION
3.
RESEARCH RESULTS
3.1. Problem statement
The optimal algorithm has been derived in [3], but a general explanation is the
following:
Let us consider a unit mass with position
with dynamics
being driven by a force
. The target is some point
. All possible
target positions are equally likely. The mass starts at
. A sensor indicates
when the target has been passed, in other words, when
. The goal is to stop
at the target in minim expected time, in other words to reach
The state is defined as
the instant
where
at which the mass passes the target, so
the velocity of the mass at this instant is denoted by
.
and
. Consider
. For convenience,
. After this instant, the
time-optimal policy is clear – it is a classical bang-bang solution of the form
(1)
for some
. It is known that
and
2=0 must be reached. An optimal policy must satisfy
(2)
where
. Solving some equations that are presented in more detail in [3], it
turns out that an optimal policy is a finite sequence of inputs
(3)
~7~
AN OPTIMAL SOLUTION
The structure of the optimal policy can be expressed as
(4)
or equivalently as
(5)
which means that during the first 17.16% of the total distance of the interval it is
necessary to accelerate at a maximum rate, and from that point until the end of the
interval a lower but still constant rate of deceleration of
must be applied.
3.2. Robot implementation
To validate the solution approach, the results were applied to hardware
experiments with the robot arm shown in Figure 3. Each link was powered by a directdrive brushless DC servo motor with encoder feedback. Planning and control were
done on an external PC with a 1 kHz control loop. The end-effector carried an inductive
proximity sensor that detected metal objects within a radius of 15 mm but did not
respond to nonmetallic objects. In the experiments a US $1 coin was placed at random
locations, determined by rolling two 10-sided dice, trying to emulate a uniform target
distribution with all possible target positions equally likely.
~8~
AN OPTIMAL SOLUTION
Figure 3: Two-link planar arm used in the hardware experiments.
The overall system can be modeled by an inner-loop/outer-loop control
architecture as mentioned in [23] and that is illustrated in Figure 4.
Figure 4: Inner-loop/outer-loop control architecture that illustrates the system.
In real life, coupling effects among the joints cannot be regarded as
disturbances to individual systems. The dynamic equations of a robot manipulator form
a complex, nonlinear and multivariable system. Therefore, the robot control problem
has to be treated in the context of nonlinear, multivariable control. Taking this into
account, in the so-called inner-loop/outer-loop control the first one computes the vector
u of input torques as a function of the measured joint positions and velocities
and the given
by the outer-loop control in order to compensate for the nonlinearities
~9~
AN OPTIMAL SOLUTION
in the plant model. The outer-loop control is designed to track a given reference
trajectory
and is in line with the notion of a feedback control in the sense of being
error-driven. The design of the outer-loop feedback control is very simplified since it is
designed for a linear system, which is represented by the dotted box in Figure 4.
3.3. Acceleration and velocity profiles
To prove that the computed optimal solution approach is correct, two alternative
control strategies that may at first seem reasonable (“bang-bang” policy and an
application of a certainty-equivalence principle that will be now explained) were
implemented and compared with the optimal. For this comparative experiment, a
straight line search path of length 1 m was considered.
As has been previously stated, the optimal control policy has the form of (5),
with dynamics
(6)
In other words, (5) defines the acceleration profile that the end-effector follows when the
optimal policy is being applied. The bang-bang alternative crosses the interval and
returns to rest in minimum time by accelerating and decelerating at a maximum rate.
The third policy is a certainty equivalence strategy (i.e. decouple the optimal estimation
and optimal control problems) that continues to accelerate across the entire path. The
explanation of this “equivalence” is the following [3]: having moved a distance d along
the straight line, the target is to be known uniformly distributed in the remaining interval
[d, 1]. The common best estimate of the location of the target is at the position
(7)
~ 10 ~
AN OPTIMAL SOLUTION
The time-optimal control policy to reach this position would be the bang-bang
strategy, which accelerates at a maximum rate during half of the distance. However, as
the end-effector moves, the target position keeps moving away to the next position
estimate in the new interval. Thus, by applying the certainty equivalence principle of
assuming that the target position estimate is correct, the robot would be accelerating
nonstop.
In Figure 5 the three end-effector acceleration profiles for the different control
alternatives are shown with respect to distance.
Figure 5: Acceleration profiles of the three different control strategies. Distance is measured in m and
acceleration is measured in
.
The unlike position points where the acceleration is switched depend on which strategy
is being applied. This difference can be observed in the graphs.
The acceleration profiles that have been previously declared yield specific
velocity profiles for each one of the strategies. These are illustrated in Figure 6, Figure
7 and Figure 8 respectively.
~ 11 ~
AN OPTIMAL SOLUTION
Figure 6: Velocity profile with respect to distance for optimal control policy. Distance in measured in m and
velocity is measured in m/s.
Figure 7: Velocity profile with respect to distance for bang-bang control policy. Distance in measured in m
and velocity is measured in m/s.
~ 12 ~
AN OPTIMAL SOLUTION
Figure 8: Velocity profile with respect to distance for decoupled control policy. Distance in measured in m
and velocity is measured in m/s.
~ 13 ~
AN OPTIMAL SOLUTION
3.4. Raster-scan search path
A natural extension of the straight-line search path is use space filling curves
to search a two-dimensional area. As an example of this, the raster-scan path that is
shown in Figure 9 was considered.
Figure 9: Raster scan search path used to perform a 2-dimensional search. Each straight line was 50 cm
long and the radius of turn was 2.5 cm.
Although now the search path is a smooth curve, the result is still a linear
problem; therefore, it can be addressed the same way as a straight line, with the
optimal solution approach. In order to do this, the real path was treated as if it were a
straight line. To emulate the curve, velocity constraints were imposed in that position
interval. Figure 10 shows the velocity profile for this new type of trajectory.
~ 14 ~
AN OPTIMAL SOLUTION
Figure 10: Velocity profile with respect to distance in a raster-scan path. The grey patch indicates the
velocity constraint (limits velocity to a maximum of 0.3 m/s). Distance is measured in m and velocity is
measured in m/s.
As can be seen in Figure 10, the total length of the trajectory is now larger
than 1 m since the switch-back turn adds to the two 50 cm long straight lines. An
optimal control strategy is being applied in the background, but due to the velocity
constraints the velocity profile diverts from the original optimal one from points a to d.
From b to c the velocity is limited to a maximum value, determined by the velocity
constraint. The time-optimal trajectory from origin to end is the curve that maximizes the
motion constraints while remaining on their boundaries [24]. Figure 11 illustrates the
end-effector x-y-coordinates acceleration profile.
~ 15 ~
AN OPTIMAL SOLUTION
Figure 11: End-effector x-y-coordinates acceleration profile with respect to distance. Distance is measured in
m and acceleration is measured in
.
3.5. Second order linear controller: Task Inverse Dynamics
As can be seen in Figure 4 and was previously mentioned, the outer-loop
controller operates as a conventional linear feedback one. It feeds the inner-loop with
task space coordinates accelerations, leaving the inner-loop control unchanged [23].
Let X
represent the end-effector pose in the plane. Since X is a function of the
joint variables q,
(8)
(9)
where J is the manipulator analytical Jacobian. Given the double integrator system in
(10),
(10)
~ 16 ~
AN OPTIMAL SOLUTION
(it is called like that because it represents n uncoupled double integrators) in joint
space, using (9) and (10) and choosing
to be
(11)
the result is then another double integrator system but in task space coordinates
(12)
which have the advantage of being joint non-dependant Since a set of desired
accelerations is known, equation (12) is very convenient and perfectly suitable for the
needed control.
Given a task space trajectory denoted as
, the outer-loop control may
then be chosen as
(13)
so that the task space tracking error defined as
satisfies
(14)
This implies that the outer-loop control achieves a linear and decoupled
system that is directly in task space coordinates, without needing to compute any joint
space trajectory and without changing the inner-loop. The tracking error goes to zero,
which means that the controller follows the desired trajectory.
~ 17 ~
AN OPTIMAL SOLUTION
3.6. Inner-loop (computed torque) and robot dynamics
The robot arm that was used for this project had already been used before.
Consequently, an inner-loop controller had already been designed for the purpose of
computing desired reference output torques as a function of the measured joint
positions and velocities
and the given set of joint accelerations (
) given by the
outer-loop control. This aims to compensate for the nonlinearities in the plant model,
and also takes into account the dynamics of the manipulator. This controller was based
on the one developed in [25] and it is known as the passivity-based adaptive control
[23].
Figure 12 illustrates the block diagram used for this task.
Figure 12: Block diagram of the inner-loop controller. Its outputs (torques) are directly connected to the
robot. Among others, the desired accelerations given by the outer-loop (Aq) are input.
Figure 13 is a zoomed in view of the inner-loop control in Figure 12, and
Figure 14 shows what is inside Figure 13.
~ 18 ~
AN OPTIMAL SOLUTION
Figure 13: Zoom in on the inner-loop controller in Figure 10.
~ 19 ~
AN OPTIMAL SOLUTION
Figure 14: Zoom in on the block in Figure 11.
The values for the gains and the different parameters are listed in Appendix B.
~ 20 ~
AN OPTIMAL SOLUTION
3.7. Trajectory planning: path motion
Robot control algorithms can be divided into two sections: path or trajectory
planning and path tracking. Multiple algorithms are developed in order to achieve the
different requirements that each problem may have. Optimal trajectory of an arbitrary
manipulator that has to follow a specified path in a one degree of freedom (1-DOF)
motion [26], time optimal motions of robotic manipulators along specified paths that
accounts for dynamic singularities [27], minimum-time control algorithm for robotic
manipulators with geometric constraints, given a specified path [28] or minimum-time
manipulator control problem for the case when a path is specified and actuator torque
limitations are known [29] are algorithm examples for trajectory planning. In the case of
this thesis, specific paths were given - a straight line or a raster scan – and the velocity
profiles that were previously addressed determine the trajectory needed for each
control strategy.
It is obvious that the implemented equations for the movement along the
straight line were those of a uniformly accelerated linear motion
(15)
where the acceleration term is the
double integrator variable in task space
coordinates that the outer-loop controller controls. This is the way the acceleration
interacts with position. In the raster-scan path, there is an extra uniformly accelerated
rotational motion that works with rotational coordinates
(16)
where
is the angular velocity and
the angular acceleration.
~ 21 ~
AN OPTIMAL SOLUTION
3.8. Kinematics
The outer-loop controller (see Figure 4) needs to convert from joint
coordinates to x-y end-effector coordinates, which will be fed into the inner-loop
controller. This is the problem of the inverse kinematics [23].
Given the two-link planar arm seen in Figure 15 there may be two solutions
for one x-y desired position of the end-effector, the so-called elbow up and elbow down
configurations. In this experiment the elbow down configuration was used.
Figure 15: Two-link planar arm used for this project.
~ 22 ~
AN OPTIMAL SOLUTION
Figure 16: Robot arm diagram for inverse kinematics.
Considering Figure 16 it is known by the law of cosines that
(17)
and therefore the sine is given by
(18)
Hence,
is determined by
(19)
and
is now given by
(20)
~ 23 ~
AN OPTIMAL SOLUTION
The same way, whenever it was necessary to convert from x-y end-effector
coordinates to joint coordinates, forward kinematics was used, which is the inverse
process of the inverse.
Following this line, in order to follow a velocity profile it is necessary to know
the relationship between the end-effector velocity and the joint velocities. There is a
matrix that relates these two, called the Jacobian [23], and it is fundamental. Therefore,
(21)
(22)
For the two-link planar arm used the Jacobian is
(23)
The Jacobian also allows determining the singular configurations that the
manipulator may have. These are important since for those configurations there are
several unachievable motions, and they also separate the multiple solutions of the
inverse kinematics (i.e. elbow up and down in this case), meaning that the manipulator
cannot go from one to another without going through one of this singular configurations.
~ 24 ~
AN OPTIMAL SOLUTION
3.9. Simulation results
Simulations were done using MATLAB. Two different kinds of experiments
were made.
3.9.1. Straight line path
First, a 1 m straight line path was considered. For this first simulation
experiment, an object (yellow marker) was randomly placed at a certain location along
the line. This location was the same for the three control strategies so that results could
be comparable (see Figure 17).
Figure 17: Straight line path with an object located at 55 cm from origin of line. Axes are in cm. Origin is in
the bottom point.
The three control strategies that were applied are the ones that were
previously mentioned: decoupled, bang-bang and optimal. The base of the robot is
considered the origin of coordinates. For the three of them, the end-effector was
initialized at the origin of the path and started the search process. Simulations were
dynamic, showing the movement of the robot arm, and a picture of the end of the
process (i.e. when the end-effector had returned to the position where the object was
~ 25 ~
AN OPTIMAL SOLUTION
located at) for each one of the control alternatives can be seen in Figure 18, Figure 19
and Figure 20.
Figure 18: Object found using decoupled control strategy. Axes are measured in cm. Time to reach target is
shown on top of graph.
As can be observed in Figure 18, the decoupled control overshoots and
exceeds the end of the path. This is because the velocity it had when it passed over the
object was very high due to the constant acceleration at maximum rate. When it comes
to a stop, it uses a bang-bang control to return to the object. The purple and cyan lines
indicate the movement of the two joints during the search process. From these lines it is
possible to get a hint of the motion: the more separated they are from each other, the
faster the arm was moving.
Figure 21 shows the acceleration profile for this trajectory.
~ 26 ~
AN OPTIMAL SOLUTION
Figure 19: Object found using bang-bang control strategy. Axes are measured in cm. Time to reach target is
shown on top of graph.
The position of the target is indicated in the graph in Figure 21. The
movement is ended whenever the manipulator comes to a stop at the position where
the target is.
~ 27 ~
AN OPTIMAL SOLUTION
Figure 20: Object found using the optimal strategy. Axes are measured in cm. Time to reach target is shown
on top of graph.
The bang-bang control decelerates at a maximum rate whenever it passes
over the object, and when it comes to a stop it uses another bang-bang control to return
to it.
As before, Figure 22 shows the acceleration profile that this control followed
to find the object.
~ 28 ~
AN OPTIMAL SOLUTION
Figure 21: Acceleration profile with respect to distance for the decoupled control used to find an object
placed at 55 cm from the origin. Distance is measured in cm and acceleration is measured in
.
Figure 22: Acceleration profile with respect to distance for the bang-bang control used to find an object
placed at 55 cm from the origin. Distance is measured in cm and acceleration is measured in
.
The optimal strategy also decelerates at a maximum rate whenever it passes
over the object, but due to its velocity profile, it was not going as fast as the bang-bang
or the decoupled alternatives when that happened. That is why it was able to stop
~ 29 ~
AN OPTIMAL SOLUTION
shorter than the other two strategies. It also uses a bang-bang control to return to the
object once the manipulator has come to a stop.
3.9.2. Summary table of straight line path results
In Table 1 there is a comparative table of the results of the three experiments.
It mainly shows the total time to find the object for each one of the experiments.
Table 1: Times to achieve target for each control strategy. Time is not measured in real-time seconds since
in the simulation there were pauses to allow a better optical view of the motion of the arm.
Decoupled control
Bang-bang
Optimal control
control
Time
3.6
3.34
3.05
It matches what was expected: the optimal strategy is the fastest one to find the object.
3.9.3. Raster scan path
The second type of experiment considered a raster scan path. Each line
segment is 50 cm long and the radius of turn is 5 cm long (i.e. total covered distance is
1.1 m).
In Figure 23 the raster scan path has been stretched out and converted into a
straight line. This shows how the robot arm follows the velocity profile described in
Figure 10 along the path. The two blue dots in the middle of the line indicate the interval
where the turn is, and therefore the velocity is constant.
~ 30 ~
AN OPTIMAL SOLUTION
Figure 23: Raster scan path stretched out in a line. Axes are measured in m.
For the search problem, an object was randomly placed at some point of the
path. The optimal control was applied in all the cases, with a maximum acceleration
rate of 1
and a maximum velocity constraint of 0.3 m/s in the turn (see Figure
10).
In the first experiment, the target was placed at 0.3 m from the origin of the
path. In Figure 24 the entire movement can be seen, and Figure 25 shows a zoomed
view of the part where the object is.
~ 31 ~
AN OPTIMAL SOLUTION
Figure 24: Object in first line segment when doing a raster scan experiment. Axes are measured in m.
~ 32 ~
AN OPTIMAL SOLUTION
Figure 25: Zoom in on the target zone. Axes are measured in m.
As can be seen in Figure 25, when the manipulator comes to a stop, it goes
back to the target in a straight line and using a bang-bang control strategy. This is the
fastest way to reach the target, and we assume that the workspace is collision free (i.e.
there are no other obstacles).
In a second experiment, the target was placed in a random location in the
curve. Figure 26 illustrates this, and there is a zoomed in view of the target zone in
Figure 27.
~ 33 ~
AN OPTIMAL SOLUTION
Figure 26: Object in curve when using a raster scan path. Axes are measured in m.
Figure 27: Zoom in on the target zone. Axes are measured in m.
~ 34 ~
AN OPTIMAL SOLUTION
The same policy is applied for this case: whenever the robot arm passes over
the target, it decelerates until it stops, and then uses a bang-bang control policy to
return to the object’s location.
~ 35 ~
AN OPTIMAL SOLUTION
3.10. Experimental results
3.10.1. Overview
The details of the experimental hardware are discussed in Appendix A. The
program used to interact with the robot was Simulink (application of Matlab) and the
block diagrams that were programmed will be shown later on.
Three experiments were done, using the control policies that were used in the
simulations in the previous section: an optimal control and a bang-bang for a straight
line path and an optimal control for a raster scan path.
For the straight line experiment, the target was located in the middle of the
linear space. Normalized path position
is plotted against path velocity
for the
optimal policy and for a reasonable alternative policy (i.e. bang-bang control). Both
controllers switch to maximum deceleration when they detect the target, just as has
previously been discussed. Given that the optimal policy has a lower velocity at the
point where the object is found, it needs a shorter distance to come to a stop, which
also results in a shorter return time. Negative velocities correspond with the robot
returning to the target. Spikes in the velocity are experimental artifacts caused by
switching accelerations. Plots have been made with real data collected from the
manipulator’s feedback information.
Figure 28 shows the velocity profile for a bang-bang control policy, whereas in
Figure 29 an optimal strategy can be seen.
~ 36 ~
AN OPTIMAL SOLUTION
Figure 28: Bang-bang velocity profile with respect to distance. Speed is measured in m/s and distance is
measured in m.
Figure 29: Velocity profile for an optimal control with respect to distance. Speed is measured in m/s and
distance is measured in m.
~ 37 ~
AN OPTIMAL SOLUTION
Figure 30 shows the third experiment, where a two-line raster scan path was
considered. The grey rectangle represents the velocity constraint of 0.3 m/s (already
mentioned in previous sections) when turning corners. The target was located at
from the origin.
Figure 30: Optimal velocity profile for a two line raster scan search. Distance is measured in m and speed is
measured in m/s.
To summarize results and prove that the experimental data matches the
theory developed in 3.1, Figure 31 shows aggregate results for the optimal policy and
both of the alternatives considered in previous sections. The graph represents the total
time to reach the target (i.e. both find and return to it) in seconds for a maximum
acceleration of 0.2
on a search path on length 0.5 m, as a function of the target
position. It can be seen that if the target is placed on the first fraction of the interval, it
takes the three control policies more or less the same amount of time to find it, but after
one point, the optimal policy is a lot faster than the other two. It is only at the very end of
~ 38 ~
AN OPTIMAL SOLUTION
the interval that the bang-bang policy becomes better than the optimal, but this is only
because at that point the robot arm is already going so slow that it is very easy to stop.
Figure 31: Total time elapsed until the object is found with respect to its position along the path. Each data
point represents the average of five trials. Time is measured in seconds, and distance is measured in m.
3.10.2. Block diagrams of experimental implementation
The block diagrams of the final implementations for both the straight line and
the raster scan search problem are included in Appendix C.
~ 39 ~
AN OPTIMAL SOLUTION
3.11. Further improvements
3.11.1. New inner-controller
In order to collect less noisy data and be able to obtain more clear plots, a
new inner-controller was derived to take account of the nonlinearities of the system.
This new controller is based on the one discussed in [23]. Let us consider the planar
manipulator shown in Figure 32.
Figure 32: Dynamic parameters in the two-link revolute joint arm. The rotational joint motion introduces
dynamic coupling between the joints.
It is known that the dynamic equations of an n-link rigid robot in matrix form
are [23], [30]
(24)
where
is the inertia matrix, and
velocities and accelerations, respectively. The matrix
~ 40 ~
and
are the joint angles,
is defined as
AN OPTIMAL SOLUTION
(25)
where
are Christoffel symbols (of the first kind). The term g(q) is a vector of
torques due to gravitational forces, and is the vector of applied joint torques.
The inertia matrix has the form
(26)
where
(27)
(28)
(29)
(30)
Values for the masses, lengths and moments of inertia are listed in Appendix B.
The Christoffel values are defined by
(31)
(32)
(33)
(34)
(35)
(36)
~ 41 ~
AN OPTIMAL SOLUTION
(37)
yielding matrix C as
(38)
The total potential energy is given by (39)
(39)
and therefore the gravitational functions
become
(40)
(41)
Finally, the dynamical equations of the system are
(42)
(43)
and
and
are the input torques to the motors.
~ 42 ~
AN OPTIMAL SOLUTION
Experimental data collection
New experiments were tested on the robot with this new inner-controller
implemented. The old inner-controller in charge of computing the output reference
torques for the motors was eliminated and substituted by the new one. The new block
diagram can be seen in Appendix C.
For the first experiment, the target was placed in the middle of a 1 m straight
line like in the experiment that has previously been mentioned, to be able to compare
the performance of the manipulator with both controllers. Figure 33 shows the velocity
profile for a bang-bang control policy and Figure 34 shows the optimal control for this
experiment.
Figure 33: Velocity of the manipulator with respect to distance for a bang-bang controller with object placed
at the middle of the interval. Speed is measured in m/s and position is measured in m.
~ 43 ~
AN OPTIMAL SOLUTION
Figure 34: Velocity of manipulator with respect to position for an optimal controller with object placed in the
middle of the interval. Speed is measured in m/s and position is measured in m.
If Figure 33 and Figure 34 are compared to Figure 28 and Figure 29
respectively, it can be seen that the first ones are less noisy than the others, thus the
performance of the robot arm was improved with the new inner-controller. The red cross
corresponds to the moment when the object was found and the robot had come to a
complete stop.
For a second experiment, the object was placed at ¾ of the distance this time
(in the same 1 m long straight line), and the same control policies were applied: bangbang and optimal. Figure 35 and Figure 36 show the experimental velocity profile plots
for this experiment.
~ 44 ~
AN OPTIMAL SOLUTION
Figure 35: velocity of the end-effector with respect to position for a bang-bang control policy and target
located at 75 cm. Speed is measured in m/s and position is measured in m.
~ 45 ~
AN OPTIMAL SOLUTION
Figure 36: Velocity of the manipulator with respect to position for an optimal control policy and target
located at 75 cm. Speed is measured in m/s and position is measured in m.
3.11.2. Friction compensation
For the control of the raster scan it was found that the use of friction
compensation helped get a smoother movement. The basic friction compensation takes
into account the Coulomb friction and the viscous. Viscous friction concerns a fluid's
internal resistance to flow. Since there is no fluid involved in the case of this project it
was ignored. Coulomb friction resists relative lateral motion of two solid surfaces in
contact and the idea is that it opposes motion, its magnitude is independent of velocity
and contact area and it is constant. In order to compensate for it, its value has to be
added to the computed desired torque to cancel its effect.
Friction compensation can be seen in Appendix C block diagrams for the
raster scan as two blocks, one for each joint. Friction values were initially set by
inputting different control signals and measuring the output velocity. This was done for
different positive and negative input voltages for each link, and then fit a straight line to
the velocity.
~ 46 ~
AN OPTIMAL SOLUTION
4
CONCLUSIONS
~ 47 ~
AN OPTIMAL SOLUTION
4. CONCLUSIONS
The experimental results have verified what was expected: the solution
approach for the linear search problem stated at the beginning minimizes the total time
to find and return to an object that is under a uniform probability distribution. Intuitively,
it may seem reasonable at first that the quickest way to find a target is to go as fast as
possible while searching for it. However, the faster the searcher is going when it
reaches the target, the longer it will take it to stop, and the later it will start returning to
the object. The solution approach takes this into account and finds the equilibrium for
this tradeoff; if the object is placed at the very end of the interval, it will take longer to
find it than with a bang-bang control policy, but other than that, the optimal algorithm is
more prompt in finding the object than any other policy. In non-technical words, it is like
the optimal algorithm is “cautious” and does not go very fast so that in case it finds the
target, it has enough time to stop.
The key part of these results is that they demonstrate that the linear search
problem is not confined to straight lines, but can also be applied to search paths with
arbitrary curves such as raster scans, and consequently they can be addressed with the
same solution approach. The only difference is that configuration-dependent velocity
constraints have to be added for the switch-back when turning corners to limit the
maximum end-effector velocity.
At last, it must be mentioned that all real trajectories have velocity constraints;
this algorithm is only optimal if working with short distances so that the velocity limit is
not reached shortly and then maintained for a long period of time. This would change
the velocity profile into a trapezoidal shape, which would result in another control policy
that is not the one being presented here.
Opportunities for future work include handling configuration-dependent
velocity and acceleration constraints to handle target distributions that are non-uniform
and to handle sensor uncertainty – situations where, depending on the configuration,
the performance of a sensor may vary.
~ 48 ~
AN OPTIMAL SOLUTION
APPENDICES
~ 49 ~
AN OPTIMAL SOLUTION
APPENDIX A. EXPERIMENTAL HARDWARE
A.1. Servos
The motors used for the experimental arm are model DM1015-B brushless
direct drive servos from Parker Compumotor. The following data are provided by [30]
and the manufacturer [31].
Peak torque 15 N-m
Max. speed 2.0 r/s
Encoder resolution 655360 lines
Rotor inertia 0.012
–
Servo mass 5.5 kg
Rotor mass 2.552 kg
Axial load:
Compression 6600 lb
Tension 2200 lb
Overhung load 148 ft – lb
Torque ripple 5%
For this application the servos are operated in “torque mode”; a signal range
of
applied to the driver corresponds to the desired torque output.
A.2. Robot links
The robot links are made of 6061 T651 aluminum. Both links are 30 cm long
(axis to axis for the first link, axis to tip for the second). Diagrams of the links are shown
in the plans attached.
In [30] rudimentary stress analysis was used in designing the first link to
ensure sufficient rigidity. The link was approximated as a uniform cantilevered beam of
length 30 cm, width 10 cm and thickness 2.4 cm, loaded at the tip with a 100 N discrete
~ 50 ~
AN OPTIMAL SOLUTION
force. The resultant maximum stress was 312 kPa, which is far below yield stress for
the material. Tip deflection was also negligible. In this project, a wooden extension was
added, but the extra stress that it applied didn’t become a problem for the robot arm to
well behave.
The second link carries a wooden part that was attached and designed
specifically for this project. Figure 35 and Figure 36 show a detailed description of each
of the links, and Figure 37 illustrates the wooden extension.
~ 51 ~
AN OPTIMAL SOLUTION
Figure 37: Link 1 of robot arm.
~ 52 ~
AN OPTIMAL SOLUTION
Figure 38: Link 2 of robot arm.
~ 53 ~
AN OPTIMAL SOLUTION
Figure 39: Wooden extension of robot arm.
~ 54 ~
AN OPTIMAL SOLUTION
APPENDIX B. PARAMETER IDENTIFICATION
Physical parameters for the links were determined by direct measurement.
Note that moments of inertia are about an axis perpendicular to the plane in which the
robot operates (the z axis). Centroids are measured from the servo axis. Original values
were taken from [30] where the moments of inertia for the composite links (link, servo
rotor, bolts, etc.) were determined by summing the inertias of the components about the
servo axis and applying the parallel axis theorem. The new values are shown in Table 2
and Table 3 and they take the wooden extension into account.
Table 2: Parameters of link 1
Measured
Mass
Moment of inertia
0.176
Centroid
Table 3: Parameters of link 2
Measured
(
Mass
is the mass of
the wooden extension)
Moment of inertia
1.7032
Centroid
~ 55 ~
AN OPTIMAL SOLUTION
APPENDIX C. BLOCK DIAGRAMS
Figures 38 and 39 illustrate how the control of the robot was implemented and
Figures 40 and 41 show the improved controller once the new inner controller was
designed. The program received data from the encoders on the motors that allowed
knowing position, velocity and acceleration at all times and also received feedback from
the proximity sensor. The interface connection between the motors (analog) and the
external PC (digital) was made through a board that had already been installed for
previous projects.
The magenta block in the diagrams includes all the programming that was
made for trajectory planning and the second order controller, task inverse dynamics,
kinematic equations, torque reference generation (inner-loop controller) and mainly all
the control for the robot. It can be seen that there is friction compensation for the raster
scan control, as it was mentioned in section 3.11.2.
~ 56 ~
AN OPTIMAL SOLUTION
Figure 40: Block diagram to move the robot along a straight line.
~ 57 ~
AN OPTIMAL SOLUTION
Figure 41: Block diagram to move the robot on a raster scan path.
~ 58 ~
AN OPTIMAL SOLUTION
Figure 42: Block diagram to move the robot along a straight line with the improved inner-controller.
~ 59 ~
AN OPTIMAL SOLUTION
Figure 43: Block diagram to move the robot on a raster scan path with the new inner-controller.
~ 60 ~
AN OPTIMAL SOLUTION
APPENDIX D. CONTROL OF THE ROBOT:
C++ CODE
In this appendix the code for the control of the robot is included. D.1. contains
the functions that generate the trajectory for the robot when it is moving along a straight
line, and D.2. is for the trajectory generation of the raster scan.
D.1. Functions for a straight line movement.
/*
* MoveRobot.c:
*
THis function computes desired XY accelerations and joint
accelerations
* Basic 'C' template for a level 2 S-function.
*
*
-----------------------------------------------------------------------*
| See matlabroot/simulink/src/sfuntmpl_doc.c for a more
detailed template |
*
-----------------------------------------------------------------------*
* Copyright 1990-2001 The MathWorks, Inc.
* $Revision: 1.26 $
*/
/*
* You must specify the S_FUNCTION_NAME as the name of your Sfunction
* (i.e. replace sfuntmpl_basic with the name of your Sfunction).
*/
#define S_FUNCTION_NAME MoveRobot
#define S_FUNCTION_LEVEL 2
/*
* Need to include simstruc.h for
SimStruct and
* its associated macro definitions.
*/
#include "simstruc.h"
#include "stdio.h"
the
definition
//constants: Parameter of Inertia and C Matrices:
const real_T
m1 = 7.848;
const real_T
m2 = 4.604075 + 1.1341; // %
mass of arm extension
const real_T
lc1 = .1554;
~ 61 ~
of
the
1.1341kg is
AN OPTIMAL SOLUTION
const real_T
lc2 = 0.0839; //%was .0275; %= (.0275*4.604075
+ 0.3127*1.1341)/m2
//const real_T
l1 = .3;
//const real_T
l2 = .3 + .15;
//%don't care
const real_T
I1 = .176;
const real_T
I2 = .03
+ 1.6732; //%AutoCAD says the
extension's Iyy around the the axis of rotation is 1.6732
kg/m^2
///////////////////////////////////////////////////////////////
/////////////////////////////////////////
//
LOCAL
FUNCTIONS
///////////////////////////////////////////////////////////////
//////////////////////
void
SlowDown(real_T
xy_Pos_des[2],
real_T
xy_Vel_des[2],
real_T xy_Acc_des[2], const real_T current_time,
const
real_T
Xstart[2],
const
real_T
Xend[2], const real_T Vs[2], const real_T maxAccel );
int Inv_Kin(real_T th[2], real_T X[2], real_T a1, real_T a2);
void DesiredJointAccel(real_T joint_out_acc[],
const real_T xy_pos[],
const
real_T
xy_des_pos[],
real_T xy_des_vel[], real_T xy_des_acc[],
const real_T joint_pos[], const
real_T joint_vel[],
const real_T kGains[], const
real_T a[], real_T xy_vel[], real_T thdot[] );
void DesiredValues(real_T xy_Pos_des[2], real_T xy_Vel_des[2],
real_T xy_Acc_des[2],
const
real_T
current_time,
const
real_T Xstart[2], const real_T Xend[2],
const real_T maxAccel, const real_T
minAccel);
//\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
/* Error handling
* -------------*
* You should use the following technique to report errors
encountered within
* an S-function:
*
*
ssSetErrorStatus(S,"Error encountered due to ...");
*
return;
*
* Note that the 2nd argument to ssSetErrorStatus must be
persistent memory.
* It cannot be a local variable. For example the following
will cause
* unpredictable errors:
*
*
mdlOutputs()
~ 62 ~
AN OPTIMAL SOLUTION
*
{
*
char msg[256];
{ILLEGAL: to fix use "static
char msg[256];"}
*
sprintf(msg,"Error due to %s", string);
*
ssSetErrorStatus(S,msg);
*
return;
*
}
*
* See matlabroot/simulink/src/sfuntmpl_doc.c for more details.
*/
/*====================*
* S-function methods *
*====================*/
/*
Function:
mdlInitializeSizes
===============================================
* Abstract:
*
The sizes information is used by Simulink to determine
the S-function
*
block's characteristics (number of inputs, outputs,
states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below
*/
ssSetNumSFcnParams(S, 0); /* Number of expected parameters
*/
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual
parameters */
return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 10)) return;
ssSetInputPortWidth(S, 0, 1); //object_detected
ssSetInputPortWidth(S, 1, 1); //init
ssSetInputPortWidth(S, 2, 1); //time
ssSetInputPortWidth(S, 3, 2); //Xstart
ssSetInputPortWidth(S, 4, 2); //Xend
ssSetInputPortWidth(S, 5, 2); //linkLen
ssSetInputPortWidth(S, 6, 2); //pAccels
ssSetInputPortWidth(S, 7, 2); //Gains (K0,K1)
ssSetInputPortWidth(S, 8, 2); //Q
ssSetInputPortWidth(S, 9, 2); //Q_dot
ssSetInputPortRequiredContiguous(S,
0,
input signal access*/
ssSetInputPortRequiredContiguous(S, 1,
~ 63 ~
true);
true);
/*direct
/*direct
AN OPTIMAL SOLUTION
ssSetInputPortRequiredContiguous(S, 2, true); /*direct
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in
either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 9)) return;
ssSetOutputPortWidth(S, 0, 2); //xy_curr_vel
ssSetOutputPortWidth(S, 1, 2);
//xy_curr_pos
//xy_Pos_des
//pParamError
//pState
//Joint Accelerations
(rad/s^2)
//xy_arm_position
//FinishingTime
//output torques
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, 0);
~ 64 ~
AN OPTIMAL SOLUTION
}
/*
Function:
mdlInitializeSampleTimes
=========================================
* Abstract:
*
This function is used to specify the sample time(s) for
your
*
S-function. You must register the same number of sample
times as
*
specified in ssSetNumSampleTimes.
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0.0);
}
#undef MDL_INITIALIZE_CONDITIONS
/* Change to #undef to
remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
/*
Function:
mdlInitializeConditions
========================================
* Abstract:
*
In this function, you should initialize the continuous
and discrete
*
states for your S-function block. The initial states
are placed
*
in
the
state
vector,
ssGetContStates(S)
or
ssGetRealDiscStates(S).
*
You can also perform any other initialization
activities that your
*
S-function may require. Note, this routine will be
called at the
*
start of simulation and if it is present in an enabled
subsystem
*
configured to reset states, it will be call when the
enabled subsystem
*
restarts execution to reset the states.
*/
static void mdlInitializeConditions(SimStruct *S)
{
}
#endif /* MDL_INITIALIZE_CONDITIONS */
#undef MDL_START /* Change to #undef to remove function */
#if defined(MDL_START)
/*
Function:
mdlStart
=======================================================
* Abstract:
~ 65 ~
AN OPTIMAL SOLUTION
*
This function is called once at start of model
execution. If you
*
have states that should be initialized once, this is
the place
*
to do it.
*/
static void mdlStart(SimStruct *S)
{
}
#endif /* MDL_START */
//%%%%%%%%%%%%%%%%%%%%%%%%%ANGLES
ARE
IN
RADIANS%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%DISTANCES
ARE
IN
CM%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
int Inv_Kin(real_T th[2], real_T X[2], real_T a1, real_T a2){
real_T
D
=
(X[0]*X[0]
a2*a2)/(2*a1*a2);
if (fabs(D) > 1){
return 1;
}
+
X[1]*X[1]
-
//% elbow up: sqrt(1-D^2), elbow down:-sqrt(1-D^2)
th[1] = atan2(sqrt(1-D*D),D); //%angle1
th[0]
=
atan2(X[1],X[0])
atan2(a2*sin(th[1]),a1+a2*cos(th[1])); //%angle2
return 0;
}
a1*a1-
-
/** function ComputeTorques
* given the current position and velocity and desired joint
acceleration, determines the necessary motor torque
* taking into consideration intertia and centrifugal forces.
* q
= Joint Position
* q_d = Joint Velocity
* q_dd
= Joins Acceleration
*/
void ComputeTorques( const real_T a[], const real_T q[], const
real_T q_d[],
const real_T q_dd[],
real_T joint_trq[] ){
// inertia and christoffel symbols
real_T
d11
=
m1*lc1*lc1
m2*(a[0]*a[0]+lc2*lc2+2*a[0]*lc2*cos(q[1])) + I1 +I2;
real_T d12 = m2*(lc2*lc2+a[0]*lc2*cos(q[1])) + I2;
real_T d21 = d12;
real_T d22 = m2*lc2*lc2 + I2; //CONSTANT
real_T c121 = -m2*a[0]*lc2*sin(q[1]);
real_T c112 = -c121;
real_T c211 = c121;
real_T c221 = c121;
//%The torques
joint_trq[0]
=
d11*q_dd[0]+
d12*q_dd[1]
(c121+c211)*q_d[0]*q_d[1] +c221*(q_d[1])*(q_d[1]);
~ 66 ~
+
+
AN OPTIMAL SOLUTION
joint_trq[1]
=
c112*(q_d[0])*(q_d[0]);
return;
}
d21*q_dd[0]+
d22*q_dd[1]
+
/** function SlowDown
* Inputs: xy_Pos_des desired (x,y) coordinate of End effector
* Description:
computes (x,y) accelerations, velocities and
positions desired to stop the arm
*/
void
SlowDown(real_T
xy_Pos_des[2],
real_T
xy_Vel_des[2],
real_T xy_Acc_des[2], const real_T current_time,
const
real_T
Xstart[2],
const
real_T
Xend[2], const real_T Vs[2], const real_T maxAccel )
{
real_T unit_vector[2];
real_T
Dist
=
sqrt(pow(Xend[0]-Xstart[0],2)
+
pow(Xend[1]-Xstart[1],2));
unit_vector[0] = (Xend[0]-Xstart[0])/Dist;
unit_vector[1] = (Xend[1]-Xstart[1])/Dist;
xy_Acc_des[0] = maxAccel*unit_vector[0];
xy_Acc_des[1] = maxAccel*unit_vector[1];
xy_Vel_des[0] = Vs[0] + xy_Acc_des[0]*current_time;
xy_Vel_des[1] = Vs[1] + xy_Acc_des[1]*current_time;
xy_Pos_des[0]
=
Xstart[0]
+
Vs[0]*current_time
0.5*current_time*current_time*xy_Acc_des[0];
xy_Pos_des[1]
= Xstart[1] + Vs[1]*current_time
0.5*current_time*current_time*xy_Acc_des[1];
+
+
return;
}
const
real_T
xy_des_pos[],
real_T joint_vel[],
real_T a[], real_T xy_vel[], real_T thdot[] ){
real_T xy_out_accel[2];
real_T Jinv[2][2], Jdot[2][2], Jdet;//, th1dot,th2dot;
real_T k0 = kGains[0];
// convert q and q_dot to Vel (velocity kinematics)
xy_vel[0]
=
(-a[0]*sin(joint_pos[0])
a[1]*sin(joint_pos[0]+joint_pos[1]))
*
joint_vel[0]
//Vel
Jacobian
+(-a[1]*sin(joint_pos[0]+joint_pos[1]))
* joint_vel[1];
~ 67 ~
AN OPTIMAL SOLUTION
xy_vel[1]
=
(
a[0]*cos(joint_pos[0])
+a[1]*cos(joint_pos[0]+joint_pos[1])) * joint_vel[0]
+( a[1]*cos(joint_pos[0]+joint_pos[1]))
* joint_vel[1];
// output xy acceleration in end effector coordinates
xy_out_accel[0]
=
xy_des_acc[0]-k0*(xy_pos[0]xy_des_pos[0])-k1*(xy_vel[0]-xy_des_vel[0]);
//eq 8.37, page
298
xy_out_accel[1]
=
xy_des_acc[1]-k0*(xy_pos[1]xy_des_pos[1])-k1*(xy_vel[1]-xy_des_vel[1]);
//GTH
//invKin(des_joint_pos, xy_pos, a);
// page 24, eq 1.10
if( joint_pos[1] == 0){
if(
xy_des_pos[1]
>
0
){
//to
avoid
the
singularity
joint_out_acc[0] = 0.2;
}
else if( xy_des_pos[1] < 0 ) {
joint_out_acc[0] = -0.2;
}
else{ //we are in an ok place
joint_out_acc[0] = 0;
}
return;
}
else{
Jdet = 1/(a[0]*a[1]*sin(joint_pos[1]));
}
Jinv[0][0] = Jdet*a[1]*cos(joint_pos[0]+joint_pos[1]);
Jinv[0][1] = Jdet*a[1]*sin(joint_pos[0]+joint_pos[1]);
Jinv[1][0]
=
Jdet*(-a[0]*cos(joint_pos[0])a[1]*cos(joint_pos[0]+joint_pos[1]));
Jinv[1][1]
=
Jdet*(-a[0]*sin(joint_pos[0])a[1]*sin(joint_pos[0]+joint_pos[1]));
thdot[0]
Jinv[0][0]*xy_des_vel[0]+Jinv[0][1]*xy_des_vel[1];
thdot[1]
=
=
Jdot[0][0]
=
-a[0]*cos(joint_pos[0])a[1]*cos(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[0][1]
=
a[1]*cos(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[1][0]
=
-a[0]*sin(joint_pos[0])a[1]*sin(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[1][1]
=
a[1]*sin(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
// desired acceleration in joint coordinates
~ 68 ~
AN OPTIMAL SOLUTION
//eq 8.35, page 298
joint_out_acc[0]
=
Jinv[0][0]*(xy_out_accel[0](Jdot[0][0]*thdot[0]+Jdot[0][1]*thdot[1]))
+Jinv[0][1]*(xy_out_accel[1](Jdot[1][0]*thdot[0]+Jdot[1][1]*thdot[1]));
joint_out_acc[1]
=
return;
}
real_T xy_Acc_des[2],
const
real_T
current_time,
const
minAccel){
real_T
V0[2];
real_T
unit_vector[2],interpoint[2];
real_T
dt,
Dist,t0,t1;
//compute t0;
Dist = sqrt(
pow(Xend[0]-Xstart[0],2) +
Xstart[1],2) );
pow(Xend[1]-
t0
=
sqrt(fabs(Dist/(0.5*
maxAccel
maxAccel*maxAccel/minAccel + 0.5*maxAccel*maxAccel/minAccel)));
t1 = -maxAccel*t0/minAccel;
unit_vector[0]
=
Xstart[0])/Dist*maxAccel/fabs(maxAccel);
unit_vector[1]
=
Xstart[1])/Dist*maxAccel/fabs(maxAccel);
interpoint[0]
=
0.5*maxAccel*t0*t0*unit_vector[0];
interpoint[1]
=
0.5*maxAccel*t0*t0*unit_vector[1];
(Xend[0](Xend[1]Xstart[0]
+
Xstart[1]
+
if ((current_time <= t0) && (current_time != 0)){
//Equations for first fraction of distance
xy_Acc_des[0]
= maxAccel*unit_vector[0];
xy_Acc_des[1]
= maxAccel*unit_vector[1];
xy_Vel_des[0]
= xy_Acc_des[0]*current_time;
xy_Vel_des[1]
= xy_Acc_des[1]*current_time;
xy_Pos_des[0]
=
Xstart[0]
+
0.5*current_time*xy_Vel_des[0];
xy_Pos_des[1]
=
Xstart[1]
+
0.5*current_time*xy_Vel_des[1];
}
else if( current_time <= t0+t1){
//Equations for
second fraction of distance
~ 69 ~
AN OPTIMAL SOLUTION
V0[0] = maxAccel*t0*unit_vector[0];
V0[1] = maxAccel*t0*unit_vector[1];
dt = (current_time-t0);
xy_Acc_des[0]
= minAccel*unit_vector[0];
xy_Acc_des[1]
= minAccel*unit_vector[1];
xy_Vel_des[0]
= V0[0] + xy_Acc_des[0]*dt;
xy_Vel_des[1]
= V0[1] + xy_Acc_des[1]*dt;
xy_Pos_des[0]
= interpoint[0] + V0[0]*dt +
0.5*dt*dt*xy_Acc_des[0];
xy_Pos_des[1]
= interpoint[1] + V0[1]*dt +
0.5*dt*dt*xy_Acc_des[1];
}
else{
xy_Pos_des[0]
= Xend[0];
xy_Pos_des[1]
= Xend[1];
xy_Vel_des[0]
= 0;
xy_Vel_des[1]
= 0;
xy_Acc_des[0]= 0;
xy_Acc_des[1]= 0;
}
return;
}
/*
Function:
mdlOutputs
=======================================================
* Abstract:
*
In this function, you compute the outputs of your Sfunction
*
block. Generally outputs are placed in the output vector,
ssGetY(S).
*/
real_T xy_Pos_obj[2];
real_T xy_Pos_zero[2];
real_T xy_Vel_obj[2];
static real_T xy_Vel[2];
static int state = 0;
real_T timeObj = 0;
real_T timeStop= 0;
static int finished = 0; //set flag to false
static real_T finishingTime = 0;
static void mdlOutputs(SimStruct *S, int_T tid)
{
//inputs
const
real_T
*pObj_detected
=
(const
real_T*)
ssGetInputPortSignal(S,0); // 1 = object detected, 0 = no
object
const real_T *pinit
=
(const
real_T*)
ssGetInputPortSignal(S,1); // 0= initing, 1 = run
~ 70 ~
AN OPTIMAL SOLUTION
const real_T *ptime
=
(const
real_T*)
ssGetInputPortSignal(S,2); //time (seconds)
const real_T *Xs
=
(const
real_T*)
ssGetInputPortSignal(S,3); // xy starting state (meters)
const real_T *Xe
=
(const
real_T*)
ssGetInputPortSignal(S,4); // xy ending state (meters)
const real_T *linkLen
=
(const
real_T*)
ssGetInputPortSignal(S,5); // length of links in m
const real_T *pAccel
=
(const
real_T*)
ssGetInputPortSignal(S,6); // maximum/min allowed acceleration
(m/s^2)
const real_T *kGains
=
(const
real_T*)
ssGetInputPortSignal(S,7); // gains K0 and K1 (position and
derivative)
const real_T *Q
=
(const
real_T*)
ssGetInputPortSignal(S,8); //joint position (radians)
const real_T *Q_dot
=
(const
real_T*)
ssGetInputPortSignal(S,9); //joint velocity (radians/s)
real_T maxAccel
real_T minAccel
= pAccel[0];
= pAccel[1];
real_T time = *ptime;
real_T init = *pinit;
real_T object_detected = *pObj_detected;
//outputs
real_T
*xy_curr_vel
= ssGetOutputPortSignal(S,0);
// current real velocity of joints
real_T
*xy_curr_pos
=
ssGetOutputPortSignal(S,1);
// current real position of joints
real_T
*xy_Pos_des
// desired position (m)
real_T
*pParamError
=
// error signal 1= err, 0 = OK
real_T
*pState
// state of system
real_T
*joint_out_acc
// desired joint accel (rad/s^2)
real_T
*X
=
// current xy arm position
real_T
*FinishTime
=
// total time to reach object
real_T
*joint_trq
=
// output torques
real_T xy_Vel_des[2];
real_T thdot[2];
// variables
//real_T X[2];
//current position of the end effector
real_T d, xy_Acc_des[2];
real_T Dist, unit_vector[2];
real_T X0[2];
X[0] = linkLen[0]*cos(Q[0]) + linkLen[1]*cos(Q[0]+Q[1]);
//Forward Kinematics
~ 71 ~
AN OPTIMAL SOLUTION
X[1] = linkLen[0]*sin(Q[0]) + linkLen[1]*sin(Q[0]+Q[1]);
if(init == 0){
//the robot needs to be sent to the
initialization setting
X0[0] = 0;
X0[1] = 0;
//check for error
*pParamError = 0;
//ensures that the line segment between Xs and Xe
is in the workspace of the robot
d
=
fabs(
((Xe[0]-Xs[0])*(Xs[1]-X0[1])-(Xs[0]X0[0])*(Xe[1]-Xs[1]))/sqrt(pow(Xs[0]-Xe[0],2)+pow(Xs[1]Xe[1],2)));
if(d < fabs(linkLen[0]-linkLen[1])){
//strErr='The line is unreachable for the
robot';
*pParamError = 1;
return;
}
else
if
(sqrt(
pow(Xs[0]-X0[0],2)+pow(Xs[1]X0[1],2)
)>(linkLen[0]+linkLen[1])
||
(sqrt(
pow(Xe[0]X0[0],2)+pow(Xe[1]-X0[1],2) )>(linkLen[0]+linkLen[1])) ){
//strErr='The line is not within the robot
workspace';
*pParamError = 2;
return;
}
xy_Acc_des[0]
xy_Acc_des[1]
xy_Vel_des[0]
xy_Vel_des[1]
xy_Pos_des[0]
xy_Pos_des[1]
Dist
X[1],2)
=
sqrt(
=
=
=
=
=
=
0;
0;
0;
0;
Xs[0];
Xs[1];
pow(Xs[0]-X[0],2)
+
pow(Xs[1]-
);
unit_vector[0] = (Xs[0]-X[0])/Dist;
if(Dist > 0.05) //limit the distance we try to go
{
xy_Pos_des[0] = X[0] + 0.05*unit_vector[0];
}
state = 0;
timeObj = 0;
timeStop= 0;
finishingTime = 0;
finished = 0;
}
else{//move to the goal
// %have we seen the object?
if( state == 0 && object_detected){
state = 1;
~ 72 ~
AN OPTIMAL SOLUTION
xy_Vel_obj[0]
=
xy_Vel[0];
//
in
XY
coordinates
xy_Vel_obj[1] = xy_Vel[1];
xy_Pos_obj[0] = X[0];
xy_Pos_obj[1] = X[1];
timeObj = time;
}
//dot product of Vel
and the unit vector from start to end. If it is positive, we
are moving along unit vector.
else if (state == 1 && ((Xe[0]-Xs[0])*xy_Vel[0] +
(Xe[1]-Xs[1])*xy_Vel[1])<=0 ){
xy_Pos_zero[0] = X[0];
xy_Pos_zero[1] = X[1];
state = 2;
timeStop = time;
}
//compute desired acceleration
if( state == 0){ //%search for object
DesiredValues(xy_Pos_des,
xy_Vel_des,
xy_Acc_des, time, Xs, Xe, maxAccel, minAccel);
}
else if (state == 1){ // %slow down arm
SlowDown(xy_Pos_des, xy_Vel_des, xy_Acc_des,
time-timeObj, xy_Pos_obj,Xe, xy_Vel_obj, -maxAccel );
}
else if (state == 2){ // % go to object
DesiredValues(xy_Pos_des,
xy_Vel_des,
xy_Acc_des,time-timeStop, xy_Pos_zero, xy_Pos_obj, -maxAccel,
maxAccel);
if(
xy_Acc_des[1]== 0)
{
finished == 0 && xy_Acc_des[0] == 0 &&
finished = 1; //set flag to true
finishingTime = time; //record finishing
time
}
}
}//end big else on <init>
*FinishTime =finishingTime;
//Inv_Kin(th_des, xy_Pos_des, linkLen[0], linkLen[1]);
*pState = (real_T) state;
xy_Vel[0] = xy_Vel_des[0]; //save previous velocity
xy_Vel[1] = xy_Vel_des[1];
DesiredJointAccel(joint_out_acc,
X,
xy_Pos_des,
xy_Acc_des,
Q, Q_dot,
kGains,linkLen,
xy_curr_vel,thdot);
// compute the torques for each link
~ 73 ~
xy_Vel_des,
AN OPTIMAL SOLUTION
//ComputeTorques( linkLen, Q, Q_dot,
joint_trq );
ComputeTorques( linkLen, Q, thdot,
joint_trq );
return;
}
joint_out_acc,
joint_out_acc,
#define MDL_UPDATE /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
/*
Function:
mdlUpdate
======================================================
* Abstract:
*
This function is called once for every major
integration time step.
*
Discrete states are typically updated here, but this
function is useful
*
for performing any tasks that should only take place
once per
*
integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
}
#endif /* MDL_UPDATE */
#define MDL_DERIVATIVES /* Change to #undef to remove function
*/
#if defined(MDL_DERIVATIVES)
/*
Function:
mdlDerivatives
=================================================
* Abstract:
*
In this function, you compute the S-function block's
derivatives.
*
The derivatives are placed in the derivative vector,
ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
}
#endif /* MDL_DERIVATIVES */
/*
Function:
mdlTerminate
=====================================================
* Abstract:
*
In this function, you should perform any actions that are
necessary
*
at the termination of a simulation.
For example, if
memory was
*
allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
~ 74 ~
AN OPTIMAL SOLUTION
{
}
/*======================================================*
* See sfuntmpl_doc.c for the optional S-function methods *
*======================================================*/
/*=============================*
* Required S-function trailer *
*=============================*/
#ifdef MATLAB_MEX_FILE
MEX-file? */
#include "simulink.c"
#else
#include "cg_sfun.h"
function */
#endif
/* Is this file being compiled as a
/* MEX-file interface mechanism */
/* Code generation registration
D.2. Functions for a raster scan movement.
/*
* MoveRobot.c:
*
This function computes desired XY accelerations and joint
accelerations
* for a 2-link robot implementing a raster scan search for an
object.
* Irene Ruano and Aaron Becker
* Basic 'C' template for a level 2 S-function.
*
* -----------------------------------------------------------------------* | See matlabroot/simulink/src/sfuntmpl_doc.c for a more
detailed template |
* -----------------------------------------------------------------------*
*
*/
/*
* You must specify the S_FUNCTION_NAME as the name of your Sfunction
* (i.e. replace sfuntmpl_basic with the name of your Sfunction).
*/
#define S_FUNCTION_NAME MoveRobot
#define S_FUNCTION_LEVEL 2
#define m_pi 3.141592653589793
/*
~ 75 ~
AN OPTIMAL SOLUTION
* Need to include simstruc.h for the definition of the
SimStruct and
* its associated macro definitions.
*/
#include "simstruc.h"
#include "stdio.h"
//constants: Parameter of Inertia and C Matrices:
const real_T
m1 = 7.848;
const real_T
m2 = 4.604075 + 1.1341; // %
1.1341kg is
mass of arm extension
const real_T
lc1 = .1554;
const real_T
lc2 = 0.0839; //%was .0275; %= (.0275*4.604075
+ 0.3127*1.1341)/m2
//const real_T
l1 = .3;
//const real_T
l2 = .3 + .15;
//%don't care
const real_T
I1 = .176;
const real_T
I2 = .03 + 1.6732; //%AutoCAD says the
extension's Iyy around the the axis of rotation is 1.6732
kg/m^2
/* Error handling
* -------------*
* You should use the following technique to report errors
encountered within
* an S-function:
*
*
ssSetErrorStatus(S,"Error encountered due to ...");
*
return;
*
* Note that the 2nd argument to ssSetErrorStatus must be
persistent memory.
* It cannot be a local variable. For example the following
will cause
* unpredictable errors:
*
*
mdlOutputs()
*
{
*
char msg[256];
{ILLEGAL: to fix use "static
char msg[256];"}
*
sprintf(msg,"Error due to %s", string);
*
ssSetErrorStatus(S,msg);
*
return;
*
}
*
* See matlabroot/simulink/src/sfuntmpl_doc.c for more details.
*/
/*====================*
* S-function methods *
*====================*/
~ 76 ~
AN OPTIMAL SOLUTION
/* Function: mdlInitializeSizes
===============================================
* Abstract:
*
The sizes information is used by Simulink to determine
the S-function
*
block's characteristics (number of inputs, outputs,
states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below
*/
ssSetNumSFcnParams(S, 0); /* Number of expected parameters
*/
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual
parameters */
return;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 12)) return;
ssSetInputPortWidth(S, 0, 1); //object_detected
ssSetInputPortWidth(S, 1, 1); //init
ssSetInputPortWidth(S, 2, 1); //time
ssSetInputPortWidth(S, 3, 2); //Xstart
ssSetInputPortWidth(S, 4, 2); //Xend
ssSetInputPortWidth(S, 5, 2); //linkLen
ssSetInputPortWidth(S, 6, 1); //radious
ssSetInputPortWidth(S, 7, 2); //pAccel(max and min for
bang bang)
ssSetInputPortWidth(S, 8, 1); //minVel
ssSetInputPortWidth(S, 9, 2); //Gains (K0,K1)
ssSetInputPortWidth(S, 10, 2);
//Q
ssSetInputPortWidth(S, 11, 2);
//Q_dot
~ 77 ~
AN OPTIMAL SOLUTION
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in
either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
if (!ssSetNumOutputPorts(S, 7)) return;
ssSetOutputPortWidth(S, 0, 2); //xy_Curr_vel
//xy_Curr_pos
//xy_Pos_des
//pParamError
//pState
//Joint Accelerations
(rad/s^2)
//output torques
ssSetNumSampleTimes(S, 1);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
ssSetNumModes(S, 0);
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, 0);
}
/* Function: mdlInitializeSampleTimes
=========================================
* Abstract:
*
This function is used to specify the sample time(s) for
your
~ 78 ~
AN OPTIMAL SOLUTION
*
S-function. You must register the same number of sample
times as
*
specified in ssSetNumSampleTimes.
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0.0);
}
#undef MDL_INITIALIZE_CONDITIONS
/* Change to #undef to
remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
/* Function: mdlInitializeConditions
========================================
* Abstract:
*
In this function, you should initialize the continuous
and discrete
*
states for your S-function block. The initial states
are placed
*
in the state vector, ssGetContStates(S) or
ssGetRealDiscStates(S).
*
You can also perform any other initialization
activities that your
*
S-function may require. Note, this routine will be
called at the
*
start of simulation and if it is present in an enabled
subsystem
*
configured to reset states, it will be call when the
enabled subsystem
*
restarts execution to reset the states.
*/
static void mdlInitializeConditions(SimStruct *S)
{
}
#endif /* MDL_INITIALIZE_CONDITIONS */
#undef MDL_START /* Change to #undef to remove function */
#if defined(MDL_START)
/* Function: mdlStart
=======================================================
* Abstract:
*
This function is called once at start of model
execution. If you
*
have states that should be initialized once, this is
the place
*
to do it.
*/
static void mdlStart(SimStruct *S)
{
}
#endif /* MDL_START */
~ 79 ~
AN OPTIMAL SOLUTION
/** function ComputeTorques
* given the current position and velocity and desired joint
acceleration, determines the necessary motor torque
* taking into consideration intertia and centrifugal forces.
* q
= Joint Position
* q_d = Joint Velocity
* q_dd
= Joins Acceleration
*/
void ComputeTorques( const real_T a[], const real_T q[], const
real_T q_d[],
const real_T q_dd[],
real_T joint_trq[] ){
// inertia and christoffel symbols
real_T d11 = m1*lc1*lc1 +
m2*(a[0]*a[0]+lc2*lc2+2*a[0]*lc2*cos(q[1])) + I1 +I2;
real_T d12 = m2*(lc2*lc2+a[0]*lc2*cos(q[1])) + I2;
real_T d21 = d12;
real_T d22 = m2*lc2*lc2 + I2; //CONSTANT
real_T c121 = -m2*a[0]*lc2*sin(q[1]);
real_T c112 = -c121;
real_T c211 = c121;
real_T c221 = c121;
//%The torques
joint_trq[0] = d11*q_dd[0]+ d12*q_dd[1] +
(c121+c211)*q_d[0]*q_d[1] +c221*(q_d[1])*(q_d[1]);
joint_trq[1] = d21*q_dd[0]+ d22*q_dd[1] +
c112*(q_d[0])*(q_d[0]);
return;
}
/** function Inv_Kin
* Description: Inverse Kinematics. Returns angle of joints
given an end effector position
* Characteristics: ANGLES IN RADIANS. DISTANCES IN M
*/
int Inv_Kin(real_T th[2], real_T X[2], real_T a1, real_T a2)
{
real_T D = (X[0]*X[0] + X[1]*X[1] - a1*a1a2*a2)/(2*a1*a2);
if (fabs(D) > 1){
return 1;
}
//% elbow up: sqrt(1-D^2), elbow down:-sqrt(1-D^2)
th[1] = atan2(sqrt(1-D*D),D); //%angle1
th[0] = atan2(X[1],X[0]) atan2(a2*sin(th[1]),a1+a2*cos(th[1])); //%angle2
return 0;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
~ 80 ~
AN OPTIMAL SOLUTION
/** function SlowDown
* --flag indicates where the object was found. Xend is where
the end of the first line is.
* -- time_incr is the time elapsed since we found the object
* Vel_o and Pos_o are the velocity and position of the robot
when it found the object
* Description: computes (x,y) accelerations, velocities and
positions desired to stop the arm after having found the object
* Characteristics: MaxAccel HAS TO BE POSITIVE WHEN FED
IN!!!!!!!!!!!!!
*/
static real_T t_aux, Vel_aux[2]; // t_aux is the time when we
move from one type of line segment onto another, Vel_aux is the
corresponding XY velocity
void SlowDown(real_T flag, real_T radious, real_T leg, const
real_T *Xstart,
const real_T *Xend, real_T maxAccel, real_T
Vel_o[2], real_T Pos_o[2],
real_T X_des[2], real_T Vel_des[2], real_T
Accel_des[2], real_T time_incr, int *state)
{
real_T unit_vector_line[2], Vel_mag;
real_T theta, theta_dot, theta_dot_dot;
real_T dt;
//printf("Flag = %f, Vel_o: %f,%f\n", (float)flag,
(float)Vel_o[0], (float)Vel_o[1]);
unit_vector_line[0] = 0;
unit_vector_line[1] = 1;
if (flag == 1) //object found in first line
{
X_des[0]
= Pos_o[0];
X_des[1]
= Pos_o[1] + Vel_o[1]*time_incr 0.5*maxAccel*pow(time_incr,2);
Vel_des[0]
= Vel_o[0] time_incr*maxAccel*unit_vector_line[0];
Vel_des[1]
= Vel_o[1] time_incr*maxAccel*unit_vector_line[1];
Accel_des[0] = -maxAccel*unit_vector_line[0];
if (X_des[1] <= (Xstart[1] + leg)) // We are still
on the first leg
{
t_aux = time_incr;
Vel_aux[0] = Vel_des[0];
}
if ((Vel_des[1] <= 0) && (X_des[1] <= Xstart[1] +
leg)) //if y coord. velocity is 0 and we have stopped in the
line
~ 81 ~
AN OPTIMAL SOLUTION
{
*state = 2;
return;
}
if (X_des[1] > Xstart[1] + leg) //we went past the
leg into the curve
{
dt = time_incr - t_aux;
theta_dot_dot = -maxAccel/radious;
theta_dot
= Vel_aux[1]/radious +
theta_dot_dot*dt;
theta
= Vel_aux[1]/radious*dt +
0.5*theta_dot_dot*pow(dt,2);
X_des[0]
= Xstart[0] - radious +
radious*cos(theta);
X_des[1]
= Xstart[1] + leg + radious*
sin(theta);
Vel_des[0]
= -radious*sin(theta)*theta_dot;
Vel_des[1]
= Vel_aux[1] +
radious*cos(theta)*theta_dot;
Accel_des[0] = radious*cos(theta)*pow(theta_dot,2) radious*sin(theta)*theta_dot_dot;
Accel_des[1] = radious*sin(theta)*pow(theta_dot,2) +
radious*cos(theta)*theta_dot_dot;
if (theta_dot <= 0) //we have stopped
{
*state = 2;
return;
}
//if object is found in b, it comes to a stop
before getting to point c AS LONG AS
//THE RADIOUS IS NOT DECREASED TOO MUCH
TO CHANGE THE VELOCITY CONSTRAINT WIDTH SIGNIFICANTLY
}
} //closes curly brackets on else if flag1
else if (flag == 2) //object found in curve
{
theta_dot_dot = -maxAccel/radious;
//constant
deceleration
Vel_mag = sqrt(pow(Vel_o[0],2) + pow(Vel_o[1],2));
theta_dot
= Vel_mag/radious +
theta_dot_dot*time_incr;
theta
= atan2(Pos_o[1]-leg-Xstart[1],
Pos_o[0]-Xstart[0]+radious) + Vel_mag/radious*time_incr +
0.5*theta_dot_dot*pow(time_incr,2);
X_des[0]
= Xstart[0] - radious +
radious*cos(theta);
X_des[1]
= Xstart[1] + leg + radious*sin(theta);
Vel_des[0]
= -radious*sin(theta)*theta_dot;
Vel_des[1]
= radious*cos(theta)*theta_dot;
Accel_des[0] = -radious*cos(theta)*pow(theta_dot,2)radious*sin(theta)*theta_dot_dot;
~ 82 ~
AN OPTIMAL SOLUTION
Accel_des[1] = radious*sin(theta)*pow(theta_dot,2)+radious*cos(theta)*theta_do
t_dot;
if (theta < m_pi) //we are still on the curve
{
t_aux = time_incr;
if (theta_dot <= 0) //we stopped on the curve
{
*state = 2;
return;
}
}
if (theta > m_pi) //we have passed the curve
{
dt = time_incr - t_aux;
X_des[0]
= Xstart[0] - 2.0*radious;
X_des[1]
= Xend[1] + Vel_aux[1]*dt +
0.5*pow(dt,2)*maxAccel;
Vel_des[0]
= Vel_aux[0] +
dt*maxAccel*unit_vector_line[0];
Vel_des[1]
= Vel_aux[1] +
Accel_des[0] = maxAccel*unit_vector_line[0];
Accel_des[1] = maxAccel*unit_vector_line[1];
if (Vel_des[1] >= 0) //we start going upwards
{
*state = 2;
return;
}
}
} //closes curly brackets of else if flag2
else if (flag == 3) //object found in second line
{
X_des[0]
= Xstart[0] - 2*radious;
X_des[1]
= Pos_o[1] + Vel_o[1]*time_incr +
0.5*pow(time_incr,2)*maxAccel;
Vel_des[0]
= 0;
Vel_des[1]
= Vel_o[1] +
time_incr*maxAccel*unit_vector_line[1];
Accel_des[0] = 0;
if (Vel_des[1] >= 0)
{
*state = 2;
return;
}
}
return;
} //closes curly brackets of function
~ 83 ~
AN OPTIMAL SOLUTION
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/** function DesiredJointAccel
* Description: Outputs the desired acceleration for each
joint, having applied a controller, given xy values
*/
const real_T xy_des_pos[],
real_T joint_vel[],
real_T a[] , real_T xy_vel[], real_T thdot[] )
{
real_T xy_out_accel[2];
real_T Jinv[2][2], Jdot[2][2], Jdet;
// convert q and q_dot to Vel (inverse velocity
kinematics)
xy_vel[0] = (-a[0]*sin(joint_pos[0]) a[1]*sin(joint_pos[0]+joint_pos[1])) * joint_vel[0] //Vel
Jacobian
+(-a[1]*sin(joint_pos[0]+joint_pos[1]))
* joint_vel[1];
xy_vel[1] = ( a[0]*cos(joint_pos[0])
+a[1]*cos(joint_pos[0]+joint_pos[1])) * joint_vel[0]
+( a[1]*cos(joint_pos[0]+joint_pos[1]))
* joint_vel[1];
// output xy acceleration in end effector coordinates
xy_out_accel[0] = xy_des_acc[0]-k0*(xy_pos[0]xy_des_pos[0])-k1*(xy_vel[0]-xy_des_vel[0]);
//eq 8.37, page
298
xy_out_accel[1] = xy_des_acc[1]-k0*(xy_pos[1]xy_des_pos[1])-k1*(xy_vel[1]-xy_des_vel[1]);
// ONLY USE POSITION INFO!
//
xy_out_accel[0] = -k0*(xy_pos[0]-xy_des_pos[0]);
8.37, page 298
//
xy_out_accel[1] = -k0*(xy_pos[1]-xy_des_pos[1]);
//invKin(des_joint_pos, xy_pos, a);
// page 24, eq 1.10
if( joint_pos[1] == 0){
if( xy_des_pos[1] > 0 ){ //avoid singularity
}
else if( xy_des_pos[1] < 0 ) {
~ 84 ~
//eq
AN OPTIMAL SOLUTION
}
else{ //we are in an ok place
}
return;
}
else{
Jdet = 1/(a[0]*a[1]*sin(joint_pos[1]));
}
Jinv[0][0] = Jdet*a[1]*cos(joint_pos[0]+joint_pos[1]);
Jinv[0][1] = Jdet*a[1]*sin(joint_pos[0]+joint_pos[1]);
Jinv[1][0] = Jdet*(-a[0]*cos(joint_pos[0])a[1]*cos(joint_pos[0]+joint_pos[1]));
Jinv[1][1] = Jdet*(-a[0]*sin(joint_pos[0])a[1]*sin(joint_pos[0]+joint_pos[1]));
thdot[0] =
thdot[1] =
Jdot[0][0] = -a[0]*cos(joint_pos[0])a[1]*cos(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[0][1] = a[1]*cos(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[1][0] = -a[0]*sin(joint_pos[0])a[1]*sin(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
Jdot[1][1] = a[1]*sin(joint_pos[0]+joint_pos[1])*(thdot[0]+thdot[1]);
// desired acceleration in joint coordinates
//eq 8.35, page 298
joint_out_acc[0] =
joint_out_acc[1] =
return;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/** function TrajectoryValues
* Description: Computes the position and velocities values
for only certain specific times and parts of the trajectory of
interest.(t0,a,b,c,d,e)
~ 85 ~
AN OPTIMAL SOLUTION
*/
void TrajectoryValues (real_T Times[6], real_T Points[2][6],
real_T leg,
const real_T radious, const
const real_T maxAccel, real_T
minAccel, const real_T minVel)
{
////////////////// REMEMBER: MaxAccel ALWAYS HAS TO BE 1!
/////////////////// Xend is where te first straight line ends,
before the turn /////////////////
real_T a, Dist, pos_a, vel_a, pos_d, vel_d, s0[400],
sd0[400];
real_T t0, t1, T0[400], timetoa, timetob, timetoc,
timetod, timetostop;
real_T x0, v0, v1, x1, x02, v02, x12;
int i;
a = 1+sqrt(2);
Dist = 2.0*leg + m_pi*radious; //Distance from origin to
end plus the turn and the come back
// times in bang-bang control
t0 = sqrt(fabs(Dist/(0.5*maxAccel 0.5*pow(maxAccel,2)/minAccel))); //time applying maxAccel from
beggining until switch
t1 = -maxAccel*t0/minAccel; //time to come to a stop
going at minAccel
//Position and velocity profiles in the bang-bang control
T0[0] = 0;
s0[0] = 0;
sd0[0] = 0;
for (i=1 ;i<400 ; i++)
{
T0[i] = T0[i-1] + t0/400.0; // creating time
vector
s0[i] = 0.5*maxAccel*pow(T0[i],2); //position
profile
sd0[i] = T0[i]*maxAccel; //velocity profile
}
//Position and velocity of point a, first intersection.
x0 = s0[399];
v0 = sd0[399];
v1 = minVel;
x1 = leg;
pos_a =
//position of
vel_a =
a
timetoa
t0 to a
timetob
to b
((pow(v0,2)-pow(v1,2)-2*(x1-x0))/(1/a-2))+ x0;
point a
sqrt(pow(v0,2)-(pos_a-x0)/a); //velocity of point
= (vel_a - sd0[399])/minAccel; //time to go from
= (vel_a - minVel)/maxAccel; //time to go from a
~ 86 ~
AN OPTIMAL SOLUTION
timetoc = radious*m_pi/minVel; //time to go from b to c
//Position and velocity of point d, second intersection
x02 = leg + m_pi*radious;
v02 = minVel;
x12 = Dist;
//v12 = 0;
pos_d = (x12-x02-pow(v02,2)*a)/(2*a+1)+x02; //position of
point d
vel_d = sqrt(pow(v02,2)+2*(pos_d-x02)); //velocity of
point d
timetod = (vel_d - minVel)/maxAccel; //time to go from c
to d
timetostop = -vel_d/minAccel; //time to come to a stop
from d
//points coordinates. points t0, a,b,c,d. First row is
position, second is velocity
Points[0][0]
Points[1][0]
Points[0][1]
Points[1][1]
Points[0][2]
Points[1][2]
Points[0][3]
Points[1][3]
Points[0][4]
Points[1][4]
Points[0][5]
Points[1][5]
=
=
=
=
=
=
=
=
=
=
=
=
s0[399];
sd0[399];
pos_a;
vel_a;
leg;
minVel;
leg + radious*m_pi;
minVel;
pos_d;
vel_d;
Dist;
0;
//Precomputed times to reach each location (t0, a, b, c,
d)
Times[0]
Times[1]
Times[2]
Times[3]
Times[4]
Times[5]
=
=
=
=
=
=
t0;
timetoa + Times[0];
timetob + Times[1];
timetoc + Times[2];
timetod + Times[3];
timetostop + Times[4];
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/** function DesiredValues
* Description: Computes the desired values for acceleration,
velocity and position along the trajectory (without object)
* Characteristics: minAccel is given, but it is the optimal
one, not the bang bang
*/
real_T xy_Acc_des[2], real_T leg,
~ 87 ~
AN OPTIMAL SOLUTION
real_T current_time, const real_T
Xstart[2], const real_T Xend[2], const real_T radious,
minVel, real_T minAccel,
real_T Times_val[6], real_T
Points_val[2][6])
{
////////////////// REMEMBER: MaxAccel ALWAYS HAS TO BE 1
/////////////////// Xend is where the first straight line
ends, before the turn /////////////////
real_T dt;
real_T unit_vector_line[2], theta, theta_dot;
unit_vector_line[0] =
Xstart[0])/sqrt(pow(Xend[0]
Xstart[1],2));
unit_vector_line[1] =
Xstart[1])/sqrt(pow(Xend[0]
Xstart[1],2));
(Xend[0] - Xstart[0],2) + pow(Xend[1] (Xend[1] - Xstart[0],2) + pow(Xend[1] -
if (current_time == 0){ //TODO: this is unecessary since
the init state is ok
xy_Acc_des[0] = 0;
xy_Acc_des[1] = 0;
xy_Vel_des[0] = 0;
xy_Vel_des[1] = 0;
xy_Pos_des[0] = Xstart[0];
}
else if (current_time <= Times_val[0]){
//Equations for
first fraction of distance, from origin to point t0
xy_Acc_des[0] = maxAccel*unit_vector_line[0];
xy_Vel_des[0] =
current_time*maxAccel*unit_vector_line[0];
xy_Vel_des[1] =
current_time*maxAccel*unit_vector_line[1];
xy_Pos_des[1] = Xstart[1] +
0.5*pow(current_time,2)*maxAccel;
}
else if(current_time <= Times_val[1]){
//Equations
for second fraction of distance, from point t0 to point a
dt = (current_time-Times_val[0]);
xy_Acc_des[0] = minAccel*unit_vector_line[0];
xy_Acc_des[1] = minAccel*unit_vector_line[1];
xy_Vel_des[0] = (Points_val[1][0] +
dt*minAccel)*unit_vector_line[0];
xy_Vel_des[1] = (Points_val[1][0] +
dt*minAccel)*unit_vector_line[1];
xy_Pos_des[1] = Xstart[1] + Points_val[0][0] +
Points_val[1][0]*dt + 0.5*pow(dt,2)*minAccel;
}
~ 88 ~
AN OPTIMAL SOLUTION
//Equations
for third fraction of distance, from point a to point b
xy_Acc_des[0] = -maxAccel*unit_vector_line[0];
xy_Acc_des[1] = -maxAccel*unit_vector_line[1];
xy_Vel_des[0] = (Points_val[1][1] dt*maxAccel)*unit_vector_line[0];
xy_Vel_des[1] = (Points_val[1][1] dt*maxAccel)*unit_vector_line[1];
xy_Pos_des[1] = Xstart[1] + Points_val[0][1] +
Points_val[1][1]*dt - 0.5*pow(dt,2)*maxAccel;
}
//Equations
for fourth fraction of distance, from point b to point c
theta = minVel/radious*dt;
theta_dot = minVel/radious;
xy_Acc_des[0] = radious*cos(theta)*pow(theta_dot,2);
xy_Acc_des[1] = radious*sin(theta)*pow(theta_dot,2);
xy_Vel_des[0] = -radious*sin(theta)*theta_dot;
xy_Vel_des[1] = radious*cos(theta)*theta_dot;
xy_Pos_des[0] = Xstart[0] - radious +
radious*cos(theta);
xy_Pos_des[1] = Xstart[1] + leg +
radious*sin(theta);
//printf(" Pos at end of curve: x: %f
y: %f \n",
(float) xy_Pos_des[0], (float)xy_Pos_des[1]);
}
//Equations
for fifth fraction of distance, from point c to point d
xy_Vel_des[0] = dt*maxAccel*unit_vector_line[0];
xy_Vel_des[1] = minVel +
xy_Pos_des[0] = Xstart[0] - 2*radious;
xy_Pos_des[1] = Xend[1] - minVel*dt 0.5*pow(dt,2)*maxAccel;
}
//Equations
for sixth fraction of distance, from point d to point end
xy_Acc_des[0] = 0;//minAccel*unit_vector_line[0];
xy_Acc_des[1] = -minAccel*unit_vector_line[1];
xy_Vel_des[0] =
0;//dt*minAccel*unit_vector_line[0];
xy_Vel_des[1] = -Points_val[1][4] dt*minAccel*unit_vector_line[1];
xy_Pos_des[0] = Xstart[0] - 2*radious;
xy_Pos_des[1] = Xstart[1] + (2*leg+m_pi*radiousPoints_val[0][4]) - Points_val[1][4]*dt .5*minAccel*dt*dt;//Xend[1] - (Points_val[0][4] -
~ 89 ~
AN OPTIMAL SOLUTION
Points_val[0][3]) - Points_val[1][4]*dt +
0.5*pow(dt,2)*minAccel;
//printf(" unitvector: %f,%f, minAccel:
Velocity: %f\n", (float) unit_vector_line[0],
(float)unit_vector_line[1], (float) minAccel,
(float)Points_val[1][4]);
}
else{
xy_Pos_des[0] = Xstart[0] - 2.0*radious;
xy_Vel_des[0] = 0.0;
xy_Vel_des[1] = 0.0;
xy_Acc_des[0] = 0.0;
xy_Acc_des[1] = 0.0;
%f,
}
return;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/** function DesiredValues_bangbang
* Description: Computes the desired values for acceleration,
velocity and position for the bang bang control,
* to go in a straight line from where the robot has stopped
to where the object is
* Characteristics: minAccel is given, it's a different one
than the optimal.
* IMPORTANT: Xstart IS WHEN THE ROBOT STOPPED, Xend IS WHERE
THE OBJECT IS
* --time_incr is the time elapsed since we stopped
* --MaxAccel DOESN'T HAVE TO BE 1 IN HERE
* maxAcc has to be positive, and minAcc has to be negative
*/
void DesiredValues_bangbang(real_T time_incr, const real_T
Xstart[2], const real_T Xend[2],
const real_T maxAccel,
const real_T minAccel_bangbang, real_T X_des[2],
real_T Vel_des[2],
real_T Accel_des[2])
{
real_T Dist, unit_vec[2], interpoint[2], V0[2];
real_T t0, t1;
//Computation of interpoint
Dist = sqrt(pow(Xend[0]-Xstart[0],2) + pow(Xend[1]Xstart[1],2)); //Distance to object in a straight line
// Computation of times
t0 = sqrt(Dist/(0.5*maxAccel + maxAccel*(maxAccel/minAccel_bangbang) + 0.5*minAccel_bangbang*pow((maxAccel/minAccel_bangbang),2))); //time to interpoint
t1 = (-maxAccel*t0/minAccel_bangbang); //time to end from
interpoint
//unit vector of straight line from stop to object
~ 90 ~
AN OPTIMAL SOLUTION
unit_vec[0] = (Xend[0]-Xstart[0])/sqrt(pow(Xend[0]Xstart[0],2) + pow(Xend[1]-Xstart[1],2));
unit_vec[1] = (Xend[1]-Xstart[1])/sqrt(pow(Xend[0]Xstart[0],2) + pow(Xend[1]-Xstart[1],2));
interpoint[0] = 0.5*maxAccel*pow(t0,2)*unit_vec[0] +
Xstart[0];
interpoint[1] = 0.5*maxAccel*pow(t0,2)*unit_vec[1] +
Xstart[1];
if (time_incr < t0)
{
//Equations for first fraction of distance
X_des[0]
= Xstart[0] +
0.5*maxAccel*pow(time_incr,2)*unit_vec[0];
X_des[1]
= Xstart[1] +
0.5*maxAccel*pow(time_incr,2)*unit_vec[1];
Vel_des[0]
= maxAccel*time_incr*unit_vec[0];
Vel_des[1]
= maxAccel*time_incr*unit_vec[1];
Accel_des[0] = maxAccel*unit_vec[0];
Accel_des[1] = maxAccel*unit_vec[1];
}
else if (time_incr > t0)
{
//Equations for second fraction of distance
V0[0] = maxAccel*t0*unit_vec[0];
V0[1] = maxAccel*t0*unit_vec[1];
if (time_incr < t0+t1)
{
X_des[0]
= interpoint[0] + V0[0]*(time_incrt0)+0.5*minAccel_bangbang*pow((time_incr-t0),2)*unit_vec[0];
X_des[1]
= interpoint[1] +
V0[1]*(time_incr-t0)+0.5*minAccel_bangbang*pow((time_incrt0),2)*unit_vec[1];
Vel_des[0]
= V0[0] +
minAccel_bangbang*(time_incr-t0)*unit_vec[0];
Vel_des[1]
= V0[1] +
minAccel_bangbang*(time_incr-t0)*unit_vec[1];
Accel_des[0] = minAccel_bangbang*unit_vec[0];
Accel_des[1] = minAccel_bangbang*unit_vec[1];
}
else
{
X_des[0]
= Xend[0];
X_des[1]
= Xend[1];
Vel_des[0]
= 0;
Vel_des[1]
= 0;
Accel_des[0] = 0;
Accel_des[1] = 0;
}
}
}
~ 91 ~
AN OPTIMAL SOLUTION
/* Function: mdlOutputs
=======================================================
* Abstract:
*
In this function, you compute the outputs of your Sfunction
*
block. Generally outputs are placed in the output vector,
ssGetY(S).
*/
static int state = 0; // 0 is init/starting search, 1 means
we saw the object
2 is bang-bang to object, 3 is finished
static real_T flag = 0;
static real_T timeObj = 0;
static real_T timeStop= 0;
static real_T xy_Vel[2] = {0,0}; //the desired velocity from
the previous step
static real_T xy_Pos_obj[2];
static real_T xy_Vel_obj[2];
static real_T xy_stop_Pos[2];
static real_T xy_prev_Pos[2] = {0,0};
static void mdlOutputs(SimStruct *S, int_T tid)
{
//inputs
const real_T *pObj_detected = (const real_T*)
ssGetInputPortSignal(S,0); // 1 = object detected, 0 = no
object
const real_T *pinit
= (const real_T*)
ssGetInputPortSignal(S,1); // 0= initing, 1 = run
const real_T *ptime
= (const real_T*)
ssGetInputPortSignal(S,2); //time (seconds)
const real_T *Xs
= (const real_T*)
ssGetInputPortSignal(S,3); // xy starting state (meters)
const real_T *Xe
= (const real_T*)
ssGetInputPortSignal(S,4); // xy ending state (meters)
const real_T *linkLen
= (const real_T*)
ssGetInputPortSignal(S,5); // length of links in m
const real_T *pradious
= (const real_T*)
ssGetInputPortSignal(S,6); // radious of turn in m
const real_T *pAccel
= (const real_T*)
ssGetInputPortSignal(S,7); // maximum acceleration and min (for
bang bang)(m/s^2)
const real_T *pminVel
= (const real_T*)
ssGetInputPortSignal(S,8); // minimum velocity (m/s)
const real_T *kGains
= (const real_T*)
ssGetInputPortSignal(S,9); // gains K0 and K1 (position and
derivative)
const real_T *Q
= (const real_T*)
ssGetInputPortSignal(S,10); //joint position (radians)
const real_T *Q_dot
= (const real_T*)
ssGetInputPortSignal(S,11); //joint velocity (radians/s)
real_T maxAccel
first element (POSITIVE)
= pAccel[0]; //maxAccel is the
~ 92 ~
AN OPTIMAL SOLUTION
real_T
bang control
SIMULINK)
real_T
real_T
minAccel_bang = pAccel[1]; //minAccel for bang
(not optimal) is the second element (POSITIVE IN
minVel
radious
= *pminVel;
= *pradious;
real_T time = *ptime;
real_T init = *pinit;
real_T object_detected = *pObj_detected;
//outputs
real_T
*xy_curr_vel
desired acceleration (m/s^2)
real_T
*xy_curr_pos
desired velocity (m/s)
real_T
*xy_Pos_des
desired position (m)
real_T
*pParamError
error signal 1= err, 0 = OK
real_T
*pState
state of system
real_T
*joint_out_acc
desired joint accel (rad/s^2)
real_T
*joint_trq
real_T thdot[2];
= ssGetOutputPortSignal(S,0); //
=
// output torques
real_T xy_Acc_des[2], xy_Vel_des[2];
// temporary variables
real_T X[2];
//current position of the end effector
//real_T d;
real_T Dist, unit_vector[2];
real_T X0[2];
real_T leg;
real_T minAccel_opt, a;
real_T Times[6], Points[2][6];
a = 1+sqrt(2); // this is the optimal slow down
acceleration
minAccel_opt = -maxAccel/(2*a); //fed to DesiredValues
and TrajectoryValues
X0[0] = 0;
X0[1] = 0;
leg = sqrt(pow(Xe[0] - Xs[0],2) + pow(Xe[1] - Xs[1],2) );
//How long is the straight line
// the current xy_position
X[0] = linkLen[0]*cos(Q[0]) + linkLen[1]*cos(Q[0]+Q[1]);
//Forward Kinematics
X[1] = linkLen[0]*sin(Q[0]) + linkLen[1]*sin(Q[0]+Q[1]);
xy_curr_pos[0] = X[0];
xy_curr_pos[1] = X[1];
~ 93 ~
AN OPTIMAL SOLUTION
TrajectoryValues (Times, Points, leg, radious, Xs, Xe,
maxAccel, minAccel_opt, minVel); //minAccel is negative
if(init == 0) //move the arm to Xs, the starting
configuration
{
xy_Acc_des[0]
= 0;
xy_Acc_des[1]
= 0;
xy_Vel_des[0]
= 0;
xy_Vel_des[1]
= 0;
xy_Pos_des[0]
= Xs[0];
xy_Pos_des[1]
= Xs[1];
Dist = sqrt(
X[1],2)
pow(Xs[0]-X[0],2) +
pow(Xs[1]-
);
if(Dist > 0.5) //limit the distance we try to go
{
}
state = 0;
timeObj = 0;
timeStop = 0;
}
else
{
//search for the object
// %have we seen the object?
if( state == 0 && object_detected)
{
state = 1;
//Save the Velocity and position of robot when
found object
// this takes the previous desired velocity
value instead of the real one that the robot
xy_Vel_obj[0] = xy_Vel[0]; // in XY
coordinates
xy_Vel_obj[1] = xy_Vel[1];
xy_Pos_obj[0] = xy_prev_Pos[0];
xy_Pos_obj[1] = xy_prev_Pos[1];
// time at which object was found
timeObj = time;
if (timeObj <= Times[2]) // we found the
object in the first line
{
flag = 1;
}
else if (timeObj <= Times[3]) //we found the
object in the curve
{
flag = 2;
}
else //we found the object in the second line
~ 94 ~
AN OPTIMAL SOLUTION
{
flag = 3;
}
}
//compute desired acceleration
if( state == 0)//search for object
{
DesiredValues(xy_Pos_des, xy_Vel_des,
xy_Acc_des, leg, time, Xs, Xe, radious, maxAccel, minVel,
minAccel_opt, Times, Points);
}
else if (state == 1) // slow down arm
{
SlowDown(flag, radious, leg, Xs, Xe, maxAccel,
xy_Vel_obj, xy_Pos_obj, xy_Pos_des, xy_Vel_des, xy_Acc_des,
time-timeObj, &state);
if (state == 2) //we have slowed the arm to
zero velocity
{
timeStop = time;
xy_stop_Pos[0] = xy_prev_Pos[0];
xy_stop_Pos[1] = xy_prev_Pos[1];
}
}
else if (state == 2) // go to object
{
DesiredValues_bangbang(time-timeStop,
xy_stop_Pos, xy_Pos_obj, maxAccel, -minAccel_bang, xy_Pos_des,
xy_Vel_des, xy_Acc_des);
}
}//end big else on <init>
//Inv_Kin(th_des, xy_Pos_des, linkLen[0], linkLen[1]);
*pState = (real_T) state;
xy_Vel[0] = xy_Vel_des[0]; //save previous velocity
xy_Vel[1] = xy_Vel_des[1];
xy_prev_Pos[0] = xy_Pos_des[0]; //save previous position
xy_prev_Pos[1] = xy_Pos_des[1];
DesiredJointAccel(joint_out_acc, X, xy_Pos_des,
xy_Vel_des, xy_Acc_des, Q, Q_dot, kGains,linkLen,
xy_curr_vel,thdot);
// compute the torques for each link
ComputeTorques( linkLen, Q, thdot,
joint_trq );
joint_out_acc,
return;
}
#define MDL_UPDATE /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
~ 95 ~
AN OPTIMAL SOLUTION
/* Function: mdlUpdate
======================================================
* Abstract:
*
This function is called once for every major
integration time step.
*
Discrete states are typically updated here, but this
function is useful
*
for performing any tasks that should only take place
once per
*
integration step.
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
}
#endif /* MDL_UPDATE */
#define MDL_DERIVATIVES /* Change to #undef to remove function
*/
#if defined(MDL_DERIVATIVES)
/* Function: mdlDerivatives
=================================================
* Abstract:
*
In this function, you compute the S-function block's
derivatives.
*
The derivatives are placed in the derivative vector,
ssGetdX(S).
*/
static void mdlDerivatives(SimStruct *S)
{
}
#endif /* MDL_DERIVATIVES */
/* Function: mdlTerminate
=====================================================
* Abstract:
*
In this function, you should perform any actions that are
necessary
*
at the termination of a simulation. For example, if
memory was
*
allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
}
/*======================================================*
* See sfuntmpl_doc.c for the optional S-function methods *
*======================================================*/
/*=============================*
* Required S-function trailer *
~ 96 ~
AN OPTIMAL SOLUTION
*=============================*/
#ifdef MATLAB_MEX_FILE
MEX-file? */
#include "simulink.c"
#else
#include "cg_sfun.h"
function */
#endif
/* Is this file being compiled as a
/* MEX-file interface mechanism */
/* Code generation registration
~ 97 ~
AN OPTIMAL SOLUTION
REFERENCES
~ 98 ~
AN OPTIMAL SOLUTION
REFERENCES
[1] R. Bellman, “Problem 63-9, an optimal search,” SIAM Review Journal, vol. 5,
no. 3, p.274, Jul. 1963.
[2] A. Beck, “On the linear search problem,” Israel Journal of Mathematics, vol. 2,
no. 4, pp. 221-228, Dec. 1964.
[3] I. Ruano de Pablo, A. Becker, and T. Bretl, “An Optimal Solution to the Linear
Search Problem for a Robot with Dynamics,” 2010 IEEE/RSJ International
Conference on Intelligent Robots and Systems, Taiwan, submitted to
publication.
[4] A. Beck, “More on the linear search problem,” Israel Journal of Mathematics,
vol. 3, no. 2, pp. 61-70, Jun. 1965.
[5] A. Beck and D. Newman, “Yet more on the linear search problem,” Israel
Journal of Mathematics, vol. 8, no. 4, pp. 419-429, Dec. 1970.
[6] A. Beck and M. Beck, “The linear search problem rides again,” Israel Journal of
Mathematics, vol. 53, no. 3, pp. 365-372, 1986.
[7] A. Beck and P. Warren, “The return of the linear search problem,” Israel
Journal of Mathematics, vol. 14, no. 2, pp. 169-183, Jun. 1973.
[8] A. Beck and M. Beck, “The revenge of the linear search problem,” SIAM
Journal on Control and Optimization, vol. 30, no. 1, pp. 112-122, 1992.
[9] S. Alpern and A. Beck, “Pure strategy asymmetric rendezvous on the line with
an unknown initial distance,” Operations Research, vol. 48, no. 3, pp. 498-501,
May – Jun. 2000.
[10] S. Alpern, V. Baston, and S. Gal, “Searching symmetric networks with
utilitarian- postman paths,” Networks archive, vol. 53, no. 4, pp. 392-402, 2009.
~ 99 ~
AN OPTIMAL SOLUTION
[11] M. G. Monticino and J. R. Weisinger, “A survey of the search theory literature,”
Naval Research Logistics Journal, vol. 38, no. 4, pp. 469-494, 1991.
[12] S. Alpern and S. Gal, The theory of search games and rendezvous, ser.
International series in operations research & management science, vol. 5.
Springer, 2003.
[13] E. D. Demaine, S. P. Fekete, and S. Gal, “Online searching with turn cost,”
Theoretical Computer Science, vol. 361, no. 2-3, pp. 342-355, Sept. 2006.
[14] A. Lopez-Ortiz, “Online target searching in bounded and unbounded domains,”
PhD dissertation, University of Waterloo, Waterloo, Ontario Canada, 1996.
[15] L. Guibas, J.C. Latombe, S. M. LaValle, D. Lin, and R. Motwani, “A visibilitybased pursuit-evasion problem,” International Journal of Computational
Geometry and Applications, vol. 9, no. 4-5, pp. 471-493, 1999.
[16] R. Vidal, O. Shakernia, H. J. Kim, D. H. Shim, and S. Sastry, “Probabilistic
pursuit-evasion games: theory, implementation and experimental evaluation,”
IEEE Trans. Robot. Autom. Journal, vol. 18, no. 5, pp. 662-669, Oct. 2002.
[17] V. Isler, S. Kannan, and S. Khanna, “Randomized pursuit-evasion in a
polygonal environment,” IEEE Trans. Robot Journal., vol. 21, no. 5, pp. 875884, Oct. 2005.
[18] B. P. Gerkey, S. Thrun, and G. Gordon, “Visibility-based pursuit- evasion with
limited field of view,” The International Journal of Robotics Research, vol. 25,
no. 4, pp. 299-315, Apr. 2006.
[19] H. Choset, “Coverage for robotics – a survey of recent results,” Annals of
Mathematics and Artificial Intelligence, vol. 31, no. 1, pp. 113-126, Oct. 2001.
[20] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. The MIT Press, 2005.
[21] J. Davidson and S. Hutchinson, “A sampling hyperbelief optimization technique
for stochastic systems,” in WAFR, 2008.
~ 100 ~
AN OPTIMAL SOLUTION
[22] H. Kurniawati, D. Hsu, and W. Lee, “SARSOP: Efficient point-based POMDP
planning by approximating optimally reachable belief spaces,” in Robotics:
Science and Systems, 2008.
[23] M. W. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Control.
John Wiley & Sons, 2006.
[24] H.Choset et al., Principles of Robot Motion: Theory, Algorithms and
Implementation. The MIT Press, 2005.
[25] N. Chopra, M. W. Spong and R. Lozano, “Adaptive Coordination Control of
Bilateral Teleoperators with Time Delay,” submitted to 2004 IEEE Conference
on Decision and Control, vol. 5, pp. 4540-4547, Dec. 2004.
[26] F. Pfeiffer and R. Johanni, “A Concept for Manipulator Trajectory Planning,”
IEEE Journal of Robotics and Automation, vol. RA-3, no.2, pp. 115-123, Apr.
1987.
[27] Z. Shilller and H. Lu, “Computation of Path Constrained Time Optimal Motions
With Dynamic Singularities,” Journal of Dynamic Systems, Measurements and
Control, vol. 114, no.34, March 1992.
[28] K. G. Shin and N. D. McKay, “Minimum-Time Control of Robotic Manipulators
with Geometric Path Constraints,” IEEE Transactions on Automatic Control,
vol. AC-30, no. 6, Jun. 1985.
[29] J. E. Bobrow, S. Dubowsky and J. S. Gibson, “Time-Optimal Control of Robotic
Manipulators Along Specified Paths,” International Journal of Robotics
Research, vol. 4, no. 3, pp. 3-17, Fall 1985.
[30] P. J. Walsh, “Feedback Linearization of a Robotic Manipulator,” M.S. Thesis,
University of Illinois at Urbana-Champaign, IL, 1994.
[31] Parker Hannifin Corporation, Compumotor Digiplan Positioning Control
Systems and Drives, 1992.
~ 101 ~
AN OPTIMAL SOLUTION TO THE LINEAR SEARCH
PROBLEM FOR A ROBOT WITH DYNAMICS
Fdo.: Irene Ruano de Pablo
MADRID, junio de 2010

and robot dynamics - IIT - Universidad Pontificia Comillas

Transcription

Similar documents

Place des Arts

From the HOTEL DES LICES (Lices square) to the Bus lines at

Joey Reznicek, Owner/Manager Salerno`s restaurant and pizzeria

Compulocks

PATTERNS - Bomanite

a. Read the English version

JagerBomb flyer-web - Smoothiecompany.com

Jardin d`enfants - St. Vital Parent Child Coalition

Commission scolaire des Samares

Read the Es Teler 77 case study