# “Don’t forget to put the milk back!”
Dataset for Enabling Embodied Agents to Detect Anomalous Situations

James F. Mullen Jr,1,2 Prasoon Goyal,1 Robinson Piramuthu,1 Michael Johnston,1
Dinesh Manocha2, and Reza Ghanadan1 1Amazon Science {
prasog,robinpir,mjohnstn, ghanadan} @amazon.com2University of Maryland
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Home robots intend to make their users lives easier. Our work assists in this
goal by enabling robots to inform their users of dangerous or unsanitary
anomalies in their home. Some examples of these anomalies include the user
leaving their milk out, forgetting to turn off the stove, or leaving poison
accessible to children. To move towards enabling home robots with these
abilities, we have created a new dataset, which we call SafetyDetect. The
SafetyDetect dataset consists of 1000 anomalous home scenes, each of which
contains unsafe or unsanitary situations for an agent to detect. Our approach
utilizes large language models (LLMs) alongside both a graph representation of
the scene and the relationships between the objects in the scene. Our key
insight is that this connected scene graph and the object relationships it
encodes enables the LLM to better reason about the scene — especially as it
relates to detecting dangerous or unsanitary situations. Our most promising
approach utilizes GPT-4 and pursues a categorization technique where object
relations from the scene graph are classified as normal, dangerous,
unsanitary, or dangerous for children. This method is able to correctly
identify over 90% of anomalous scenarios in the SafetyDetect Dataset.
Additionally, we conduct real world experiments on a ClearPath TurtleBot where
we generate a scene graph from visuals of the real world scene, and run our
approach with no modification. This setup resulted in little performance loss.
The SafetyDetect Dataset and code will be released to the public upon this
papers publication.

##  I Introduction

Detecting anomalies consisting of unsafe and unsanitary conditions in the home
is key functionality required for home robots to be useful for users. For
instance, if you forget to put your milk back in your fridge, or leave the
front door ajar, it would be great if a home robot notified you, or solved the
problem itself. Additionally, users with children will materially benefit from
a robot that can monitor the environment for their children’s safety.

Figure 1: In this work we aim to enable embodied agent to detect unsafe and
unsanitary conditions in the home. For this, we first create a new, unique
dataset with unsafe and unsanitary conditions to detect. We then hypothesize
that Large Language Models (LLMs) contain the knowledge needed to logically
operate on these conditions. Our approach creates LLM prompts that leverage
object relationships in the scene from a scene graph (like those in the right
image with objects being nodes and relationships being edges), and classifies
them. In addition to testing on our dataset, we tested in the real world using
the ClearPath TurtleBot in scenarios like that shown here.

These types of scenarios can present a real danger to people in the home. For
example, 31% of home cooking fires are caused by unattended equipment [1],
over 42,000 people died from falls sustained at home or at work [2], and
accidents, including poisoning and suffocation, are the leading cause of death
for children in the United States [3]. If a home robot can monitor the stove
to make sure it is properly turned off, police the environment for tripping
hazards, and monitor the home for accessible poisons or suffocation hazards,
many of these fires, injuries, or deaths can be prevented.

Building embodied agents that can perform household tasks has seen a lot of
recent interest from the robotics and embodied AI communities. Some commonly
approached problems include navigation [4, 5], instruction following, and
embodied question answering [6, 7, 8]. Each of these tasks defines a precise
goal, e.g. navigating to a set location, moving objects to a set location, or
answering a question correctly. However, our use case, detecting anomalies in
the home, is a poorly formulated problem consisting of an incredibly diverse
and disparate set of examples. As such, it is difficult to extensively hard-
code into a robots behavior. One would have to record a rule for where every
object can or cannot be kept, and what state it must be in. Additionally,
these rules would have to adapt to an unknown and unique set of objects and
personal preferences in a household. We hypothesize that recent advances in
Large Language Models (LLMs) [9, 10, 11] could enable our use case by
providing diverse knowledge on the home that can be leveraged to detect
potentially unsafe or unsanitary conditions. However, there are no sources of
data that contain environments with these conditions to test potential methods
on. Additionally, we show in this work that previous approaches to prompting
LLMs are unable to effectively extract the knowledge necessary to detect
unsafe and unsanitary conditions.

Main Contributions: We introduce the SafetyDetect dataset to benchmark the
ability of embodied AI agents to infer unsafe or unsanitary situations in the
house. Figure 1 showcases our task where an agent is spawned randomly in an
unknown environment, with an unknown set of potentially unsafe or unsanitary
conditions. Without explicit instructions, the agent must discover any
potential anomalies and report them to the user.

The SafetyDetect dataset is challenging due to the diverse and unrelated
nature of the possible anomalies in the scene. For example, medicine on the
ground (poison hazard for children) is hardly connected to moldy produce on
the counter (sanitation hazard). Additionally, agents are not trained on a
base environment allowing them to simply detect changes. Agents are required
to use some form of knowledge to deduce the danger to the user.

We propose baseline solutions that encapsulate knowledge leveraged from LLMs
and demonstrate that this serves as an effective solution for the SafetyDetect
dataset. Our key finding is that utilizing a scene graph is the most effective
way of informing the LLMs of the proper scene context and semantic information
for scene understanding and spatial reasoning. Baseline methods that do not
use a scene graph perform very poorly on the SafetyDetect dataset.

Dataset | Goal | Scenarios | Object Categories | Object Models | Scenes
---|---|---|---|---|---
Transport Challenge | Geometric | Inf | 50 | 112 | 15
Behavior | Predicate | 1000 | 391 | 1217 | 15
My House, My Rules | Human Preferences | 75 | 12 | 12 | 2
Housekeep | Human Preferences | 585 | 268 | 1799 | 14
Ours | Anomalies | 1000 | 192 | 1163 | 7
TABLE I: Comparison of SafetyDetect to other similar benchmarks. SafetyDetect
is comparable to the other datasets in scale while approaching a different end
task. Note that we can use procedural generation to go beyond 1000 scenarios
and 7 scenes.

Our main contributions are as follows:

  1. 1.

We present the SafetyDetect dataset, a new, first-of-its-kind dataset, built
off the VirtualHome simulator[12], aimed at enabling researchers to create
embodied agents that can detect unsafe or unsanitary conditions in the home.
This dataset, at release, contains 1000 scenarios for users to explore and
solve. We additionally provide information about each scenario which informs
user preference and how the robot should report a given anomaly to the user.

  2. 2.

We present an LLM-based method that leverages a scene graph, categorization,
and chain of thought prompting to perform exceptionally well on a simplified
version of this task, with an anomaly detection rate of over 95%.

  3. 3.

We show that use of the scene graph when creating the LLM prompt is important
for performance on our SafetyDetect dataset through its ability to provide
scene information to the LLM in a concise way.

  4. 4.

We explore the sim-to-real transfer of our method tested on the SafetyDetect
dataset and demonstrate that the SafetyDetect dataset is an adequate
representation of real world scenarios. We do this by running our method on a
ClearPath TurtleBot and having it detect unsafe or unsanitary anomalies in a
real-world home environment.

##  II Related Work

###  II-A Similar Datasets or Benchmarks

While to our knowledge no previous work has approached household anomaly
detection as a task, there are some alternate tasks that are similar in scope
or implementation. Behavior1K [13] is a simulation benchmark where the robots
must complete 1000 everyday tasks. [14] and [15] both focus on the idea of
cleaning up clutter in the home and placing things in their proper locations.
[14] is the most similar to our work as in both tasks, agents are placed
randomly into the environment and must find the issues, unsafe situations in
our case and misplaced objects in theirs. Unique to our work is the specific
use case of the detection of unsafe or unsanitary conditions. Approaches on
the HouseKeep dataset are built solely around their task and would need
significant modification to work on SafetyDetect.

###  II-B Language-Based Embodied AI

Using language to inform robotic agents is a popular task in literature, with
work including using generalized grounding graphs [16] for robot manipulation
[17, 18] to performing language-guided navigation [4, 5]. Tellex et al. [19]
recently presented a useful survey on using language from a robotics
perspective.

More recent work tackling this problem by Thomason et al. [6, 7] and Gao et
al. [8] has explored the use of human-robot dialogue to gather relevant
information for completing tasks. Different from these works, we are focused
on using natural language derived from a scene graph as a medium for scene
understanding. However, parsing and utilizing natural language is very
relevant in our work, and we are motivated by the techniques developed by
these papers.

With the advent of ChatGPT [9], LLaMA [11], FLAN-T5 [20], and other LLMs, the
field of Embodied AI worked to leverage them to improve performance on their
tasks. Dorbala et al. [21] use language models to inform navigation for object
goal navigation. Singh et al. [22] use language models to write code that
solves a given task. We differ from these methods in terms of task and
prompting technique. Specifically, creating the SafetyDetect dataset to
explore the detection of unsafe conditions is substantially different from the
prior work which is generally focused on question-answering [23, 24], task-
completion [22, 25], or object goal navigation [21]. Additionally, our
utilization of the scene graph to provide context to the LLM when prompting is
a key differentiator from these methods.

