TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

1

Online Trajectory Planning and Force Control for
Automation of Surgical Tasks
Takayuki Osa, Member, IEEE, Naohiko Sugita, and Mamoru Mitsuishi, Member, IEEE

Abstract—Automation of surgical tasks is expected to improve
the quality of surgery. In this paper, we address two issues
that must be resolved for automation of robotic surgery: online
trajectory planning and force control under dynamic conditions.
By leveraging demonstrations under various conditions, we
can model the conditional distribution of the trajectories given
the task condition. This scheme enables generalization of the
trajectories of spatial motion and contact force to new conditions
in real time. In addition, we propose a force tracking controllerthat robustly and stably tracks the planned profile of thecontact force by learning the spatial motion and contact force
simultaneously. The proposed scheme was tested with bimanualtasks emulating surgical tasks that require online trajectory
planning and force tracking control, such as tying knots andcutting soft tissues. Experimental results show that the proposed
scheme enables planning of the task trajectory under dynamic
conditions in real time. Additionally, the performance of theforce control schemes was verified in the experiments.
Note to Practitioners — This study addresses the problem of
motion planning and control for automation of surgical tasks.
In surgical tasks, it is necessary to manipulate objects underconditions where positions or shapes of objects often change
during the task. Thus, trajectories for surgical tasks need to beplanned and updated according to the change in the conditions
in real time. In this study, we propose a framework for learning
both spatial motion and force profile from human experts. The
proposed system can plan and update task trajectories in realtime and robustly control the contact force under dynamic
conditions. On the other hand, generalization of trajectories islimited to the conditions which are close to the conditions where
the demonstrations were performed. In future work, we will
investigate reinforcement learning approaches in order to enable
autonomous improvement of the performance.
Index Terms—Motion planning, force control, surgical robot.

I. I NTRODUCTIONOBOTIC surgery has established its position within
medicine in recent years, and its benefits have beenclarified through clinical studies [1]. On the other hand, there is
still potential that is not leveraged in current commercialized
systems. One such possibility is to automate surgical tasks
in robotic surgery, which would reduce surgeons’ mental andphysical fatigue and improve the quality of surgery. This
study addresses two issues that needs to be resolved in orderto achieve automation of robotic surgery: online trajectory
planning and force control.

R

T. Osa is with Intelligent Autonomous Systems Group at TU Darmstadt,
Germany (e-mail: takayuki@robot-learning.de).
N. Suigta and M. Mitsuishi are with Department of Mechanical Engineering, The University of Tokyo, 113-8656, Japan (e-mail: sugi@nml.t.utokyo.ac.jp; mamoru@nml.t.u-tokyo.ac.jp).
Manuscript accepted, February 2017.

Fig. 1. Automation of knot tying in robotic surgery requires both online
trajectory planning and force control.

The planning of task trajectories under dynamic conditions
in real time is one of the major issues. When a robotic surgicalsystem assists a surgeon through autonomous motions, it must
collaborate with a human surgeon and work with movableand flexible objects such as threads and soft tissues during
operations. For example, surgical knot tying involves making
loops around a surgical instrument with a surgical thread (Fig.
1). If the surgical instrument to be looped is manipulated bya surgeon and moving during the task, the trajectory must berecalculated and updated online according to the motion of the
instrument, thereby maintaining the topological feature of the
trajectory. However, such online trajectory planning presents
a difficult challenge, for which a solution has not yet been
established.
The other issue is force control. Haptic information plays
important roles in surgical operations. In some tasks wherecontacts between instruments and objects are necessary, thecontact force must be explicitly controlled using the haptic
information. For instance, when a surgeon cuts a membraneusing a surgical knife, the contact force must be controlled to
cut the target appropriately; or, when a surgeon makes a knot, athread must be tightened with appropriate force to make a tightknot without breaking the thread. In these tasks, the desired
profile of the contact force must be planned online and tracked
robustly by the robotic system. Such planning and trackingof the contact force are essential to autonomously performtasks that require explicit force control. When both online
trajectory planning and force tracking control are necessary,the execution of the task becomes extremely challenging formany robotic systems.
In this paper, we present a framework for online trajectory planning and force control under dynamic conditions
to achieve automation of surgical tasks. In our earlier work[2], [3], we presented a system that learns time- and spacedependent motions from trajectories demonstrated under vari-

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

ous conditions. Although such online trajectory planning wasaddressed in our prior work, we only showed how to learn
spatial motions from the demonstration and the quantitativeevaluation of the scheme was not exhaustive. The scheme
proposed in this paper enables planning trajectories of both
spatial motion and contact force in real time under dynamic
conditions by leveraging demonstrations under various conditions. By learning the distribution of the spatial motions andcontact force simultaneously, the developed system updates
the profile of the contact force in real time under dynamic
conditions and robustly tracks the planned contact force . The
proposed scheme enables autonomous execution of tasks that
require force control, such as cutting soft tissues and tying aknot.
The main contributions of this paper are (i)presentationof the framework to achieve online trajectory planningand force tracking control for surgical task automation;(ii)describing implementation details of the proposed schemeand (iii)quantitative evaluation of the developed scheme
through experiments with tasks that emulate surgical tasks.
This paper is structured as follows. The next section describes the previous studies related to trajectory planningby learning from demonstrations and force control. Section
III describes the details of the proposed trajectory planningscheme and force tracking control. Section IV provides theexperiments and simulations to evaluate the performance of
our online trajectory planning and force control. Section V
discusses the results presented in this study. The conclusions
can be found in Section VI.
II. R ELATED W ORK
A. Trajectory Planning for Automation of Surgical Tasks
Numerous studies have addressed the automation of surgical tasks [4]–[10]. Learning-from-demonstration (LfD) approaches have often been used in these previous studies. LfD
is an approach that leverages human demonstration to achieveautonomous motion in robotic systems [11]. Robotic systems
for surgery often employ a master-slave system in which
an operator inputs motions from the master system for theslave manipulator to execute. In this typical configuration, themotion inputs from the human operator are easily obtained;
therefore, LfD is a reasonable approach to achieve autonomous
motion in robotic surgery. Mayer et al. developed a scheme tomodel demonstrated trajectories, using a recurrent neural network [5]. Berg et al. developed a system that learns the motionof tying a knot from multiple demonstrations and performs
it faster than the given demonstration [4]. Recently, Murali
et al proposed an autonomous assistance system based on
observation of human experts [10]. However, these schemes do
not address the generalization of the demonstrated trajectories
to new situations. Since the conditions in a surgical operationvary for each operation, the task trajectory must be planned
according to the given condition.
Generalization of demonstrated trajectories is one of primary problems in LfD. Khansari et al. developed a scheme
to model demonstrated motions as a time-invariant dynamic
system using Gaussian mixture models (GMMs) [12], [13].

2This scheme allows the motion to be generalized for a newstate in the robotic system. In a reinforced learning framework,
schemes to adapt the motion primitives for new situations were
proposed in [14]. These studies focused on motion that canbe defined by the starting state and the end state. However,some surgical operations will be difficult to describe using
such frameworks designed primarily for point-to-point tasks.
Making a loop around an instrument in order to tie a knot, forinstance, is difficult to decompose into point-to-point motions,because its trajectory is seamless, and the topological shapeof the entire trajectory must adapt to new situations. Schulman et al. developed a scheme to generalize demonstrated
trajectories for new situations [9], [15], [16]. Their scheme
is not limited to point-to-point motions, and the geometricalmapping from demonstrated situation to a new situation iscomputed by non-rigid registration. This scheme was applied
to a suturing task in a simplified situation [9]. However,finding the best demonstration for the new condition is timeconsuming; hence, their method is difficult to use for online
trajectory planning under dynamic conditions. Recent work
developed methods for learning task-parameterized policies
such as [17] and [18]. Although these methods can be used togeneralize the learned skills, they are not focused on online
trajectory planning. The method in [18] estimate the model
parameter using GMMs, and the estimated model parameters
are utilized to generate trajectories. This method is indirectcompared to our approach, which learns the direct mappingfrom the task condition to the trajectory. As a consequence,
the method in [18] may require more computational time
when compared to our approach. Parachos et al. proposeda framework of Probabilistic Movement Primitive(ProMP) torepresent the distribution of demonstrated trajectories [19].
This method enables the generalization of the trajectory to
new via-points or new goal points by conditioning the distribution of the trajectory. However, the study in [19] does notdiscuss the distribution of the situation where demonstrations
are performed. Recent work in [20] proposed a trajectory
optimization framework to generate collision-free motions bylearning from demonstration. Although the method in [20] cangeneralize the trajectory to new scenes, it does not address
online trajectory planning.
Although many studies have reported schemes for generalizing demonstrated trajectory for new via-points or goal points,
specifying the via-points or goal points are not always trivial
in motion planning for surgical tasks. For example, when thesystem needs to make a loop with a surgical thread around the
instrument, it is challenging to plan via-points according to theposition of the surgical instrument, maintaining the topologicalfeature of the trajectory.
In this study, we extend an approach of learning fromdemonstrations under various situations in [2]. In the schemein [2], the conditional distribution of the demonstrated trajectory given the situation is modeled using Gaussian Process
Regression (GPR). This enables the modeling of time- andspace-dependent task trajectories and their generalization to
new situations. We extend this scheme to learning and planningfor the desired force in real time during the task. We willshow that the proposed scheme enables planning for both

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

3

