This article has been accepted for publication in a future issue of this journal, but has not been
fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
Hybrid Force Tracking Impedance Control-based
Autonomous Robotic System for Tooth Brushing
Assistance of Disabled People
Oladayo S. Ajani, and Samy F. M. Assal,
Abstract—Due to the increase of the world ageing population
and several forms of disabilities, assistive robots have received a
lot of attention lately. Although, different assistive robotic systems
have been developed to perform essential tasks around the head,
these existing robotic systems are either manually controlled,
shared controlled or interactive ones. Although, fully autonomous
robotic systems can have the potential of eliminating several
limitations of the existing systems, such systems are limited in
design due to critical issues of increased complexities and the
associated risks. In this paper, a hybrid impedance controller-
based autonomous assistive robotic system for tooth brushing as-
sistance is developed. A fixed base manipulator with an attached
automatic tooth brush and Kinect RGB-D sensor are proposed
as a platform to achieve the brushing task. An online face
detection, mouth detection, head pose estimation and trajectory
planning for the brushing task are achieved. Additionally, online
re-planning the trajectories in case of patients’ head pose changes
is taken into account. The controller performance is evaluated
regarding tracking the generated brushing trajectories and the
desired force trajectory by co-simulating the manipulator’s dy-
namics in ADAMS and SIMULINK. The results demonstrate
the capability of the proposed robotic system to achieve tooth
brushing autonomously.
Index Terms—Tooth Brushing Assistance, Hybrid impedance
control, Mouth Detection, Autonomous Robotic Assistance.
I. INTRODUCTION
The increase in world ageing population contributes to
several human disabilities amidst other factors such as neuro-
logical disorders and some other diseases. Due to disabilities
that result into upper arm manipulation challenges, disabled
people often need assistance of caregivers to perform the
activities of daily living (ADLs). Due to the lack of caregivers,
the need for practical approaches to provide assistance for
disabled people has received a lot of attention. Interestingly,
assistive robotics which aims to provide human assistance
has been a major remedy to such problem. It has been
demonstrated that, by the use of the existing assistive robotic
systems for ADLs, patients are able to perform several ADLs
without the help of caregivers and this has been associated
with improvement in the quality of life and independence
1 Ajani S. Oladayo is with the Department of Mechatronics and
Robotics Engineering, School of Innovative Design Engineering, Egypt -
Japan University of Science and Technology (EJUST), Alexandria, Egypt.
oladayo.solomon@ejust.edu.eg
2 Samy F. M. Assal is with the Department of Mechatronics and Robotics
Engineering, School of Innovative Design Engineering, Egypt - Japan Univer-
sity of Science and Technology (EJUST), Alexandria, Egypt., on leave from
the Department of Production Engineering and Mechanical Design, Faculty of
Engineering, Tanta University, Egypt samy.assal@ejust.edu.eg
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
1
[1]. The main limitation of these existing assistive robotic
systems is that they are developed based on the assumption
that patients can participate in the control of such systems.
Therefore these systems are either controlled manually by the
patients through input interfaces such as joysticks, graphical
user interface (GUI) and head or hand gestures [2], [3],
interactive such that patients have to interactive with them to
perform ADLs [2] or controlled using shared control approach
which aims at delegating part of the control operation to
the systems [4], [5]. These assumptions are not practical,
because based on the severity of the patients disability, some
patients cannot participate in the control of these systems and
thus these existing systems are limited for some levels of
disability. Although the use of fully autonomous systems was
recommended [2], autonomous systems are limited in design
due to the increased complexities and safety issues that are
associated with such systems. For instance, tooth brushing task
requires an intricate coordination of contact force and motion
generation to be performed. This requires sophisticated control
algorithms as well as vision-based sensory system to facilitate
such physical human-robot interaction (HRI), which should be
developed.
A comprehensive review of the current assistive robotic
systems for assistive tasks around the head shows that these
systems can be categorized and evaluated based on their
usability and safety, interaction and control, economic and
task prioritization [6]. Robotic assistances for tasks around the
head such as feeding, drinking, beard shaving, teeth brushing,
hair combing, face washing and scratching have received
considerable attention in recent times [7], [2], [4], [8]. For
instance, the first multi-function robot for tasks around the
head assistance known as Handy 1 was developed to enable
patients with cerebral palsy to gain some level of independence
during meal time [9]. However this system was controlled
manually and required the user to interact with the system
to perform such tasks. Due to its nature of control, users were
frustrated by the time delay due to sensory feedback. Also,
a shaving task was performed by a mobile manipulator [2]
which provided the assistance by holding the tool near the
patient head while the patient moves his head against the tool.
This process was shown to be long with unintuitive interface.
Based on this, autonomous system for tasks around the head
was recommended. Among the tasks around the head, teeth
brushing is of interest in this work.
Since task priority for ADLs is classified based on the
frequency of performing a task, tooth brushing is considered as
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
high priority task. Although that, assistive robotic systems for
this task have received little attention due to the complexities
associated with the coordination of contact force and motion
as well as safety issue within the mouth for effective tooth
brushing. Additionally, performing the tooth brushing properly
by the conventional way add more complexity due to the
high dexterity needed. However, the advent of electric tooth
brushes which have been reported to be more efficient than
the conventional ones in terms of the removal of plague [10]
has open up some possibilities to be used for disabled people.
Since these electric brushes are hand held tools, their use for
disabled people is limited based on the level of disability.
Basically all currently available assistive robotic systems for
tooth bushing involve the participation of the user to perform
this task. Based on the assumption that users have the ability
to sit such that a forward- posture is possible, an interactive
teeth cleaning robot driven by cam mechanism was designed
[11]. In this system, the user was required to place his mouth
on the jaw support and adjust the tooth cleaning module to fit
the mouth to enable cleaning of the plaques on the teeth. Em-
bedded force sensors were used to stop brushing automatically
whenever the user lift his jaw from the jaw supporter. Thus,
the system cannot be used by patients with different levels
of disabilities autonomously. An Assistive Robot Manipulator
(ARM) which relies on effective interfaces for control was also
employed to provide tooth brushing assistance [12]. Because
this system is manually controlled by the users, difficulty in
orienting the tooth brush to the right direction of the mouth
using the control interface was one of the drawbacks in this
system. The Desktop Vocational Assistant Robot (DeVAR)
[13] was used for tooth brushing task. The approach of using
this DeVAR system was not proved to be autonomous since
appropriate commands from its user were required to position
the electric tooth brush in the mouth to perform the tooth
brushing. An average time of 5:25 minutes over 24 disabled
volunteers was recorded to complete tooth brushing task.
Based on Proprio and Tele-operation concept [14], in which
the master and the remote areas are the same, a system that
is known as ASIBOT (Assistive robot), a snake-like robotic
arm, was proposed for tasks such as tooth brushing, drinking,
applying make-up, shaving and feeding. Based on the different
levels of user disabilities, different Human-Machine interfaces
(HMI) were needed for such system such as voice command,
simple switches or joysticks. This shows an increasing demand
for developing fully autonomous robotic systems for assistive
tasks around the head.
Recently a commercially available automatic tooth brush
has been used by many abled users. Because this brush
does not apply mechanical motion, it is considered very safe.
Additionally, because of its mouth piece, several sub-tasks
such as putting tooth paste on the brush and picking up the
tooth paste are totally taken care of [12] except the problems
of manipulating and holding the brush during brushing. There-
fore, with the availability of such automatic tooth brush, much
effort are needed to achieve autonomous detection of the users
mouth as well as autonomous brush manipulation in order to
realize an autonomous robotic system for tooth brushing that
can handle different levels of disabilities.
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
2
On the context of tooth brushing task, the goal is to develop
vision system to enable mouth detection and consequently
compute the pose of the mouth in 3D domain. This vision
system serves similar purposes as those for other tasks such
as feeding and drinking [4], [8]. Mouth detection techniques
which have been applied basically involve one of the following
three categories; those that involve the use of trained cascades
to detect the mouth directly from input images, those which
involve face detection and consequently mouth detection using
features such as the golden ratios and finally those which
involve the use of models for face detection which have been
trained to detect landmarks such as eye, mouth, etc. The first
approach was applied with several recorded limitations [15]
such as panning error along the Z direction, error in the
height of the patients mouth and the mouth detection. The
second approach which involves face and mouth detections
was recorded to outperform the first approach [4]. Recently,
the third approach which involves landmarks has been widely
applied in several application scenarios and several open
source tools such as Dlib and openCV have supported such
features. This approach was applied in drinking assistance
[16]. Unfortunately, those three approaches are often applied
in 2D. However, for 3D applications as the one in hand,
more computational effort and time would be required to map
the colour and depth information of the face. Also some of
these methods work well only at certain configurations for the
cascades that have been trained. Recently the Microsoft Kinect
software development kit (SDK) has enabled a face tracking
engine known as the HD FACE. This face tracking engine
enables face detection and returns the face as 3D point-cloud
model. These point cloud models are also labeled and can be
used for landmarks detection and segmentation of several parts
of the face in 3D domain with no need of mapping coloured
information to the depth information.
Considering the need for the development of fully au-
tonomous robotic systems for assistive tasks around the head
that can achieve contact force regulation and motion gener-
ation for the intended task, compliance control becomes a
necessity here. This will ensure that these systems can be
used for any level of disabilities and also without the need
for the patient to participate in the control of such systems.
Thus, the problems associated with control, ease of use, safety
and efficiency can be tackled. Generally compliance can be
introduced to a system through active or passive compliance
[17]. Passive compliance is basically introduced into a system
as compliant mechanical elements [18]; while active ones can
be introduced through active interaction control schemes [19].
Passive compliance was used for feeding assistance system in
[20] in which, the spoon is connected to the robotic arm using
a spring or a magnet which falls off when large forces are
exceeded. Although such system can provide some levels of
safety and compliance, it is still limited in terms of flexibility.
On the other hand, the most efficient active compliance is
the impedance control which mimics the human behavior by
imposing mass-spring-damper-like disturbance response and
can be designed to comply with varying stiffness and envi-
ronmental location. To realize this adaptive nature, different
frameworks have been proposed within the impedance control
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
scheme which can also be combined into a single structure
through the hybrid impedance scheme.
From the aforementioned literature, it can be concluded that,
real-time autonomous teeth brushing robotic system that is
safe and efficient has not yet been developed. Additionally,
the existing robotic systems for teeth brushing assistance either
manually controlled, interactive or shared controlled are based
on the assumption that patients have the abilities to participate
in the control of such systems via GUI with video stream from
a camera. Moreover, their teeth brushing task is characterized
as poor, unsafe and tedious due to the generated improper con-
tact force trajectories. So, unlike the existing robotic systems, a
hybrid impedance controller-based autonomous robotic system
that is capable of autonomous and reliable teeth brushing
assistance for patients who have no abilities to participate in
the system controller is proposed in this paper. In this proposed
system, the related problems of unintended patient head pose
changes during teeth brushing which result in unintended force
exertion on all the other force uncontrolled directions are
tackled for the first time. Rather than the manual selection
of the task region by the user as in existing systems, the
developed system involves autonomous mouth detection as
well as online trajectories planning. On the context of this
teeth brushing assistance, vision sub-system is used for the
face detection, mouth detection and head pose estimation.
Rather than edge- or colour-based segmentations which are
mostly used and appropriate in unconstrained background for
2D segmentation, 3D semantic segmentation based on known
labels of each point on the face is developed to be used for this
mouth detection. Additionally, online trajectories re-planning
with a hybrid force tracking impedance control for the brush-
ing task are developed to ensure safety during unintended face
pose changes and to ensure achieving the desired force for
brushing within the mouth of the user. In the proposed system,
a Kinect RGB-D sensor and a general purpose light weight
UR-10 6-DOF manipulator with an automatic tooth brush are
proposed as a platform for achieving the teeth brushing task.
Furthermore, the performance of the proposed hybrid force
tracking impedance controller is evaluated in terms of tracking
the generated tooth brushing trajectories and the desired force
trajectory by co-simulating the full dynamics of the proposed
robotic system in ADAMS and MATLAB.
Following this introduction, a brief overview of the pro-
posed robotic system is presented in Section II. Section III
explains in details the face detection, mouth detection and
end-effector orientation. In Section IV, trajectory planning
and online re-planning are detailed. Section V elaborates on
the dynamic model of the robotic system in ADAMS and
the active force tracking impedance control. The hybrid force
tracking impedance control is also developed in Section VI.
Consequently, results of tracking trajectories with contact force
regulation are presented in Section VII. Finally conclusions
and future work are presented in Section VIII.
II. T HE P ROPOSED AUTONOMOUS ROBOTIC S YSTEM
As illustrated in Fig. 1, the setup of the proposed au-
tonomous tooth brushing robotic system includes a general
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
3
Fig. 1: The proposed robotic system setup for tooth brushing.
purpose collaborative UR10 6-DOF manipulator whose end-
effector is attached with an automatic electric tooth brush such
as the commercial one shown in Fig. 1. and a vision-based
perception system which is a RGB-D kinect camera.
The RGB-D kinect camera enables mouth detection of the
user with respect to the Kinect frame, while transformations
are applied to the resulting 3D pose of the mouth to be related
to the manipulator base frame. Consequently, the required
brushing tool orientations are computed using the normal
vectors to the patient face. These position and orientations
relative to the manipulator base frame are the desired poses
for the automatic tooth brush. This tooth brush is designed
to brush the whole teeth at once in 10 seconds [21]. The
movements of the brush bristles back and forth are enabled by
subtle vibrations which make the brush able to remove plaque
and keep the gums undamaged. Since the bristles movements
are required to enable proper brushing of the tooth, the aim of
the proposed system is to manipulate this brush perpendicular
to the user mouth with orientation suits the mouth orientation
and with proper compliant interaction. To achieve this, the
following conditions should be satisfied
1) The base of the manipulator and the camera are fixed at
the sagittal plane of the system’s user
2) The camera is tilted to ensure that the view of the camera
covers the entire manipulator workspace. It also ensures
that occlusion during brushing is brought to the barest
minimal. Specifically, it enables online face and mouth
detections of the user even if the brush is in the mouth
of the user since the lower lip as well as the right and
left parts of the upper lip are still visible.
By the use of the RGB-D kinect camera, the resulting pose
of the mouth relative to the manipulator coordinate frame are
set as inputs to the trajectory planner which plans the required
trajectories to complete the brushing task. Also the trajectory
planning includes the required penetration path of the tooth
brush into the mouth of the users. This penetration distance
can be as recommended in literature [22]. The output of the
trajectory planner is considered as inputs to the robot controller
which is a hybrid force tracking-based impedance controller.
The robot controller is based on the dynamics of the UR10
6-DOF manipulator co-simulated in ADAMS and MATLAB.
Online re-planning the trajectory has also been enabled to re-
plan the trajectory required for the withdrawal of the tooth
brush from the mouth after the brushing task. The reason is
that, during the time of brushing there might be unintended
changes in the pose of the brushing tool due to movements of
the users head.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
III. FACE D ETECTION AND M OUTH D ETECTION
Computation of the desired end-effector positions and ori-
entations required to complete the brushing task based on the
mouth detection is the focus of this section. This desired end-
effector pose is specifically the positions of the mid point
of the mouth and the orientations of the normal vector and
the mouth based on the normal to the face surface at the
facial points of the mouth respectively. Therefore, this stage
involves face detection, mouth detection and computations of
the desired end-effector positions and orientations.
A. Face Detection and Tracking
The high-definition (HD) real-time face detection and track-
ing engine provided in the Microsoft kinect Software Devel-
opment Kit (SDK) referred to as HD face is used here for
face detection and tracking. Utilizing the infrared and color
streams, the Kinect sensor can accurately track faces with
varying poses [23] and create a 3D point-cloud that represents
it as shown in Fig. 2.
The 3D point-cloud face model of the detected face is
returned by the camera in a matrix form as P ∈ ℜ3X1347 ,
where the matrix row represents the XYZ coordinates of
the facial point and the matrix column is the index label of
each point. Consequently, the labels of these facial points can
be accessed and used to enable the mouth detection. Kin2,
a Kinect 2 toolbox for MATLAB which contains wrapper
function which is used for communication with the camera to
enable face detection. Consequently, from the 3D point-cloud
face model which is extracted in a matrix form, the mouth can
be detected using the known labels [24].
The accuracy of the face and mouth detections is based on
the face tracking algorithm of the Microsoft Kinect and the
accuracy of this algorithm is guaranteed when the face is far
from the camera between 0.4 to 2m and the head rotation
is within 0 to 50◦ [25]. For this application, all the above
mentioned factors are fully met and thus the face and the
mouth detections are considered as highly accurate.
B. Mouth Detection
To detect the mouth, face detection is first carried out as
described in III-A. The resulting point cloud representation of
the face can now be used for the detection of several landmarks
within the face based on the known labels of these facial
landmarks. For the case of mouth detection, the labels of the
points representing the mouth are used to determine the 3D
positions of the mouth region as illustrated in Fig. 3, the labels
of the points within the entire region are depicted in black.
Fig. 2: Illustration of the face detection for different head poses
showing a point-cloud model distribution of 1347 points which
represents the face.
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
4
(a) (b)
Fig. 3: Illustration of the mouth detection; (a) illustration of
the upper and lower mid point positions of the mouth in red,
(b) mouth detection with automatic toothbrush in the mouth.
However, since it is required to position the tooth brush at
the middle of the mouth and also make room for different
variations of the mouth in terms of opening and closing the
mouth, the upper and lower mid points of the mouth are
considered as depicted in red in Fig. 3(a). Consequently,
the position of the center point of these two points can be
computed which is considered as the desired 3D point position
of the mouth. Here, it is worthy to mention that Fig. 3(b)
illustrates that the mouth can still be detected even if an
object is in the user mouth. This center point of the mouth
is the starting point by the trajectory planner. Furthermore, to
pave way for the trajectory planning, it is important to know
that, there is a need to compute the distance between the
3D position of this center point and the final point position
of the brush’s penetration into the mouth of the user. The
desired average penetration distance from this detected 3D
point position of the mouth is know as the total arch length of
the mouth with an average of 0.0426m [22]. This arch length,
shown in Fig. 4, will be used here as the desired penetration
distance of the tooth brush into the mouth.
C. End-Effector Orientation
To obtain the required end-effector orientations for the
brushing task, the normal vectors to the facial points should
first be obtained. Here K nearest neighbor (KNN) is applied
as described in a previous work by the authors [26].
Having obtained the normal n i to facial point q i of the
mouth region, with respect to the camera frame C, the brush
frame B that is attached to the end-effector frame E is needed
to be aligned to the normal and oriented to suit the mouth
orientation. The required end-effector orientation to achieve
this can be calculated with respect to the manipulator base
frame M by solving for the homogeneous transformation C TE ,
from which the Roll-Pitch-Yaw angles can be obtained. From
Fig. 5, the homogeneous transformation from the end-effector
E with respect to frame C C TE , can be obtained as follows
Fig. 4: Dental arch measurements. [22]
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
C
TE =C TB B TE
where B TE is the homogeneous transformation from the end-
effector with respect to frame B which is considered constant
and assumed to be known since the brushing tool is fixed to
the end-effector frame E and C TB is the transformation from
the brush with respect to frame C which is obtained form
mouth detection. Specifically, Z-axis of the rotational matrix
C R is aligned with the normal at the center point of the mouth
B
while X-axis is along points ”a” and ”b” of the mouth shown
in Fig. 6. Y-axis can be obtained by the cross product of Z and
X axes. The origin of the coordinate frame C TB is the position
of the mouth center point. Once obtained the transformation
C T from (1), the rotational matrix C R can be obtained as in
E E
(2), from which the Roll-pitch-Yaw angles of the end-effector
with respect to the camera frame can be obtained as in (3).
For cases where θ = ∓90, (4) can be used instead of (3).
 