###  II-C Using the Scene Graph

Scene graphs are a common method of representing a scene in computer graphics
and 3D modeling where, generally, nodes of the graph are objects and edges are
relationships. Additionally, creating a scene graph from images is a popular
problem in the computer vision community [26, 27]. Many simulation platforms
for embodied AI are built on top of a scene graph including both Habitat [28]
and VirtualHome [12]. This makes a scene graph a relatively easy to access
source of information, both in simulation and real world environments, for
methods trying to solve embodied AI tasks. However, few works have attempted
to leverage the scene graph for scene understanding with LLMs.

In contrast to our approach with scene graphs, existing LLM literature uses
knowledge graphs as a means of finding information to the LLM as context.
These methods generally conduct a semantic search of the knowledge graph to
find said information [29, 30, 31]. The closest work to ours is SayPlan [32]
which in fact uses a scene graph, but only uses it as a knowledge graph and a
means of conducting a semantic search on the scene to provide relevant
information to the LLM based on the task at hand. In contrast to this, we
utilize the scene graph to create strings that encapsulate object
relationships to then feed into the LLM alongside a classification approach
that does not allow for filtering the graph extensively.

##  III Adding Anomalies to the Home: The SafetyDetect Dataset

###  III-A Creating the SafetyDetect Dataset

Task Definition: In SafetyDetect, an embodied agent is tasked with finding any
unsafe or unsanitary conditions in the home, and reporting them to the user.
Additionally, the SafetyDetect dataset requires the agent to only report
conditions that meet the users preferences. For example, if there are no
children in the home, the agent should not report a situation that is only
dangerous for children. The agent may also be provided a graph representation
of the scene, similar to that which can be created by [26] in the real world,
with nodes of the graph being the objects that make up the scene, and edges of
the graph being relationships like ‘ON,’ ‘INSIDE,’ and ‘FACING.’

Figure 2: A sampling of images from the SafetyDetect dataset showing unsafe
conditions. In one of the images, medication and alcohol are on the floor and
dangerous for children. In another, a pile of clothes are in the doorway - a
tripping hazard for users.

Simulator and Scenes: We use VirtualHome [12] as the basis for the
SafetyDetect dataset. VirtualHome was chosen for its ease of use, existing
support community, and relative simplicity of adding novel objects.
VirtualHome contains seven pre-built scenes, but supports procedural
generation to create an unlimited number of valid home environments. For the
SafetyDetect dataset we rely on the pre-built scenes, but we release code to
allow users to procedurally generate new scenes and data samples with
unlimited diversity.

Category | Class Name | Number of Occurances
---|---|---
Safety | Spills | 195
Tripping Hazard | 202
Broken Items | 214
Candle On | 190
Front Door Open | 181
Stove On | 207
Sanitation | Refrigerated/Frozen foods out | 210
Expired Produce | 199
Fridge/Freezer Open | 184
Safety for Children | Choking Hazard | 201
Sharp Objects | 201
Poison: Cleaning Products | 192
Poison: Medication & Beauty Products | 198
TABLE II: Outline of all of the classes of anomalies in the dataset as well as
the number of times they occur in the dataset.

Anomalies and Objects: To create SafetyDetect, we first outlined our target
set of hazards that fit the overarching categories of unsafe conditions,
unsanitary conditions, and conditions which are dangerous for children. The
set of hazards, which are the classes of the dataset, is outlined in Table II,
alongside the number of occurrences of each in the dataset. This was created
primarily by referencing large scale statistical studies of major household
dangers for users or their children. For example, [3] outlines the major
causes of death for children in the United States. After filtering out dangers
to children not relevant to the household environment (auto accidents), and
those we did not feel comfortable addressing (firearm related incidents), we
chose to cover the most common remaining examples including choking, poison
hazards, and sharp objects. Additional individual examples were created
through a small scale user study soliciting examples from prospective users.
This added classes like ‘broken items’ which adds the detection of items like
shattered glasses or mugs that may have been broken by pets.

To embody and visualize these hazard classes in the simulator, we had to
determine what objects were necessary to recreate each hazard and where those
objects could be placed to constitute the hazard while remaining logically
plausible. We began by manually noting obvious examples. For example, a candle
flame hazard would require a candle object with an active flame, and a
tripping hazard must be on the floor. For each hazard class, we expanded our
samples by prompting an LLM to first produce sample objects, and then for
sample placement location examples that would create the target hazard using
the other objects present in the scene. The LLM’s output was manually filtered
to examples that made logical sense and could be produced in the simulator.

Many objects we required were not present in the VirtualHome simulator. To
address this, we added relevant objects from Google Scanned Objects [33],
ReplicaCAD [28], Fantastic Breaks [34], and YCB Objects [35]. We additionally
created new objects and modified similar objects using Blender to cover cases
where suitable objects could not be found. For example, we created a spill
object and texture, and re-textured various fruits and vegetables to appear as
their expired or rotten counterparts. In total, we add over 30 new objects to
VirtualHome while leveraging many objects native to the simulator. The final
assortment of objects and placement locations results in 967 unique anomalies
split among the 13 hazard classes. Through combining these anomalies,
countless unique scenes can be created.

Generating the Dataset: To generate the final dataset, we randomly sampled
1000 i.i.d scenarios, each of which utilizes one prebuilt scene, contains 0-5
hazards, and shares a set of user preferences. Each hazard consists of the
hazard class, a selected object affiliated with said class, and a valid
placement of that object to embody the hazard. The final scene graph after
placing the object into the scene in VirtualHome is provided. Future users can
visualize our scenarios, add more samples with the prebuilt environments, or
procedurally generate new scenes and scenarios.

Agent: VirtualHome contains an agent for use in exploring the scene. The agent
is embodied as a human but contains a RGB camera who’s location on the agent
and Field of View (FOV) we can alter. This agent can navigate around the scene
using the VirtualHome API. This includes commands for walking towards a room
or object, forward in the direction of travel, and turning a certain angle.

###  III-B Using the SafetyDetect Dataset

The central challenge of the SafetyDetect dataset is understanding how to
detect unsafe or unsanitary conditions in the home and notify the user of
their existence. Our goal is to capture a comprehensive but not necessarily
all-inclusive set of these conditions so researchers can test their methods
before deploying them into a consumer environment. These scenarios were
collected through surveys of prospective users and expert documentation. This
allowed us to produce a diverse set of scenarios for exploration.

Episodes: Each episode in the SafetyDetect dataset instantiates one of the
seven base scenes of VirtualHome before extending it with 0-5 unsafe or
unsanitary conditions. The agent itself is spawned randomly into the
environment with no prior knowledge of it and can explore the environment to
find potentially unsafe or unsanitary conditions. We call this set of
conditions $\mathcal{A}$, where each individual anomaly is denoted with
$a_{i}$. An agent is then placed randomly into the scene. Next, we define for
the agent the relevant context of the scene, specifically the presence of
children in the house. The researcher must then create a strategy for the
agent that allows it to detect the anomalies in the home, and reproduce
$\mathcal{A}$. Our solution is described in the next section.

Evaluations: We evaluate agents/detection schemes for effectiveness and
efficiency. All metrics are reported per episode and are then aggregated
across multiple episodes to report the averages and standard errors. These
metrics are utilized in similar literature including [14].

  1. 1.

Anomaly Success (AS): Fraction of all anomalies found in a given episode. This
is essentially the true positive detection rate.

  2. 2.

Conditioned Anomally Success (CAS): Fraction of anomalies reported correctly
minus those reported incorrectly depending on the given context. For example,
in a home without children, a knife being left on the counter is not a problem
and should not be reported. Reporting in a situation like this could be
annoying to the user, hampering the agent’s effectiveness.

Additionally, we track the true positive rate for each anomaly individually.
Tracking of false positive and false negative results is also conducted
automatically but analysis must be conducted manually. Code released with the
SafetyDetect dataset allows users to easily glean this information for their
own methods.

##  IV Finding Anomalies in the Home

For our proposed method we employ the GPT-4 Large Language Model (LLM) [10] to
apply knowledge about what scenarios may be unsafe, unsanitary, or dangerous
for children in a scene. Specifically, we hypothesize that LLMs contain domain
knowledge about what is safe in a home environment, and what might be
dangerous, unsanitary, or dangerous for children. To enable performance from
the LLM, we provide two key insights: 1) utilizing a scene graph provides the
context needed for scene understanding and reasoning, and 2) classification is
an effective approach for detecting specific situations in a scene.

Figure 3: The flow of our method is depicted here. We first get the scene
graph before using it to formulate a prompt with asks the LLM to categorize
the object relations. The model must then use commonsense reasoning to
categorize these object relations effectively.