spatial motion and force profile in real time under dynamic
conditions.
B. Planning and Tracking the Contact Force
Force tracking is a classical control problem in the field of
robot manipulation [21]–[24]. Seraji et al. described schemes
for force tracking control by impedance control and admittancecontrol [25], [26]. Tracking the desired force requires consideration of the stiffness of the contact object and the ability
to predict the motion for tracking the desired force. However,this problem remains non-trivial despite decades of research.
The learning and planning of the contact force trajectory
have been addressed by some studies in the literature regarding learning from demonstration and reinforcement learning.
Some studies reported robotic systems that learn the required
impedance from demonstrations to react to the force exerted
on the manipulator [27]–[29]. However, surgical tasks require
explicit force control beyond the mere ability to react tothe contact force caused by a disturbance. Reinforcement
learning methods for tasks that require force control havebeen reported in [30], [31]. These previous studies proposed
schemes for adapting motion parameters to new situations
to achieve appropriate force and motion control. However,these methods require iterative learning to track the planned
contact force in a new situation. Recently, Lee et al proposeda scheme for learning force control from demonstration in[32]. However, this method is based on the trajectory planningscheme in [9], and this scheme is not applicable to the taskthat require online trajectory planning due to its computationalcost. Likewise, Rozo et al. proposed a method for learning andcontrolling both force profile and spatial motions [33]. The
method in [33] is based on the trajectory learning in [18], andthe methods in [33] may not be suitable for online trajectory
planning for the same reason as the methods in [18].
In this work, we leverage the models learned from demonstrations to plan the trajectory of the contact force and track
the planned contact force robustly. By learning the trajectoryof the contact force and spatial position simultaneously, theproposed scheme plans the desired contact force and estimates
the spatial motion to track the planned contact force. This trajectory learning scheme was then combined with the conceptof the sliding mode control to achieve robust force tracking
under dynamic conditions [34].
III. P ROPOSED M ETHOD
A. Overview of System and Algorithm
1) System: We integrated the proposed online trajectory
planning and force control schemes with a teleoperated robotic
system developed for laparoscopic surgery [35], [36] (Fig. 2).
The robotic surgical system consists of master and slave
systems, in which the motions input at the master system by anoperator are executed by the slave manipulator. In this type ofsystem, the LfD approach is expected to be efficient since thetrajectory executed by experts is easily obtained. A standard
system for robotic surgery employs rate control, where the(a)Fig. 2. A robotic system for teleoperated surgery.

(b)

Slave site

Master site
Mastercontroller

ξ&
m

ξ&
m

Motion/Forcecontroller

Mastermanipulator

r
ξ&
s

ξs

Ξs*

Trajectory
planner

Velocity
controllerξ&s , ξsInteraction

ξc

Human
operator

sensorFs , ξsus

Slave
manipulator
Interaction
Environment

Fig. 3. Overview of proposed system.

motion of the slave manipulator is proportional to the motion
input at the master system asξ˙s = Kms ξ˙m ,(1)

where ξ˙s , ξ˙m , and Kms are the velocity of the slave manipulator, velocity of the master manipulator, and motion scaling gain
between the master and slave systems, respectively. To achieve
this control, a velocity/position control is often implemented
in a robotic surgery system. Hence, we developed a motion
planning and execution system based on the master-slave
system with a velocity controller.
Fig. 3 shows the structure of the proposed system. A
trajectory planner and a motion-force controller are implemented in the master-slave system. The trajectory planner
estimates a trajectory Ξ∗s required to perform the learned task
according to a given condition ξc . The motion-force controller
is implemented as an outer loop based on an underlying
velocity controller as shown in Fig. 3. To track the planned
trajectory, the motion-force controller determines the reference
velocity for the velocity controller ξ˙sr using the feedback of
the sensory information, such as the measured contact force
Fs . The torque input, us , to each joint in the slave manipulator
is determined by the underlying velocity controller.
Force control is usually categorized into two types:
impedance control and admittance control. Admittance control
works well for soft environments and robots with a highgear ratio, whereas impedance control works well for hardenvironments and robots with a low gear ratio [37]. Surgicalrobots often have a high gear ratio and a velocity controller toachieve precise motions by rate control. In addition, the contact
force is relatively small in laparoscopic surgery because therobotic manipulator is expected to cope with soft tissues. For

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

these reasons, we developed a force control strategy based on
admittance control in this study. Hence, the force controller
was implemented as an outer loop with an inner loop that
implemented the velocity controller.
2) Learning and Planning of Trajectory: In the proposed
trajectory planning scheme, we assume that demonstrations
performed by a human expert under various task conditions
ξc are available. The task condition was fixed for a single
demonstration. Thereafter, the system models the conditional
distribution of the demonstrated trajectories given the task
condition at time t as P (ξs (t)|ξc ). Subsequently, if the newtask condition is given as ξc∗ , the optimal task trajectory Ξ∗sunder the given condition can be predicted as a conditional
expectation asΞ∗s = [ξs∗ (0), . . . , ξs∗ (N )] ,(2)ξs∗ (t) = E[ξs (t) | ξc∗ ]

(3)

where
and N is the total number of time steps of the trajectory. For
modeling the distribution of the demonstrated trajectories, thesystem normalizes the demonstrated trajectories in the time
domain and estimates the reference trajectory. In this step,
dynamic time warping (DTW) was employed to eliminate
the temporal variance of the demonstrated trajectories [38],and the Kalman smoother was used to estimate the reference
trajectory [4], [39], [40]. Thereafter, the distribution of thedemonstrated trajectories is modeled using GPR.
During the task execution, the task condition ξc is constantly
measured and sent to the trajectory planner, and the system
plans and updates the trajectory Ξ∗s in real time according to
the task condition ξc .
This trajectory learning scheme can be used for learning
trajectories of various sensory information. In this work, weapplied this scheme to learn both spatial motion and contact
force simultaneously. The details of our trajectory planningscheme are described in Section III-B.
3) Motion and Force Control: In this study, we implemented the force tracking control scheme, which is a prerequisite for tasks that require force control such as cutting soft
tissues and tying knots. These types of motions involve both
spatial motions and contact force. The proposed system learns
the trajectory of the contact force and the spatial position
simultaneously from human demonstration by representing thestate of the system as follows:ξspaceξs =(4)Fswhere ξspace and Fs represent the kinematic state of thesystem and state of the contact force, respectively. The motionforce controller determines the reference velocity ξ˙sr to track
the planned trajectory of the contact force Fs∗ by leveraging∗
under the task condition
the planned spatial trajectory ξspace∗ξc . This enables planning of the contact force trajectory undera given condition and robust tracking of the planned trajectory.
In robotic surgery, importance of spatial motion control
and force control is different between tasks. In addition, theworkspace is severely limited in robotic surgery, and it is4

difficult to measure the contact force loaded on the endeffector in every direction. Therefore, we need to combine
spatial motion control and force tracking control according
to the availability of force information and the type of thegiven tasks. We implemented spatial motion control and force
tracking control in the developed system, and both controls
were used in the experiments to verify the proposed scheme as
discussed in Section IV. We employed the spatial motion control, as presented in our prior work [2]. For self-containment,
we describe the spatial motion control in Section III-C. The
novel force tracking control is then described in Section III-D.
B. Online Trajectory Planning by Learning from Demonstration
1) Normalization in Time Domain and Estimation of Reference Trajectory: Demonstrated trajectories usually containtemporal variations because the execution speed of the task
varies for each demonstration. Therefore, these variations mustbe eliminated to model the distribution of the trajectory. For
this purpose, we normalize the demonstrated trajectories in thetime domain and estimate the reference trajectory by using themethod described in [4], [39], [40]. As in [4], [39], [40], we
regard the demonstrated trajectory as a noisy “observation”
of the “reference” trajectory. We assume a linear system withprocess noise and measurement noise, each of which has aGaussian distribution. We represent the state of the system in
the reference trajectory z(t) as follows: refξs (t)(5)z(t) =
uref (t)where ξsref (t) is the state of the robotic system at time t, anduref (t) is the input to the system in the reference trajectory.
As in (4), ξs (t) can be represented by kinematic state (e.g. theCartesian or configuration space) and sensory outputs (e.g.
force/torque sensor). We represent the system as a stochastic
model asA Bz(t + 1) =z(t) + w(t),(6)
0 I
where A is the state matrix, and B is the input matrix, andw(t) is the process noise of the system. In our system, ξs (t) is
given by the state of the slave manipulator, and u(t) is givenby the input from the master manipulator. Since we assume
the velocity-velocity control as in (1), A is a identity matrix,
B is given by Kms , and u(t) = ξ˙m . We then assume that theith demonstrated trajectory y i is drawn from this stochastic
system, and y i is given as i
ξs (t)iy (t) =,(7)ui (t)
where ξsi (t) is the state of the system and ui (t) is the input tothe system at time t in the ith demonstrated trajectory. WhenM demonstrated trajectories are given, the reference trajectory z can be associated with the demonstrated trajectories
y 1 , · · · , y M as
 

  1
y 1 (τt1 )RI  ..  ..
 =  .  z(t) + v(t), v(t) ∼ N 0,  0.
Iy M (τtM )
0


0
..
.
0

0




0 ,
M
R

(8)

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

where τti is the time mapping from the demonstrated trajectoryy i to the reference trajectory z, and Ri is the variance of the ith
trajectory from the reference trajectory. Therefore, we can estimate the reference trajectory z by using the Kalman smoother.
In our implementation, we set Ri = I for i = 1, · · · , M sothat the importance of each demonstrated trajectory is equal.
To estimate the reference trajectory, we initialized the timealignment as τti = Nt T i , where N is the total number of time
steps of the reference trajectory and T i is the length of the ith
demonstrated trajectory.
After the time alignment is updated, the reference trajectory
is also updated using the updated time alignment. By alternately repeating the updating of the reference trajectory andthe time alignment, we can properly align the demonstrated
trajectories in the time domain.
2) Modeling the conditional distribution of the demonstrated trajectories given the task condition: We employed
GPR, a statistical approach, to model the distribution of thedemonstrated trajectories as a function of the task condition.
Although other regression methods such as Gaussian mixture
regression [41], [42] and locally weighted regression [43] areavailable, we selected GPR because it handles nonlinear relationships using relatively little training data [44]. To model thedeviation of the demonstrated trajectory y from the reference
trajectory z by using GPR, we represent the dataset as follows:


T 
T 
ξs1 (τt1 ) − ξsref (t)
ξc1




..
..
,
 , Yt = 
X=
.
.






(9)
T
T
ref
M
M
M
ξs (τt ) − ξs (t)
ξc
h
i
h
T i
T
X ∗ = (ξc∗ ) , Yt∗ = ξs∗ (t) − ξsref (t)where X is the set of task conditions, Yt is the set of deviationsof the demonstrated trajectories from the reference trajectoryat the tth time step, X ∗ is the test task condition, and Yt∗ is thedeviation from the reference trajectory under the test condition
X ∗ at the tth time step. When the state of the system is D
dimensional, the dimension of Yt is M × D. In GPR, the joint
distribution of Yt and Yt∗ is modeled as a Gaussian distribution
with zero-mean as

p

Yt,j
∗
Yt,j


∼N

 
G(X, X) + σn2 I
0,G(X ∗ , X)

G(X, X ∗ )G(X ∗ , X ∗ ) + σn2 I



(10)
∗
where Yt,j is the j th column of Yt , Yt,jis the j th column ofYt∗ , and σn is a constant. G is the kernel matrix defined asfollows:
G(X, X) ∈ RM ×M , (G(X, X))i,j = g(ξci , ξcj ),G(X ∗ , X) ∈ R1×M , (G(X ∗ , X))1,j = g(ξc∗ , ξcj ),G(X, X ∗ ) ∈ RM ×1 , (G(X, X ∗ ))i,1 = g(ξci , ξc∗ ),G(X ∗ , X ∗ ) ∈ R, G(X ∗ , X ∗ ) = g(ξc∗ , ξc∗ ),where ξci is the ith demonstrated task condition, and g(xi , xj )represents the kernel function. In our implementation, we used
the square exponential kernel function defined as
1g(xi , xj ) = σf exp − 2 (xi − xj )T (xi − xj ) , (11)2l5where σf and l are parameters that define the shape of thekernel function. The performance of the GPR depends on
the selection of the hyperparameters [σf , σn , l], which are
obtained by maximizing the marginal likelihood defined as
1
log pmarg = − yjT G(X, X)yj −log det |G(X, X)|−M log 2π.
2(12)For details of selecting the hyperparameters of GPs, please refer to [44]. Using the models described above, the conditional∗
distribution of Yt,jis expressed as:∗p(Yt,j|Yt,j , X, X ∗ ) ∼ N µ∗j , Σ∗j
(13)
where
µ∗j = G(X ∗ , X)(G(X, X) + σn2 I)−1 Yt,jΣ∗j = G(X ∗ , X ∗ ) + σn2 I
− G(X

∗

(14)

, X)(G(X, X) + σn2 I)−1 G(X, X ∗ )Therefore, the trajectory under the given task condition isestimated as∗∗ξs,j(t) = ξsref (t) + E[Yt,j|Yt,j , X, X ∗ ]= ξsref (t) + G(X ∗ , X)(G(X, X) + σn2 I)−1 Yt,j

T

(15)

∗
for j = 1, . . . D, where ξs,j(t) is the state of the system in theththj dimension at the t step in the trajectory predicted for thetask condition X ∗ . The entire trajectory under the given task
condition is estimated by computing (15) for t = 0, · · · , N .
Since we employed the standard single output GP, we requireN × D models to obtain the whole trajectory.
Once the hyperparameters [σf , σn , l] and covariance matrix
G(X, X) are obtained in the offline phase, the trajectory
planning requires computing (15) for t = 0, · · · , N , which
is a matrix computation with relatively little computationalcost. Therefore, the proposed scheme enables planning of the
trajectory under dynamic task conditions with a sufficiently
short computation time.

C. Spatial Motion Control for tracking the planned trajectory1) Control Scheme for Spatial Motion: We present the
spatial motion control scheme to stably track the spatial
trajectory which is constantly updated during the task. This
controller is necessary for tasks where the planned spatial
motion needs to be achieved robustly.
The motion controller is implemented as an outer loop basedon the underlying velocity controller, and it determines thereference velocity ξsr in order to track the task trajectory in
the proposed system (Fig. 3). When the task trajectory is
updated during the task execution, the desired trajectory of
the manipulator changes discontinuously. Given such changes,
the planned trajectory cannot be input to the velocity controller
directly as a reference value since this would cause unstablesystem behavior. Tracking the updated trajectory stably androbustly requires that we determine the appropriate reference
velocity for the velocity controller. For this purpose, we usedthe concept of sliding mode control [34]. In this framework,
the desired system behavior is expressed as a sliding surface
as follows:
S(t) = ξs (t) − ξs∗ (t) = 0(16)

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

where ξs (t) is the kinematic state of the system at time tand ξs∗ (t) is the kinematic state at time t obtained fromthe planned trajectory. The desired velocity for tracking theplanned trajectory is expressed as follows:ξ˙sr = ξ˙s∗ − K · sat (ξs (t) − ξs∗ (t))
where K = 


k1


0
0

0
..
.
0

0

(17)




 , n is the dimension of ξs (t), and0 
knki > 0 for i = 1, . . . , n. The saturation function sat(S) is
defined assat(s1 /c1 ),...
sat(S) = (18)
sat(sn /cn )where S = [s1 , · · · , sn ]T , 1xisat(xi ) =
−1(xi > 1)(|xi | < 1)(xi < −1)(19)

and [c1 , · · · , cn ] are positive constants that determine theboundary layer on the sliding surface.
In (17), the first term represents the feed-forward control
input to ensure the system slides on the sliding surface; thesecond term represents the feedback control input to ensure
the system converges to the sliding surface.
2) Stability of the Control Scheme: In the framework of
the sliding mode control [34], the system converges to thesliding surface within a finite time if the following conditionis satisfied for a certain positive value [η1 , · · · , ηn ],d 2
s = −ηi |si |.
(20)
dt i
From (16) and (17), we obtain the following equations.
1 d 2
s
2 dt i= si · ṡi= −ki si · sat (si )(21)When |si | > ci , (21) becomes
1 d 2
s = −ki |si | .
(22)
2 dt i
Therefore, the system reaches the neighborhood of the sliding
surface within a finite time. Meanwhile, when |si | < ci , (21)can be rewritten as
1 d 2
s = −ki si · si .
(23)
2 dt iThus, the system exponentially converges to the sliding surface
in the neighborhood of the sliding surface. Therefore, it converges from any state to the desired trajectory within a finite
time and can perform tracking in stable manner according thecontrol law in (17).

6challenges: planning the desired trajectory of the contact force,and tracking the planned contact-force trajectory.
These problems can be solved by extending the trajectory
planning scheme described in the previous section. The systemsimultaneously learns the spatial position trajectory of themanipulator and the contact force by defining the system state
as in (4). By using this redundant expression, the system learns
the trajectory of the spatial position and the contact force
simultaneously using the method described in the previous
section. Given the task condition ξc∗ , the trajectory is estimated∗∗as ξs∗ = [ξspace, Fs∗ ]T where ξspace
is a spatial trajectory and∗Fs is the desired contact force. Thus, the desired value of thecontact force can be planned by using the online trajectory
planning scheme described in the previous section.
As we described in the previous section, we use the force
tracking controller based on admittance control; the force
controller is implemented as an outer loop based on anunderlying velocity controller. In this study, we consider thecontact force exerted at the tip of the end effector, which
is given by Fs = [Fx , Fy , Fz ]. To track the planned contact
force, the force tracking controller determines the appropriate
reference velocity for the velocity controller. Here, we define
the error function e as follows:e = Fs − Fs∗(24)where Fs is the current value of the contact force. To design
the force tracking control scheme, we used the concept of thesliding mode control again. The control law to achieve therobust force tracking control in a linear system is expressed
as follows:
uf = û − ksat(e/e0 )(25)
where uf is the control input, û is the control input that
achieves ė = 0, sat(e/e0 ) is the saturation function defined
as in (19), k is a feedback gain, and e0 is a constant that
determines the boundary layer on the sliding surface.
In our force control framework, the desired contact force
Fs∗ is planned under the task condition ξc∗ , and the reference
velocity for the velocity controller, ξ˙sr , can be considered asa control input uf . When the contact object is modeled as aspring and the state of the system is given by the translational
position as ξspace = [x, y, z], the trajectory learned from thedemonstration should satisfy the following equation:∗Ḟs∗ = K demo ξ˙space,(26)

D. Force Tracking Control by learning both spatial motion
and contact force∗
where ξs∗ = [ξspace, Fs∗ ]T is the planned trajectory under the∗
given condition ξc , and K demo is the stiffness of the contact
object under the condition in which the demonstration was∗
performed. Assuming that (26) holds, û is replaced with ξ˙spacein (25). Hence, the reference velocity for the velocity controllerξ˙sr is determined as follows:Fs − Fs∗∗(27)
ξ˙sr = ξ˙space− ksate01) Control Scheme for Force Tracking Control: In thissection, we describe the force tracking control scheme fortasks that require force control such as cutting soft tissue andtying a knot. The execution of such a task involves two main

As in (17), the first term represents the feedforward input, andthe second term represents the feedback input. Although it
is known that the transition between motions without contact
to motions with contact can be unstable [45], the proposed

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