r11 r12
RPY (φ , θ , ψ) = r21 r22
r31 r32
q r21 r11
2 + r 2 ), φ = atan2(
θ = atan2(−r31 , r11 21 , ), ψ = atan2(
cθ cθ
θ = ∓90, φ = 0, ψ = ∓ atan2(r12 , r22 )
The desired transformation matrix of the end-effector E
with respect to the manipulator based frame M (M TE ) can be
obtained as in (5). Thus, the desired pose of the end-effector
at each point p i on the brushing area can be obtain. This paves
way for planning trajectories for the brushing tool to track.
M
TE =M TC C TE
where MTis the transformation matrix of the camera C with
C and an average time of 3.9242 sec is computed. It is remark-
respect to the manipulator based frame M.
IV. T RAJECTORY P LANING
Having obtained the pose of the mouth, the required brush-
ing tool orientation at the center point position of the mouth
and the required penetration distance in the direction of the
normal to the mouth known as the total arch length, smooth
Fig. 5: Illustration of the transformations required to align the
brush B to the face normal and orient it to suit the mouth
orientation.
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
Transactions on Medical Robotics and Bionics
5
and continuous trajectories required to achieve the tooth brush-
(1) ing task can be planned. The trajectory planning involves two
major stages, the first stage involves approaching the center
point position of the user mouth and then penetrating along the
normal to the mouth to complete the brushing task. The second
stage involves online re-planning the trajectory required to
withdraw the tooth brush from the user mouth. This second
stage is necessary because during the period of brushing, the
pose of the user mouth might be unintentionally changed. The
waypoints from the initial end-effector position to the user
mouth as well as the end-point along the penetration distance
are fed in as inputs to the trajectory planner. Cubic spline
which can satisfy the constraints of smooth and continuous
trajectories x(t) with zero initial and final velocities are used
with the waypoints to realize on-line planning trajectories for
the brushing task. Trajectory T1 illustrated in Fig. 6 is a
representative cubic splines-based trajectory passing through
the way-points for the cleaning stage.
After the cleaning stage, the trajectory is re-planned for the
r13
r23  (2) withdrawal of the brush from the user mouth, since uninten-
r33
tional change in the head pose of the user might have occurred.
r32 r33
The trajectory re-planning is triggered based on brushing
, ) (3) time. As illustrated in Algorithm 4.1, after completing the
cθ cθ
cleaning trajectory, the withdrawal trajectory is re-planned
(4) after 10 seconds based on the cleaning time specified and
recommended by the brush producer. Based on the new status
of mouth detection, mouth center point position estimation and
normal vector calculation, the new withdrawal trajectory can
be online re-planned. Trajectory T2 depicted in Fig. 6 is a
representative online re-planned trajectory for the withdrawal
of the tooth brush. To evaluate the time required to re-plan
the brush withdrawal trajectory after completing the cleaning
(5) stage, twenty independent runs of re-planning different brush
withdrawal trajectories are performed at varying head poses
able to note that, this trajectory re-planning time involves the
mouth detection, computation of normal vectors, computation
of the desired end-effector orientations and finally generation
of cubic splines to be tracked using the way-points within
the mouth and the final desired location of the brush. This
Fig. 6: Representative trajectories for the brushing task stages;
namely, cleaning stage (blue trajectory) and withdrawal stage
(red trajectory).
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 6

Algorithm: 4 . 1 pseudo-code showing the online plan-

ning of brushing trajectories
1 Input : f (x, y)
2 Output: t j,T , X j,T ∈ ℜ6 for j = 1, ....
3 Initialize Kinect sensor;
4 Initialize Mouth Upper and lower Mid-Points (ul);
5 Initialize initial Robot pose R p
6 while Kinect sensor is initialized do
7 g(x, y) ← detectFace(f(x,y));
8 PointCloud p ∈ ℜ3∗1347 ← extract3DFace(g(x,y));
9 M p , M pen , N p ∈ ℜ3∗1 ← DetectMouth(p ∈ ℜ3∗1347 , ul);
10 Mo ∈ ℜ3∗1 ← ComputeOrientation(M p , N p )
11 t j,T 1 , X j,T 1 ∈ ℜ6 ← Tra jPlannerT 1(M p ,Mo , M pen R p ,)
12 return t j,T 1 , X j,T 1 // Return T1 for teeth cleaning
stage
13 Wait for 10 seconds // Time-based trajectory
Fig. 7: Active force-tracking impedance control.
re-planning
14 g(x, y) ← detectFace(f(x,y)); different control schemes. The input torques in ADAMS are
15 PointCloud p ∈ ℜ3∗1347 ← extract3DFace(g(x,y)); supplied from MATLAB as variables based on the control
16 M p , M pen , N p ∈ ℜ3∗1 ← DetectMouth(p ∈ ℜ3∗1347 , ul); architecture while the outputs of this model are basically the
17 Mo ∈ ℜ3∗1 ← ComputeOrientation(M p , N p )
18 t j,T 1 , X j,T 2 ∈ ℜ6 ← Tra jPlannerT 2(M p ,Mo , M pen R p ,)
joint angles, velocities and accelerations.
19 return t j,T 2 , X j,T 2 // Return T2 for brushing tool
withdrawal
20 end B. Active Force Tracking-based Impedance Control
In the brushing task, the interaction required between the
manipulator and the human is basically a force-position re-
indicates that the proposed approach is suitable for real-time lation. Impedance control provides effectively this level of
tooth brushing application. interaction because it enables motion trajectory tracking and
In terms of contact force regulation, the desired force trajec- realizes a desired dynamic interaction between the end-effector
tory Fd is specified only in the Z direction of the manipulator position and the contact force [27]. The desired dynamic
end-effector coordinates that is aligned with the normal to the behavior between the interaction force of the brushing tool and
face surface. This is appropriate because only the Z direction desired trajectory within the mouth region can be modeled as
constitutes the desired force controlled direction to achieve the follows
tooth brushing based on the proposed approach.
f d − f = Kd (xxd − x ) + Bd (ẋxd − ẋx) + Md (ẍxd − ẍx) (7)
V. ROBOT DYNAMIC M ODEL AND I MPEDANCE C ONTROL where Kd , Bd , Md are the desired stiffness, damping and inertia
A. ADAMS-based Dynamic Modelling of the Robot impedance diagonal matrices respectively; in which, Md is
generally assumed to be equal to zero. fd is the desired
The dynamic equations of an n-DOF robot manipulator can interaction force between the automatic tooth brush and the
be written generally as mouth of the user. The block diagram of the closed loop
M(qq)q̈q + H(qq, q̇q)q̇q + g (qq) = τ m − τ ext (6) impedance control system is shown in Fig. 7.
The impedance control law can be given as follows
where q , q̇q, q̈q ∈ Rn are the joint position, velocity and accel-
U = J T (qq)[K p (xxd − x ) + Kv (ẋxd − ẋx) + f d + K f ( f d − f )] + g (8)
eration vectors respectively; M(qq) ∈ Rn×n is the mass matrix;
H(qq, q̇q)q̇q ∈ Rn denotes the Coriolis and centrifugal matrix; where K f , K p and Kv are force, position and velocity gain
g (qq) ∈ Rn is the gravity vector; τ m ∈ Rn is the motor torque diagonal matrices respectively. Since
vector which is the control input and τ ext = J T (qq) f ext ∈ Rn , in
which f ext ∈ Rm denotes the external force vector measured (xxd − x ) ≈ J(qqd − q ) (9)
in the m-Cartesian space and mapped to the joint space with
the Jacobian transpose J T (qq) ∈ Rn×m . and
To obtain (6) using either the Newton-Euler or the Lagrange J T (qq)K p (xxd − x ) = K p (qqd − q ) (10)
method becomes often difficult for high number of n DOF and Therefore, (8) can be rewritten as
irregular shape manipulators. So, ADAMS software is used
here to dynamic simulate the UR10 6-DOF manipulator. To U = K p (qqd − q) + Kv (q̇qd − q̇q) + J T (qq)[ f d + K f ( f d − f )] + g (11)
achieve this, the manipulator CAD model is obtained from
For the controller to be asymptotically stable, the following
the manufacturer in Solidworks and exported to ADAMS. In
conditions must be satisfied as proposed in [28].
ADAMS, revolute joints are added to each of the joint loca-
K p + (I + K f )Kd > 0, Kv + (I + K f )Bd > 0, Md = 0
tions with necessary inputs and measurements parameters set.
or
Consequently, the ADAMS model is exported to MATLAB
K p > 0, Kv > 0, K f = −1
for co-simulation between ADAMS and MATLAB, which
is important for achieving virtual environment for validating

2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
1) GA-based Gains Tuning: Classical tuning approaches
such as Nichols and Ziegler were widely studied techniques
in the past, but this class of tuning techniques often becomes
very complex and unable to give proper and stable responses
in cases of non-linear or higher-order plants. Therefore meta-
heuristic approaches such as Genetic Algorithm (GA), Particle
Swarm Optimization (PSO) and Simulated Annealing (SA)
algorithms, which are more simple to be implemented, have
been developed to obtain optimal control gains. GA which is
based on natural selection and genetics is proposed to be used
in this study because GA is faster than SA and does not tend
to have problem of premature convergence as PSO. The GA
basically minimizes an objective function. Here the objective
function which minimizes both the tracking error e = x d − x
and settling time ts is considered as given below [29]
Z ∞
Fob j = ts + e T (t)ee(t)
0 Hybrid impedance control basically combines two or more
To ensure a fast convergence of the GA, the gain boundaries
of the search space must be set. Hence, the lower and the upper
limits of the controller gains K p and Kv must be specified.
To estimate this range, the basic PD independent joint control
algorithm is developed in ADAMS. Joint angles of 20 degrees
was set as inputs to this controller for all the six joints of the
manipulator to be tracked. Figures 8 shows the responses of
this basic PD independent joint controller. The upper limits
obtained for K p and Kv are diag[2500 60000 25000 1000 80
50] and [500 6000 2500 40 10 5] respectively while the lower
limits obtained for K p and Kv are diag[1000 40000 15000 500
20 10] and [300 4000 1000 10 2 1] respectively. Those gain
limits are used for the GA to obtain the optimal controller
gains of the force tracking-based impedance control.
While performing simulations for such controller, it is
assumed that the tooth brush is attached to the end-effector via
some sort of spring mechanism which in turn serves as passive
compliance to cope with the unintended head pose changes
that can result in unintended force exertion on all the other
force uncontrolled directions. However, this controller can not
be applied practically in such case of unintentional head pose
changes. This motives developing a hybrid impedance control
to tackle such problem.
VI. H YBRID I MPEDANCE C ONTROL
Safety is one of the underlying considerations limiting
the deployment of autonomous assistive robotic systems. In
robotic tasks with physical HRI, safety must be taken into
account such that the forces exerted during such interactions
should be within allowable limits. Several safety methods
have been proposed in literature which includes safety through
control, motion planning, prediction and psychological con-
siderations [30]. Among those, the only method that deals
with safety under such situations is the one which introduces
compliance into the system through control. Active compli-
ance schemes are those which implement compliance at the
control level and the most advanced one is the impedance
control which mimics human manipulation capabilities to
ensure safe in HRI [19]. Hence to ensure safety in tooth
brushing task, rather than the ordinary concept of using certain
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
7
(a) (b) (c)
(d) (e) (f)
Fig. 8: Closed loop time response of the PD independent joint
control developed in ADAMS.
force thresholds which might not be sufficient to ensure safety,
hybrid impedance control is implemented here.
Comprehensive review of active compliance control presents
(12) different varieties of the impedance control schemes [31].
of such schemes into a single structure to achieve the desired
level of HRI [32]. Specifically, it combines impedance control
and hybrid position and force control into one strategy. This
enables compliance and safe interaction between the robot and
its environment by adjusting the mechanical impedance of the
system while tracking the planned trajectories.
For the brushing task, it is required that some directions
are force controlled while the other directions are force ac-
commodating. So, it is important to adapt the existing control
schemes to suit achieving the brushing task and ensure that all
positions and orientations are controlled under the scheme by
combining impedance and admittance controls thus ensuring
that force tracking and force accommodating are achieved in
their respective directions. For such task, the force controlled
direction is the one along which the required brushing force
should be applied while the other directions should be force
accommodating to cope with the unintentional changes in
the head pose of the user during brushing. To achieve this
task using a single control structure, it is proposed to adapt
the hybrid impedance control which combines the structures
of force tracking-based impedance control subsystem and
admittance control subsystem to suit the problem as follows.
A. Admittance Control Subsystem
Although admittance control is often referred to an
impedance control, it is the dual or reciprocal of the widely
known impedance control. Specifically, admittance control
uses measured interaction force to modify the original desired
motion trajectory to achieve some preferred dynamic inter-
action responsive behavior [33]. Due to its force to desired
motion relation, admittance control is also often referred to
as position-based impedance control. Generally, admittance
control can be referred to as force accommodating control.
The general implementation of the admittance control is
shown in Fig. 9(a) [34]. The position control block is a PD
controller, whose gains, K p and Kv are the same as those of
the obtained from the GA and applied in Section V-B. The
admittance block relation can be generally given as
Md (x¨d − x¨o ) + Dd (x˙d − x˙o ) + Kd (xxd − x o ) = F ext (13)
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
((a)) General concept of admittance control.
((b)) General concept of impedance control
Fig. 9: General concept of admittance and impedance control.
where Md is the desired inertial matrix which is usually
assumed to be zero for simplicity, Dd is the desired damping
matrix, Kd is the desired stiffness matrix, x o is the original
planned trajectory, x d is the modified trajectory and F ext is
the measured external force. The control law of the admittance
control is given as follows, in which τ is the input torque to
the manipulator dynamics.
τ = J T (qq)[K p (xxd − x ) + Kv (ẋxd − ẋx) − F ext ]
where x is the measured position vector, J T is the Jacobian
transpose, K p and Kv are the proportional and derivative gains
of the PD position control respectively. This admittance con-
trol is used as a subsystem of the propose hybrid impedance
control to achieve the tooth brushing task. Specifically, it
is used for the subspace that is designated for the force
accommodating directions to cope with the unintentional head
pose changes.
B. Force-based Impedance Control
The basic concept of the force-based impedance control is
that, the controller uses the measured position to modify the
force input of the system directly. Figure 9(b) shows the basic
implantation of the force-based impedance control. Several
modifications have been proposed within the framework of
the force-based impedance control to enable force tracking in
changing and unchanging environments.
The active force tracking-based impedance control dis-
cussed in Section V-B and illustrated in Fig. 7 is applied here
directly as a subsystem of the proposed hybrid impedance con-
trol to achieve a singe structure of this controller. Specifically,
the active force tracking-based impedance control is used for
the subspace that is designated for the force control to achieve
the brushing task.
C. Hybrid Force Tracking-based impedance control
The hybrid impedance control combines the admittance
control and the impedance control into a single control scheme
using the switching matrix S ∈ Rn×n that distinguishes be-
tween the force and position controlled sub-spaces, where
S = diag[si ], for i = 1, ..., n in which n is the DOF of the
manipulator task space and si is either 1 or 0 based on
the force or position controlled directions. In this hybrid
impedance control, the impedance relation of force-control
can be distinguished from that of the position-control and thus
achieves more flexibility in choosing those impedances. In this
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
8
Fig. 10: Closed loop hybrid impedance control.
tooth brushing task, the position-based control is applied to
all end-effector position and orientation except the Z position
direction which is a force controlled direction.
The details of the proposed impedance control are illustrated
in Fig. 10, where Z mp −1 is the admittance relation in equation
(13), Z m f −1 is the active force tracking relation in equation (7)
and I is an n × n identity matrix. The modified trajectory x d is
composed of the modified position subspace trajectory x d p and
the modified force subspace trajectory x d f . The measured force
F = Ke (xx − x e ), where Ke is the environment stiffness and x e
(14) is the contact environment trajectory or perturbed trajectory.
The position-based impedance control or admittance control
subsystem ensures that the end-effector trajectory x closely
tracks the modified trajectory x d for not to generate any
exertion forces in all directions that are position controlled.
For tooth brushing task in hand, this will ensure no harm for
the patient in case of head pose changes that results in any
perturbed trajectory while grantee achieving perfect brushing.
VII. S IMULATION R ESULTS
The aforementioned active force tracking-based impedance
control and the hybrid impedance control based on the full
dynamics of the UR10 manipulator presented in Sections
V-B and VI respectively are used in Co-simulations between
ADAMS and MATLAB Simulink R installed on an AMD A6-
4400M with Radeon (TM ) HD Graphics, 2.70GHz processor.
For those co-simulations, the head model is not developed in
ADAMS, but the detected head model with face and mouth
detections, the planning of the desired trajectory and the
control laws are implemented in MATLAB and the outputs
of the control algorithms are sent to the manipulator model
in ADAMS. Consequently, the motion of the manipulator
along the planned trajectory are set as inputs to MATLAB
from ADAMS to evaluate the results in terms of tracking the
planned trajectory as illustrated in Fig. 10.
The desired force fd is also chosen as diag[0 0 2 0 0 0]N.
The gains obtained from GA; namely, K p and Kv used in
those simulations are diag[2000 50000 20000 800 50 25] and
diag[400 5000 1500 25 5 1.8] respectively. First, the results
of position and force tracking capabilities of the active force
tracking impedance control are presented. Second, the results
of the position and force tracking capabilities of the hybrid
impedance controller are illustrated.
A. Simulation Results of the Active Force Tracking Impedance
Control
The position and force trajectories for the cleaning stage and
the withdrawal stage are set as inputs to this controller. The
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
desired stiffness and damping matrices, Kd and Bd are chosen
as diag[0 0 150 0 0 0] and diag[0 0 20 0 0 0]N respectively;
while k f = −1 as previously stated.
1) Simulations Results for the Cleaning Stage: The force
and position trajectories generated for the cleaning stage,
illustrated in Fig. 6, are set as inputs to the active force-
based impedance controller. Figure 11 shows the force tracking
results of the manipulator since the force tracking error fd − f
in the Z direction is regulated to zero over time and this
ensures that brushing is performed with the desired force fd
and consequently safety is guaranteed. The large force of ap-
proximately 50N measured at the beginning of the simulation
shown in Fig. 11 is due to measuring the force in terms of
the position error, since large force measurement results from
large position error. This occurs only at the free space where
the end-effector is at the initial position far from the mouth
point. This can be easily overcome in practise by measuring
the force directly by a force sensor. The little spike in the
actual force noticed at about 3 secs is due to the transition
in the trajectory of the brushing tool attached to the UR10
manipulator from free space into contact space (contact of the
brushing tool with the mouth of the user) and this perturbation
is shown to be very small and thus acceptable.
Figure 12(a) - (c) shows the position tracking results of
a typical cleaning trajectory in XYZ coordinates while Fig.
12(d)-(f) shows orientation tracking in terms of Roll, Pitch
and Yaw angles, respectively. Those results of the position and
orientation tracking illustrate a rapid system response which
is often required in autonomous systems.
The errors of the force, position and orientation tracking
from the initial position to the end of the penetration distance
inside the mouth by the active force tracking impedance
control are shown in Fig. 13. Figure 13(a) shows that the force
tracking error of fd − f in the Z direction is regulated to zero
over time and this ensures that brushing is performed with
the desired force fd and safety is guaranteed. Furthermore, it
should be noted that tracking errors of the position and orien-
tation shown in Figs. 13(b) and (c) respectively, are necessary
as the force tracking requirements modifies the trajectory to
satisfy the force constraints and thus ensuring overall safety
of the system. Keeping in mind that the desired impedance
stiffness and damping constants, that can be changed, affect
the resulting tracking errors, the obtained negligible tracking
errors especially in the contact region inside the mouth can
ensure the feasibility of the proposed trajectory control and
ensure interaction safety.
2) Simulation Results for the Withdrawal Stage: The re-
planned trajectories for the withdrawal of the brushing tool
after completing the cleaning stage are set as inputs to the
active force-based impedance controller. Figure 14(a) - (c)
Fig. 11: Force tracking of a typical cleaning trajectory by the
active force-tracking impedance control
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
9
shows the position tracking results of a typical re-planned tra-
jectory illustrated in Fig 6 for the withdrawal of the tooth brush
in XYZ coordinates while Fig. 14(d)-(f) shows orientation
tracking in terms of Roll, Pitch and Yaw angles, respectively.
Those results prove successful tracking with very negligible
errors. Thus, the results illustrated in Figs. 12 and 14 prove
the capability of the proposed system for achieving the tooth
brushing task autonomously.
B. Simulation Results of the Hybrid Impedance Control
In this simulation, force and position trajectories generated
for the brushing task, illustrated in Fig. 6, are set as inputs
to the hybrid impedance controller. The only force controlled
direction is the Z- direction, while all other directions are force
accommodating and therefore are controlled by the position-
based impedance scheme subsystem. Additionally, to illustrate
the force accommodation capability of the hybrid impedance
controller, the trajectory is assumed to be slightly perturbed be-
tween simulation time from 3.2 seconds to 3.7 seconds during
cleaning stage to signify changes in head pose. Consequently,
this leads to forces exertion in all other directions which are
controlled by the position-based impedance controller. Note
that, those exerted forces due to the slight perturbation are
functions of the position errors between the end-effector and
perturbed trajectories as F = K e (xx − x e ). To accommodate
those forces, the end-effector trajectory x will closely track
the modified trajectory x d for not to generate any exertion
forces in all directions that are position controlled. Since
the impedance terms are selected by the user, the desired
stiffness and damping matrices, Kd and Dd for the force-based
impedance control are diag[0 0 150 0 0 0] and diag[0 0 20
0 0 0] respectively, while those of the admittance control are
chosen as diag[40 40 0 40 40 40] and diag[10 10 0 10 10 10]
respectively.
1) Simulation Results for the Cleaning Stage: The original
trajectory x o and modified trajectory by the admittance control
x d p during the entire simulation time are shown in Fig. 15.
Due to the slight perturbation of the trajectory x e , the end-
effector will closely track x d p to cancel the forces in all
position controlled directions indicating the accommodating
capabilities of the admittance control. For better clarification,
Fig. 16 shows a zoomed portion of Fig. 15 in the time span
from 3.2 seconds to 3.7 seconds. This shows clearly how the
original trajectory is adapted to accommodate the exerted force
due to the trajectory perturbation.
To show force exertion as a results of the slight perturbation,
the force is calculated in between the time span from 3.2 to 3.7
sec as F = Ke (xx − x e ) where Ke is the environment stiffness
matrix, as Ke = diag [40 40 40 45 45 45], x is the end-effector
trajectory and x e is the perturbed trajectory. As illustrated in
Fig. 17, the external forces and moments are generated in all
position controlled directions in the time span from 3.2 to 3.7
sec due to trajectory perturbation.
On the other hand, Fig. 18 shows the force tracking results
of the manipulator since the force tracking error fd − f in the
Z direction is regulated to zero over time by the force-based
impedance control subsystem and this ensures that brushing is
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 10