Using the Scene Graph. For the scene graph, we assume that other methods
exist, or can be created, to generate a scene graph that represents object
relations in the scene. Some examples of these methods include [27], and [26].
The VirtualHome [12] simulator provides a scene graph of this type natively.
In the scene graph, each object is a node and the relation is an edge. For
example, an apple sitting on a kitchen table would have its initial node with
a class of apple, and then two edges, on showing it ‘ON’ the kitchen table,
and another showing it ‘INSIDE’ the kitchen. The use of the scene graph is
imperative to our method and what sets it apart from many other approaches
which generally provide either a textual description of the environment, or a
listing of classes found in the immediate area [21].

For our LLM prompts, we specifically parsed the scene graph for the edge
representing the room the object was inside, and the edge for what the object
was on top of. This produced object relation strings like ‘apple INSIDE
kitchen ON counter’ or ‘medication INSIDE livingroom ON floor.’ These strings
were in turn fed directly into GPT-4 alongside the rest of the prompt
outlining the classification problem and providing some examples to elicit
chain-of-thought.

Classifying the Scene Graph. The second key aspect of our proposed method is
classifying relationships in the scene graph as a way of differentiating
normal and dangerous situations. Specifically, our prompt for GPT-4 asks it to
classify object relations as ‘normal’, ‘unsafe’, ‘unsanitary’, or ‘unsafe for
children’. This classification approach improved performance as similar
methods that asked the LLM to directly detect anomalous object relationships
would frequently produce false negatives.

Building the Prompt. We also provide the LLM with a series of example
anomalies given as an object relation pulled from the scene graph, followed by
the correct classification and an explanation. We employ the popular chain-of-
thought prompting technique to provide these examples. The prompt we utilize
for our method is below, where [CONTENT] is replaced by the object relations
for the LLM to classify:

> Q: Classify the following object relations in a home into either ‘normal’,
> ‘unsafe’, ‘unsanitary’, or ‘unsafe for children’ - [‘medication INSIDE
> livingroom ON floor’, ‘rawmeat INSIDE kitchen ON counter’]
>
> A: ‘medication INSIDE livingroom ON floor’: ‘unsafe for children’ as
> medication is a poison hazard
>
> ‘rawmeat INSIDE kitchen ON counter’: ‘unsanitary’ as rawmeat requires
> refrigeration and will go bad if left unrefrigerated.
>
> Q: Classify the following object relations in a home into either ‘normal’,
> ‘unsafe’, ‘unsanitary’, or ‘unsafe for children’ – [CONTENT]
>
> A:

Parsing and Evaluating the Response. To parse through the verbose responses,
we again prompt a LLM, this time GPT-3.5 Turbo for cost savings with minimal
performance loss. We ask the model to simplify the previous response into a
one word classification fitting the categories above. For example, a response
including “‘medication INSIDE livingroom ON floor’: ‘unsafe for children’ as
it presents a possible poison hazard,” would be filtered into just ‘unsafe for
children.’ This allows us to evaluate the models performance effectively
despite the verbose responses provided through chain-of-thought prompting.
This parsing method injected a small amount of error into the system, with
around 1-2% of responses being improperly parsed in our method.

##  V Experiments and Results

###  V-A Baseline Results

Method | AS $\uparrow$ | CAS $\uparrow$
---|---|---
Object List + E, C | 12.4 | 5.1
Scene Description + E, C | 21.5 | 13.6
SG, no C | 18.8 | -
E + SG, no C | 38.8 | -
SG & C | 83.2 | 80.8
E + SG & C | 94.0 | 88.8
CoT + SG & C (Ours) | 96.0 | 90.5
TABLE III: Comparison of performance on SafetyDetect. E represents the inclusion of example, SG represents the use of the Scene Graph, C represents the inclusion of categorization. Note that the methods that utilize the scene graph and categorization are significantly more competitive than those that do not. Method | AS $\uparrow$ | CAS $\uparrow$
---|---|---
FLAN-T5-Large | 81.3 | 51.2
GPT-3.5-Turbo | 94.0 | 88.8
GPT-4 (Ours) | 96.0 | 90.5
TABLE IV: Comparison of performance on SafetyDetect for different Large
Language Models. LLaMA is not included as we could not get it to work well on
this task.

Use of The Scene Graph and Classification. We first tested against the use of
the scene graph and classification techniques in the prompts. We find that
when providing scene information to the LLMs, standard techniques like using
object lists or a scene descriptions had difficulty understanding when an
anomaly was present. For example, when providing a simple object list, its
difficult for the model to know whether an object is in a safe space or not.
As such, it tended to err on the side of reporting normalcy. It did relatively
well with specific cases like spills or rotten fruit as the class name was
sufficient to detect an anomaly. The scene description-based prompts performed
better, with a 21.5% detection rate, but it still struggled to pick out
dangerous and unsanitary conditions. It performed similarly to the object list
method for objects whose class names alone were enough to detect the anomaly
and added some performance in other cases like sharp items and refrigeration
required items.

When utilizing the scene graph as the basis for communicating the scene
context, performance nearly doubles when continuing to provide examples but
removing classification. When adding classification back in, performance
doubles again to a nearly 4.5$\times$ improvement over the scene description-
based method for a 94% detection rate. Reformatting the prompt to fit a chain-
of-thought scheme further boosted the performance slightly to a 96% detection
rate.

Upon further inspection, the logic provided by GPT-4 in response to our method
was consistent with real-world commonsense. Anomalies were not only correctly
classified, but the logic behind their classification matched that in expert
sources and from our sample users. In fact, it found anomalies built into the
scene graph that we had not placed there, but were potential hazards. Some
scenes were built in VirtualHome through stacking objects on top of each other
to create a visually new object, but the scene graph retained this stacked
structure and produced illogical relationships. Our method would consistently
pick out these scenarios as anomalous. As the underlying structure of the
simulator had many of these anomalies, it became difficult to track a false
positive rate. Anecdotally, we saw very few false positives in the our
method’s responses.

Ablation on Models. We also test against the specific use of GPT-4 by
deploying the same Chain-of-Thought + Scene Graph & Classification prompt on
GPT-3.5-Turbo, FLAN-T5-Large, and LLaMA-2. We find that GPT-3.5 produces a
similar level of performance to GPT-4 with FLAN-T5-Large also producing
respectable detection rates above 80%. We had difficulty applying LLaMA to
this task as it frequently hallucinated new object relations that we did not
provide. This made parsing the response increasingly difficult and severely
impacted performance. Further experiments with different prompting
approachesay be able to improve upon these LLaMA results.

###  V-B Real World Experimentation

The goal of our dataset is to enable new use cases in home robotics. As such,
it is important to validate the use of the scene graph in the real world and
explore how best to create the scene graph for our purposes. For this
experiment, we set up a ClearPath TurtleBot to navigate the local environment
while running a slightly modified version of [26] that builds scene graphs
similar to that from VirtualHome for use alongside our CoT + SG & C method.

Our real world scene consisted of one room with a small number of objects,
including 1-3 anomalies. The TurtleBot captures images from $360^{\circ}$ and
feeds them into our modified version of [26] that produces an adequate scene
graph. We then run our algorithms verbatim on the scene graph. We conduct 20
experiments that covered a subset of eight of our anomaly classes in the
SafetyDetect task.

We find that performance is similar between the simulator and the real world.
Creating the scene graph through [26] also enabled increased performance on
some anomalies, such as obstructions, which were not captured well in the
scene graph created by the VirtualHome simulator. Some anomalies suffered
worse performance as the scene graph generator we utilized was unable to
distinguish between good and rotten produce, and often did not detect spills.
We believe that many of these issues are easily solved by retraining the
object detection components of the scene graph builder. Overall, our detection
rate dropped to 82.7%. Through this experiment, we believe that if provided
with a ground truth scene graph, there is little reason why our LLM-based
approach would not be able to sustain our simulator-based detection rates of
over 95% with few false positives.

Figure 4: We deploy a ClearPath TurtleBot in the real world. We use existing
methods to generate an effective scene graph before utilizing our method to
detect anomalies in the scene. In this example, medication and cleaning
products are on the table. This is captured in the scene graph and detected by
the model as unsafe for children.

##  VI Conclusions, Limitations, and Future Work

Home robots intend to make their users lives easier. One way they may do this
is by informing users of issues or anomalies in the home. In this work, we
moved towards enabling home robots with these abilities by creating a new
dataset, built on top of the popular VirtualHome platform, that contains 1000
dangerous or unsanitary scenarios for an agent to detect. We also propose an
LLM-based approach that utilizes a scene graph and classification approach.
This methods identifies over 90% of anomalous scenarios in our dataset.
Additionally, we show that these techniques remain viable in the real world.

Limitations. The methods we proposed are limited by perception, specifically
how the scene graph is created. For example, in the scene graph created by the
VirtualHome simulator, there was nothing indicating obstructions in the scene.
To illustrate this, a relation of ‘box INSIDE livingroom ON floor’ is okay,
but physically the box may be in the doorway creating a tripping hazard. While
we assumed perfect perception in creating the scene graph in the simulator,
alternate approaches without this assumption would produce weaker results.