7control scheme does not require switching control schemes
for dealing with contacts.
2) Stability of the Proposed Force Tracking Control: The
robustness of the proposed force tracking control law in (27) isshown below. We introduce K test as the stiffness of the contact
object under the given test condition. We then assume that thedifference of the object stiffness between the test condition
and the demonstration condition is bounded as∗|(K test − K demo )ξ˙space| < ∆,

Z
Y(28)

where ∆ is a constant that indicates the upper bound of thedifference between the test condition and the demonstration
condition. From (27) and (28), the following relationship canbe obtained.

th

(29)

component of e = Fs − Fs∗ . When

where ei denotes the i|ei | > e0 , (29) is rewritten as:

1 d 2
e ≤ |ei | · ∆ − K test k|ei |2 dt i(30)

Hence, if we set k as k > ∆+ηK test , the condition (20) is satisfied.
When |ei | ≤ e0 , (29) is rewritten as:
1 d 2
e ≤ |ei | · ∆ − K test ke2i
2 dt i(31)

Therefore, if we set k as k > ∆+ηK test , the norm of the error
function |ei | monotonically decreases to K∆
test k . Thus, theproposed force tracking scheme enables robust tracking of
the planned force trajectory with a disturbance by setting ksufficiently large.
Obviously, (28) is rewritten as|K test − K demo | <∆∗|ξ˙space
|

.

(32)

Hence, if the stiffness of the contact object in the test scene
satisfies the condition of (32), the system robustly tracks theplanned contact force.
IV. E XPERIMENTS
We evaluated the developed system through experimentswith tasks that emulate tying a knot and cutting soft tissues.
First, we describe the experiment to evaluate the online trajectory planning scheme. Next, the performance of the proposed
force tracking scheme is discussed.
A. Evaluation of Trajectory Planning Scheme
1) Evaluation with a unimanual looping task: The firstexperiment was performed to evaluate the performance ofthe proposed trajectory planning scheme. In this experiment,
the system learned a unimanual DOUBLE LOOP task, which
involves making a loop around the instrument on the left witha thread held by an instrument on the right. This task was

X

(a)

X

(b)

1 d 2
ei = ei · ėi
2 dt∗= ei · (Ḟs,i − Ḟs,i
)




test
∗
∗
˙
= ei · Kξspace,i − ksat(ei /e0 ) − K demo ξ˙space,i≤ |ei | · ∆ − K test kei · sat(ei /e0 )

Z
Y

Z
Y

Z
X

Y

X

(c)
(d)
Fig. 4. Datasets for learning DOUBLE LOOP task: (a) dataset1, (b) dataset2,(c) dataset3, and (d) dataset4. Specific conditions for each dataset are shownin Appendix.

designed to demonstrate trajectory generation for a time- andspace-dependent task that required adaptation of the topological shape of the trajectory to the environmental conditions.
In this experiment, in this unimanual task, the state of thesystem is given by the position of the right instrument asξs = [xr , yr , zr ](33)

where [xr , yr , zr ] is the positions of the tip of the right
instruments. The task condition was defined as the positionof the left instrument, as given below :ξc = [xl , yl , zl ](34)

where [xl , yl , zl ] is the positions of the tip of the left instruments. The position of the left instrument was fixed for a single
demonstration. The transformation between the coordinates of
the right and left instruments was unknown. In this experiment,
we set N = 100 in (15).
We used the four sets of demonstrations shown in Fig. 4 totest whether the quality of the dataset affected the results. Nine
demonstrations each were performed for dataset1, dataset2,
and dataset3. Among these datasets, dataset1 had the largest
variance, while dataset2 had the smallest variance (Fig. 4).
Dataset4 includes all of the demonstrations used in dataset1,
dataset2, and dataset3. Specific conditions for each dataset are
shown in Table I - IV in the Appendix. The initial position of
the right instrument was the same throughout this experiment.
To test the performance of trajectory planning, we varied the
distance between the left and right instruments. The distancewas changed from 10 to 40 mm along the Z and Y axes.
To quantify the smoothness of the planned trajectories, we
computed... norm of jerk of the planned trajectory as,
PT the
1/T t=0 ξ s (t) .

100

xx [mm][mm][mm]
x [mm]

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

0
-100

0

500

1000

100
0
-100

0

100
0
-100

0

500

1000

0
-100

0

zz[mm][mm]zz [mm]

0

500

50

100

Time step

100

0

100

100

Time step

-100

50

Time stepyy[mm][mm][mm]
yy [mm]Time step

1000

100
0
-100

0

50

Time stepTime step

(a)

(b)

100

Fig. 5. Results of dataset1 (a) before and (b) after the DTW process. The
red thick lines in (b) represent the reference trajectory z.

The result of the DTW is shown in Fig. 5. The thick red line
in Fig. 5(b) represents the reference trajectory estimated by
the Kalman smoother. Although the demonstrated trajectories
contain temporal variations (Fig. 5(a)), the motion was synchronized with the reference trajectory after the DTW process(Fig. 5(b)). The distribution of the demonstrated trajectories
was modeled by GPR from the data shown in Fig. 5(b).
The trajectories planned in the simulation are visualized
in Fig. 6. The trajectories show that they were successfullyplanned under the given conditions when the dataset1 wasused as demonstration. On the other hand, when the dataset2
was used, the trajectories became unnecessarily wavy if thecondition was far from the demonstrated condition as ξc =[0, 0, 40] shown in Fig. 6 (h). Such unnecessary motions
are not preferable because they may lead to collisions withsurrounding objects during an actual surgery.
The jerk of the planned trajectories is shown in Figs. 7. As
the distance between the left and right instrument increased,
the jerk of the planned trajectory increased. This indicates thatthe quality of the estimated trajectory deteriorates when the
difference between the test condition and the demonstration
condition is large. As for the effects of variance of the training
dataset, the jerks of the planned trajectories were smaller thegreater the variance.
These are natural results because our framework considers
trajectory planning to be a regression problem. These results
indicate that the demonstration should be performed undersufficiently various conditions.
The average computation time for planning an entire trajectory when using a 64-bit machine with an Intel Core i7-4600U
CPU 2.1 GHz is shown in Fig. 8. The computation time wasnearly proportional to the number of demonstrated trajectories
given as inputs. Therefore, a trade-off exists between thecomputation time and quality of the planned trajectory. The
demonstrated trajectory is often sub-optimal and contains
unnecessary motions. If we have more demonstrations, unnecessary motions in the demonstrated trajectories are often

8canceled out, and we can estimate better trajectories. However,increasing the number of demonstrations leads to increase ofcomputational time for planning trajectories.
We performed trajectory planning under a dynamic condition to evaluate the proposed trajectory planning scheme. In
this experiment, the developed system performed the DOUBLE LOOP task autonomously according to changing task
conditions, and the spatial motion controller described in
Section III-C was employed. Dataset1 in Fig. 4 was used forlearning the DOUBLE LOOP task in this experiment. During
the experiment, the left instrument was moved to disrupt thetask execution while the system planned and updated the task
trajectory under the dynamic conditions. The left instrument
started moving approximately 4 seconds after the motion of
the right instrument started.
Figure 9 shows the experimental procedure of this experiment. The system performed the task successfully despite
the motion of the left instrument. The planned and executed
trajectories in this experiment are visualized in Fig. 10.
In Fig. 10(a), the displacement of the left instrument was[∆x, ∆y, ∆z] = [+12mm, −28mm, +36mm]. In Fig. 10(b),the displacement of the left instrument was [∆x, ∆y, ∆z] =[−23mm, +20mm, +44mm]. The trajectories were updated
approximately every 50 ms. During the task, the system
updated the trajectory 205 times in less than 11 seconds. The
figure shows that the trajectories were updated according to the
motion of the left instrument, and the right instrument tracked
the planned trajectories in a stable manner. Fig. 11 shows theplots of the same trajectories as in Fig. 10 over time. From
this figure, one can see that the trajectory converged to updated
trajectories smoothly and quickly. This quick convergence tothe updated trajectory shows the benefit of our tracking control
scheme.
2) Application Example: Knot Tying Task : To demonstrate
the capability of the proposed trajectory planning, we implemented autonomous knot tying in the developed system. The
developed system learned two bimanual motions: making aloop with a thread, and then grabbing and pulling the thread.
In the bimanual motion for looping, the relative position of
the two arms was given as the task condition:ξc = [xr − xl , yr − yl , zr − zl ](35)

We performed the demonstration of the bimanual motion formaking a loop nine times from the different initial positions.
In the bimanual motion for grabbing and pulling the thread,the position of the stage was tracked by the stereo camera andgiven as the task condition:ξc = [xcam , ycam , zcam ](36)

We placed a color marker on the stage that was visually
tracked by the KLT tracker implemented in OpenCV [46]. The
measurement from the visual tracking was smoothed by using
the Kalman filter. The motion for grabbing and pulling thethread was demonstrated nine times with the different stage
position.
The procedure of the autonomous knot-tying is shownin Fig. 12. In this experiment, spatial motion control described in Section III-C was employed. The developed system

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

9

Z
Y

X

(a)

(b)

(c)

(d)

Z
Y

X

5

5

4

4

3

dataset 1

2

dataset 2

1

dataset 3
dataset 4

0
10

20
30
Z [mm]

Jerk

Jerk

(e)
(f)
(g)
(h)
Fig. 6. Trajectory planned under test conditions. (a)–(d):Trajectories planned using dataset1. (e)–(h):Trajectories planned using dataset2. (a)and(e), ξc =[0, 0, 10]; (b)and(f), ξc = [0, 0, 20];(c)and(g), ξc = [0, 0, 30]; (d)and(h), ξc = [0, 0, 40]. Green dots represent the trajectory planned using the proposed
scheme. Red dots represent the trajectory demonstrated by a human operator under the same condition.