(a) (b) (c)

(d) (e) (f)

Fig. 12: Pose tracking of a typical cleaning trajectory by the active force-tracking impedance control
0.8 0.8
Error in x-direction Error in x-direction

Orientation Error
Position Error(m)
1

Error(m)
Error in y-direction Roll Tracking
Error Error
in y-direction
0.6 0.6
Error in z-direction Pitch
ErrorTracking Error
in z-direction

(rad)
Yaw Tracking Error
0.4 0.4
0.5

Position
0.2 0.2

0 0
0 1 2 3 4 0 1 22 33 44
Time(s) Time(s)
(a) (b) (c)

Fig. 13: Tracking errors of the active force tracking impedance controller for the cleaning stage (a) Force tracking error, (b)
Position tracking error (c) Orientation tracking error

(a) (b) (c)

(d) (e) (f)

Fig. 14: Pose tracking of a typical brush withdrawal trajectory by the active force-tracking impedance control

(a) (b)
(a) (b)
Yaw(rad)

(C) (d) (e)

(c) (d) (e)
Fig. 16: Zoomed portion of the original x o and modified
Fig. 15: Original and modified trajectories of a typical cleaning x d p trajectories of a typical cleaning trajectory by the hybrid
trajectory by the hybrid impedance control impedance control

performed with the desired force fd and consequently safety
is guaranteed. The little spike in the force noticed at about 3
secs is due to the transition of the trajectory of the brushing
tool from free space into contact space (contact of the brushing
tool with the mouth of the user) and this perturbation is shown
to be very small and thus acceptable.
The tracking capabilities of the inner position controller of
the hybrid impedance controller are shown in Figs 19(a) -
(c) and 19(d)-(f) for the XYZ coordinates and Roll, Pitch
and Yaw angles, respectively. Those results of the position
and orientation tracking illustrate that the controller is robust
enough to track the modified trajectories x d , accommodate
changes in the external force and thus enable the needed
complaint human-robot interaction during the brushing task.