Future Work. Continuation work with multi-modal LLMs like GPT-V could be
valuable to both address the perception perception and solve the problem in
one shot. The next logical step in this work is to get the agent to
preemptively solve the anomaly by adding on a task planning and completion
task. For the real world deployment of this work, significant effort needs to
be put into optimizing prompts and altering the scheme to lower the costs of
querying the LLM.

## References

  * [1] M. Ahrens, “Home cooking fires.” [Online]. Available: https://www.nfpa.org/News-and-Research/Data-research-and-tools/US-Fire-Problem/Home-Cooking-Fires
  * [2] [Online]. Available: https://www.nsc.org/work-safety/safety-topics/slips-trips-and-falls
  * [3] J. E. Goldstick, R. M. Cunningham, and P. M. Carter. [Online]. Available: https://www.nejm.org/doi/full/10.1056/NEJMc2201761
  * [4] Z. Hu, J. Pan, T. Fan, R. Yang, and D. Manocha, “Safe navigation with human instructions in complex scenes,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 753–760, 2019.
  * [5] V. S. Dorbala, A. Srinivasan, and A. Bera, “Can a robot trust you?: A drl-based approach to trust-driven human-guided navigation,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 3538–3545.
  * [6] J. Thomason, A. Padmakumar, J. Sinapov, N. Walker, Y. Jiang, H. Yedidsion, J. Hart, P. Stone, and R. J. Mooney, “Improving grounded natural language understanding through human-robot dialog,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 6934–6941.
  * [7] J. Thomason, M. Murray, M. Cakmak, and L. Zettlemoyer, “Vision-and-dialog navigation,” in _Conference on Robot Learning_. PMLR, 2020, pp. 394–406.
  * [8] X. Gao, Q. Gao, R. Gong, K. Lin, G. Thattai, and G. S. Sukhatme, “Dialfred: Dialogue-enabled agents for embodied instruction following,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 10 049–10 056, 2022.
  * [9] T. Brown, B. Mann, N. Ryder, M. Subbiah, J. D. Kaplan, P. Dhariwal, A. Neelakantan, P. Shyam, G. Sastry, A. Askell _et al._ , “Language models are few-shot learners,” _Advances in neural information processing systems_ , vol. 33, pp. 1877–1901, 2020.
  * [10] OpenAI, “Gpt-4 technical report,” 2023.
  * [11] H. Touvron, T. Lavril, G. Izacard, X. Martinet, M.-A. Lachaux, T. Lacroix, B. Rozière, N. Goyal, E. Hambro, F. Azhar, A. Rodriguez, A. Joulin, E. Grave, and G. Lample, “Llama: Open and efficient foundation language models,” 2023.
  * [12] X. Puig, K. Ra, M. Boben, J. Li, T. Wang, S. Fidler, and A. Torralba, “Virtualhome: Simulating household activities via programs,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_ , 2018, pp. 8494–8502.
  * [13] C. Li, R. Zhang, J. Wong, C. Gokmen, S. Srivastava, R. Martín-Martín, C. Wang, G. Levine, M. Lingelbach, J. Sun _et al._ , “Behavior-1k: A benchmark for embodied ai with 1,000 everyday activities and realistic simulation,” in _Conference on Robot Learning_. PMLR, 2023, pp. 80–93.
  * [14] Y. Kant, A. Ramachandran, S. Yenamandra, I. Gilitschenski, D. Batra, A. Szot, and H. Agrawal, “Housekeep: Tidying virtual households using commonsense reasoning,” in _European Conference on Computer Vision_. Springer, 2022, pp. 355–373.
  * [15] J. Wu, R. Antonova, A. Kan, M. Lepert, A. Zeng, S. Song, J. Bohg, S. Rusinkiewicz, and T. Funkhouser, “Tidybot: Personalized robot assistance with large language models,” _arXiv preprint arXiv:2305.05658_ , 2023.
  * [16] S. Tellex, T. Kollar, S. Dickerson, M. Walter, A. Banerjee, S. Teller, and N. Roy, “Understanding natural language commands for robotic navigation and mobile manipulation,” _Proceedings of the AAAI Conference on Artificial Intelligence_ , vol. 25, no. 1, pp. 1507–1514, Aug. 2011. [Online]. Available: https://ojs.aaai.org/index.php/AAAI/article/view/7979
  * [17] J. S. Park, B. Jia, M. Bansal, and D. Manocha, “Efficient generation of motion plans from attribute-based natural language instructions using dynamic constraint mapping,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 6964–6971.
  * [18] R. Paul, J. Arkin, N. Roy, and T. M Howard, “Efficient grounding of abstract spatial concepts for natural language interaction with robot manipulators,” _Robotics: Science and Systems_ , 2016.
  * [19] S. Tellex, N. Gopalan, H. Kress-Gazit, and C. Matuszek, “Robots that use language,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 3, pp. 25–55, 2020.
  * [20] J. Wei, M. Bosma, V. Zhao, K. Guu, A. W. Yu, B. Lester, N. Du, A. M. Dai, and Q. V. Le, “Finetuned language models are zero-shot learners,” in _International Conference on Learning Representations_ , 2022.
  * [21] V. Sashank Dorbala, J. Mullen, James F., and D. Manocha, “Can an Embodied Agent Find Your “Cat-shaped Mug”? LLM-Based Zero-Shot Object Navigation,” _arXiv e-prints_ , p. arXiv:2303.03480, Mar. 2023.
  * [22] I. Singh, V. Blukis, A. Mousavian, A. Goyal, D. Xu, J. Tremblay, D. Fox, J. Thomason, and A. Garg, “Progprompt: Generating situated robot task plans using large language models,” _arXiv preprint arXiv:2209.11302_ , 2022.
  * [23] D. Driess, F. Xia, M. S. Sajjadi, C. Lynch, A. Chowdhery, B. Ichter, A. Wahid, J. Tompson, Q. Vuong, T. Yu _et al._ , “Palm-e: An embodied multimodal language model,” _arXiv preprint arXiv:2303.03378_ , 2023.
  * [24] W. Huang, F. Xia, T. Xiao, H. Chan, J. Liang, P. Florence, A. Zeng, J. Tompson, I. Mordatch, Y. Chebotar _et al._ , “Inner monologue: Embodied reasoning through planning with language models,” _arXiv preprint arXiv:2207.05608_ , 2022.
  * [25] M. Ahn, A. Brohan, N. Brown, Y. Chebotar, O. Cortes, B. David, C. Finn, C. Fu, K. Gopalakrishnan, K. Hausman, A. Herzog, D. Ho, J. Hsu, J. Ibarz, B. Ichter, A. Irpan, E. Jang, R. J. Ruano, K. Jeffrey, S. Jesmonth, N. Joshi, R. Julian, D. Kalashnikov, Y. Kuang, K.-H. Lee, S. Levine, Y. Lu, L. Luu, C. Parada, P. Pastor, J. Quiambao, K. Rao, J. Rettinghouse, D. Reyes, P. Sermanet, N. Sievers, C. Tan, A. Toshev, V. Vanhoucke, F. Xia, T. Xiao, P. Xu, S. Xu, M. Yan, and A. Zeng, “Do as i can and not as i say: Grounding language in robotic affordances,” in _arXiv preprint arXiv:2204.01691_ , 2022.
  * [26] J. Yang, J. Lu, S. Lee, D. Batra, and D. Parikh, “Graph r-cnn for scene graph generation,” in _Proceedings of the European conference on computer vision (ECCV)_ , 2018, pp. 670–685.
  * [27] D. Xu, Y. Zhu, C. B. Choy, and L. Fei-Fei, “Scene graph generation by iterative message passing,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2017, pp. 5410–5419.
  * [28] M. Savva, A. Kadian, O. Maksymets, Y. Zhao, E. Wijmans, B. Jain, J. Straub, J. Liu, V. Koltun, J. Malik, D. Parikh, and D. Batra, “Habitat: A Platform for Embodied AI Research,” Nov. 2019.
  * [29] S. Pan, L. Luo, Y. Wang, C. Chen, J. Wang, and X. Wu, “Unifying large language models and knowledge graphs: A roadmap,” 2023.
  * [30] T. Bratanic, “Knowledge graphs; llms: Multi-hop question answering,” Jun 2023\. [Online]. Available: https://neo4j.com/developer-blog/knowledge-graphs-llms-multi-hop-question-answering/
  * [31] J. Zhang, “Graph-toolformer: To empower llms with graph reasoning ability via prompt augmented by chatgpt,” _arXiv preprint arXiv:2304.11116_ , 2023.
  * [32] K. Rana, J. Haviland, S. Garg, J. Abou-Chakra, I. Reid, and N. Suenderhauf, “Sayplan: Grounding large language models using 3d scene graphs for scalable task planning,” _arXiv preprint arXiv:2307.06135_ , 2023.
  * [33] L. Downs, A. Francis, N. Koenig, B. Kinman, R. Hickman, K. Reymann, T. B. McHugh, and V. Vanhoucke, “Google scanned objects: A high-quality dataset of 3d scanned household items,” 2022.
  * [34] N. Lamb, C. Palmer, B. Molloy, S. Banerjee, and N. K. Banerjee, “Fantastic breaks: A dataset of paired 3d scans of real-world broken objects and their complete counterparts,” in _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2023.
  * [35] B. Calli, A. Walsman, A. Singh, S. Srinivasa, P. Abbeel, and A. M. Dollar, “Benchmarking in manipulation research: Using the yale-CMU-berkeley object and model set,” _IEEE Robotics & Automation Magazine_, vol. 22, no. 3, pp. 36–52, sep 2015.