3

dataset 1

2

dataset 2

1

dataset 3
dataset 4

0
10

40

20
30
Y [mm]

(a)

40

(b)

Computation time [ms]Fig. 7. Jerk of planned trajectory as a function of distance between the left
and right instruments: (a) Z axis and (b) Y axis.
250
200
150
100
50
0

dataset 1

dataset 2

dataset 3

dataset 4

Fig. 8. Computation times for planning an entire trajectory to perform
DOUBLE LOOP task, using dataset1, dataset2, dataset3, and dataset4.

autonomously performed knotting by executing the learned
looping motion and grasping motion in sequence. In addition,by tracking the position of the object, the system planned andupdated the trajectory in real time, and grabbed the threadeven when the position of the thread was disturbed during theautonomous motions.
The trajectories planned and executed during the autonomous knotting is visualized in Fig. 13. As shown, thetrajectories were planned and updated during the motion for
grabbing the thread. The trajectory was updated 372 times inabout 20 seconds. This result indicates the capability of the
proposed scheme that enables planning the task trajectory ina sufficiently short time under dynamic conditions.
B. Evaluation of Online Trajectory Planning and Force Tracking Control
1) Performance of Proposed Force Tracking Control:
Here we evaluated the system in which the online trajectory
planning and robust force tracking control schemes were
integrated. The system learned the TOUCH-LINE task, which
involves touching a line depicted on the contact object witha certain contact force (Fig. 14). This task was designed toemulate the motion of cutting an organ by using a surgicalknife. The task requires the precision to touch along a straightline on the object and robustness to track the planned contact
force trajectory under disturbances. This task is time- andspace-dependent and difficult to describe as a single pointto-point task.
An operator demonstrated the TOUCH-LINE task ninetimes by controlling the slave manipulator from the mastersystem. The force exerted on the slave manipulator wasreflected on the master system, and the operator performed
the task based on the haptic feedback from the master system.
The force tracking control was implemented in the Z axis
indicated in Fig. 14(a). The position of the contact object wasvaried for each demonstration.
In this experiment, the state of the system is defined asfollows:
ξs = [xr , yr , zr , Fz,r ](37)
where [xr , yr , zr ] is the position of the right instruments, andFz,r is the force exerted on the tip of the right instrumentin the direction of the z axis. In this experiment, the force

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

1

2

10

3

4

Fig. 9. Procedure of the DOUBLE LOOP task under dynamic conditions. The left instrument was moved upward to disrupt the task execution. The plannedand executed trajectories are visualized in Fig. 10(a).

Z

Z

X

Y

Y

(a)

X

(b)

50

xx [mm][mm][mm]
xx [mm]Fig. 10. Visualization of the planned and executed trajectories. The red
and green dots represent motions executed by the left and right instruments,
respectively. The blue, purple, orange, and yellow dots represent trajectories
planned in this order during the experiment. The left instrument moved
differently between (a) and (b).

0
-50

-100

0

100

200

300

400

500

50
0
-50

-100

50
0
-50

0

100

200

300

0

100

400

zz [mm]zz [mm]
[mm]
200

300

500

0

100

200

300

400

500

400

500

Time step

0

100

400

50
0

500

100

0

300

100

Time step

-100

200

Time step

100

[mm]
yy [mm][mm]
y [mm]

Time step

400

500

100
0

-100

0

100

200

300

Time stepTime step

(a)

(b)

Fig. 11. Plots of trajectories over time. Green lines represent motions executedby the left and right instruments, respectively. Blue, purple, orange, and yellow
lines represent trajectories planned in this order during the experiment. (a) and(b) correspond to Fig. 10(a) and (b).

tracking control described in Section III-D was employed in
the direction of the z axis, and the spatial motion control
described in Section III-C was employed in the direction of
the x and y axis. The task condition is given asξc = [xr (0), yr (0), zr (0)],(38)

where [xr (0), yr (0), zr (0)] represents the initial position of the
tip of the right instrument. The demonstration was performed
nine times using different initial positions. The demonstrated
trajectories are visualized in Fig. 15. Specific conditions forthe demonstration is found in Table V in the Appendix. The
trajectories were generalized to new initial positions of the end

effector using the proposed trajectory planning method. The
performance of the force tracking control was then examined.
To examine the robustness of the proposed force tracking
scheme, we examined the system under three conditions:(a) the same as the demonstration; (b) a condition with adisturbance of the contact object stiffness (Fig. 14(b)); and (c)a condition with a disturbance of the contact object posture(Fig. 14(c)). We used a rubber sheet as the contact object,under which was placed a sponge in the demonstration. For
the condition with disturbance of the contact object stiffness,
we placed a steel plate between the rubber sheet and sponge(Fig. 14(b)). For the condition with disturbance of the contact
object posture, we placed another sponge under the stage to tilt
the contact surface (Fig. 14(c)). We performed the experimentwith two systems: the system that learns and controls both
spatial motion and contact force with the proposed scheme,
and the system that learns only spatial motion with theproposed trajectory planning scheme.
To measure the contact force at the tip of the instrument,strain gauges were attached as shown in Fig. 16. In practice,
it is not trivial to measure the contact force at the tip of asurgical instrument [47]. We used an experimental setup toallow for measuring the contact force in a stable manner.
Fig. 17 shows the results under the same condition as that ofthe demonstration, the condition with a disturbance of the contact object stiffness, and the condition with a disturbance of thecontact object posture, respectively. In these experiments, theinitial position of the end effector was set to ξc = [0, 0, 30]T ;hence, the initial position of the end effector was different fromthe demonstration. Because the demonstrated trajectories were
statistically modeled, the planned trajectories of the contact
force were smooth despite the noisiness of the measured
contact force in the demonstrations.
The system without force tracking control followed theplanned spatial trajectory regardless of the disturbance. Under
the same condition as the demonstration, the planned trajectoryof the contact force was roughly achieved by the system that
only learned the spatial motion (Fig. 17(a)) This indicatesthat it is reasonable to assume that (26) is satisfied. However,under conditions with a disturbance, the contact force largely
deviated from the planned trajectory in the experiment withthe system that learned only spatial motion (Fig. 17(b) and
Fig. 17(c)).
In contrast, the proposed system with the force tracking
control robustly tracked the planned trajectory of the contact
force even under conditions with disturbances. Under conditions with disturbance, the contact force deviated from theplanned contact force at the beginning of contact; however, thecontact force converged to the planned trajectory in a stable

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

11

1

2

3

4

5

6

Fig. 12. Experimental procedure for autonomous knot tying. The position of the stage was moved manually to disrupt the task execution.

X

Z

Z

Y

Y

(a)
(b)
Fig. 13. Visualization of trajectory planned and executed during the autonomous knotting: (a) trajectories for making a loop and (b) trajectories for
grabbing and pulling the thread. In (b), the green dots represent the executed
trajectory. The Blue, orange, and yellow dots represents the trajectories
planned in this order.

Z

X

Y

Z

X

X
Y
Fig. 15. Visualization of demonstrated trajectories for TOUCH-LINE task.
Specific conditions are shown in Appendix.

Strain gauges are attached
to measure the contact force

Fig. 16. Robotic instrument with force sensors. Strain gauges were attachedto the base of the rod.

(a)

(b)

(c)

Fig. 14. Setup for TOUCH-LINE task. The task involves pushing along theline indicated in (a); (b) shows the setup for experiment with a disturbancein the contact object stiffness; (c) shows the setup for experiment with adisturbance in the contact object posture.

manner (Fig. 17(b) and Fig. 17(c)). Following convergence, thesystem robustly tracked the planned contact force even when
the planned contact force began to decrease. These results
show that the proposed force tracking control scheme enables
robust tracking and planning of the contact force even underconditions with disturbances. In this experiment, the trajectory
does not change the directions where the position control isused because the disturbance from the contact object does notaffect the trajectory in the direction where the position control
is used.
2) Force Tracking Control with Online Trajectory Planning:
We performed online trajectory planning with force tracking
control under dynamic conditions. In this experiment, we
disturbed the position of the contact object during automatic
execution of the TOUCH-LINE task.

1
0.5
0
0 1 2 3 4 5 6 7 8 9-0.5Time [s]12

1.5Contact force [N]Contact force [N]Contact force [N]TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

1
0.5
0
-0.5

0 1 2 3 4 5 6 7 8 9Time [s]

3
2
1
0
-1

Planned force
Force tracking control
No force control

Planned force
Force tracking control
No force control

0 1 2 3 4 5 6 7 8 9Time [s]Planned force
Force tracking control
No force control

(a)
(b)
(c)
Fig. 17. Contact force in force tracking experiment under three conditions: (a)result under the same condition as the demonstration, (b)results under thecondition with disturbance of the contact object stiffness, and (c)results under the disturbance of the contact object posture.

1

2

3

4

5

6

Fig. 18. Experimental procedure for the TOUCH LINE task to evaluate online trajectory planning with the proposed force tracking control.

In this experiment, the position of the contact object was
tracked by a stereo camera with a KLT tracker implemented
in OpenCV. The measurement from the visual tracking wassmoothed by using Kalman filter. The state of the system wasgiven as the same as the previous experiment:ξs = [xr , yr , zr , Fz,r ].

X

Z

Y

(39)

We used the task condition given asξc = [xcam , ycam , zcam ],(40)

