diff --git a/.gitignore b/.gitignore index a0f2dbd..454d569 100644 --- a/.gitignore +++ b/.gitignore @@ -16,5 +16,16 @@ CMakeLists.txt.user # Ignore PDFs on master literature/ -# Pictures stuff +# Pictures *.png +# But not the necessary ones +!docs/figures/**/* + +# Presentation stuff +*.pptx + +# fuck apple +.DS_Store + +# Latex Stuff +build/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..5369275 --- /dev/null +++ b/README.md @@ -0,0 +1,64 @@ +# Teleoperation NAO + +## Project description + +This ROS package allows you to remotely control a NAO robot by standing in +front of a webcam and having two ArUco markers on the hands and one ArUco +marker on the chest. + +You can move a NAO around using a "Human Joystick" approach and make NAO +imitate your arm motions. For more details, read our +![report](docs/report.latex). + +Our package relies on the NAO being reachable from the computer and the +environment variable `NAO_IP` being set to the IP address of the NAO. + +Our main code is written in Python and located in `script/` with the ArUco +detection and rviz GUI logic in C++ in `src/`. + +## Requirements + +* ROS Indigo on Ubuntu 14.04 +* [aruco_ros](http://wiki.ros.org/aruco_ros) +* [nao_robot](http://wiki.ros.org/nao_robot) +* [nao_meshes](http://wiki.ros.org/nao_meshes) +* [usb_cam](http://wiki.ros.org/usb_cam) + +We installed the dependencies by cloning them to our workspace and running +`catkin_make`, but `sudo apt install ros-indigo-package-name` should work even +better. + +## Usage + +Hang an ArUco #0 marker on the chest, take the #1 in the left hand and #2 in +the right hand. The markers should be 15cm high/wide. Then you can run + +```sh +$ roslaunch teleoperation calibration.launch +``` + +The NAO will tell you what to do. + +After that you can launch the teleoperation routine by running. + +```sh +$ roslaunch teleoperation fulltest.launch +``` + +If you want our awesome GUI, run + +```sh +$ roslaunch nao_bringup nao_full.launch +$ roslaunch teleoperation with_gui.launch +``` + +If any problems arise with the GUI, such as the NAO not getting properly +displayed, try pointing the rviz to the correct NAO `.urdf`, which should be +somewhere in the `nao_description` package. + +This diagram depicts what you should expect from the teleoperation routine. + +![State Machine](docs/figures/teleoperation_overview.png) + +Since this package relies on the NAO voice recognition, you must be in the same +room with NAO, and the room must be quiet. diff --git a/docs/figures/aruco.png b/docs/figures/aruco.png new file mode 100644 index 0000000..9c45960 Binary files /dev/null and b/docs/figures/aruco.png differ diff --git a/docs/figures/interface_nao.png b/docs/figures/interface_nao.png new file mode 100644 index 0000000..70189b9 Binary files /dev/null and b/docs/figures/interface_nao.png differ diff --git a/docs/figures/joint_jacobian.png b/docs/figures/joint_jacobian.png new file mode 100644 index 0000000..693e025 Binary files /dev/null and b/docs/figures/joint_jacobian.png differ diff --git a/docs/figures/joint_jacobian.xml b/docs/figures/joint_jacobian.xml new file mode 100644 index 0000000..de206f0 --- /dev/null +++ b/docs/figures/joint_jacobian.xml @@ -0,0 +1 @@ +7Vddb9sgFP01fqwU48RpHpu06Sat06RsWrc3aqhNho2HceP01+9i8Ad2+rUlmjTtpeUe7sVwzj2IeMEqra4lzpMbQSj30IRUXnDpIbSYTeCvBvYGmE99A8SSEQP1gA17pBa0dXHJCC2cRCUEVyx3wUhkGY2Ug2Epxc5Nuxfc/WqOYzoCNhHmY/QrIyqxqB8uuol3lMWJ/fQ5mpuJFDfJ9iRFgonY9aDgygtWUghlRmm1olxz1/Bi6tZPzLYbkzRTrym4/Xj9ucDft3SP56vtl/z9I0nOzs0qD5iX9sB2s2rfMACrANkQLHcJU3ST40jP7EBuwBKVcoh8GOIiNwrcs4rCR5f3IlNWUT/UMeN8JbiQAGQi00sSXCQ6t16gUFL8aGkGgpZ2d1QqWj15bL8lE5qQipQquYcUW4Cmln/bgCi08a6TM2hykp6SLYhtC8Xt2h3LMLBEv4H06YhjKcqM1DxMXE5pxdRtb/zNpujxZdUP9k2QwRZv+8G3pl4HXVEdNVVbqtTeaoVLJQASUiUiFhnmH4TI2zXIhXZVpyAga6bPX6/jShi0ElIystlAQOBDlDKiz/DW+B/LmKpn8tDhhpCUY8Ue3H0cXVz/pI7qOchDQRAsFut1S/vAW0cwT7BwzTP1x+Y55J2TWcf3x2xCa21saE/+op0mb7BT2/ARx0XBIqfn/Td4Z3gdntQrf+oBW/pJMPhy2w/T2eAyDQZCG2/aqoHW7TZ+X370L5lr5g/IXPxlc81G7EoPhVxpDso7h+bwZ6nfLXVPnxV1U19AAgrzqianmYdRrP9vTUeYxWBz9XpmaqQgMKpcqQ4qcOA1gTmLM+1UUIACvtT6MHjMXdiJlBHCn+oN99bouRWFNl7jlHGt1gqnd5Jh2PdN/cwz2fYI6Ai9EQxeLa3xer0xO/RoOVVvhKfrDbhN/3fG62+N6ezF9+z8OLcGhN3PE3OFd7/xgqtf \ No newline at end of file diff --git a/docs/figures/posture_retargeting.xml b/docs/figures/posture_retargeting.xml new file mode 100644 index 0000000..a85b276 --- /dev/null +++ b/docs/figures/posture_retargeting.xml @@ -0,0 +1 @@ +5VjbUtswEP2azNDOpOMLdsojCYE+AO1MygCPSqzYBttrZJk4/fpK1voWN6kJAdzpSyIdaVfalfbsygNzEmYXjMTeFTg0GBiakw3Ms4FhnFia+JXAWgGjY10BLvMdBdWAmf+LIohybuo7NGlM5AAB9+MmuIAoogvewAhjsGpOW0LQXDUmLm0BswUJ2uit73APUd0+qQa+Ud/1cOmvxkgNhKSYjJYkHnFgVYPM6cCcMACuWmE2oYH0XeEXJXe+ZbTcGKMR7yJgLWazZP40dTPTCafT6/Hd481QN5SaZxKkaPHAsAOhcLwEoVd4jCzUgP2Uyq2OJyScM5+IoStpYomLliv/WaEgSefSbL4Omgqk4mGSn/SpmKCP4qytJR0Yk8SDNHBoqVDYlutUM8plYhLtv84RzB8+1RfItZUrkDAWjWieyL9cu3Y4+xYeTXhfjBu+l2lHxbHWd6YuW7Ezo7G4QR0RktgFxj1wISLBtELHDNLIofKia6Ln8TAQTV00aebzOwl/sbB3j5Nk+yyrd9ZFJ+JsXROS3fv6WCWW9wq5B8r5GhmMpBwEVO32EiDGPSWcwWPJJga6FQX1UemBemhjtCeQMgzHLfGMFEmYS/mOeRj20rO1BZA4LiiEVFgmbwQNCPefm2RIkFPdcl5FO6KBzPMSFmqR0E0iw16bqGukTQCY40eEi1SweTuqs5fOXXk+p7NYkdZKpKTmfah72t7l6WfKOM12+qYYHSHBY4YrCX9VyxcF5tVSha29kTtPtnL66+iEbaeS17NGROA/oPze2ScumqFdHqUq3ggLhaT2OceuT7+X0OEYuStL7svCf2dX2T8noR9IkY1q5jXca3TkXqsj9yKfDHUTiaIzGaOuH+DnJRwSlbVJVNYGAamdo1S9kiwUFRNhuUwob/FUuff9qatdjmImmJWB0+tkYG/6WPvoZKC3k2s/KnxF+BxYAr1hwwOyfU/zmTE+sIU7DvBdK/3DVd9/fjMUr4mXvRnKTLYlW+2fbayO2ab4kNKTUt9qsZEqM/5RfjePP5zftS0e/akCs+lOFYZz1inqbRH1ebhvBH7vTmXzCWa+4RNMdKtvdqrsqT58mtPf \ No newline at end of file diff --git a/docs/figures/rviz_human.png b/docs/figures/rviz_human.png new file mode 100644 index 0000000..fa333fa Binary files /dev/null and b/docs/figures/rviz_human.png differ diff --git a/docs/figures/teleoperation_overview.png b/docs/figures/teleoperation_overview.png new file mode 100644 index 0000000..c96247b Binary files /dev/null and b/docs/figures/teleoperation_overview.png differ diff --git a/docs/figures/teleoperation_overview.xml b/docs/figures/teleoperation_overview.xml new file mode 100644 index 0000000..48b7912 --- /dev/null +++ b/docs/figures/teleoperation_overview.xml @@ -0,0 +1 @@ +7Vvbdps4FP0aPyYLEDc/xs61ndtaTlamfZNBNrQYUZATu18/Eki2BCQmDhjSTh5idBACztna2joSIzBdbW5SmAR/Yh9FI0PzNyNwOTKMsaXR/8ywLQyOqReGZRr6hUkyzMKfiBv5dct16KNMqUgwjkiYqEYPxzHyiGKDaYqf1WoLHKl3TeASVQwzD0ZV62Pok4BbdXu8P3GLwmXAb+0aTnFiBUVl/iZZAH38LJnA1QhMU4xJcbTaTFHEfCf8Ulx3/cLZ3YOlKCZNLggenW+WHmzd61t9dm+ZiXH99cw0imaeYLTmb8yflmyFC1K8jn3EWtFGYPIchATNEuixs8805tQWkFVESzo9XOCY8CjqNi9PcYTTvC2g5X/MHkaRsMc4pvUnPsyC/Dasnerb8Rd+QilBG8nE3/YG4RUi6ZZWEWeF57ciaLz8vA8kcLktkGJoCCPk4Fnu2t77lx5wF7/B3bpZ4277x5phYPI5ZA2IUiUMyKfA5EWckgAvcQyjq711ogZKCgrahORfZj53LF78wmux48uNXNiKQkzfWL6Klb+IFllhf11eEhd+Q4RsOQbgmmBq2j/vHxgnAioSBCg0fAu5vkntGUnxdySdcY05sO3dGdEPjcNwMy/H4xxuL8Ipw+vUQ6+FjJMOTJeIvFLPLeqxKL0KzhRFkIRPKr20DjQBfQlo15Bd1gWoZEzpTTGlQErrBlIL10OeVwepuWuZlnYMpBigTgQpevtBYQp0AR9dAo/ET4fwo6BHIqiWOQkid1ELINtz0XxRC6DugWEMCxdu3aAWEd53cnEm/LYf3YDp59wsmewl/+WWi3SVKeej4ve6aFZU74DSFEwaDTGpq5g02sJkA4p680haJjXnUnOck2DXHBR29Qp073yKkZclsN5AAh8VDTmeLYhfYKviFxhV8bsTyLL4tbvSvmZVkrRDEze4AUkU5nl6OtpoPJRJTNGmFmpRRtfO2mRoH08bblMt1BdtnH2azIJ5cOVk+K+b2QMaaz/cGtroSl0fJ49KQ1FX8ugE+roU8I5BZr/Kqdo5cDR+z8Y4y1u7SFO4lSokOIxJJt3sH2bY38lxVPY2bEvG7MH6pmuVMF48Qelq8Th4scioY8q9YueB40m/Kg1nEULJLzDAWoZ5bqlB6n2I1TuZoQ1pgt/qbOtjpnb06uAzhVE4T+m9cfzOntVFP1F7SV0W9sS9pEpKw8qNqYP3xx27j+mU5puG6sF0SlDtlO1MbmaEBrCPHMiw8nIfIwfSGLx9YRc62tndY+QC78L9Oru/n2+de5FN7G1N6pglKRVmXaba3q0121+SagqzYemW6tLn3SokbYiWIUwHdDC0fJtRHZJmBMY+NT1UJ2B9iZ2ukuYfQ+0IFXP6rnxUWsIs7aiwQGnLSTktMX5ffQO8ksZoK0UhYlCZDlBnepgywLZVeloskF2PRN8Zz4ssa/fLAabZNz3p42HPwn5vYjI+5jTMMDvBjzIV6ns5uN8tCo2BMSxcVLjmEUbfw3jZrvI8HIoOqN0enPIEoOLuhwyl1LLCtXKfvilRPZuiLPwJ53kF1jm4MqG1rcnIuqQW1kUy7klWjMJlTI8jtGBNMfeFHowuuJllUWhcaCRp0O/zvnRmHmZSTQPFjKGFOFl6KU51Q7BWtx21szh1tXFnGuEM/Z+1ancJniOx/SV40DSfwNX4UEgdVDMKt3R+S+cvGk5Qyysh704v7vZQtEAl5WSDbvVO+VU13w6V/J1Hsswk8j6e33pX4OC5xWrKLcPaFyieu4ZbPDa4+b8wu5RzBf2zi1nP9Dmr0M6DWQMJzrKQCUaJIqT4CAZhfjsrdOMFraC7yUbhmxcJpKpR1eDwj5hqvmsS0tSjIaEquCpOV6Hv5xRUhxqVlpRvpGQUud1iwnAPr+JToFQxAd6OCVrcfxVXZPn2nxaCq/8A \ No newline at end of file diff --git a/docs/figures/usr_pt.png b/docs/figures/usr_pt.png new file mode 100644 index 0000000..57d1ff2 Binary files /dev/null and b/docs/figures/usr_pt.png differ diff --git a/docs/figures/usr_pt.xml b/docs/figures/usr_pt.xml new file mode 100644 index 0000000..e05044e --- /dev/null +++ b/docs/figures/usr_pt.xml @@ -0,0 +1 @@ +7Vjbcps6FP0aPzoDiJsfbSfuSafttEkvc85LRwGBdSIjKuRbvr5bIDAXuXETZ5qH+iFBS0KCvdZe2mKE5qvdG4Hz5XseEzZyrHg3QpcjxwkmDvxVwL4CfDusgFTQuILsA3BLH4gGLY2uaUyKzkDJOZM074IRzzISyQ6GheDb7rCEs+6qOU7JALiNMBui32gslxUaOsEB/4fQdFmvbPuTqmeF68H6TYoljvm2BaGrEZoLzmV1tdrNCVOxq+NS3bc40ts8mCCZPOWG/N9FLL6FfHY9QeRt+vXDJ+9hrMnYYLbWL6wfVu7rCMAsEGxozLZLKsltjiPVswW6AVvKFYOWDZe4yCsGErojsOiM4TvCZji6TwVfZ/GcMy6gO+OZmi3hmdR8265u10NGDrIshHxf4ZSx3q0xLpZqhXJZ/QZESLI7Ghq7CTgIlfAVkWIPQ/QNjqc50iK16/b2QLlraWzZorsBsZZZ2sx9YAIuNBlmYsZR8ANfvp3z63e3/mZPbz7NrLHzksS0QwqhTsKIRBHghRT8nrR67kLPhVhosvTSx8JrIOFoxFHgdSLu1O1WxBsW2hFvwPNHPBiE/D3fEECUhLdYxAMGIKVzdVnQLGVkquxGqZMKCDflmerha/U6jzH060ypermIiTDlUEMLTMkFfQAMq4kVb6UF1sZlXbjOxPabX1DmXTlEJ6J1gQLkhy7Yr+f4qMrLrlhiTMLEKBY/Csld8otMPoNuXLubqa4z1E1okk097uyyQciQqT6TOg4dwfg/1rzuGBdlyKcwwA7zXRmeuh+uUvV/tk4SImDEf4puPSs8ZTVxNWYgSoik7Kqry1QtnaGtYkZTJdoI+IFl0UzxQmEvnOqOFY1jdsxwSuWWrjw0jK7bhyftDW1l+Rb8zqWgrvMgg4ICg4LO4TvTq0nOP38c///9y9SXb/brxfX92KSfvtNnsTaXy4jhoqBV/mEhh/DjrgIBXYTzq/m8R4sH7WgtNs3OWummdg/HwOrQAx5lisSdgmvIU4sH0wZQY4IwLOmmW6aZyNErfOS0zMZ6A5p0jQTZPYMA5xYR0Xe1y6reRJ79yERAU0rkYCJgDu9bw3I1oDj+wG6vRnE8rye9asaDEJuYnqRNY3k41KbeEm9KQn5/PyS4kH9oO7Rf53b4rMLVc3pmZg8L12bra2dReAY3MyrGPaaYdyR5imC25K9gziiY/knnzwvGOyaYBRdPLLozLv4W3WcUTb/oNh2PX6zoNorGH4jmhktc0v86y+FnfQppiirDKfyZxPZO4ZPg4rRq2HkpO5gY7WBFylKoOga9SoI7n6ROZzs0Ze5Lsd0reRsS295v+sr1hKMPNA9fNqtq9PB5GF39BA== \ No newline at end of file diff --git a/docs/report.latex b/docs/report.latex new file mode 100644 index 0000000..886b677 --- /dev/null +++ b/docs/report.latex @@ -0,0 +1,352 @@ +\documentclass[conference]{IEEEtran} + +% \IEEEoverridecommandlockouts +% The preceding line is only needed to identify funding in the first footnote. +% If that is unneeded, please comment it out. + +\usepackage{cite} +\usepackage{amsmath,amssymb,amsfonts} +% \usepackage{algorithmic} +\usepackage{graphicx} +\usepackage{textcomp} +\usepackage{xcolor} +\usepackage{subcaption} +\usepackage{todonotes} +\usepackage{hyperref} + +\def\BibTeX{{\rm B\kern-.05em{\sc i\kern-.025em b}\kern-.08em + T\kern-.1667em\lower.7ex\hbox{E}\kern-.125emX}} + +\begin{document} + +\title{Humanoid Robotic Systems - ``Teleoperating NAO''} + +\author{Pavel Lutskov, Luming Li, Lukas Otter and Atef Kort} + +\maketitle + +\section{Project Description} + +In this semester the task of our group was to program a routine for +teleoperation of a NAO robot. Using the ArUco markers, placed on the operator's +chest and hands, the position and the posture of the operator should have been +determined by detecting the markers' locations with a webcam, and then the +appropriate commands should have been sent to the robot to imitate the motions +of the operator. The overview of the +process can be seen in \autoref{fig:overview}. The main takeaway from +fulfilling this objective was practicing the skills that we acquired during the +Humanoid Robotic Systems course and to get familiar with the NAO robot as a +research and development platform. + +\begin{figure}[h] + \centering + \includegraphics[width=\linewidth]{figures/teleoperation_overview.png} + \caption{Overview of the defined states and their transistions.} + \label{fig:overview} +\end{figure} + +In closer detail, once the markers are detected, their coordinates relative to +the webcam are extracted. The position and the orientation of the user's +chest marker is used to control the movement of the NAO around the environment. +We call this approach a ``Human Joystick'' and we describe it in more detail in +\autoref{ssec:navigation}. + +The relative locations of the chest and hand markers can be used to determine +the coordinates of the user's end effectors (i.e.\ hands) in the user's chest +frame. In order for the NAO to imitate the arm motions, these coordinates need +to be appropriately remapped into the NAO torso frame. With the knowledge of the +desired coordinates of the hands, the commands for the NAO joints can be +calculated by using the Cartesian control approach. We present a thorough +discussion of the issues we had to solve and the methods we used for arm motion +imitation in \autoref{ssec:imitation}. + +Furthermore, in order to enable the most intuitive teleoperation, a user +interface was needed to be developed. In our system, we present the operator +with a current estimation of the operator's pose, a sensor feedback based robot +pose, as well as with the camera feed from both NAO's cameras and with the +webcam view of the operator. In order for the user to be able to give explicit +commands to the robot, such as a request to open or close the hands or to +temporarily suspend the operation, we implemented a simple voice command system. +Finally, to be able to accommodate different users and to perform control in +different conditions, a small calibration routine was developed, which would +quickly take a user through the process of setting up the teleoperation. +We elaborate on the tools and approaches that we used for implementation of the +user-facing features in \autoref{ssec:interface}. + +An example task, that can be done using our teleoperation package might be the +following. The operator can safely and precisely navigate the robot through an +uncharted environment with a high number of obstacles to some lightweight +object, such as an empty bottle, then make the robot pick up that object and +bring the object back to the operator. Thanks to the high precision of the arm +motions and the constant operator input, the robot is able to pick up an object +of different shapes and sizes, applying different strategies when needed. We +demonstrate the functioning of our system in the supporting video. + +We used ROS as a framework for our implementation. ROS is a well-established +software for developing robot targeted applications with rich support +infrastructure and modular approach to logic organization For interacting +with the robot we mainly relied on the NAOqi Python API. The advantage of using +Python compared to C++ is a much higher speed of development and a more concise +and readable resulting code. + +\section{System Overview} + +\subsection{Vision}\label{ssec:vision} + +- Camera calibration +- Aruco marker extraction +- TF world coordinate publishing + +\begin{figure} + \centerline{\includegraphics[width=0.8\linewidth]{figures/aruco.png}} + \caption{ArUco marker detection on the operator.} + \label{fig:aruco_detection} +\end{figure} + +\subsection{Interface}\label{ssec:interface} + +\paragraph{Speech State Machine} + +Based on NAOqi API and NAO built-in voice recognition + +\begin{table} +\caption{Commands of the speech recognition module} +\begin{center} +\begin{tabular}{|c|c|c|} +\hline +\textbf{Command}&\textbf{Action}&\textbf{Available in state} \\ +\hline +``Go'' & Wake Up & Sleep \\ +\hline +``Kill'' & Go to sleep & Idle, Imitation \\ +\hline +``Arms'' & Start imitation & Idle \\ +\hline +``Stop'' & Stop imitation & Imitation \\ +\hline +``Open'' & Open hands & Idle, Imitation \\ +\hline +``Close'' & Close hands & Idle, Imitation \\ +\hline +\end{tabular} +\label{tab_speech_states} +\end{center} +\end{table} + +\paragraph{Teleoperation Interface} + +In order to make it possible to operate +the NAO without visual contact, a teleoperation interface was developed. This +interface allows the operator to receive visual feedback on the NAO as well as +additional information regarding his own position. + +The NAO-part contains video streams of the top and bottom cameras on the robots +head. These were created by subscribing to their respective topics (FIND NAME) +using the \textit{rqt\_gui} package. Moreover, it also consists of a rviz +window which gives a visual representation of the NAO. For this, the robot's +joint positions are displayed by subscribing to the topic tf where the +coordinates and the different coordinate frames are published. We further used +the \textit{NAO-meshes} package to create the 3D model of the NAO. + +\begin{figure} + \centering + %\hfill + \begin{subfigure}[b]{0.4\linewidth} + \includegraphics[width=\linewidth]{figures/rviz_human.png} + \caption{} + %{{\small $i = 1 \mu m$}} + \label{fig_human_model} + \end{subfigure} + \begin{subfigure}[b]{0.4\linewidth} + \includegraphics[width=\linewidth]{figures/interface_nao.png} + \caption{} + %{{\small $i = -1 \mu A$}} + \label{fig_nao_model} + \end{subfigure} + \caption{Operator and NAO in rviz.} + \label{fig_interface} +\end{figure} + + +\subsection{Navigation}\label{ssec:navigation} + +- Human Joystick (3dof) + +One of the two main feature in our robot is an intuitive navigation tool, which +allows the robot to navigate the environment by tracking the user movements. + +By fixing an ArUco marker on the user's chest, we can continuously track its +position and orientation in a three dimensional space and so capture its +motion. + +In order to simplify the task we define a buffer zone where the robot can only +track the orientation of the user then depending on which direction the user +will exit the zone the robot will either go forward, backward, left or right. +Also the covered distance will influence the speed of the robot, the further +the user is from the center of the buffer zone the faster the movement of the +robot will be. The extent of the movement and buffer zone are determined +automatically through calibration. + +\begin{figure} + \centering + \includegraphics[width=0.8\linewidth]{figures/usr_pt.png} + \caption{User position tracking model} + \label{fig_user_tracking} +\end{figure} + +\subsection{Imitation}\label{ssec:imitation} + +One of the main objectives of our project was the imitation of the operator +arm motions by the NAO. In order to perform this, first the appropriate mapping +between the relative locations of the detected ArUco markers and the desired +hand positions of the robot needs to be calculated. Then, based on the +target coordinates, the robot joint rotations need to be calculated. + +\paragraph{Posture retargeting} + +First, let us define the notation of the coordinates that we will use to +describe the posture retargeting procedure. Let $r$ denote the 3D $(x, y, z)$ +coordinates, then the subscript defines the object which has these coordinates, +and the superscript defines the coordinate frame in which these coordinates are +taken. So, for example, $r_{NAO hand}^{NAO torso}$ gives the coordinate of the +hand of the NAO robot in the frame of the robot's torso. + +After the ArUco markers are detected and published on ROS TF, as was described +in \autoref{ssec:vision}, we have the three vectors $r_{aruco,chest}^{webcam}$, +$r_{aruco,lefthand}^{webcam}$ and $r_{aruco,righthand}^{webcam}$. We describe +the retargeting for one hand, since it is symmetrical for the other hand. We +also assume that all coordinate systems have the same orientation, with the +z-axis pointing upwards, the x-axis pointing straight into webcam and the +y-axis to the left of the webcam. Therefore, we can directly calculate the hand +position in the user chest frame by the means of the following equation: + +$$r_{hand,user}^{chest,user} = r_{aruco,hand}^{webcam} - +r_{aruco,chest}^{webcam}$$. + +Next, we remap the hand coordinates in the chest frame into the user shoulder +frame, using the following relation: + +$$r_{hand,user}^{shoulder,user} = +r_{hand,user}^{chest,user} - r_{shoulder,user}^{chest,user}$$ + +We know the coordinates of the user's shoulder in the user's chest frame from +the calibration procedure, described in \autoref{ssec:interface}. + +Now, we perform the retargeting of the user's hand coordinates to the desired +NAO's hand coordinates in the NAO's shoulder frame with the following formula: + +$$r_{hand,NAO}^{shoulder,NAO} = +\frac{L_{arm,NAO}}{L_{arm,user}} r_{hand,user}^{shoulder,user}$$ + +As before, we know the length of the user's arm through calibration and the +length of the NAO's arm through the specification provided by the manufacturer. + +A final step of the posture retargeting is to obtain the coordinates of the +end effector in the torso frame. This can be done through the following relation: + +$$r_{hand,NAO}^{torso,NAO} = +r_{hand,NAO}^{shoulder,NAO} + r_{shoulder,NAO}^{torso,NAO}$$ + +The coordinates of the NAO's shoulder in the NAO's torso frame can be obtained +through a call to the NAOqi API. + +Now that the desired position of the NAO's hands are known, the appropriate +joint motions need to be calculated by the means of Cartesian control. + +\paragraph{Cartesian control} + +For this a singular robust cartesian controller was build. + +The output of our cartesian controller are the 4 angles of the rotational +joints for the shoulder and the elbow part of each arm of the NAO robot, which +is described by the inverse kinematic formula + +$$\Delta\theta = J^{-1,robust}\Delta r$$ + +To build the cartesian controller first the Jacobian matrix is needed. The +content of the Jacobian matrix describes an approximation for the movement of +each joint of the robot. There are 2 main ways to determine the Jacobian +matrix. The first way is the numerical method, where this approximation is done +by checking how the end effector moves with small angles for rotational joints. +For this we can approximate each column of the Jacobian Matrix as followed: + +$$\frac{\partial r}{\partial\theta} \sim \frac{\Delta r}{\Delta\theta} = +\left( + \begin{array}{ccc} + \frac{\Delta r_x}{\Delta\theta} & + \frac{\Delta r}{\Delta\theta} & + \frac{\Delta r}{\Delta\theta} + \end{array} + \right)^{T}$$ + +The other method is the analytical method, which was used in this project. +Since only rotational joints were available, the approximation for the +Jacobian matrix, which is the tangent in rotational joints, can be calculated +using the cross product between the rotational axis $e$ and the rotational +vector \\ $r_{end}-r_{joint}$. + +$$ +\frac{\partial r_{end}}{\partial\theta _{joint}} = +(e \times (r_{end}-r_{joint})) +$$ + +which gives us one column of the Jacobian matrix. This can be repeated for +each rotational joint until the whole matrix is filled. + +The next step for the Cartesian controller is to determine the inverse Jacobian +matrix for the inverse kinematic. For this singular value decomposition is +used. + +\section{System Implementation and Integration} + +Now that the individual modules were designed and implemented, the whole system +needed to be assembled together. It is crucial that the states of the robot and +the transitions between the states are well defined and correctly executed. The +state machine, that we designed, can be seen in the \autoref{fig:overview}. + +The software package was organized as a collection of ROS nodes, controlled by +a single master node. The master node keeps track of the current system state, +and the slave nodes consult with the master node to check if they are allowed +to perform an action. To achieve this, the master node creates a server for a +ROS service, named \verb|inform_masterloop|, with this service call taking as +arguments a name of the caller and the desired action and responding with a +Boolean value indicating, whether a permission to perform the action was +granted. The master node can then update the system state based on the received +action requests and the current state. Some slave nodes, such as the walking or +imitation nodes run in a high-frequency loop, and therefore consult with the +master in each iteration of the loop. Other nodes, such as the fall detector, +only inform the master about the occurrence of certain events, such as the fall +or fall recovery, so that the master could deny requests for any activities, +until the fall recovery is complete. + +\section{Drawbacks and conclusions} + +% \begin{table}[htbp] +% \caption{Table Type Styles} +% \begin{center} +% \begin{tabular}{|c|c|c|c|} +% \hline +% \textbf{Table}&\multicolumn{3}{|c|}{\textbf{Table Column Head}} \\ +% \cline{2-4} +% \textbf{Head} & \textbf{\textit{Table column subhead}}& \textbf{\textit{Subhead}}& \textbf{\textit{Subhead}} \\ +% \hline +% copy& More table copy$^{\mathrm{a}}$& & \\ +% \hline +% \multicolumn{4}{l}{$^{\mathrm{a}}$Sample of a Table footnote.} +% \end{tabular} +% \label{tab_sample} +% \end{center} +% \end{table} + +% \begin{thebibliography}{00} + +% \bibitem{b1} + +% G. Eason, B. Noble, and I. N. Sneddon, +% ``On certain integrals of Lipschitz-Hankel type involving +% products of Bessel functions,'' +% Phil. Trans. Roy. Soc. London, vol. A247, pp. 529--551, April 1955. + +% \end{thebibliography} + +\end{document} diff --git a/script/calibrator.py b/script/calibrator.py index 5eb5d57..d4c78bb 100755 --- a/script/calibrator.py +++ b/script/calibrator.py @@ -1,4 +1,10 @@ #! /usr/bin/env python +"""The script for calibrating the arm lengths and 'joystick' extents. + +You will normally not call this script directly but rather with +`roslaunch teleoperation calibration.launch` +""" + import os import json @@ -12,6 +18,23 @@ from teleoperation.msg import RequestSpeechAction, RequestSpeechGoal def calib_field(sentences, src_frames, targ_frames): + """Retrieve a relative transform of some marker. + + Parameters + ---------- + + sentences : list of str + What to say. + src_frames : list of str + The names of tf source frames (from where to transform) + targ_frames : list of str + The names of tf target frames (to where to transform) + + Returns + ------- + list of transforms + + """ ok = False t_and_q = [] @@ -34,6 +57,7 @@ def calib_field(sentences, src_frames, targ_frames): def calib_center(): + """Retrieve the coordinates of the 'joystick' zero state.""" trans, q = calib_field([ 'Stand in front of camera', 'Far enough to see your arms' @@ -43,6 +67,7 @@ def calib_center(): def calib_arms(): + """Retrieve the hand positions relative to the chest.""" ts_and_qs = calib_field( ['Now stretch your arms', 'Make sure the markers are detected'], ['odom'] * 3, @@ -58,6 +83,7 @@ def calib_arms(): def calib_shoulders(): + """Retrieve the shoulder positions relative to the chest.""" ts_and_qs = calib_field( ['Now place the markers on the corresponding shoulders', 'Make sure the markers are detected'], @@ -74,6 +100,7 @@ def calib_shoulders(): def calib_rotation(): + """Retrieve the limits of the z-axis of the 'joystick'.""" rots = [] for side in ('left', 'right'): _, q = calib_field( @@ -86,6 +113,7 @@ def calib_rotation(): def calib_extremes(): + """Retrieve the limits of the x and y axes of the 'joystick'.""" transs = [] for direction in ('forward', 'backward', 'to the left', 'to the right'): trans, _ = calib_field( @@ -100,6 +128,7 @@ def calib_extremes(): def calibration(): + """Run full calibration and dump the results to the config file.""" center = calib_center() larm, rarm = calib_arms() lsh, rsh = calib_shoulders() diff --git a/script/controller.py b/script/controller.py index 910e1ad..dabb64f 100755 --- a/script/controller.py +++ b/script/controller.py @@ -1,5 +1,6 @@ #! /usr/bin/env python -"""Controller should execute control for a given effector""" +"""Library for executing control for a given effector""" + from __future__ import division import os import json @@ -28,6 +29,7 @@ NAO_ARM = 0.22 def xyz(joint): + """Get 3D coordinate of a joint in torso frame.""" return np.array(mp.getPosition(joint, FRAME_TORSO, False), dtype=np.float64)[:3] @@ -53,12 +55,14 @@ best_guess = { def get_transform(joint): + """Retrieve a transform matrix of a joint in the torso frame.""" return np.array( mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64 ).reshape((4, 4)) def diff_jacobian(side): + """Update and return the differential Jacobian. NOT GOOD.""" new_end_xyz = xyz('{}Arm'.format(side)) delta_r = new_end_xyz - crt_xyz[side] crt_xyz[side] = new_end_xyz.copy() @@ -75,6 +79,7 @@ def diff_jacobian(side): def jacobian(side): + """Calculate the analytical Jacobian for side 'L' or 'R'.""" end_xyz = xyz('{}Arm'.format(side)) xyzs = np.array([xyz(side + j) for j in JOINTS]) @@ -84,6 +89,7 @@ def jacobian(side): ax_order = (y_axis, z_axis, x_axis, z_axis) + # Calculate the Jacobian after the formula from the report axes = np.concatenate( [get_transform(side + j).dot(ax) for j, ax in zip(JOINTS, ax_order)], axis=1 @@ -94,6 +100,7 @@ def jacobian(side): def to_nao(my_xyz, side): + """Transform object coordinates from operator chest frame to NAO torso.""" sh_offs = np.array(MY_SHOULDERS[side]) my_sh_xyz = my_xyz - sh_offs nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM @@ -102,6 +109,7 @@ def to_nao(my_xyz, side): def inv_jacobian(j): + """Singluarity robust inverse Jacobian.""" u, s, vt = np.linalg.svd(j) s_inv = np.zeros((vt.shape[0], u.shape[1])) np.fill_diagonal(s_inv, 1 / s) @@ -110,6 +118,22 @@ def inv_jacobian(j): def _some_cartesian(my_xyz, side, jfunc): + """A generic cartesian controller. + + Parameters + ---------- + my_xyz : numpy.array + Coordinates of the object/target in the operator chest frame. + side : str, 'L' or 'R' + jfunc : func + A function that will return a jacobian matrix for the given side. + + Returns + ------- + numpy.array + The control to execute in the joint space. + + """ nao_xyz = to_nao(my_xyz, side) delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side))) crt_q = mp.getAngles([side + j for j in JOINTS], False) @@ -118,19 +142,23 @@ def _some_cartesian(my_xyz, side, jfunc): def our_cartesian(my_xyz, side): + """Our cartesian controller.""" return _some_cartesian(my_xyz, side, jacobian) def our_diff_cartesian(my_xyz, side): + """Our differential cartesian controller.""" return _some_cartesian(my_xyz, side, diff_jacobian) def _elbow(arm_): + "Dumb way to calculate the elbow roll.""" quot = min(1.0, arm_ / 0.70) return radians(180 * (1 - quot)) def dumb(my_xyz, side): + """Our dumb controller that directly maps 3D coords into joint space.""" sign = 1 if side == 'L' else -1 roll = asin(my_xyz[1] / np.linalg.norm(my_xyz)) roll -= sign * radians(25) @@ -142,11 +170,24 @@ def dumb(my_xyz, side): def movement(my_xyz, side, control): + """Execute the movement. + + my_xyz : numpy.array + Coordinates of the object/target in the operator chest frame. + side : str, 'L' or 'R' + control : func + A function to calculate the conrol for the side, depending on the + target coordinates. It must return returning the `numpy.array` + joint vector of format + [ShoulderPitch, ShoulderRoll, ElbowYaw, ElbowRoll]. + + """ ref = control(np.array(my_xyz), side).tolist() mp.setAngles([side + j for j in JOINTS], ref, 0.3) def nao_cart_movement(my_arm_xyz, side, *_): + """Execute the movement using the NAO cartesian controller.""" nao_xyz = to_nao(my_arm_xyz, side) mp.setPositions('{}Arm'.format(side), FRAME_TORSO, tuple(nao_xyz.tolist()) + (0, 0, 0), diff --git a/script/fall_detector.py b/script/fall_detector.py index 0cc3f3c..86ec023 100755 --- a/script/fall_detector.py +++ b/script/fall_detector.py @@ -1,4 +1,10 @@ #! /usr/bin/env python +"""A module for graceful fall handling. + +ROS node: `fall_detector`. +Uses `inform_controller` service. + +""" import os import rospy @@ -15,6 +21,7 @@ _inform_masterloop = inform_masterloop_factory('fall_detector') class FallDetectorModule(ALModule): + """An ALModule responsible for fall handling.""" def __init__(self, name): ALModule.__init__(self, name) @@ -23,7 +30,7 @@ class FallDetectorModule(ALModule): _inform_masterloop('falling') def on_robot_fallen(self, *_args): - if not _inform_masterloop('fallen'): + if not _inform_masterloop('fallen'): # Seize the control return mp.rest() rospy.Rate(0.5).sleep() @@ -32,9 +39,10 @@ class FallDetectorModule(ALModule): am.setExpressiveListeningEnabled(False) pp = ALProxy('ALRobotPosture') while not pp.goToPosture('StandInit', 1.0): + # Try to stand up indefinetely pass rospy.Rate(1).sleep() - _inform_masterloop('recovered') + _inform_masterloop('recovered') # Release the control if __name__ == "__main__": diff --git a/script/hand_ler.py b/script/hand_ler.py index 6693d9a..5d2d383 100755 --- a/script/hand_ler.py +++ b/script/hand_ler.py @@ -1,4 +1,9 @@ #! /usr/bin/env python +"""A small module for hand controls service. + +ROS Node: `hand_ler` +Provides `hands` service. +""" import os import rospy @@ -12,6 +17,12 @@ mp = None def do_in_parallel(func): + """Open hands in parallel. + + The robot's hand function blocks and only accepts one hand. This is a + thread-based hackaround. + + """ tl = Thread(target=func, args=('LHand',)) tr = Thread(target=func, args=('RHand',)) tl.start() @@ -21,6 +32,7 @@ def do_in_parallel(func): def handle_hands(request): + """Hand service routine.""" if request.target == 'open': do_in_parallel(mp.openHand) return HandsResponse('opened') diff --git a/script/imitator.py b/script/imitator.py index 02a8193..24e5194 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -1,4 +1,10 @@ #! /usr/bin/env python +"""The node performing the imitation. + +ROS Node: `imitator` +Uses `inform_masterloop` service. +""" + from argparse import ArgumentParser from math import radians @@ -18,6 +24,7 @@ TORSO = False def controller_factory(ctrl): + """Create a controller depending on the command line argument.""" if ctrl == 'nao_cartesian': return lambda my_arm_xyz, side: nao_cart_movement(my_arm_xyz, side) @@ -45,8 +52,8 @@ if __name__ == '__main__': ll = tf.TransformListener() while not rospy.is_shutdown(): - rospy.Rate(20).sleep() - if not _inform_masterloop('imitate'): + rospy.Rate(20).sleep() # Run at 20 Hz or something + if not _inform_masterloop('imitate'): # Try to seize the control continue rospy.logdebug('IMITATOR: ACTIVE') @@ -61,7 +68,7 @@ if __name__ == '__main__': # except Exception as e: # rospy.logwarn(e) - for i, side in enumerate(['L', 'R'], 1): + for i, side in enumerate(['L', 'R'], 1): # One arm at a time try: a0, _ = ll.lookupTransform( 'odom', @@ -76,9 +83,12 @@ if __name__ == '__main__': except Exception as e: rospy.logwarn(e) continue + + # Calculate the position of the hand in my chest frame my_arm_xyz = np.array(arm) - np.array(a0) - # rospy.loginfo('{}'.format(my_arm_xyz)) - # rospy.loginfo('{}'.format(dumb(my_arm_xyz, side))) - imitation_cycle(my_arm_xyz, side) + + imitation_cycle(my_arm_xyz, side) # Do the actuation + + # Set hands to sane positions mp.setAngles(('LWristYaw', 'RWristYaw'), (radians(-70), radians(70)), 0.3) diff --git a/script/masterloop.py b/script/masterloop.py index 369c589..3b19dc1 100755 --- a/script/masterloop.py +++ b/script/masterloop.py @@ -1,4 +1,6 @@ #! /usr/bin/env python +"""The master state machine node.""" + import os from argparse import ArgumentParser @@ -37,6 +39,7 @@ mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) def init_voc_state_speech(): + """Initialize the transitions.""" global VOC_STATE, SPEECH_TRANSITIONS VOC_STATE = { 'idle': [KILL] + ([IMITATE] if not AI else []), @@ -55,6 +58,7 @@ def init_voc_state_speech(): def hands(what): + """Handle the command to do something with the hands.""" try: _hands = rospy.ServiceProxy('hands', Hands) newstate = _hands(what).newstate @@ -64,6 +68,7 @@ def hands(what): def speech_done_cb(_, result): + """Handle the recognized command.""" global speech_in_progress, state, hand_state _state_old = state rospy.loginfo('SPEECH: {}'.format(result)) @@ -83,6 +88,12 @@ def speech_done_cb(_, result): def inform_masterloop_factory(who): + """Create a function to inform the masterloop. + + Every node infroms the master, so it's nice to have a factory for + these functions. + + """ def inform_masterloop(what): try: inform_masterloop = rospy.ServiceProxy('inform_masterloop', @@ -96,6 +107,10 @@ def inform_masterloop_factory(who): def handle_request(r): + """Handle a node's request to seize/release control. + + Update the state if needed. + """ global state module = r.module message = r.message @@ -164,6 +179,8 @@ if __name__ == '__main__': ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request) + # Necessary initializations all have been performed by now + def _shutdown(): if speech_in_progress: speech.cancel_goal() @@ -171,7 +188,7 @@ if __name__ == '__main__': rospy.on_shutdown(_shutdown) - while not rospy.is_shutdown(): + while not rospy.is_shutdown(): # Run the speech detection loop if state in ('idle', 'imitate', 'dead'): if not speech_in_progress: speech_in_progress = True @@ -184,4 +201,4 @@ if __name__ == '__main__': speech.cancel_goal() speech_in_progress = False - rospy.Rate(10).sleep() + rospy.Rate(10).sleep() # Run at 10 Hz or something diff --git a/script/speech_server.py b/script/speech_server.py index 1d695ee..55fc3f0 100755 --- a/script/speech_server.py +++ b/script/speech_server.py @@ -1,4 +1,10 @@ #! /usr/bin/env python +"""A module for speech detection. + +ROS Node: `speech_server` +Provides `request_speech` action + +""" import os import rospy @@ -13,6 +19,7 @@ almem = None def request_speech(goal): + """Handle the request for speech detection.""" rospy.loginfo('SPEECH SERVER: NEW GOAL: {}'.format(goal)) if not speech_detector.start_speech(goal.vocabulary): sas.set_succeeded(RequestSpeechResult(word='')) @@ -33,6 +40,7 @@ def request_speech(goal): class SpeechDetectorModule(ALModule): + """ALModule for accessing the NAO's speech recognition.""" def __init__(self, name): ALModule.__init__(self, name) @@ -52,6 +60,7 @@ class SpeechDetectorModule(ALModule): return almem.getData('ALSpeechRecognition/Status') def start_speech(self, voc, resume=False): + """Start recognizing the given vocabulary.""" if self._busy != resume: return False if not resume: @@ -63,14 +72,17 @@ class SpeechDetectorModule(ALModule): return True def have_word(self): + """Check if something was recognized.""" return self.recognized is not None def get_recognized_and_erase(self): + """Retrieve the recognized word and erase it from the variable.""" result = self.recognized self.recognized = None return result def stop_speech(self, pause=False): + """Stop detecting or just pause detection.""" if not self._busy: return self.asr.pause(True) @@ -79,6 +91,7 @@ class SpeechDetectorModule(ALModule): self._busy = False def on_word_recognized(self, *_args): + """Callback for word recognized event.""" word, conf = almem.getData('WordRecognized') if conf > 0.4: self.stop_speech(pause=True) diff --git a/script/walker.py b/script/walker.py index df21d54..1f2895a 100755 --- a/script/walker.py +++ b/script/walker.py @@ -1,4 +1,10 @@ #! /usr/bin/env python +"""Module for walking in the environment (Human Joystick). + +ROS Node: `walker`. +Uses `inform_masterloop` service. + +""" from __future__ import division import os @@ -23,6 +29,7 @@ VMAX = 1.0 def n_way(a, b, n=3): + """A point which is (b - a)/n away from a.""" return a + (b - a) / n @@ -45,6 +52,7 @@ _inform_masterloop = inform_masterloop_factory('walker') def _speed(pos, interval): + """Calculate speed based on the operators position.""" int_dir = 1 if interval[1] > interval[0] else -1 if int_dir * (pos - interval[0]) < 0: return 0.0 @@ -81,6 +89,7 @@ if __name__ == '__main__': ref_vec = trans[:2] + rot[2:] #-1 1 -1 1 -1 1 for i, dr in enumerate((BK, FW, RT, LT, RR, LR)): + # figure out in which direction to move idx = i // 2 sign = 1 if (i % 2) else -1 speed = _speed(ref_vec[idx], dr) @@ -94,16 +103,17 @@ if __name__ == '__main__': if not any(movement): rospy.logdebug('WALKER: STOP') - _inform_masterloop('stop') + _inform_masterloop('stop') # release the control # mp.move(0, 0, 0) mp.stopMove() continue - permission = _inform_masterloop('move') + permission = _inform_masterloop('move') # try to seize the control if not permission: mp.stopMove() else: + # set the head so that the scene is clearly visible on cameras mp.setAngles(('HeadYaw', 'HeadPitch'), (0.0, 0.35), 0.3) mp.moveToward(*movement) # DON'T DELETE THIS ONE pass diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index e996d54..78e67cf 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -12,6 +12,17 @@ #include #include +/* + * This is the ROS Node for detecting the ArUco markers. + * + * Node: aruco_detector + * + * It subscribes to a webcam, detects the markers, transforms them + * into the correct coordinate frame (z-up) and publishes on TF + * relatively to the odom frame + * + */ + static const std::string ARUCO_WINDOW = "Aruco window"; class ImageConverter { @@ -41,6 +52,7 @@ class ImageConverter { } void imageCb(const sensor_msgs::ImageConstPtr& msg) { + // This function gets called when an image arrives cv_bridge::CvImageConstPtr cv_ptr; try { @@ -54,7 +66,7 @@ class ImageConverter { return; } - // Do Aruco + // Define parameters of markers and camera cv::Mat cam_mat = (cv::Mat_(3, 3) << 690., 0.0, 320, 0.0, 690., 225, @@ -70,7 +82,7 @@ class ImageConverter { float m_size = 0.15; detector.detect(cv_ptr->image, markers, cam, m_size); - // Draw and print aruco + // Draw all aruco into the window and publish on tf cv::Mat aruco_demo = cv_ptr->image.clone(); for (int i = 0; i < markers.size(); i++) { @@ -79,6 +91,8 @@ class ImageConverter { cv::Mat rot_cv; cv::Rodrigues(markers[i].Rvec, rot_cv); + // Transform the coordinates of the aruco markers so that they + // are in the Z-up, X-forward frame tf::Matrix3x3 rot( rot_cv.at(0, 0), rot_cv.at(0, 1), @@ -106,7 +120,7 @@ class ImageConverter { 0, -1, 0 ); - rot = hmat * rot * another; // This all somehow works as expected + rot = hmat * rot * another; // This transforms the coords trans = hmat * trans; tf::Transform aruco_tf(rot, trans); @@ -115,6 +129,7 @@ class ImageConverter { id += stuff_to_str(markers[i].id); id += "_frame"; + // Publish the transform this->aruco_pub.sendTransform(tf::StampedTransform( aruco_tf, ros::Time::now(), "odom", id.c_str()));