2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

Macau, China, November 4-8, 2019

An Asynchronous Multi-Body Simulation Framework for Real-Time

Dynamics, Haptics and Learning with Application to Surgical Robots
Adnan Munawar and Gregory S. Fischer

Abstract— Surgical robots for laparoscopy consist of several for collecting da Vinci Research Kit (dVRK) manipulators
patient side slave manipulators that are controlled via surgeon and vision data, primarily for training learning agents by
operated master telemanipulators. Commercial surgical robots motion decomposition of sub-tasks is developed in [8].
do not perform any sub-tasks – even of repetitive or non-
invasive nature – autonomously or provide intelligent assistance. Recent developments in deep learning and AI have sparked
While this is primarily due to safety and regulatory reasons, the the interest of researchers from a variety of fields. Until very
state of such automation intelligence also lacks the reliability recently, the scope of deep learning algorithms were limited
and robustness for use in high-risk applications. Recent develop- to discretized problems, and thus, most real world control
ments in continuous control using Artificial Intelligence and Re- problems remained out of reach. However, the introduction
inforcement Learning have prompted growing research interest
in automating mundane sub-tasks. To build on this, we present of Deep Deterministic Policy Gradients (DDPG) model [9] –
an inspired Asynchronous Framework which incorporates real- an improvement over Deterministic Policy Gradients – broke
time dynamic simulation – manipulable with the masters of a new grounds while making the realization of smart agents
surgical robot and various other input devices – and interfaces for continuous control problems seemingly possible. These
with learning agents to train and potentially allow for the advancements led to the successful training of a simulated
execution of shared sub-tasks. The scope of this framework
is generic to cater to various surgical (as well as non-surgical) human rag-doll capable of running, jumping and avoiding
training and control applications. This scope is demonstrated obstacles [10]. Unsurprisingly, there has been an increase
by examples of multi-user and multi-manual applications which in the number of software libraries targeted for machine
allow for realistic interactions by incorporating distributed con- and reinforcement learning developed by the open source
trol, shared task allocation and a well-defined communication community. Many of these libraries provide Python API’s
pipe-line for learning agents. These examples are discussed in
conjunction with the design philosophy, specifications, system- and are capable of utilizing high-bandwidth system resources
architecture and metrics of the Asynchronous Framework and for faster training of data. Zamora et al. [11] present a useful
the accompanying Simulator. We show the stability of Simulator reinforcement learning toolkit catered towards mobile robots
while achieving real-time dynamic simulation and interfacing that employs such a Python interface for reinforcement
with several haptic input devices and a training agent at the learning by interconnecting the Gazebo simulator with Open-
same time.
AI’s GYM [12].
I. INTRODUCTION
Partial autonomy of sub-tasks has exciting prospects for
research aimed for the next generation of surgical robotics.
Research in this area focuses on assisting the surgeon in
accomplishing sub-tasks, thereby making the automation
passive in nature. Some notable research in this area includes
autonomous algorithms for performing soft-tissue suturing
[1], an automated approach for sinus surgery using computer
navigation techniques [2], characterization and automation
of soft-tissue suturing using a curved needle guide [3] and
automation of cutting/creasing sub-tasks while employing
learning by observation [4]. Additionally, [5] presents a Fig. 1: An overview of components selected for the Asynchronous Frame-
work for Assistive intelligence, simulation and collaborative control.
holistic approach to simplifying the task of manipulator
positioning prior to surgeon interaction, and [6] demonstrates
We propose the use-case of an Intelligent Agent for col-
a telemanipulated surgical simulation designed for heart
laborative control of real-time tasks, specialized for robotic
surgery. A trainable infrastructure is presented in [7] with
surgery. Given that we intend to assist the Master with
controllable dominance and aggression factors for automat-
collaboration rather than fully automatic control, we use
ing repetitive surgical tasks. Lastly, a shared infrastructure
the more appropriate term of Assistive Intelligence. The
coordination can range between two target types, (1) multi-
Adnan Munawar & Gregory S. Fischer are with the Department of sensory feedback to the Master – visual, haptic and tactile
Robotics Engineering, Worcester Polytechnic Institute, MA, 01609, USA – and (2) cooperative control of one or more slaves in
[amunawar, gfischer]@wpi.edu
This work is supported by the National Science Foundation through conjunction with user-controlled manipulators. We propose a
National Robotics Initiative Grant (NRI): IIS-1637759 framework to achieve both forms of assistance by providing