# Linear Kinematics for General Constant Curvature and Torsion Manipulators

Bill Fan∗1 Farhan Rozaidi2, Capprin Bass2, Gina Olson3, Melinda Malley1, Ross
L Hatton2 *: Corresponding author1 Bill Fan and Melinda Malley are with Olin
College of Engineering, Needham, MA, USA. {wfan<EMAIL_ADDRESS>Rozaidi, Capprin Bass, and Ross L Hatton are with the Collaborative Robotics
and Intelligent Systems (CoRIS) Institute, Oregon State University, Corvallis,
OR, USA. {nikahman, basscap<EMAIL_ADDRESS>Gina Olson is with
the University of Massachusetts Amherst, Amherst, MA, USA<EMAIL_ADDRESS>
###### Abstract

We present a novel general model that unifies the kinematics of constant
curvature and constant twist continuum manipulators. Combining this kinematics
with energy-based physics, we derive a linear mapping from actuator
configuration to manipulator deformation that is analogous to traditional
robot forward kinematics. Our model generalizes across manipulators with
different sizes, types of bending, and types of actuators, without the need
for parameter re-fitting. The combination of generality and linearity makes
the model useful for control and planning algorithms. Finally, our model is
shown to be accurate through experimental validation on manipulators with
pneumatic artificial muscles.

##  I INTRODUCTION

While the motion of traditional robots comes from their discrete joints, a
continuum manipulator moves by deforming along its entire arc. These
manipulators are often composed of rigid skeletons and soft actuators. When
the actuators are activated, the interplay of forces within the manipulator
creates stretching, bending, and sometimes twisting of the whole manipulator.
These actuators commonly take the form of tendons or wires actuated by
pulleys, pneumatic or hydraulic artificial muscles which contract when
inflated, or shaped metal alloys which contract when electricity is applied.

The design space of possible actuator routings and skeleton geometry is
infinitely large. Within this space, a common configuration consists of a few
segments connected in series, where each section is composed of actuators that
are arranged parallel to each other and the base-curve of the manipulator. The
manipulator cross-section geometry is consistent throughout the length of each
section (Fig 1). The simple geometry of these manipulators makes their
kinematics analytically tractable: using entirely geometric methods, the
configuration of the actuators can be algebraically mapped to the length,
curvature, and bending plane of the manipulator. Prior work has developed
models for use in control and motion planning, which has enabled applications
in surgical robots, manipulation, and inspections [1].

However, most bending continuum manipulators lack a torsional degree of
freedom, and thus are incapable of separately controlling position and
orientation, or performing motion primitives such as wrapping. Inspired by
muscle structures in cephalopod tentacles, recent works [2][3][4] have shown
that helically routing actuators at a constant pitch and diameter drastically
enlarges the manipulator workspace by enabling constant twisting: a
combination of bending and torsion.

This relatively simple design change invalidates constant curvature
kinematics, thus requiring the use of the far more complex models such as
Cosserat rod mechanics. [3][5]. The use of numerical integration makes
Cosserat rod mechanics general and inclusive of non-constant deformation;
however, it also makes them opaque. The reliance on integration to solve for
manipulator deformation makes the approach computationally expensive for use
in control and planning algorithms, which require repetitive kinematic
solutions.

Figure 1: Manipulators that exhibit constant bending and twisting. Left to
right: Planar, spatial, and helical arms. Top: Physical systems with Apriltags
attached for motion capture. Bottom: Model (dashed) versus measured (solid)
geometry. All shown axes are in meters. The same model and parameters were
used for all above experiments.

In this work, we derive an extension of constant curvature kinematics to
include constant twist, while maintaining the simplicity of the approach. This
kinematic model is purely algebraic and does not require numerical
integration. Paired with an energy-based physics model, we produce a linear
mapping between the actuator lengths and the deformed state of an unloaded
manipulator. The model’s form is analogous to traditional robot forward
kinematics, and through our kinematic extension generalizes between constant
curvature and constant twist manipulators.

This paper is structured as follows. Section II begins with an overview of
kinematics, physics, and hybrid modeling approaches. Section III reviews
Cosserat-rod kinematics and derives the kinematic relation that unifies
constant curvature kinematics to include constant twist. Section IV derives
our novel equilibrium model. Section V presents results comparing the model to
measured manipulator deformation from experiments. Finally, we end with a
discussion of results and considerations of future work in Section VI.

##  II Prior Work

The most well established class of continuum manipulator models are models
that assume constant curvature [6], which parameterize manipulator
deformations by their length, curvature, and bending-plane. By assuming
perfect knowledge of actuator lengths, the kinematics problem is reduced to
pure geometry, thus enabling the development of models that, albeit nonlinear,
function similarly to traditional robot forward kinematics.

These models have proven useful for implementation in control and planning
applications. However, they rely on an extrinsic parameterization of
manipulator geometry. The notions of bending planes and tangent circles exist
within a broader embedding space, and are specific to the structure of the
manipulator. Thus, they do not generalize well to changes in manipulator
design, such as additional actuators, or helically routed actuators. While
recent works [2] have developed purely geometric kinematic models for constant
twist manipulators, these models still parameterize the twisting geometry
extrinsically as a winding radius and pitch. Therefore they suffer from the
same lack of generality as models for constant curvature manipulators.

Cosserat rod kinematics present an alternative formulation of kinematics
[5][7]. By representing rod geometry intrinsically as the body-frame spatial
velocity of a frame moving along the length of a rod, Cosserat rod kinematics
is able to generalize to manipulators of arbitrary designs, including ones
with non-constant deformation. While this requires numerical integration in
the general case, restricting the model to constant deformation makes the
kinematics analytically integrable. Thus, recent works have begun to apply
this intrinsic parameterization to constant curvature kinematics [8] to derive
more general models. The generalization of this parameterization to constant
twist kinematics is natural, but has not yet been formulated in the
literature. Thus, there does not currently exist a general form of constant
twist kinematics.

A general constant deformation kinematics allows for the simplification of
physics-based models. In [9], the authors derived a linear mapping from tendon
tensions to actuator deformation analogous to traditional forward kinematics.
Combining physics-based methods with constant curvature kinematics lead to the
development of a model applicable to manipulators with an arbitrary number of
actuators while maintaining the simplicity of linearity. More recently, in [8]
the authors instead focus on how manipulator behavior arises geometry between
individual actuators. Similar to [9], force-moment-balances are solved for
within a manipulator with constant curvature kinematics. This time, by
formulating the physics upon individual actuator mechanics the model
generalizes across manipulators with different designs without the need for
parameter re-fitting.

Due to the lack of a general constant twist kinematics, a similar work that
applies physics-based modeling to constant twist manipulators does not yet
exist. Thus, in this work we present the first generalized formulation of
constant-twist kinematics, which we use to derive the first physics-based
model applied specifically to constant-twist manipulators.

##  III ACTUATOR KINEMATICS

We begin with a review of the Cosserat-rod parameterization of rod geometry to
provide background and establish our notation. We then present a re-framing of
constant curvature kinematics that leads to a straightforward derivation of a
model of manipulator geometry at unloaded equilibrium.

###  III-A Individual Rod Kinematics

In 3D Cosserat-rod theory, a rod is characterized by its center-line curve in
3D space $\bm{p}(s)\in\mathbb{R}^{3}$ and its material orientation $R(s)\in
SO(3)$. The reference parameter $s\in[0,1]$ denotes the location of the
current position along the length of the arm; ie, $\bm{p}(0)$ is the position
at the base of the rod while $p(1)$ the position at the tip.

The positions and orientations of frames along the rod can be combined into a
single 6-DOF pose $g(s)$, and element of the Lie group $SE(3)$, whose matrix
representation is a homogeneous transformation matrix. We denote the matrix
representation of a Lie group or Lie algebra element with $\rho(\cdot)$:

$\rho(g(s))=\begin{bmatrix}R(s)&\bm{p}(s)\\\ \mathbf{0}&1\end{bmatrix}$ (1)

Alongside the poses $g(s)$, we consider its rate of change along the length of
the rod:

$\frac{\partial}{\partial
s}\rho(g)=\rho(\dot{g})=\rho(g)\rho(\accentset{\scriptscriptstyle{\circleright}}{g})$
(2)

where the twist-vector $\accentset{\scriptscriptstyle{\circleright}}{g}(s)$ is
an element of the Lie algebra $\mathfrak{se}(3)$ \- a vector
$\begin{bmatrix}\bm{v}&\bm{\omega}\end{bmatrix}^{T}$ that contains the body-
frame linear and angular velocities $\bm{v}$ and $\bm{\omega}$. The matrix
representation is:

$\rho(\accentset{\scriptscriptstyle{\circleright}}{g}(s))=\begin{bmatrix}\rho(\bm{\omega}(s))&\bm{v}(s)\\\
\mathbf{0}&0\end{bmatrix}$ (3)

where $\rho(\bm{\omega})$ is the skew-symmetric matrix representation of an
angular velocity in $\mathfrak{so}(3)$.

The rate of change $\accentset{\scriptscriptstyle{\circleright}}{g}(s)$ alone
can parameterize a rod’s poses $g(s)$ along the entire rod for all $s$. In
general, body-frame twists
$\accentset{\scriptscriptstyle{\circleright}}{g}(s)$ along the length of the
rod can be converted to world-frame and numerically integrated to recover the
rod geometry. In the special case where the
$\accentset{\scriptscriptstyle{\circleright}}{g}(s)=\accentset{\scriptscriptstyle{\circleright}}{g}$
is constant over $s$, this integration can be done analytically through
exponentiation.

$g(s)=g(0)\circ\exp(s\ \accentset{\scriptscriptstyle{\circleright}}{g})$ (4)

Or, in matrix representation, with $\exp_{M}$ as the standard matrix
exponential:

$\rho(g(s))=\rho(g(0))\circ\exp_{M}(s\
\rho(\accentset{\scriptscriptstyle{\circleright}}{g}))$ (5)

Figure 2: Let $g_{A}(\cdot)$ and $g_{B}(\cdot)$ refer to poses along actuators
$A$ and $B$. Assuming constant curvature means assuming the cross-section
geometry ${}_{B}g_{BA}$ is uniform throughout the arm. This enables relating
the twist-vectors of different actuators
$\accentset{\scriptscriptstyle{\circleright}}{g}_{A}$ and
$\accentset{\scriptscriptstyle{\circleright}}{g}_{B}$ as the velocities of
segments on different paths (blue and yellow) that lead to the same
destination.

As an example, consider the twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{0}$ of an actuator in its
neutral configuration. In its neutral state, we assume an actuator takes on
its free-contraction length with zero shearing, torsion, or bending. This
free-contraction length $l(q)$ can be experimentally measured as a function of
the actuator input $q$, such as muscle pressure or tendon pulley angle. The
actuator’s neutral twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{0}$ is thus
$\begin{bmatrix}l(q)&0&0&0&0&0\end{bmatrix}^{T}$. Integrating this quantity
along the unit interval $[0,1]$ recovers the positions along the undeformed
rod.

Any rod with a constant twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}$ that deviates from this
neutral configuration $\accentset{\scriptscriptstyle{\circleright}}{g}_{0}$
must therefore entail deformation. Taking the difference
$\accentset{\scriptscriptstyle{\circleright}}{g}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0}$
gives us the deformation vector
$\Delta\accentset{\scriptscriptstyle{\circleright}}{g}=\begin{bmatrix}\lambda&\gamma_{y}&\gamma_{z}&\tau&\omega_{y}&\omega_{z}\end{bmatrix}^{T}$,
which respectively contains the stretch, y- and z-shearing, x-curvature
(torsion)-, and y- and z- curvature. We will later combine this deformation
vector with physical laws to calculate quantities such as force and elastic
energy.