where [xcam , ycam , zcam ] represents the position of the contact
object in the stereo camera coordinates. The transformation
between the stereo camera coordinates and the robot coordinates was unknown. The demonstration was performed ninetimes with different positions of the contact object. The initial
position of the end effector was fixed in this experiment.
The experimental procedure is shown in Fig. 18. The
manipulator tracked the line on the contact object even though
the contact object was moved during the task execution. The
trajectory was updated 188 times in less than 11 seconds. This
indicates that the task trajectory was appropriately generalized
to the new conditions in real time.
The planned and executed trajectories during the experimentare visualized in Fig. 19. The trajectories were planned andupdated approximately every 50 ms according to the movement of the contact object. The system updated the trajectory

Fig. 19. Visualization of the planned trajectories and the executed trajectoryduring the experiment of online trajectory planning with force tracking control.
The green dots represent trajectory executed by right instrument. The blue,
orange, and yellow dots represent trajectories planned in this order during theexperiment.

188 times in approximately 11 seconds. The trajectory of thecontact force and the position of the manipulator are shown in
Fig. 20. Although the contact force is disturbed by the motionof the contact object, the planned contact force was robustly
achieved by the manipulator. This result indicates that the
proposed system is applicable to the online trajectory planningand force tracking control under dynamic task conditions.

1
0.8
0.6
0.4
0.2
0
-0.2 0 1 2 3 4 5 6 7 8 9Time [s]Measured force Planned force40
200-20 0 1 2 3 4 5 6 7 8 9
-40
-60
-80
Time [s]Position [mm]Contact force

TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

X

Y

(a)

13

The thread was not tightened sufficiently.

Z(b)Fig. 20. Data recorded in the experiment when the contact object was moving:(a) Contact force and (b) the position of the slave manipulator.
Z(a)

The thread was successfully tightened.
X

Y

Fy,r
X
Y

Z

Fig. 21. Experimental setup for thetightening-a-knot task.

Fig. 22. Example of the trajectorydemonstrated for the tightening-aknot task.

3) Application Example: Tightening a Knot: To demonstrate the performance of the force control scheme, we implemented an autonomous motion for tightening a knot. We
performed a motion for tightening a knot nine times fromthe different initial positions of two instruments (Fig. 21).
In the demonstration, the position of the thread held by the
instrument was fixed. In this experiment, the system learned
the force in one direction as shown in Fig. 21. The state of
the system was given as:ξs = [xr , yr , zr , xl , yl , zl , Fy,r ](41)

where Fy,r is the force exerted on the tip of the right instrumentin the direction of the y axis. The initial positions of the tipsof the right and left instruments are used as the task condition:ξc = [xr (0), yr (0), zr (0), xl (0), yl (0), zl (0)].

(42)

By using this representation, the system generalized the task
trajectories to different initial positions of the right and left
instruments. One of the demonstrations is visualized in Fig. 22.
In this experiment, the force tracking control was employed in
the direction of the y axis, and the spatial motion control was
employed in the direction of the x and z axis. The conditionsof the demonstrations are shown in the Appendix. In theseexperiments, the initial position of the end effector was set toξc = [0, 0, 0, 0, 0, 0]T .
To examine the robustness of the proposed scheme, we
varied the length of the thread and performed autonomous
tightening for several scenarios: (1)The thread was the samelength as that of the demonstration; (2)The thread was 10mm
shorter than that of the demonstration; and (3)The thread was
10mm longer than that of the demonstration.
The result of the experiment is shown in Figs. 23 and 24. As
shown, the system using the proposed force tracking control
apparently outperforms the system without force control. Despite the difference of the thread length, the proposed systemtightened the knot appropriately. Figure 24 shows the force
profile recorded in this experiment. The length of the thread(b)Fig. 23. Results of autonomous tightening under the condition where thethread was held to be longer than that of the demonstration: (a)Result without
force control, and (b)result with the proposed force control.

significantly affected the force profile during the task. If force
control is not implemented, the measured force was nearlyzero at the end of the task when the thread is longer thanthat of the demonstration, and when the thread is shorter, themeasured force largely exceeded the planned value at the endof the task.
In contrast, the force was appropriately controlled to be
the desired value at the end of the task regardless of thelength of the thread if the proposed force control method was
implemented. This result indicates that the proposed scheme
achieves robust tracking of the planned force profile even whenexperiencing disturbances.
V. D ISCUSSION
Our scheme enables online planning and robust tracking of
spatial motion and contact force based on demonstrations. In
addition, our force control scheme achieves robust tracking of
the planned contact force under dynamic conditions. Although
we updated the trajectories every 50-60 ms in the experiments,
it might not be necessary to update the trajectory as often
for simple tasks. However, we think that the faster updateof the trajectory leads to more adaptive behaviors of thesystem. Our approach solves the trajectory planning problem
as a regression problem. Although we use GPR to model the
distribution of the demonstrated trajectories, our framework is
not limited to specific regression methods. Sparse GPR and
GMR can be also used in our framework, thereby reducing
the computational costs [48], [49].
With regard to the use of GPR, the work by Calinon etal. described a method using GPR to retrieve the trajectory
parameters from task conditions [18]. In order to generalize
the motion to a new task condition, the method in [18] requires
two steps: 1) estimate the parameters of GMMs with GPR,0 1 2 3 4 5 6 7 8 9-0.5Time [s]Planned force
Force tracking control
No force control

00 1 2 3 4 5 6 7 8 9-0.5Time [s]Planned force
Force tracking control
No force control

14

Force [N]0Force [N]Force [N]TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

0
0 1 2 3 4 5 6 7 8 9-1Time [s]Planned force
Force tracking control
No force control

(a)
(b)
(c)
Fig. 24. Results of autonomous tightening: (a)Result under the condition where the thread was held to be the same length as that of the demonstration,(b)result under the condition where the thread was held to be shorter than that of the demonstration, and (c)result under the condition where the thread washeld to be longer than that of the demonstration.

and 2) generate a trajectory with the GMMs. On the contrary,
we use GPR to directly retrieve the state of the system fromthe task conditions. Since our approach directly estimates a
trajectory from a given task condition, the computational cost
will be less, which is suitable for online trajectory planning.
In the DOUBLE LOOP and TOUCH-LINE tasks, the relationship between task conditions and trajectories is nearly linear. For this reason, the trajectory distribution was successfullymodeled from only nine demonstrated trajectories in the experiments. When the relationship between task conditions andtrajectories is nonlinear, more demonstrations will be required.
In addition, task conditions in our experiments comprised threeor six dimensions. When the dimension of the task condition
increases, the system will require more demonstrations, which
in turn will lead to greater computational costs. For example,
if the orientation of the object needs to be additionally taken
into account, demonstrations with various object orientations
are necessary.
In prior work in [12], a spatio-temporal perturbation isreferred to as a perturbation that affects the length of a task,
which results in changes in both a spatial trajectory andtemporal dynamics. Pushing away a target object during atask is presented as an example of such a spatio-temporal
perturbation in [12]. The experiment presented in Section IV-A
shows the performance of our system under spatio-temporal
perturbations. Moving the target instrument away from thecontrolled instrument during the DOUBLE LOOP task is aspatio-temporal perturbation that affects both the spatial andtemporal dynamics of the movement. As shown in the experiment in Section IV-A, when a spatio-temporal perturbation was
introduced, our system adapted the behavior without unnatural
behaviors. In our framework, spatio-temporal perturbationis addressed using online trajectory planning and tracking
control. By updating a spatial trajectory on the fly, our system
plans natural spatial trajectory according to the change ofthe condition. Simultaneously, our tracking control adapts thevelocity and tracks the planned trajectory in a stable manner.
This adaptation of the velocity can be interpreted as theadaptation of the temporal dynamics to the updated trajectory.
Therefore, our system stably adapts its behavior even when aspatio-temporal perturbation is introduced.
In the step where we align the trajectory in the time domainusing DTW, the trajectory distribution is assumed to be a
Gaussian distribution in (8). Therefore, if the distribution of
the demonstrated trajectories does not follow this assumption,

it may be necessary to modify the method for aligning the trajectories in the time domain. When we compute the variance in(8), we assumed that every demonstration has equal relevanceto the reference trajectory. However, one can use the variancethat encapsulates the information about the task condition. One
simple way to implement this condition-related variance isto set the variance as Ri ∝ exp( ξci − ξctest ) for the given
context ξctest in (8). This extension needs to be investigated infuture work.
In Section IV-B, the performance of the force tracking
control is discussed. The results show that the proposed
scheme can track the planned trajectory of the contact force
robustly even under disturbances. The approaches describedin [30], [31] enable explicit force control, but they require
iterative learning to adapt the controllers for new situations.
In contrast to these approaches, our approach enables robust
control of the contact force without iterative learning. As
shown in the tightening-a-knot task, small changes of thecondition significantly affect the profile of the contact force,
especially when working with a flexible and non-stretchable
object such as a thread. Thus, the robustness shown in theexperiments are very important to perform such tasks that
require the force tracking control.
This study shows that the force tracking control with
online trajectory planning is potentially feasible by using the
proposed scheme. Additionally, it can be used for automationof surgical tasks such as tying a knot and cutting soft tissues.
Although experiments in this paper emulated surgical tasks,
the tested situation shown in this paper is still very simple
compared with real surgery. Experiments in a more realistic
situation should be performed in future work. One of the main
challenges for applying our framework to actual surgery is
sensing. Although our framework requires sensing the task
condition in real time, it is challenging to track a surgicalthread and measure the contact force at the tip of an instrument
during actual robotic surgery.
Regarding the application to actual clinical use, full automation of surgical task is not realistic at this time because supervision by human operators is essential to ensure the safety.
Several recent studies developed shared control frameworks
for teleoperation [50]–[53]. The method described in this paper
is potentially applicable to such frameworks. Although weapplied our method to automation of robotic surgery, ourmethod is also applicable to various applications. The results
shown here will contribute to various fields, which are notTRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