2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015
Moment (Nm)
Force(N)
(a)
Moment (Nm)
Moment (Nm)
Moment Along Roll Orientation Moment Along Pitch Orientation
(c)
Fig. 17: External forces and moments exerted in the position
controlled directions of the hybrid impedance control during
the cleaning stage
The errors of the force, position and orientation tracking from
the initial position to the end of the penetration distance inside
the mouth by the active force tracking impedance control are
shown in Fig. 20. Figure 20(a) shows that the force tracking
error of fd − f in the Z direction is regulated to zero over time
and this ensures that brushing is performed with the desired
force fd and safety of the system is guaranteed.
Fig. 18: Force tracking of a typical cleaning trajectory by the
hybrid impedance control
2) Simulation Results for the Withdrawal Stage: The tra-
jectories generated after re-planning the trajectories for with-
drawal of the brushing tool after completing the cleaning
stage are set as inputs to the hybrid impedance controller.
Figure 21(a) - (c) shows the position tracking results of a
typical trajectory illustrated in Fig 6 for the withdrawal of the
tooth brush in XYZ coordinates while Fig. 21(d)-(f) shows
orientation tracking in terms of Roll, Pitch and Yaw angles,
respectively. Those results prove successful tracking with very
negligible errors. Thus, the results illustrated in Figs. 19 and
21 prove the capability of the proposed system for achieving
the tooth brushing task autonomously.
VIII. C ONCLUSION AND F UTURE W ORK
In this paper, an autonomous assistive robotic systems for
tooth brushing of disabled people have been developed. On
the context of developing this robotic system, autonomous
face detection and mouth detection have been developed for
the estimation of the mouth of the user. Furthermore online
planning the trajectories has been enabled. Also a hybrid force
tracking impedance controller has been designed to achieve
the required impedance interaction between the manipulator
and the human. The hybrid force tracking impedance control
ensures task execution at the desired force in the force con-
trolled direction and accommodation of force in the position
controlled directions. Thus, it guarantees system safety and
effectiveness in terms of tooth brushing. The simulation results
demonstrate the capability of the proposed robotic system to
2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.
Transactions on Medical Robotics and Bionics
11
Moment (Nm)
achieve tooth brushing autonomously that could be extended
Force(N)
for fulfilling other assistive tasks around the head. In future,
experimental implementation of the proposed robotic system
(b) shall be carried out.
Moment (Nm)
ACKNOWLEDGEMENTS
Moment Along Yaw Orientation
This research is supported by Egypt Ministry of Higher
Education (MoHE) through Egypt-Japan University of Science
(d) (e)
and Technology Scholarship for the first author.
R EFERENCES
[1] A. Hall, U. Backonja, I. Painter, M. Cakmak, M. Sung, T. Lau,
H. Thompson, and G. Demiris, “Acceptance and perceived usefulness
of robots to assist with activities of daily living and healthcare tasks,”
Assistive Technology, vol. 31, 2017.
[2] K. P. Hawkins, P. M. Grice, T. L. Chen, C. H. King, and C. C.
Kemp, “Assistive mobile manipulation for self-care tasks around the
head,” IEEE Symposium on Computational Intelligence in Robotic
Rehabilitation and Assistive Technologies, pp. 16–25, Orlando, USA,
2014.
[3] M. A. Haseeb, M. Kyrarini, S. Jiang, D. Ristic-Durrant, and A. Graeser,
“Head gesture-based control for assistive robots,” Proceedings of the
11th PErvasive Technologies Related to Assistive Environments Confer-
ence, pp. 379–383, Corfu, Greece, 2018.
[4] S. Schröer, I. Killmann, B. Frank, M. Völker, L. Fiederer, T. Ball, and
W. Burgard, “An autonomous robotic assistant for drinking,” Proceed-
ings - IEEE International Conference on Robotics and Automation, pp.
6482–6487, Seattle, WA, USA, 2015.
[5] H. El-Hussieny, S. F. M. Assal, A. A. Abouelsoud, and S. M. Mega-
hed, “A novel intention prediction strategy for a shared control tele-
manipulation system in unknown environments,” Proceedings - 2015
IEEE International Conference on Mechatronics, ICM 2015, pp. 204–
209, 2015.
[6] A. Bilyea, N. Seth, S. Nesathurai, and H. A. Abdullah, “Robotic
assistants in personal care: A scoping review,” Medical Engineering and
Physics, vol. 49, pp. 1–6, 2017.
[7] T. L. Chen, M. Ciocarlie, S. Cousins, P. Grice, K. Hawkins, K. Hsiao,
C. Kemp, C. H. King, D. Lazewatsky, A. E. Leeper, H. Nguyen,
A. Paepcke, C. Pantofaru, W. Smart, and L. Takayama, “Robots for
humanity: Using assistive robotics to empower people with disabilities,”
IEEE Robotics and Automation Magazine, vol. 20, no. 1, pp. 30–39,
2013.
[8] A. Candeias, T. Rhodes, and M. Marques, “Vision augmented robot feed-
ing,” European Conference on Computer Vision, pp. 50–65, Amsterdam,
Netherlands, 2016.
[9] M. J. Topping and J. K. Smith, “The development of Handy 1. A robotic
system to assist the severely disabled,” Technology and Disability,
vol. 10, no. 2, pp. 95–105, 1998.
[10] P. Robinson, S. A. Deacon, C. Deery, M. Heanue, A. D. Walmsley, H. V.
Worthington, A.-M. Glenny, and B. C. Shaw, “Manual versus powered
toothbrushing for oral health,” Cochrane Database of Systematic Re-
views, no. 2, 2005.
[11] G. Sakaeda, T. Matsubara, H. Ishii, and A. Takanishi, “Development of
automatic teeth cleaning robot driven by cam mechanism,” 2017 IEEE
International Conference on Mechatronics and Automation, ICMA 2017,
pp. 536–540, Takamatsu, Japan, 2017.
[12] C.-S. Chung, H. Wang, D. Ding, and R. A. Cooper, “Feasibility Analysis
of Daily Activities Using Assistive Robotic Manipulators,” RESNA
Annual Conference, VA, USA, 2016.
[13] J. Hammel, K. Hall, D. Lees, L. Leifer, M. Van Der Loos, I. Perkash, and
R. Crigler, “Clinical evaluaton of a desktop robotic assistant,” Journal
of Rehabilitation Research and Development, vol. 26, no. 3, pp. 1–16,
1989.
[14] C. Balaguer, A. Giménez, A. Jardón, R. Correal, S. Martı́nez, A. M.
Sabatini, and V. Genovese, “Proprio and teleoperation of a robotic
system for disabled persons’ assistance in domestic environments,”
Springer Tracts in Advanced Robotics, vol. 31, pp. 415–427, 2007.
[15] C. J. Perera, T. D. Lalitharatne, and K. Kiguchi, “Eeg-controlled meal
assistance robot with camera-based automatic mouth position tracking
and mouth open detection,” IEEE International Conference on Robotics
and Automation, pp. 1760–1765, Singapore, 2017.
This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. Citation information: DOI 10.1109/TMRB.2020.3030317, IEEE
Transactions on Medical Robotics and Bionics
JOURNAL OF LATEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 12