Having covered both the twist-vector parameterization of elastic rod geometry
and deformation, we conclude our review of Cosserat rod kinematics theory for
individual rods.

###  III-B Relating Actuator Twists

For a constant-twist manipulator we derive a novel linear mapping from the
twist-vectors of each actuators to the twist-vector of the manipulator base-
curve. We do so by generalizing the geometric constraints between the
actuators.

It is established that for manipulators assumed to be constant-curvature, the
uniformity of the cross-section geometry is crucial to the validity of the
constant-curvature assumption [10]. Works on constant-twist manipulators
similarly rely on designs with a uniform helical winding pitch [2]. In designs
where actuators are attached to a backbone with consistent cross-sections,
this uniformity is inherited from the backbone. Other designs lack a
continuous backbone and employ a set of inter-actuator separators: enough
separators are used so that constraints between actuators are still enforced,
even at points not directly constrained by separators, as in Fig. 2. For
either design, the result is the same: a uniform cross-section geometry leads
to the constant deformation throughout the manipulator.

Note that this is only true in the un-loaded case: the addition of external
loads causes deformation that is no longer uniform throughout.

Consider a planar arm with two actuators $A$ and $B$, as depicted in Fig. 2.
We can represent the cross-section geometry as the transformation from a frame
attached to actuator $B$ to a frame attached to actuator $A$:

$g_{A}(s)=g_{B}(s){}_{B}g_{BA}$ (6)

where ${}_{B}g_{BA}$ is an element of $SE(3)$ representing the transformation
from a frame on actuator $B$ to a frame on actuator $A$, with respect to the
frame on $B$. Note that due to the uniform geometry, this transformation is
constant across the entire manipulator. We can exploit this fact to relate the
twist-vectors between different actuators.

Expressing the poses $g(s)$ along each actuator by exponentiating the twist-
vectors using Eqn. 4 gives:

$g_{A}(0)\circ\exp(s\
\accentset{\scriptscriptstyle{\circleright}}{g}_{A})=g_{B}(0)\circ\exp(s\
\accentset{\scriptscriptstyle{\circleright}}{g}_{B})\circ{}_{B}g_{BA}$ (7)

Eqn. 6 gives us $g_{A}(0)^{-1}g_{B}(0)={}_{B}g_{BA}^{-1}$, which we substitute
in:

$\exp(s\
\accentset{\scriptscriptstyle{\circleright}}{g}_{A})={}_{B}g_{BA}^{-1}\circ\exp(s\
\accentset{\scriptscriptstyle{\circleright}}{g}_{B})\circ{}_{B}g_{BA}$ (8)

We have thus arrived at the common definition for the Adjoint matrix. Thus the
twist-vectors $\accentset{\scriptscriptstyle{\circleright}}{g}_{A}$ and
$\accentset{\scriptscriptstyle{\circleright}}{g}_{B}$ are related via the
Adjoint matrix in $SE(3)$:

$\accentset{\scriptscriptstyle{\circleright}}{g_{A}}=\textrm{Ad}_{BA}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{B}$
(9)

The structure of the Adjoint matrix $\textrm{Ad}_{BA}$ depends on the Lie
group used to parameterize actuator geometry, such as $SE(2)$ or $SE(3)$. The
values of the matrix depend on the cross-section geometry: ${}_{B}g_{BA}$. For
a full derivation of the Adjoint map and matrix representations in common
groups we refer the reader to [11]. 3 Note that so long as the cross-section
geometry is uniform, one can apply equation 9 to any imagined actuator given
its location in the manipulator cross-section. For a general manipulator with
an arbitrary number of actuators, we therefore relate all actuators to a
single reference actuator. If the reference actuator is considered at the
center of the manipulator, it traces the base-curve of the manipulator. Thus,
for any actuator $i$ with transformation ${}_{i}g_{oi}$ relative to the base-
curve, its twist-vector $\accentset{\scriptscriptstyle{\circleright}}{g}_{i}$
can be related to the base-curve twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{o}$ by the Adjoint matrix
$\textrm{Ad}_{oi}$

$\accentset{\scriptscriptstyle{\circleright}}{g}_{i}=\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}$
(10)

We have thus shown that assuming the cross-section geometry is uniform
throughout the continuum manipulator, the twist-vector of every actuator in
the manipulator can be related to the twist-vector of the manipulator base-
curve via the Adjoint map. This concludes the derivation of our kinematic
model.

##  IV Equilibrium Mechanics Model

We now derive our equilibrium manipulator model by formulating the elastic
energy stored within each actuator, and solving for the manipulator
configuration that minimizes the total energy. Note once again that this model
does not consider external loading.

Combining the deformation vector
$\Delta\accentset{\scriptscriptstyle{\circleright}}{g}$ from before with a
Hookean spring law with stiffness matrix
$K=\textrm{diag}(\begin{bmatrix}K_{\epsilon}&K_{\gamma}&K_{\gamma}&K_{\tau}&K_{\kappa}&K_{\kappa}\end{bmatrix})$,
we arrive at the standard expression for local wrench moments throughout the
length of the rod:

$\bm{f}=K(\accentset{\scriptscriptstyle{\circleright}}{g}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0})$
(11)