strictly limited to medical applications.
In our framework, the performance of the automated motions is dependent on the quality of the demonstrated motions.
In future work, we will investigate reinforcement learning
approaches, which would enable autonomous improvement of
the performance of automated motions [54], [55].
VI. C ONCLUSION
We presented a framework of online trajectory planningand force tracking control for automation of surgical tasks.
The proposed scheme enables the planning of trajectories for
spatial motion and contact force in real time under dynamic
conditions by leveraging demonstrations under various conditions. By learning the spatial motion and contact force simultaneously, the planned profile of the contact force can be tracked
robustly and stably. The proposed scheme enables performing
surgical tasks that require online trajectory planning and force
tracking control, such as cutting soft tissue and tying knots.
The robustness of the proposed force tracking control scheme
was analytically described.
The performance of the trajectory planning and force control
schemes was evaluated through experiments that emulatetying a knot and cutting soft tissues. In the experiments, thedeveloped system planned and updated both spatial trajectoryand force profiles under dynamic conditions. Additionally, theplanned contact force was tracked robustly against changes inthe stiffness and position of the contact object. Experimental
results indicate the feasibility of surgical task automation
under dynamic conditions using our framework.
A PPENDIX
Tables I–IV present the specific conditions of the demonstrations for DOUBLE LOOP task in Section IV-A. Table
V presents the specific conditions of the demonstrations for
TOUCH LINE task in Section IV-B1. Table VI presents the
specific conditions of the demonstrations for the tightening-aknot task in Section IV-B1.
ACKNOWLEDGMENT
This work was supported by JSPS Grant-in-Aid for Scientific Research(S) Number 23226006 and Grant-in-Aid for
JSPS Fellows Number 25-7106.
R EFERENCES[1] J. Nix, A. Smith, R. Kurpad, M. E. Nielsen, E. M. Wallen, and R. S.
Pruthi, “Prospective randomized controlled trial of robotic versus open
radical cystectomy for bladder cancer: Perioperative and pathologic
results,” European Urology, vol. 57(2), pp. 196–201, 2010.
[2] T. Osa, N. Sugita, and M. Mitsuishi, “Online trajectory planning in
dynamic environments for surgical task automation,” in Proceedings of
Robotics: Science and Systems (RSS), 2014.
[3] T. Osa, K. Harada, N. Sugita, and M. Mitsuishi, “Trajectory planningunder different initial conditions for surgical task automation by learningfrom demonstration,” in Proceedings of IEEE International Conferenceon Robotics and Automation (ICRA), 2014, pp. 6507–6513.
[4] J. van den Berg, S. Miller, D. Duckworth, H. Hu, A. Wan, X.-Y. Fu, K. Y.
Goldberg, and P. Abbeel, “Superhuman performance of surgical tasks
by robots using iterative learning from human-guided demonstrations,”in Proceedings of IEEE International Conference on Robotics andAutomation (ICRA), 2010, pp. 2074–2081.

15

TABLE IC ONDITIONS FOR DATASET 1
No.
1
2
3
4
5
6
7
8
9

xl
0
20
-20
20
-20
20
20
-20
-20

yl
0
20
20
-20
-20
20
-20
20
-20

zl
0
20
20
20
20
-20
-20
-20
-20

No.
1
2
3
4
5
6
7
8
9

TABLE IIIC ONDITIONS FOR DATASET 3
No.
1
2
3
4
5
6
7
8
9

xl
0
20
-20
20
-20
20
20
-20
-20

yl
0
20
20
-20
-20
20
-20
20
-20

zl
0
20
20
20
20
0
0
0
0

TABLE II
C ONDITIONS FOR DATASET 2
xl
0
10
-10
10
-10
10
10
-10
-10

yl
0
10
10
-10
-10
10
-10
10
-10

zl
0
10
10
10
10
-10
-10
-10
-10

TABLE IV
C ONDITIONS FOR DATASET 4

No.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
TABLE V

xl
0
20
-20
20
-20
20
20
-20
-20
20
20
-20
-20
10
-10
10
-10
10
10
-10
-10

yl
0
20
20
-20
-20
20
-20
20
-20
20
-20
20
-20
10
10
-10
-10
10
-10
10
-10

zl
0
20
20
20
20
0
0
0
0
-20
-20
-20
-20
10
10
10
10
-10
-10
-10
-10

C ONDITION FOR THE TRAINING DATASET FOR THE TOUCH LINE TASK INS ECTION IV-B1.
No.
1
2
3
4
5
6
7
8
9

xr (0)
0
20
20
-20
-20
20
20
-20
-20

yr (0)
0
20
-20
20
-20
20
-20
20
-20

zr (0)
20
20
20
20
20
40
40
40
40

[5] H. Mayer, F. Gomez, D. Wierstra, I. Nagy, A. Knoll, and J. Schmidhuber,“A system for robotic heart surgery that learns to tie knots using
recurrent neural networks,” in Proceedings of IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2006, pp. 543–
548.
[6] H. Mayer, I. Nagy, A. Knoll, E. Braun, R. Lange, and R. Bauernschmitt,“Adaptive control for human-robot skill transfer: Trajectory planning
based on fluid dynamics,” in Proceedings of IEEE International Conference on Robotics and Automation (ICRA), 2007, pp. 1800–1807.
[7] H. Mayer, I. Nagy, D. Burschka, A. Knoll, E. Braun, R. Lange, and
R. Bauernschmitt, “Automation of manual tasks for minimally invasive
surgery,” in Proceedings of International Conference on Autonomic andAutonomous Systems (ICAS), 2008, pp. 260–265.
[8] F. Nageotte, P. Zanne, C. Doignon, and M. de Mathelin, “Stitchingplanning in laparoscopic surgery: Towards robot-assisted suturing,”
International Journal of Robotics Research, vol. 28, no. 10, pp. 1303–
1321, 2009.
[9] J. Schulman, A. Gupta, S. Venkatesan, M. Tayson-Frederick, and
P. Abbeel, “A case study of trajectory transfer through non-rigid registration for a simplified suturing scenario,” in Proceedings of IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS),TRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

TABLE VI
C ONDITION FOR THE TRAINING DATASET FOR THE TIGHTENING - A - KNOTTASK IN S ECTION IV-B3.
No.
1
2
3
4
5
6
7
8
9

xl (0)
0.5
-14.2
8.8
15.3
-16.3
9.2
7.9
-4.2
-4.5

yl (0)
-0.5
2.0
2.1
1.9
1.5
8.9
3.8
1.1
6.6

zl (0)
0.5
-5.0
6.5
-12.0
7.3
-3.9
17.5
-2.9
13.2

xr (0)
0.5
10.4
-20.1
-24.3
10.7
5.2
7.5
11.2
-5.4

yr (0)
0.5
-8.3
-7.8
-7.3
-8.2
-16.1
-7.9
-9.5
-7.5

zr (0)
-0.5
4.5
-8.1
6.0
-11.2
-4.8
14.5
8.276
-3.7

2013, pp. 4111–4117.
[10] A. Murali, S. Sen, B. Kehoe, A. Garg, S. McFarland, S. Patil, W. Boyd,
S. Lim, P. Abbeel, and K. Goldberg, “Learning by observation for surgical subtasks: Multilateral cutting of 3d viscoelastic and 2d orthotropic
tissue phantoms,” in Proceedings of IEEE International Conference on
Robotics and Automation (ICRA), 2015, pp. 1202–1209.
[11] B. D. Argall, S. Chernova, M. Veloso, and B. Browning, “A survey ofrobot learning from demonstration,” Robotics and Autonomous Systems,
vol. 57, no. 5, pp. 469–483, May 2009.
[12] S. Khansari-Zadeh and A. Billard, “Learning stable nonlinear dynamicalsystems with gaussian mixture models,” IEEE Transactions on Robotics,
vol. 27, no. 5, pp. 943–957, 2011.
[13] S.-M. Khansari-Zadeh and A. Billard, “A dynamical system approach torealtime obstacle avoidance,” Autonomous Robots, vol. 32, pp. 433–454,
2012.
[14] J. Kober, A. Wilhelm, E. Oztop, and J. Peters, “Reinforcement learning
to adjust parametrized motor primitives to new situations,” Autonomous
Robots, vol. 33, no. 4, pp. 361–379, 2012.
[15] J. Schulman, J. Ho, C. Lee, and P. Abbeel, “Learning from demonstrations through the use of non-rigid registration,” in Proceedings of the
16th International Symposium on Robotics Research (ISRR), 2013.
[16] A. X. Lee, S. H. Huang, D. Hadfield-Menell, E. Tzeng, and P. Abbeel,“Unifying scene registration and trajectory optimization for learningfrom demonstrations with application to manipulation of deformableobjects,” in Proceedings of IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2014, pp. 4402–4407.
[17] B. C. da Silva, G. Baldassarre, G. Konidaris, and A. Barto, “Learningparameterized motor skills on a humanoid robot,” in Proceedings ofIEEE International Conference on Robotics and Automation (ICRA),
2014, pp. 5239–5244.
[18] S. Calinon, T. Alizadeh, and D. G. Caldwell, “On improving theextrapolation capability of task-parameterized movement models,” inProceedings of IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), 2013, pp. 610–616.
[19] A. Parachos, C. Daniel, J. Peters, and G. Neumann, “Probabilistic
movement primitives,” in Advances in Neural Information Processing
Systems (NIPS), 2013, pp. 2616–2624.
[20] T. Osa, G. E. A. M., R. Stolkin, R. Lioutikov, J. Peters, and G. Neumann,“Guiding trajectory optimization by demonstrated distributions,” IEEE
Robotics and Automation Letters, vol. 2, no. 2, pp. 819–826, 2017.
[21] M. T. Mason, “Compliance and force control for computer controlled
manipulators,” IEEE Transactions on Systems, Man and Cybernetics,
vol. 11, no. 6, pp. 418–432, 1981.
[22] N. Hogan, “Impedance control: An approach to manipulation,” inAmerican Control Conference, 1984, June 1984, pp. 304–313.
[23] D. E. Whitney, “Historical perspective and state of the art in robot force
control,” in Proceedings of IEEE International Conference on Robotics
and Automation (ICRA), 1985, pp. 262–268.
[24] D. Lawrence, “Impedance control stability properties in common implementations,” in Proceedings of IEEE International Conference on
Robotics and Automation (ICRA), 1988, pp. 1185–1190.
[25] H. Seraji, “Adaptive admittance control: an approach to explicit force
control in compliant motion,” in Proceedings of IEEE International
Conference on Robotics and Automation (ICRA), 1994, pp. 2705–2712.
[26] H. Seraji and R. Colbaugh, “Force tracking in impedance control,” TheInternational Journal of Robotics Research, vol. 16, no. 1, pp. 97–117,
1997.
[27] P. Evrard, E. Gribovskaya, S. Calinon, A. Billard, and A. Kheddar,“Teaching physical collaborative tasks: object-lifting case study witha humanoid,” in Proceedings of IEEE-RAS International Conference on
Humanoid Robots (Humanoids), 2009, pp. 399–404.