(a) (b) (c)

(d) (e) (f)

Fig. 19: Pose tracking of the hybrid impedance controller for the cleaning stage
0.8
Error in x-direction Roll Tracking Error

Position Error(m)
1

Orientation Error
0.6 Error in y-direction Pitch Tracking Error
Error in z-direction Yaw Tracking Error

(rad)
0.4
0.5
0.2

0 0
0 1 2 3 4 0 1 2 3 4
Time(s) Time(s)
(a) (b) (c)

Fig. 20: Tracking errors of the hybrid tracking impedance controller for the cleaning stage (a) Force tracking error, (b) Position
tracking error (c) Orientation tracking error

(a) (b) (c)

(d) (e) (f)

Fig. 21: Pose tracking of a typical brush withdrawal trajectory by the hybrid impedance control

[16] F. Arrichiello, P. Di Lillo, D. Di Vito, G. Antonelli, and S. Chiaverini,
“Assistive robot operated via P300-based brain computer interface,” Pro-
ceedings - IEEE International Conference on Robotics and Automation,
pp. 6032–6037, Marina Bay Sands, Singapore, 2017.
[17] C. Schindlbeck and S. Haddadin, “Unified Passivity-Based Cartesian
Force / Impedance Control for Rigid and Flexible Joint Robots via
Task-Energy Tanks,” IEEE International Conference on Robotics and
Automation (ICRA), pp. 440–447, Seattle, WA, USA, 2015.
[18] T. Morita and S. Sugano, “Design and development of a new robot joint
using a mechanical impedance adjuster,” vol. 3, pp. 2469–2475 vol.3,
May 1995.
[19] A. S. Sadun, J. Jalani, and J. A. Sukor, “An overview of active
compliance control for a robotic hand,” ARPN Journal of Engineering
and Applied Sciences, vol. 11, no. 20, pp. 11 872–11 876, 2016.
[20] W.-K. Song and J. Kim, “Novel Assistive Robot for Self-Feeding,”
Robotic Systems - Applications, Control and Programming, pp. 43–6–0,
2012.
[21] M. Musialek, M. Forstenpointner, A. Stummer, H. Haririan, and
K. Tacaks, “Amabrush - World’s First Automatic Toothbrush,” 2017,
accessed 30/2/2020. [Online]. Available: https://bit.ly/2GxYl3B
[22] N. M. Al-Zubair, “Determinant factors of Yemeni maxillary arch dimen-
sions,” Saudi Dental Journal, vol. 27, no. 1, pp. 50–54, 2015.
[23] E. Tsakiraki, “Real-time Head Motion Tracking for Brain Positron Emis-
sion Tomography using Microsoft,” KTH Royal Institute of Technology,”
Ph.D. dissertation, 2016.
[24] J. R. Terven and M. C. Diana, “Kin2 . A Kinect 2 Toolbox for
MATLAB,” SCI COMPUT PROGRAM, vol. 130, pp. 97–106, 2017.
[25] M. Rahman, Beginning Microsoft Kinect for Windows SDK 2.0, 2017.
[26] A. S. Oladayo, S. F. M. Assal, and H. El-hussieny, “Towards Develop-
ment of an Autonomous Robotic System for Beard Shaving Assistance
for Disabled People,” 2019 IEEE International Conference on Systems,
Man and Cybernetics (SMC), Bari, Italy, pp. 3435–3440, 2019.
[27] P. Song, Y. Yu, and X. Zhang, “A Tutorial Survey and Comparison of
Impedance Control on Robotic Manipulation,” Robotica, vol. 37, no. 5,
pp. 801–836, 2019.
[28] H. Mehdi and O. Boubaker, “Stiffness and Impedance Control Using
Lyapunov Theory for Robot-Aided Rehabilitation,” international Jour-
nal of Social Robotics, no. 4, pp. 107–119, 2012.
[29] M. A. Naeem and S. F. M. Assal, “Development of a 4-DOF cane
robot to enhance walking activity of elderly,” Proceedings of the Institute
of Mechanical Engineers, Part C: Journal of Mechanical Engineering
Sciences, pp. 1–19, 2019.
[30] P. A. Lasota, T. Song, and J. A. Shah, A Survey of Methods for Safe
Human-Robot Interaction, 2017.
[31] G. Zeng and A. Hemami, “An overview of robot force control,”
Robotica, vol. 15, no. 5, pp. 473–482, 1997.
[32] E. Akdoğan, M. E. Aktan, A. T. Koru, M. Selçuk Arslan, M. Atlıhan, and
B. Kuran, “Hybrid impedance control of a robot manipulator for wrist
and forearm rehabilitation: Performance analysis and clinical results,”
Mechatronics, vol. 49, no. July 2017, pp. 77–91, 2018.
[33] D. E. Whitney, “Force feedback control of manipulator fine motions,”
Journal of Dynamic Systems, Measurement and Control, Transactions
of the ASME, vol. 99, no. 2, pp. 91–97, 1977.
[34] C. Ott, R. Mukherjee, and Y. Nakamura, “Unified impedance and
admittance control,” in 2010 IEEE International Conference on Robotics
and Automation, 2010, pp. 554–561.

2576-3202 (c) 2020 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
Authorized licensed use limited to: Carleton University. Downloaded on November 05,2020 at 12:02:50 UTC from IEEE Xplore. Restrictions apply.