Note that by being a spatial-velocity, the linear component $\bm{v}$ of
$\accentset{\scriptscriptstyle{\circleright}}{g}$ has units [m/m] and is thus
unitless, while the angular component $\bm{\omega}$ has units [rad/m]. Thus
the linear stiffness constants $K_{\epsilon}$ and $K_{\gamma}$ are have units
of [N], and the rotational stiffness constants $K_{\tau}$ and $K_{\kappa}$
have units of [N m2], because radians are unitless.

One can also integrate these local wrenches to find the elastic energy stored
within a rod:

$\displaystyle U(q)$
$\displaystyle=\int_{\accentset{\scriptscriptstyle{\circleright}}{g}_{0}}^{\accentset{\scriptscriptstyle{\circleright}}{g}}K\bm{r}\cdot
d\bm{r}$ (12)
$\displaystyle=\frac{1}{2}(\accentset{\scriptscriptstyle{\circleright}}{g}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0})^{T}K(\accentset{\scriptscriptstyle{\circleright}}{g}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0})$
(13)

Remember that $\accentset{\scriptscriptstyle{\circleright}}{g}_{0}$ is
dependent on the actuator configuration $q$. We will now use the Adjoint
mapping between actuator twist-vectors to derive our equilibrium model.

The total elastic energy stored within a manipulator is the sum of the elastic
energy stored in each actuator. For a manipulator with $n$ actuators, using
Eqn. 12 we can express the total energy as:

$U_{T}=\sum_{i=1}^{n}U_{i}=\frac{1}{2}\sum_{i=1}^{n}(\accentset{\scriptscriptstyle{\circleright}}{g}_{i}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})^{T}K(\accentset{\scriptscriptstyle{\circleright}}{g}_{i}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})$
(14)

Using Eqn. 10, we can then represent each actuator’s twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{i}$ in terms of the base-
curve twist-vector:

$\displaystyle U_{T}$
$\displaystyle=\frac{1}{2}\sum_{i=1}^{n}(\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})^{T}K(\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}-\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})$

Expanding the quadratics gives:

$\displaystyle
U_{T}=\frac{1}{2}\sum_{i=1}^{n}(\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{T}\textrm{Ad}_{oi}^{\textrm{-}T}K\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}-2\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{T}\textrm{Ad}_{oi}^{\textrm{-}T}K\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}}+\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}}^{T}K\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})$

Recall that the reference twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}}$ for each rod is a
function of its configuration $q_{i}$, where
$\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}}=\begin{bmatrix}l(q_{i})&0&0&0&0&0\end{bmatrix}^{T}=l(q_{i})\mathbf{e_{1}}$
and $\mathbf{e_{1}}$ is the first basis vector. The equilibrium base-curve
twist-vector $\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}$ is the
collective actuator configuration where the gradient of the elastic energy is
the zero vector. Taking the gradient, setting it to zero, and solving for
$\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}$ gives the following:

$\displaystyle\nabla U_{T}$
$\displaystyle=\sum_{i=1}^{n}(\textrm{Ad}_{oi}^{\textrm{-}T}K\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}-\textrm{Ad}_{oi}^{\textrm{-}T}K\accentset{\scriptscriptstyle{\circleright}}{g}_{0_{i}})=\mathbf{0}$
$\displaystyle\Rightarrow\sum_{i=1}^{n}\textrm{Ad}_{oi}^{\textrm{-}T}K\textrm{Ad}_{oi}^{\textrm{-}1}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}=\sum_{i=1}^{n}\textrm{Ad}_{oi}^{\textrm{-}T}K\mathbf{e_{1}}l(q_{i})$

The $\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}$ in the LHS of
the above equation can be factored out of the sum. The RHS equation can be
simplified by noting that it is a linear combination with each $l(q_{i})$
element. Combining each $l(q_{i})$ element into a vector gives the following:

$\sum_{i=1}^{n}(\textrm{Ad}_{oi}^{\textrm{-}T}K\textrm{Ad}_{oi}^{\textrm{-}1})\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}=\\\
\begin{bmatrix}\textrm{Ad}_{o1}^{\textrm{-}T}K\mathbf{e_{1}}&\textrm{Ad}_{o2}^{\textrm{-}T}K\mathbf{e_{1}}&\dots&\textrm{Ad}_{on}^{\textrm{-}T}K\mathbf{e_{1}}\end{bmatrix}\begin{bmatrix}l(q_{1})\\\
l(q_{2})\\\ \vdots\\\ l(q_{n})\end{bmatrix}$ (15)

Using
$\mathbf{A}=\sum_{i=1}^{n}(\textrm{Ad}_{oi}^{\textrm{-}T}K\textrm{Ad}_{oi}^{\textrm{-}1})$
as the sum of the Adjoints scaled with elastic constants, the matrix
$\mathbf{D}=\begin{bmatrix}\textrm{Ad}_{o1}^{\textrm{-}T}K\mathbf{e_{1}}&\textrm{Ad}_{o2}^{\textrm{-}T}K\mathbf{e_{1}}&\dots&\textrm{Ad}_{on}^{\textrm{-}T}K\mathbf{e_{1}}\end{bmatrix}$,
and vector
$\mathbf{l}=\begin{bmatrix}l(q_{1})&l(q_{2})&\dots&l(q_{n})\end{bmatrix}^{T}$
as the vector of actuator neutral lengths dependent on pressure, we can
represent Eqn. 15 in compact matrix form:

$\mathbf{A}\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}=\mathbf{Dl}$
(16)

As a final step, the neutral configuration can be solved for by taking the
pseudo-inverse of $\mathbf{A}$:

$\accentset{\scriptscriptstyle{\circleright}}{g}_{o}^{*}=\mathbf{A^{+}Dl}$
(17)

This concludes our derivation of our simplified rod model for the equilibrium
configuration of an unloaded constant twist manipulator.

###  IV-A Example In Coordinates

We now provide the coordinate representation of our model of a two-actuator
planar arm with width $2d$. Since the planar arm exists entirely in two
dimensions, we embed the model in $SE(2)$. By using the $SE(2)$ twist-vector
form
$\accentset{\scriptscriptstyle{\circleright}}{g}=\begin{bmatrix}\lambda&\gamma&\kappa\end{bmatrix}$
respectively containing length, shear, and curvature, and the $SE(2)$ Adjoint
as derived in [11], Eqn. 17 takes on the following form:

$\begin{bmatrix}2K_{\epsilon}&0&0\\\ 0&2K_{\gamma}&0\\\
0&0&2K_{\kappa}+2K_{\epsilon}d^{2}\end{bmatrix}\begin{bmatrix}\lambda\\\
\gamma\\\ \omega\end{bmatrix}=K_{\epsilon}\begin{bmatrix}1&1\\\ 0&0\\\
-d&-d\end{bmatrix}\begin{bmatrix}l(q_{1})\\\ l(q_{2})\end{bmatrix}$ (18)

Because $K_{\epsilon}l(q_{i})$ is proportional to the stress experienced in
each actuator, our model is equivalent to that of [9]. However, our model
generalizes to different manipulator designs, and crucially is able to predict
how the overall manipulator bending stiffness $2K_{\kappa}+2K_{\epsilon}d^{2}$
changes with width - something that was left experimentally determined in [9].
As that work was performed on cable-driven manipulators while ours is with
pneumatic ones, our model generalizes to manipulators with different types of
actuation.

##  V EXPERIMENT RESULTS

We now describe the setup, methodology, and results of validating our
constant-twist equilibrium model on planar, spatial, and helical arms. We
first describe the construction of our three manipulators with adjustable
dimensions. We then outline the procedure of each experiment, as well as the
parameter characterization process. Finally, we present the experiment
results, concluded by a discussion of the sources of error.

###  V-A Manipulator Construction

Individual actuators were constructed identically to [4], but with an
effective length of 460mm. These actuators were mounted to three different
types of actuator-separators to form our array of manipulators. Constraining a
constant distance between two muscles creates a planar arm capable of purely
planar bending. A spatial arm which can bend in three dimensions is created by
adding a third muscle to form a radial arrangement. Finally, tilting the
actuator mount relative to the separator creates a pitch, and thus a helical
arm that both bends and twists. Pictures of the three actuators can be found
in Fig. 1.

For all manipulators, the muscles were held in place by clamps which
constrained their position but allowed them to rotate in place. These clamps
were then rigidly attached to muscle separators with adjustable mounting
positions, enabling the effective width or diameter of the arms to be easily
reconfigured. The planar arm was tested with widths of 50.8mm, 76.2mm, and
101.6mm widths. The spatial arm was tested with diameters of 50.8mm and
101.6mm. The helical arm was only tested at a diameter of 50.8mm.

###  V-B Experiment Procedure

Laminated Apriltag [12] fiducial markers were first attached to the
manipulators using adhesive putty to enable motion capture. The experiments
were then conducted as a series of static measurements. Using a bicycle pump,
the manipulator was inflated to a desired pressure, which was maintained
constant by an inline pressure regulator. Then, with a Logitech c920 webcam,
we capture video footage of the fiducials which we later localize using
TagSLAM [13]. Finally, the twist-vector
$\accentset{\scriptscriptstyle{\circleright}}{g}_{o}$ of the manipulator base-
curve is fitted based on the measured tag positions through a nonlinear
optimization solved with Matlab’s `fminsearch` function. Between three to five
such captures are performed for each muscle inflation pressure, and their
median is taken as the final result.