16[28] E. Gribovskaya, A. Kheddar, and A. Billard, “Motion learning andadaptive impedance for robot control during physical interaction withhumans,” in Proceedings of IEEE International Conference on Robotics
and Automation (ICRA), 2011, pp. 4326–4332.
[29] K. Kronander and A. Billard, “Learning compliant manipulation throughkinesthetic and tactile human-robot interaction,” IEEE Transactions on
Haptics,, vol. 7, no. 3, pp. 367–380, July 2014.
[30] J. Kober, B. Mohler, and J. Peters, “Learning perceptual coupling for
motor primitives,” in Proceedings of IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), 2008, pp. 834–839.
[31] Y. Chebotar, O. Kroemer, and J. Peters, “Learning robot tactile sensing
for object manipulation,” in Proceedings of IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2014, pp. 3368–
3375.
[32] A. Lee, H. Lu, A. Gupta, S. Levine, and P. Abbeel., “Learning forcebased manipulation of deformable objects from multiple demonstrations,” in Proceedings of IEEE International Conference on Robotics
and Automation (ICRA), 2015.
[33] L. Rozo, D. Bruno, S. Calinon, and D. G. Caldwell, “Learning optimal
controllers in human-robot cooperative transportation tasks with position and force constraints,” in Proceedings of IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2015, pp. 1024–
1030.
[34] J. J. E. Slotine and W. Li, Applied nonlinear control. Prentice Hall,
1991.
[35] J. Arata, H. Takahashi, S. Yasunaka, K. Onda, K. Tanaka, N. Sugita,
K. Tanoue, K. Konishi, S. Ieiri, Y. Fujino, Y. Ueda, H. Fujimoto,
M. Mitsuishi, and M. Hashizume, “Impact of network time-delay andforce feedback on tele-surgery,” International Journal for Computer
Assisted Radiology and Surgery, vol. 3, pp. 371–378, 2008.
[36] M. Mitsuishi, M. Hashizume, P. Navicharern, Y. Fujino, K. Onda,
S. Yasunaka, N. Sugita, J. Arata, H. Fujimoto, K. Tanimoto, K. Tanoue,
S. Ieiri, K. Konishi, and Y. Ueda, “A telesurgery experiment between
japan and thailand,” in Proceedings of International Conference on
Ubiquitous Robots and Ambient Intelligence (URAI), 2009.
[37] C. Ott, R. Mukherjee, and Y. Nakamura, “Unified impedance andadmittance control,” in Proceedings of IEEE International Conferenceon Robotics and Automation (ICRA), 2010, pp. 554–561.
[38] H. Sakoe and S. Chiba, “Dynamic programming algorithm for spoken
word recognition,” in IEEE Transactions on Acoustics, Speech andSignal Processing, A. Waibel and K.-F. Lee, Eds., San Francisco, CA,
USA, 1978, pp. 159–165.
[39] A. Coates, P. Abbeel, and A. Y. Ng, “Learning for control from multiple
demonstrations,” in Proceedings of the 25th International Conference on
Machine Learning (ICML). ACM, 2008, pp. 144–151.
[40] P. Abbeel, A. Coates, and A. Y. Ng, “Autonomous helicopter aerobatics
through apprenticeship learning,” International Journal of Robotics
Research, vol. 29, no. 13, pp. 1608–1639, Nov. 2010.
[41] S. Calinon, F. Guenter, and A. Billard, “On learning, representing, andgeneralizing a task in a humanoid robot,” IEEE Transactions on Systems,
Man, and Cybernetics, Part B: Cybernetics, vol. 37, no. 2, pp. 286–298,
2007.
[42] S. Calinon, F. D’halluin, E. Sauser, D. Caldwell, and A. Billard,“Learning and reproduction of gestures by imitation,” IEEE Robotics
Automation Magazine, vol. 17, pp. 44–54, 2010.
[43] D. A. Cohn, Z. Ghahramani, and M. I. Jordan, “Active learning withstatistical models,” Journal of Artificial Intelligence Research, vol. 4,
pp. 129–145, 1996.
[44] C. E. Rasmussen and C. K. I. Williams, Gaussian Processes for Machine
Learning. MIT Press, 2006.
[45] R. L. Williams, D. W. Repperger, J. M. Henry, and M. A. Murphy,“Naturally-transitioning rate-to-force control in free and constrained
motion,” Journal of Dynamic Systems Measurement and Control, vol.
121(3), pp. 425–432, 1999.
[46] [Online]. Available: http://opencv.org/[47] A. L. Trejos, R. V. Patel, and M. D. Naish, “Force sensing and
its application in minimally invasive surgery and therapy: A survey,”
Proceedings of the Institution of Mechanical Engineers, Part C: Journalof Mechanical Engineering Science, vol. 224, pp. 1435–1454, 2010.
[48] E. Snelson and Z. Ghahramani, “Sparse gaussian processes using
pseudo-inputs,” in Proceedings of Advances in Neural Information
Processing Systems (NIPS), 2005, pp. 1257–1264.
[49] L. Weruaga and J. Via, “Sparse multivariate gaussian mixture regression,” IEEE Transactions on Neural Networks and Learning Systems,
vol. 26, no. 5, pp. 1098–1108, 2015.
[50] K. Shamaei, Y. Che, A. Murali, S. Sen, S. Patil, K.Goldberg, andA. M. Okamura, “A paced shared-control teleoperated architecture forTRANSACTION ON AUTOMATION SCIENCE AND ENGINEERING, ACCEPTED IN FEBRUARY 2017.

supervised automationof multilateral surgical tasks,” in Proceedings ofIEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2015, pp. 1434–1439.
[51] N. Padoy and G. Hager, “Human-machine collaborative surgery using
learned models,” in Proceedings of IEEE International Conference on
Robotics and Automation (ICRA), 2011, pp. 5285–5292.
[52] K. A. Nichols and A. M. Okamura, “A framework for multilateralmanipulation in surgical tasks,” IEEE Transactions on Automation
Science and Engineering, vol. 13, no. 1, pp. 68–77, Jan 2016.
[53] F. B. Farraj, T. Osa, N. Pedemonte, J. Peters, G. Neumann, and
P. Giordano, “A learning-based shared control architecture for interactive
task execution,” in Proceedings of the IEEE International Conferenceon Robotics and Automation (ICRA), 2017.
[54] M. P. Deisenroth, G. Neumann, and J. Peters, “A survey on policy search
for robotics,” Foundations and Trends in Robotics, vol. 2, no. 1-2, pp.
1–142, 2013.
[55] J. Kober, J. A. Bagnell, and J. Peters, “Reinforcement learning inrobotics: A survey,” The International Journal of Robotics Research,
vol. 32, pp. 1238–1274, 2013.

Takayuki Osa received his Master’s degree anddoctoral degree in Mechanical Engineering from the
University of Tokyo in 2010 and 2015, respectively.
He worked for Terumo Corporation, Japan, from2010 to 2012. Currently, he is working as a postdoctoral fellow at Intelligent Autonomous Systems institute, Technische Universität Darmstadt, Germany.
His research interests include machine learning for
object manipulation, teleoperation, and intelligent
surgical systems.

Naohiko Sugita received a Master’s degree in Mechanical Engineering from the University of Tokyoin 1996. He worked for NEC, Japan, from 1996 to
2003. He became a research associate in 2003 andan associate professor in 2007 at the University ofTokyo. He received a doctoral degree in MechanicalEngineering in 2005 from the University of Tokyo.
His research interests include machining of biomaterials, robot-assisted surgical systems, and intelligent
manufacturing systems.

Mamoru Mitsuishi (M93) received his Master’sand D.E. degrees in Mechanical Engineering fromthe University of Tokyo, Tokyo, Japan, in 1983 and1986, respectively. In 1986, he was a lecturer at the
University of Tokyo, where he also held the post of
an associate professor in 1989; he has been a professor there since 1999. From 1987 to 1988, he wasa visiting researcher at the Fraunhofer Institute for
Production Technique and Automation, Germany.
His research interests include computer integrated
surgical systems and manufacturing systems.

17