978-1-7281-4004-9/19/$31.00 ©2019 IEEE 6268

Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 the means to integrate modern surgical robots/haptics devices
with high fidelity asynchronous control, haptic feedback and
the implementation of a distributed asynchronous framework
for manipulating simulated dynamic bodies that allow for the
training of learning agents.
II. H IGH L EVEL S YSTEM A RCHITECTURE
An essential step towards the realization of the proposed Fig. 2: A visual representation of the Asynchronous Framework with
regards to the C++ AMBF Simulator where each simulated dynamic
Asynchronous Framework is the stable and robust control of object is represented as an afObject. The afObjects utilize independent
multiple input devices in the simulated dynamic environment. communication pipelines by exposing State/Command interfaces which
Section III-A discusses the challenge associated with this allow isolated control
multi-device control. The second, but equally important, step
is the selection of the right software components which is
driven either by the compatibility in our use-case (CISST- utilizes the communication interfaces exposed by the AMBF
SAW and dVRK – an open-source research kit based on the Simulator and complements it to provide robust and easy-to-
clinical da Vinci surgical robot – [13] [14]) or the popularity use interfaces for training on real-time dynamic tasks with
and adoption of the components in the research community. simulation in the loop haptic-feedback (Section III-E).
As such, the existing components which have been chosen III. IMPLEMENTATION DETAILS
to complement the Asynchronous Framework include CHAI-
3D [15], Bullet [16], Keras [17], Keras-RL [18], and Open- A. Asynchronous Control of Multiple Haptic Devices
AI’s GYM [12]. Figure 1 shows a holistic view of the Bullet Physics is used as a module in CHAI-3D to simulate
inclusion of the aformentioned components in Asynchronous dynamic bodies while using haptic/input devices to interact
Framework. with them. This built-in module is designed to interface a
The motivation behind the selection of each component is single haptic device while using a fixed time-step for the
presented briefly, starting with the two integral components, dynamic update. The fixed time-step allows for a stable
Bullet [16] and CHAI-3D [15]. Bullet’s Dynamics Engine performance, but it lacks accuracy as it does not track the real
is already used in some open-source robotics simulators, world clock. A realistic multi-manual task requires multiple
including Gazebo - the preferred dynamic simulator for the input devices interacting with each other and bodies in the
Robot Operating System (ROS) community. While Open simulation, all while maintaining a recommended haptic
Dynamics Engine (ODE) is another competitive physics update-rate (≥ 1kHz) and the dynamic simulation clock
library, Bullet provides a built-in collision detection library. in sync with the real-world clock. Achieving this setup is
CHAI-3D is an open-source library that supports a vast not trivial as the challenges are inherent to the implemen-
majority of commercial haptic devices and offers a device tations of rigid body dynamics using numerical integration
agnostic interface to applications rendered in Open-GL [19]. and constraint solving methods. Regardless of the dynamic
CHAI-3D lacks a built-in physics computation library but engine used, the overall simulation typically has an update-
has preliminary support for modules built around Bullet and step in which it applies the forces/constraints/collisions to
ODE. children objects and numerically integrates over the given
Keras [17] is chosen because of its compatibility with time-step. In a sense, this update-step synchronizes all the
modern libraries for training Neural Networks, and its ease dynamic bodies, and hence, all the body constraints require
of use. Keras-RL [18] is built to support Keras and provides resolution prior to each step.
the implementations of various Reinforcement Learning al- The computational time of each update-step depends on
gorithms. OpenAI’s GYM allows for the creation of envi- several internal, as well as external, (OS scheduling) factors.
ronments and agents that expose an action-state interface for The internal factors are mainly the magnitude of the time-
input-output and is the default frontend for utilizing Keras- step δt and the collision computation of high-density meshes
RL (and consequently Keras for training Neural Networks during contact. To keep in sync with the real world clock,
using TensorFlow). the time-step needs to be calculated simultaneously on each
The Asynchronous Framework is realized in an application update-step. Adding several input devices to simulation adds
called the Asynchronous Multi-Body Framework (AMBF) to the computational time, thereby increasing the duration of
Simulator which provides a dynamic-haptic simulation (ren- successive time-steps. Getting bounds on the time-step is not
dered by utilizing CHAI-3D’s interfaces and Bullet as the trivial in a non real-time OS and unbounded time-steps lead
dynamics solver) and allows for high-fidelity control via to cyclic deterioration.
robust inter-process communication interfaces. The AMBF Each haptic device is represented by a simulated dynamic
Simulator utilizes a ground-up design philosophy that al- end-effector (SDE) and is controlled using a dynamic control
lows for control of each dynamic object in an intuitive law. In a trivial implementation, all the devices are sequen-
asynchronous fashion (Figure 2). The details, discussions, tially read to calculate and apply the action forces on their
and metrics of the simulator are presented in Section III-B. SDEs, next, the simulated world is stepped forward by δt
Additionally, we present a Python Client (AMBF Client) that which updates the states of dynamic bodies. Finally, the