###  V-C System Characterization

Figure 3: Fitted artificial muscle free-contraction line.

Our equilibrium model is characterized by two sets of parameters. The first is
the mapping from muscle inflation pressure to neutral length $l(q)$, which we
refer to as the free-contraction model. The second parameter is the individual
muscle stiffness matrix $K$.

The contraction model was experimentally measured by inflating a single
unladen muscle to various pressures within our activation range and measuring
its length. As in [8], we then fit a fifth-order polynomial over the measured
values to arrive at our fitted model. The results can be seen in Figure 3a.

For the muscle stiffness matrix $K$, appropriate assumptions help simplify the
full matrix to a single value: the ratio of strain stiffness $K_{\epsilon}$ to
bending stiffness $K_{\kappa}$. First, because the muscles are not actually
rigidly clamped to the separators and instead are allowed to rotate in place,
we assume the effective torsional stiffness $K_{\tau}$ of a single muscle is
0. Second, we assume the muscles experience zero shearing, and thus have
infinite shearing stiffness $K_{\gamma}$; in practice, we’ve found that using
any shearing stiffness $K_{\gamma}$ more than ten times the strain stiffness
$K_{\epsilon}$ had the same effect. Finally, we note that in equation 15 any
scale factor of K would cancel each other out; we can therefore eliminate
$K_{\epsilon}$ by normalizing K with $K_{\epsilon}$, and just consider the
normalized bending stiffness $\bar{K_{\kappa}}=K_{\kappa}/K_{\epsilon}$. To
recap, our final stiffness matrix is therefore $K=\textrm{diag}(1,\ 10,\ 10,\
0,\ 0,\ \bar{K_{\kappa}})$. Recall the units of $K_{\epsilon}$ and
$K_{\gamma}$ are [N], and the units of $K_{\tau}$ and $K_{\kappa}$ are [Nm2].

In practice, our value for $\bar{K_{\kappa}}$ is taken as the value that
minimizes tip position error for a single calibration dataset. This value is
then used for all other datasets.

###  V-D Model Accuracy

Figure 4: Planar (Top), and Spatial (Bottom) arm model accuracy for a range of
arm diameters. Left: Model vs. experimentally measured length-scaled
curvature, which for a 2D arm is equivalent to the bending angle. Right:
Manipulator tip position error normalized by the length of the manipulator.

We reiterate that the model in all following tests are parameterized by the
same stiffness matrix $K$ and contraction model $l(q)$.

For each type of manipulator tested, we provide two metrics. The first is the
accuracy of the arc-length-scaled-curvature over pressure, which we directly
get through the corresponding component in the base-cure twist-vector. The
authors of [8] used the same metric but with the true-curvature. The second is
the tip position error: the difference between predicted and measured
positions of the manipulator tips, normalized by the actuator’s length. This
metric is used in [5].

Figures 4 shows the experiment results of a two-muscle planar and a three-
muscle spatial arm at all dimensions tested. For each arm a single muscle was
actuated to induce a planar bending. The bending plane was oriented such that
the arms were evenly supported by a flat tabletop at all times, eliminating
the effects of gravity.

Figure 5: Expected vs. measured geometry of a helical arm. Left to right: red
muscle inflated at 103kpa, 241kpa, and 345kpa.

Figure 6: Helical arm results model accuracy results. Left: Model vs.
experimentally measured X- and Z-curvature over pressure. Right: Same as
before - manipulator tip position error normalized by the length of the
manipulator.

Unlike the planar and spatial arms, helical arm bending is not relegated to a
single bending plane. Thus, the effects of gravity could not be prevented, and
there are now also two curvature components to consider: the X- and
Z-curvature. Figure 6 shows the model accuracy results from actuating a
helical arm, where the model has less than 6% normalized tip position error at
maximum inflation..

Prior works on helical manipulators also considered the helix winding radius
and helix pitch error. At maximum inflation (380kpa), we measured a winding
radius error of 4%, and a pitch error of 16%.

###  V-E Sources of Error

Because our model does not account for external loading, the largest sources
of error were external forces such as static friction or gravity, which were
most prominent at lower pressures. This can be seen in the planar and helical
dataset tip position error trends. Our use of TagSLAM and manually attached
Apriltags also likely introduced $\sim$1cm of error to each tag’s position
estimate. Finally, our assumption of zero torsional stiffness is likely the
cause of the incorrectly-trending x-curvature in Fig. 6. This can be accounted
for in future experiments by actuator mounts that prevent them from rotating
in-place, as done in [8].

##  VI CONCLUSION

In this work, we presented a linear kinematics model that generalizes across
all constant curvature and twist manipulators. Using these kinematics, we
derived a physics-based mechanics model that is analogous to traditional robot
forward kinematics, and generalizes across multiple manipulator designs
without the need for re-parameterization. This model was implemented and
tested for a wide array of manipulators, and showed less than 10% of error
when using a single characterization across all designs.

We are excited for future work that applies this model to control and planning
of complex constant curvature or twist manipulators that fully exploits the
linear forward and inverse kinematics. Potential projects can include finding
manipulators and use-cases where the unloaded model is useful for planning and
control, as well as extending this model to account for loading while
maintaining simplicity. This will bring us another step closer to building
continuum manipulators that can reach the full capacity of cephalopod
tentacles and elephant trunks.

## ACKNOWLEDGMENT

This work was supported by the National Science Foundation (NSF) Research
Experience for Undergraduates (REU) program. The authors thank Emma Waters for
being a wonderful project partner and friend, Luke Raus for his feedback on
figure design, and all friends for their support.

## References

  * [1] Xiaoqian Chen et al.  “A review of soft manipulator research, applications, and opportunities”  In _Journal of Field Robotics_ 39.3 John WileySons Inc, 2022, pp. 281–311  DOI: 10.1002/rob.22051
  * [2] Laura H. Blumenschein et al.  “Helical actuation on a soft inflated robot body”  In _2018 IEEE International Conference on Soft Robotics (RoboSoft)_ IEEE, 2018, pp. 245–252  DOI: 10.1109/ROBOSOFT.2018.8404927
  * [3] Julia Starke, Ernar Amanov, M. Chikhaoui and Jessica Burgner-Kahrs  “On the merits of helical tendon routing in continuum robots”  In _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ IEEE, 2017, pp. 6470–6476  DOI: 10.1109/IROS.2017.8206554
  * [4] Farhan Rozaidi et al.  “A Locomotive Study of the Helical Inflating Soft Snake Robot (HISSbot)”, 2022
  * [5] D. Rucker and Robert J. Webster  “Statics and dynamics of continuum robots with general tendon routing and external loading”  In _IEEE Transactions on Robotics_ 27.6, 2011, pp. 1033–1044  DOI: 10.1109/TRO.2011.2160469
  * [6] Robert J. Webster and Bryan A. Jones  “Design and kinematic modeling of constant curvature continuum robots: A review”  In _International Journal of Robotics Research_ 29.13, 2010, pp. 1661–1683  DOI: 10.1177/0278364910368147
  * [7] Federico Renda et al.  “Dynamic model of a multibending soft robot arm driven by cables”  In _IEEE Transactions on Robotics_ 30.5 Institute of ElectricalElectronics Engineers Inc., 2014, pp. 1109–1122  DOI: 10.1109/TRO.2014.2325992
  * [8] Gina Olson, Ross L. Hatton, Julie A. Adams and Yiğit Mengüç  “An Euler–Bernoulli beam model for soft robot arms bent through self-stress and external loads”  In _International Journal of Solids and Structures_ 207 Elsevier Ltd, 2020, pp. 113–131  DOI: 10.1016/j.ijsolstr.2020.09.015
  * [9] David B. Camarillo et al.  “Mechanics modeling of tendon-driven continuum manipulators”  In _IEEE Transactions on Robotics_ 24.6, 2008, pp. 1262–1273  DOI: 10.1109/TRO.2008.2002311
  * [10] Changquing Li and Christopher D. Rahn  “Design of Continuous Backbone, Cable-Driven Robots”  In _Journal of Mechanical Design_ 124.2, 2002, pp. 265–271  DOI: 10.1115/1.1447546
  * [11] Joan Solà, Jeremie Deray and Dinesh Atchuthan  “A micro Lie theory for state estimation in robotics”, 2018
  * [12] John Wang and Edwin Olson  “AprilTag 2: Efficient and robust fiducial detection”  In _2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ IEEE, 2016, pp. 4193–4198  DOI: 10.1109/IROS.2016.7759617
  * [13] Bernd Pfrommer and Kostas Daniilidis  “TagSLAM: Robust SLAM with Fiducial Markers”, 2019