6269
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 To counteract these issues, an asynchronous control
scheme is implemented where the loop delays are isolated
from each other to prevent cyclic deterioration. A block
diagram representing this control scheme is shown in Figure
3. Implementation wise, the dynamic update-loop runs in a
separate thread and all of the haptic update-loops in separate
individual threads. Each input device owns a data-structure
which is shared to allow for asynchronous reads and writes.
Fig. 3: A block diagram depicting the Design of Asynchronous Control This data-structure maintains the device’s states and has
Scheme, the Simulated end-effectors and Devices maintain independent and fields to store the commanded forces. A similar, but non-
mutually exclusive Data Structures (DS) that are updated on successive identical, data-structure is defined for each SDE. The novelty
writes and are capable of asynchronous reads
in this implementation is the application of forces/commands
in dynamic and haptic threads, as the execution counters of
reactionary forces are applied to the corresponding input each thread are asynchronous by design. The difference be-
devices in the same time-step. tween the two control schemes is analyzed by experimenting
with a multi-manual task of grasping, picking and placing
objects and recording the dynamic and haptic update-rates.
In one example configuration, the framework is stressed
by simultaneously testing five haptic devices including two
Novint Falcons, a Geomagic Touch, and two master telema-
nipulators (MTMs) from (Intuitive Surgical Inc., Sunnyvale,
CA, USA). As shown in Figure 4(a), (c) in the sequential
implementation, the update-rate never meets the 1 kHz set-
(a) (b)
point. On the other hand, in Figure 4(b), and (d), the device
update-rates stay close to 1 kHz but the dynamic update-
rate can swing depending upon the collision computation
for high-density meshes during contact. The states and com-
mands are stored outside the haptic/dynamic update-loops
and are then used as “set-points” in the relevant threads to
prevent saturating the forces in both simulation and haptic
feedback loops.
(c) (d)
Fig. 4: Figure (a) and (b) show the haptic update-rate of 5 devices when B. Design of Asynchronous Framework (AMBF) Simulator
controlled ’sequentially’ vs ’asynchronously’, respectively. Figure (c) and
(d) show the corresponding rates for physics update-loops for ’sequential’ We used a design philosophy, motivated by several differ-
vs ’asynchronous’ control ent sources, which assimilates the concept of bodies in dy-
namic simulation as independent objects with self-contained
Controlling each device in task-space requires a large num- kinematic & dynamic properties, thereby mimicking real-
ber of matrix operations, including similarity transforms to world objects. This philosophy is to distinguish from the
enable hand-eye (camera) coordination and offset-transforms practical implementation where the simulated bodies are part
for clutch engaging/disengaging (presented in Section III- of an interconnected graph in a unified simulation and require
C and equation 4). The drivers for several commercial sequential updates. The goal of this design philosophy is to
devices – Geomagic Phantom/Touch from (3D Systems Corp, allow for asynchronous manipulation and control of each
Rock Hill, SC, USA) and Falcon (Novint Technologies Inc., simulated body independently. As a result, objects in the
NY, USA) – impose a delay while commanding forces to simulation are classified as either afObject or afWorld, where
restrict the update-rate. Tracker devices (such as Razer Hydra ‘af’ stands for ‘Asynchronous Framework’. An afObject is a
from (Razer Inc., CA, USA) operate at lower update-rates kinematic or dynamic rigid body which can have any number
(≤ 400Hz), and hence pose additional challenges. There- of movable parts (including 0). At their core both afObject
fore, the issue with the “sequential” implementation is that and afWorld have two interfaces for communication, utilizing
reading/writing multiple devices throttles the update-rate of afState / afCommand for state / command pair. These two
the dynamic and haptic feedback loops. Moreover, mixing interfaces implement a generic input-output design that is
devices with different update-rates makes the dynamic sim- easy to scale and communicate in parallel through an Inter
ulation unusable. This can be alleviated by withholding the Process Communication (IPC) medium (Figure 2).
force commands in the main loop and executing them con- The AMBF Simulator has a single world instance which
currently in a separate thread. However, while this improves is responsible for managing all the visual, kinematic and
the update-timing, it makes the devices and SDEs unstable dynamic objects. This world instance supports features such
due to a non-deterministic delay between the computation of as step-throttling, step-skipping and reporting metrics (dis-
control laws and the application of output forces. cussed in more detail in Section III-F).

6270
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 offset. These quantities are calculated from equations 3 and
5.

δx = xsn−1 + Rc (xh − xp+

h ) − xg (3)

Rs = Rsp− Rc (Rhp+ )T Rh RcT (4)

(z, δθ) = AxisAngle(RgT Rs ) (5)

(a) (b)
In equations 3 and 5, R denotes the rotation matrix. The
Fig. 5: These figures show the simulated end-effectors controlled by dVRK
Master with clutch/camera foot-pedals enabled. The clutch is used to move super-script for the rotation matrix is ignored when it is
the haptic device disengaged, and the camera foot-pedal is used to re-orient represented w.r.t. the world frame. Special use is made of
the view-direction without affecting the end-effector the superscripts p+ and p−. p+ denotes the event when the
clutch/camera button is pressed, and p− denotes the release
event. In this sense, Rhp+ is the recorded rotation matrix
when the camera/clutch button is pressed, whereas, Rsp−
is the simulated rotation recorded when the camera/clutch
button is released. The remaining subscripts are defined
as follows: l = linear, a = angular, s = simulated,
h = haptic device, c = camera and g = gripper.
The force feedback on the dVRK Masters is computed ac-
Fig. 6: This block diagram depicts a plugin based interface for dVRK cording to equation 6. The term (J(q)T )† Υ(q, q̇, q̈)ΠE is the
manipulators using ROS as an IPC. The ROS functionality is sealed in estimated gravity wrench and is appended to the computed-
the Arm Bridge Class whereas the ARM Interface exposes API for user
applications. wrench from the simulation. We have done previous work
on the development of a haptic interface for the dVRK
manipulators [20] for this purpose. The dVRK masters are
C. Integration of Constraints interfaced using a plugin called DVRK Arm demonstrated
in Figure 6 which abstracts the CISST-SAW [14] imple-
The AMBF Simulator may contain sets of constrained
mentation. This plugin based interface fully supports the
bodies connected via sliding or rotating joints. Additionally,
Asynchronous Framework and hides all ROS-functionality
realistic multi-manual simulations targeting surgical applica-
from the AMBF Simulator. The details of DVRK Arm are
tions may require grippers, forceps or retractors as simulated
beyond the scope of the current manuscript, but the source
dynamic end-effectors (SDEs). For such SDEs, the abstract
code can be found at [21].
control of the jaw angle is preferable over explicit joint
position or effort control. As a result, rather than providing
angular limits and independent joint control for the SDE’s, Fmtm = (J(q)T )† Υ(q, q̇, q̈)ΠE + Tsim
mtm
(−Fsim ) (6)
we use a more straightforward range between 0.0 and 1.0 for
fully closed and fully open SDE jaw position. Interpolation D. The Communication Medium
is used for the values in between. We experimented with shared memory and sockets as
Figure 5 shows a pair of SDEs which act as proxies for IPCs in Linux. The complexity involved at the cost of
dVRK MTMs and are controlled using a modified PD control communication speed was not justified in creating a scal-
law Fsim = [f ; η], where: able solution for shared memory. Socket communication,
although relatively slower, is scalable and provides similar
f = Kl δxts + Bl δ 2 x/dtd (1)
implementations across all dominant Linux flavors and even
η = (Ka δθts + Ba δ 2 θ/dtd )z (2) other operating systems and programming languages. It does,
however, require data-serialization and de-serialization. In
Here f and η are the force and torque, while K and B addition to enjoying substantial community support, ROS
dt
are Stiffness and Damping coefficients. The term ts = dtfd [22] has the ability for serialization and socket communi-
enables us to scale the time-step for asynchronous control cation together and thus, made it an appropriate candidate
by taking the fraction of custom fixed time-step (dtf ) by the for our Asynchronous Framework.
dynamic time-step (dtd ). The control law outputs a spatial Each afObject (and the single instance of afWorld) utilizes
wrench which is added at each dynamic update-step of the a plugin for afObjComm (afWorldComm in afWorld’s case)
physics simulation. Since the output wrench is added to the while using ROS as a middleware. The communication
existing external forces on a simulated body, ts prevents plugins use the same thread as their owners, and unlike other
the saturation of external forces for slower update-rates. solutions for independent nodes over a distributed network
Conveniently, it is often the case that, ts = 1 (such as the (ROS Nodelets http://wiki.ros.org/nodelet), the
dynamic simulation running at intended speed). δx and δθ AMBF Simulator uses a single node and distributes/isolates
are the linear and angular offsets, and z is the axis of angular the callbacks using custom callback queues. This isolation

6271
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 provides the flexibility to launch the communication in- Ena Throttle is used to control the flow of simulation based
stances from within the AMBF Simulator without the need on the toggle of Clock. The Jump Steps is the number of
for ros-launch files. steps the simulation must take between each clock toggle
Each communication instance owns a WatchDog timer (event). The requirement for throttling the simulation comes
which has a primary and a secondary function for data from the action-reward pair for the valid Markov States in
transmission control. The primary function of the Watch- RL problems. This requirement mandates the states to have
dog is to reset the afCommand if the timing condition – associated rewards which are meaningless if the simulation is
the invocation frequency of the afObjComm/afWorldComm not throttled between the update-steps of training (forward
callback – is not met. This keeps the asynchronous control and backward pass of the Neural Network). Server Time
safe for physical devices connected to AMBF Simulator. is the time (to nano secs precision) of the clock running
The Watchdog timer is re-initiated once a stream of new in AMBF Simulator, and the Sim Time is the time of the
commands starts flowing in. The secondary purpose of the inner-clock of dynamic simulation. This time is incremented
watchdog timer is to limit the publishing frequency of at each iteration of the dynamic solver such that:
afStates to lower values if the watchdog timer expires, thus
reducing the use of computing resources. n−1
tnsim = tsim + dtd (7)
E. The Python Client
Since the dynamic update-loop (Dyn Freq) runs asyn-
As discussed in section I, many of the popular libraries for chronously without any real-time constraints, using a fixed dt
learning and training agents have Python interfaces (Keras, causes time dilation between the wall (world) and simulation
GYM, Tensorflow/Theano, and Keras-RL). In alignment with clock (shown in Figure 8a with one input device and dynamic
these preferred interfaces, we present a stand-alone Python time-step dt = 0.001). The reason for this dilation is evident.
client that complements the AMBF Simulator. This client is Lacking a real-time kernel and custom sleep function makes
capable of creating callable instances of afObjects and af- it harder to meet the desired frequency which shifts the two
World (using ROS Communication) which are isolated from clocks. Even with a real-time kernel, the start-up time for
one another to reduce communication and computational initializing haptic devices can throw off the simulation clock.
overheads. We outlined various specifications that prioritize Moreover, the nature of collision computation techniques in
robustness in handling load. These design specifications are physics simulation libraries makes the computational time
intended for real-time training on data as well as closed-loop variable, and in effect, non-deterministic as it depends on the
control by accounting for the communication overheads and varying number of bodies in contact and their geometries.
slower execution speeds of Python applications. Based on
these specifications, the Python Client uses data sequencing
techniques and payload time-stamps to keep track of states,
actions and rewards. The consequence of the design speci-
fications is reflected not only in the Python Client itself but
also in the AMBF Simulator and the Payload Types (Section
III-F). A block diagram representing the Python Client is
shown in Figure 7.
For safety reasons, each callable instance of afObect and
afWorld in the client inherits a WatchDog timer which
enforces command resetting if the timing condition fails. The
Python Client is capable of throttling the dynamic update-
Fig. 7: The Python Client communicates with the AMBF Simulator using
loop of the AMBF Simulator, in which case, it provides a ROS as a middle-ware, AMBF ENV retrieves the requested handles for
clock to step the dynamic update-loop. This clock is provided objects from Python Client and provides a GYM compatible interface
using “Clock” field in the afCommand message for afWorld
and number of jump steps can be set to > 1. All this is
done automatically by the Python Client as the user/training
agent sets the corresponding parameters at run-time.

F. Online Training and Control and Associated Challenges

Based on the considerations in Section III-E, the Commu-
nication Payloads for online training and control are designed
accounting for the communication overheads and the slower
execution speeds of Python applications. In this regard, the
contents of afObjects and afWorlds communication payload (a) (b)
are shown in table I. The naming convention is designed to Fig. 8: (a) Time dilation between Application Clock & Simulation Clock
be self-explanatory, however, some fields pertaining to the using fixed time-step (dt=0.001) (b) Time tracking between Application
scope of this manuscript are explained. The message field Clock & Simulation Clock using dynamic time-step

6272
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 TABLE I: afObjects and afWorlds payloads for closed loop control and
training via online data

afWorld afObject
afState afCommand afState afCommand
- - Base Frame Base Frame
Msg Num - Msg Num Msg Num
Server Client Time Server Time Client Time
Time
Sim Time - Sim Time -
Num Devs Ena Throttle Name Ena Pos Ctrl
Dyn Freq Clock Mass & Inertia Pose
- Jump Steps Transform Wrench
- - Children[] Pos Ctrlr Mask[]
- - Joint Positions[] Joint Cmds[]

Fig. 9: Reponse of haptic controllers with degrading dynamic-loop’s

The necessity of using the dynamic time-step is that frequency
the simulation can run at any frequency while keeping the
two clocks synchronized. Figure 8b depicts this behavior.
It is important to mention the case where the dynamic- the Asynchronous Framework for Assistive Intelligence and
loop’s frequency drops to < 100Hz, this mostly occurs Control, we analyzed the response of haptics and control of
during collision computation for a relatively high number the Simulated Dynamic end-effectors (SDEs) as the dynamic
of objects during contact or explicit throttling by learning simulation slows down. Since such a scenario is difficult to
agents using the Ena Throttle. In such cases, the time- reproduce for performance metrics without careful prepara-
step is still calculated dynamically, however, the number of tion, we explicitly throttled the dynamic-loop’s frequency by
sub-iterations the dynamic solver is allowed to progress at using the “step throttling” functionality discussed in Section
once needs extra attention. Bullet uses 3 parameters which III-F. Next, to generate consistent input motion profiles, we
include the time-step δt, maximum number of sub-iteration use mock devices by exploiting the DVRK Arm plugin
Nmax and the default integration time-step δti . The goal is interface (shown in Figure 6) to spawn two input devices
to interpolate rather than recalculate the motion for δt if: (MTM-R and MTM-L) in the AMBF Simulator. We then
generated a customizable trajectory of the form:
δt < δti × N ; N ∈ Z+ & N ≤ Nmax (8)
Ideally, δt should not exceed δti Nmax , but there is no Pinput = Pof f + [ap sin(tc t), bp cos(tc t), cp sin(tc t)]′ S (9)
guarantee this will not occur. Therefore, in order to have
an accurate motion calculation for the degraded dynamic- In the above equation Pinput is the commanded position
loop’s frequency, Nmax needs to be updated accordingly. of the Input Device while the R.H.S consists of offset Pof f ,
However, increasing Nmax also increases the computational time constant tc , scale S, system time t and ap , bp , cp , which
time which leads to circular deterioration. Finding the right are the major/minor axes but in a 3 Dimensional space.
balance between Nmax and δt is challenging. However, to In order to generate a high velocity, during our testing
mitigate this limitation, Nmax is capped at a soft maximum. procedures, we set tc = 4.0, S = 0.1m and ap = 1, bp =
Another proposed solution to avoid the degradation of 1, cp = 2. A script systematically throttles the dynamic-
the dynamic update-loop’s frequency is to use collision loop’s frequency and records the controllers’ performance
primitives. Relatively advanced shapes can be created using as the magnitude of error from set-point. Figure 9 shows the
a compound of various collision primitives. Primitive shapes output of the controllers performance for n = 5000 readings.
have the advantage of utilizing implicit collision techniques It is evident that controllers’ response only begins to suffer as
rather than the costlier computation using state of the art the dynamic-loop’s frequency falls below 60 Hz. The reason
algorithms such as Minkowski Distance and Gilbert Johnson we are using the controller performance as such for haptic
Keerthi (GJK) algorithm [23]. Implicit collision computation response is due to the shared-data structures discussed in
is significantly faster and more reliable than explicit collision Section III-A. For the force-feedback computation for both
techniques, especially for a low-frequency dynamic update- the device and SDEs, we only used the error from their set-
loop. However, since creating collision primitives can be points. The remaining control schemes, such as the gravity
undesirable for complex shapes, mesh decimation techniques compensation, were added modularly from the device drivers
may be preferred. or distributed controllers that were running in conjunction
with the dynamic update-loop.
IV. R ESULTS AND D ISCUSSIONS Next, we demonstrate an example for manipulating and
A PC setup consisting of an Intel(R) Core(TM) i7-3770 solving a complex puzzle using two users controlling a
CPU (3.40GHz), Fujitsu 32 GB DDR3 RAM (1333 MHz) pair of devices each (Figure 10). Two devices had haptic
and an Nvidia GTX 1060 (8 GB RAM) GPU running Ubuntu feedback (dVRK MTMs visible as PIP on the top right) while
18.04 was used for the demonstration of results. To test the remaining two were simply tracker devices with lower

6273
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 Fig. 10: These sub-figures show the progression (left to right) of a bi-manual task using the AMBF Simulator. The Two end-effectors holding the green
multi-link puzzle piece are controlled by dVRK Masters (shown as Picture in Picture on top right) and the other two end-effectors are controlled via Razer
Hydra (shown as Picture in Picture on top left)

Figures 10 show the progression of a sub-task that involves

manipulation of a multi-link puzzle. The multi-link puzzle
requires at least two inputs to be lifted and placed on the Puz-
zle Base. This is followed by the Single link Puzzle pieces
being placed on top. All of the four simulated end-effectors
can interact with each other and the remaining puzzles. The
close-loop constraint formed by the multi-link Puzzle is felt
Fig. 11: Communication speed of several afObjects for an overloaded by the dVRK Masters which constrains the range of motion,
dynamic environment. The desired communication frequency is set to 2
kHz Dynamic-Loop’s Frequency ∼ 300 Hz, afObjComm frequency ∼ 2
thus allowing better control and manipulation.
kHz Requirements that drive the design guidelines of the puzzle
pieces are multi-manual manipulation, moderately complex
maneuvering and ease of grasping. Given that the goal of
update-rates (PIP on the top left). The puzzle involved several this study was to demonstrate the flexibility and utility
pieces including a multi-link Puzzle (Green Plate with Or- of the Asynchronous Framework, the puzzles utilized here
ange Handles), a Puzzle Base (Yellow Mesh) and three single were not developed formally for a particular clinial sub-
link rigid body puzzles which included the Triangle Puzzle task. Moreover, designing these types of puzzles based on
(Green cylindrical shape), Square Puzzle (Blue cylindrical any specification is trivial using Solidworks / Blender or any
shape) and the Circle Puzzle (Red cylindrical shape). The mesh creation software. The corresponding lower-resolution
Puzzle base and the multi-link Puzzle had a matching set collision meshes were generated using mesh-decimation
of extrusions and holes respectively, while the three rigid techniques. The lower resolution collision meshes are helpful
body puzzles had intruded cuts to match the three extrusions for maintaining higher frequency dynamic update-loop. In
of the puzzle base. It is important to note that all of the this study we used CPU for Physics computation and a GPU
grasping interactions in the simulation were purely dynamic. for graphics processing. The addition of Open-CL (Open
Hence, they involved a combination of friction due to contact Compute Language) for both rigid-body solvers and collision
geometry, grip force, slip, and slide. We did not use any computation is a future goal.
simplification techniques for appending the grasped object For the purpose of training neural networks and agents
as a fixed body to the end-effector link. While this dynamic for assistive intelligence, we discuss the interfaces exposed
grasping helps provide a natural feel by allowing gripping by the Python Client (Section III-E). The focus here is the
slack, it makes puzzle solving more challenging. ease of creating learning agents using tools such as GYM,
Keras, TensorFlow/Theano and Keras-RL from the Python
Client and not result of the trained models. The Python
Client exposes compatible interfaces for Keras since each
dynamic body in the simulation can be probed using afState
and manipulated using afCommand. While it is trivial to use
a limited number of dynamic objects (shown in Figure 10)
for training NN or RL agents, we show the possibility of
training a larger number of simulated bodies. In Figure 11,
200 dynamic boxes were added to the AMBF Simulator.
ROS introspection tools were used to probe the frequency
of afState for a few boxes with the desired communication
frequency set to 2 kHz. Due to the excessive load on the
Fig. 12: (a) Histogram of the time difference between the embedded time of
a received packet and the current time for synchronous communication using AMBF Simulator, the dynamic-loop’s frequency dropped to
Step Throttling (b) Difference between embedded times of consequently around 300 Hz. However, the communication speed for all
received packets (green dots) vs the time they are read (red dots). afObjects was ∼ 2kHz as shown in Figure 11.

6274
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.
 An essential feature of the Python Client is controlling Proceedings. ICRA’02. IEEE International Conference on, vol. 4.
the AMBF Simulator synchronously for satisfying Markov’s IEEE, 2002, pp. 3769–3774.
[6] R. Bauernschmitt, E. U. Schirmbeck, A. Knoll, H. Mayer, I. Nagy,
action-reward pair property under the umbrella of the Asyn- N. Wessel, S. Wildhirt, and R. Lange, “Towards robotic heart surgery:
chronous Framework. Immediate feedback is difficult to Introduction of autonomous procedures into an experimental surgi-
achieve in a distributed architecture since it involves delay cal telemanipulator system,” The International Journal of Medical
Robotics and Computer Assisted Surgery, vol. 1, no. 3, pp. 74–79,
due to (1) round-trip communication of packets and (2) pro- 2005.
cessing time in between for computing kinematics, dynamics [7] K. Shamaei, Y. Che, A. Murali, S. Sen, S. Patil, K. Goldberg, and
and updating packet data. The latency caused by the round- A. M. Okamura, “A paced shared-control teleoperated architecture
for supervised automation of multilateral surgical tasks,” in Intelligent
trip of data is presented in Figure 12(a) and (b). As illustrated Robots and Systems (IROS), 2015 IEEE/RSJ International Conference
in Figure 12 (b), the bottleneck is caused by the execution on. IEEE, 2015, pp. 1434–1439.
speed of Python. This jitter is expected as a result of the [8] T. D. Nagy and T. Haidegger, “An open-source framework for surgical
subtask automation,” Robotics and Automation (ICRA) Workshops,
longer queue sizes. Hence, the latency increases as newer 2018 IEEE International Conference on, 2018.
data needed to wait in a queue while the program execution [9] T. P. Lillicrap, J. J. Hunt, A. Pritzel, N. Heess, T. Erez, Y. Tassa,
processed older data. For synchronous control, however, we D. Silver, and D. Wierstra, “Continuous control with deep reinforce-
ment learning,” arXiv preprint arXiv:1509.02971, 2015.
are concerned with the latest data, and thus, the queue-size [10] N. Heess, S. Sriram, J. Lemmon, J. Merel, G. Wayne, Y. Tassa, T. Erez,
is set to 1. The limited queue-size helps to drive down the Z. Wang, S. Eslami, M. Riedmiller, et al., “Emergence of locomotion
round-trip communication latency to ≤ 0.001secs for 2 kHz behaviours in rich environments,” arXiv preprint arXiv:1707.02286,
2017.
of communication speed (Figure 12(a)). [11] I. Zamora, N. G. Lopez, V. M. Vilches, and A. H. Cordero, “Extending
With regard to the evolution of the Asynchronous Frame- the openai gym for robotics: a toolkit for reinforcement learning using
work, we intend on integrating flexible body dynamics ros and gazebo,” arXiv preprint arXiv:1608.05742, 2016.
[12] G. Brockman, V. Cheung, L. Pettersson, J. Schneider, J. Schulman,
and visualizations to create realistic surgical training ap- J. Tang, and W. Zaremba, “Openai gym,” 2016.
plications using simulated body tissues, organs, cloths and [13] A. Deguet, R. Kumar, R. Taylor, and P. Kazanzides, “The cisst libraries
threads. The foreseeable challenges include the complexity for computer assisted intervention systems,” in MICCAI Workshop
on Systems and Arch. for Computer Assisted Interventions, Midas
of implementation, stability, performance, dynamic-update Journal, vol. 71, 2008.
to track real-world clock and manipulation using haptic [14] P. Kazanzides, Z. Chen, A. Deguet, G. S. Fischer, R. H. Taylor,
devices. Moreover, new guidelines for the communication and S. P. DiMaio, “An open-source research kit for the da vinci R
surgical system,” in 2014 IEEE International Conference on Robotics
interfaces afState-afCommand need to be developed. Lastly, and Automation (ICRA). IEEE, may 2014.
the inclusion of flexible body dynamics should employ a [15] C. et al., “The CHAI libraries,” in Proceedings of Eurohaptics 2003,
generic framework design to allow for universal adaptation Dublin, Ireland, 2003, pp. 496–500.
[16] E. Coumans, “Bullet physics simulation,” in ACM SIGGRAPH 2015
as opposed to a more targeted application. Courses, ser. SIGGRAPH ’15. New York, NY, USA: ACM, 2015.
In this manuscript, we discussed the motivation behind [Online]. Available: http://doi.acm.org/10.1145/2776880.2792704
the design philosophy of an Asynchronous Framework for a [17] C. et al., “Keras,” https://github.com/keras-team/keras, 2015.
[18] M. Plappert, “Keras-rl,” https://github.com/matthiasplappert/keras-rl,
distributed application that is intended for training learning 2016.
agents via real-time input from a dynamic-haptic simulation. [19] D. Shreiner and T. K. O. A. W. Group, OpenGL Programming Guide:
The entire framework is available at the public repository The Official Guide to Learning OpenGL, Versions 3.0 and 3.1, 7th ed.
Addison-Wesley Professional, 2009.
[24]. The challenges to such an implementation are discussed [20] A. Munawar and G. Fischer, “Towards a haptic feedback framework
throughout the text, and we have concluded with the perfor- for multi-dof robotic laparoscopic surgery platforms,” in Intelligent
mance analysis of the proposed Asynchronous Framework. Robots and Systems (IROS), 2016 IEEE/RSJ International Conference
on. IEEE, 2016, pp. 1113–1118.
[21] A. Munawar, “Plugin based Interface for the da Vinci Research Kit
R EFERENCES (dVRK) MTMs,” https://github.com/WPI-AIM/dvrk arm, 2016.
[22] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs,
[1] A. Shademan, R. Decker, J. Opfermann, S. Leonard, A. Krieger, and R. Wheeler, and A. Y. Ng, “Ros: an open-source robot operating
P. C. W. Kim, “Supervised autonomous robotic soft tissue surgery,” system,” in ICRA workshop on open source software, vol. 3, no. 3.2.
Science Translational Medicine, vol. 8, pp. 337ra64–337ra64, 05 2016. Kobe, Japan, 2009, p. 5.
[2] K. Bumm, J. Wurm, J. Rachinger, T. Dannenmann, C. Bohr, [23] M. Sagardia, T. Stouraitis, and J. L. e Silva, “A new fast and robust
R. Fahlbusch, H. Iro, and C. Nimsky, “An automated robotic approach collision detection and force computation algorithm applied to the
with redundant navigation for minimal invasive extended transsphe- physics engine bullet: Method, integration, and evaluation,” in Prof.
noidal skull base surgery,” Minimally invasive neurosurgery : MIN, of the Conf. and Exhibition of the European Association of Virtual
vol. 48, pp. 159–64, 07 2005. and Augmented Reality (EuroVR), 2014, pp. 65–76.
[3] S. Sen, A. Garg, D. V. Gealy, S. McKinley, Y. Jen, and K. Goldberg, [24] A. Munawar, “The Asynchronous Multi-Body Framework,” https://
“Automating multi-throw multilateral surgical suturing with a mechan- github.com/WPI-AIM/ambf, 2019.
ical needle guide and sequential convex optimization,” in Robotics and
Automation (ICRA), 2016 IEEE International Conference on. IEEE,
2016, pp. 4178–4185.
[4] A. Murali, S. Sen, B. Kehoe, A. Garg, S. McFarland, S. Patil, W. D.
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 Robotics and Automation (ICRA),
2015 IEEE International Conference on. IEEE, 2015, pp. 1202–1209.
[5] A. Krupa, J. Gangloff, M. de Mathelin, C. Doignon, G. Morel, L. Soler,
J. Leroy, and J. Marescaux, “Autonomous retrieval and positioning of
surgical instruments in robotized laparoscopic surgery using visual
servoing and laser pointers,” in Robotics and Automation, 2002.

6275
Authorized licensed use limited to: LAHORE UNIV OF MANAGEMENT SCIENCES. Downloaded on July 19,2023 at 18:06:39 UTC from IEEE Xplore. Restrictions apply.