Saving arm poses and safely sending the robot to a pose

[mayacakmak]

As a basis of two functionalities requested by Henry (automatic stow/prep and automated robot motions) we need to be able to save the state of the robot arm (position of each joint) and then send the robot to a previously saved pose.

We can look into the arm stowing script for how that is handled.

We might want to have poses saved on Firebase.

We might need to be extra cautions during execution of the movement, going from an arbitrary current pose to a saved pose.

[mayacakmak]

This issue depends on addressing Issue #16 first, since we'll need to remember poses saved by the user. We might want a separation of global poses ("stowed" "ready for manipulation") higher in the Firebase tree versus user's saved actions under the user branch of Firebase.

[mayacakmak]

Once we figure out the infrastructure for this and have stow/prep implemented (Issue #13) then we can figure out and implement the user interface for saving a pose (e.g. a button that says save current pose and a text field for naming the pose, perhaps in a popup) as well as sending the robot to a pose (e.g. dropdown menu with a list of all saved poses and a button "Send robot to pose").

[kavidey]

This is a general update on the state of pose saving/loading right now

---

OAuth

Google OAuth is set up to allow users to log in with their email. I set it up as described in this comment (once the image issue is fixed we can probably close that issue).

I am having some trouble loading the button image from the server, so right now it displays an empty image (worst case we can load it from an external source, but I think this is just a node.js configuration issue or I'm loading the wrong path here)

Because the robot cannot access firebase, or the operator's OAuth login, more data needs to be passed from the operator to the robot when a pose is executed (I'll touch on this more later).

Database

Refactor

In order to get OAuth working, I had to refactor most of the database code. I also updated the class syntax style. The biggest change is that it is now referenced using the local instance db rather than the Database prototype.

For example:

stretch_web_interface/shared/commands.js

Lines 57 to 64 in b737d6f

	function lookRight() {
	var cmd = {type:"command",
	subtype:"head",
	name:"right",
	modifier:currentV};
	sendData(cmd);
	db.logEvent("LookRight", currentV);
	}

Pose Saving

Poses are stored in two locations in firebase.

• Global poses are stored in /poses
• User poses are stored in /users/{uid}/poses

Each pose has an id, a description, and a key-value pair for each joint it sets. I moved the two global poses (stow & prep) from being hardcoded in ros_connect.js to firebase:

I added a few new functions to the database to manage poses.

• addPose adds a pose to the users local pose list
• getGlobalPoses & getUserPoses get the global/user poses from firebase respectively

Because there is no admin interface, the only way to add global poses currently is by entering them into firebase manually.

Pose manager

The core of the front-end pose system is a the new PoseManager class in operator_pose.js

It interfaces with firebase through db to

• Get the poses in firebase and turn them into buttons on page load
  • The HTML for the buttons is generated by the pose manager here and added to this div in operator.html
• Handle adding new poses to firebase and their corresponding buttons

This is what it looks like when all the buttons are loaded (a, b, and c are just testing poses)

Currently there is no visual difference between global and user poses, that could be added by changing the styling here

Using existing poses

As previously mentioned, the robot cannot access firebase, this means that all of the pose joint data needs to be sent from the operator to the robot via the WebRTC connection. When any pose button is pressed, it calls moveToPose in commands.js and passes in the id of the pose.
moveToPose uses the id to get the joint information for that pose from the pose manager, and sends it as the modifier for the pose message.

stretch_web_interface/shared/commands.js

Lines 192 to 199 in b737d6f

	function moveToPose(id) {
	var cmd = {type:"command",
	subtype:"full",
	name:"pose",
	modifier:poseManager.getPose(id)};
	sendData(cmd);
	db.logEvent("Pose", id);
	}

It is a bit ugly, but the name has to be pose (as opposed to id or something), because the code that interprets the messages on the robot side, uses name to figure out what function to call. And because the robot doesn't have access to firebase, it has no way of knowing which poses even exit.

Once the robot receives the pose info, it calls goToPose in ros_connect.js (We should probably refactor the command functions are setup so that we don't need two different names for each action, one for the operator and one for the robot).
goToPose just creates a pose goal and sends it to the robot.

In simulation, moving multiple joints at a time is much more complicated than on the actual robot, because each type of joint movement needs its own goal that has to be sent to a different rostopic. I completely rewrote generatePoseGoal to support this. It works in simulation, will likely need to be debugged on the actual robot.
One other note: because there is more than one goal for a single pose, this will also likely make detecting when poses are complete more difficult (I assume we will need this for a dead man's switch).

Adding new poses

The front end side of this is currently functional. When the Save pose button is pressed, it opens a modal dialog that allows the operator to set the pose description, and select which parts of the robot should be saved to the pose (I figured this would be useful so that the arm and gripper position of the robot could be saved without the camera or vice versa).

When Add pose is pressed, it triggers the addPose function in poseManager. This function is not at all finished yet. It currently gets the description, and state of the checkboxes, and uploads that all to firebase (the id id generated by taking the description and removing spaces)

The problem is that the operator doesn't have access to the robot state, so it doesn't know what position each of the joints is in. The data that is currently being uploaded to firebase is just the state of the checkboxes which doesn't actually do anything.

What I was planning on doing next

The biggest thing is probably setting up communication between the operator and robot, so that the operator can request specific pose data from the robot and then upload it to firebase. Once this is working, the full pose pipeline should be complete.

I think we had also talked about implementing a deadman's switch so that either the operator had to hold down a button until stretch is done moving to a pose, or at any time before it is done, the operator can click to cancel. I was planning on looking into the goal's result callback for that.

Finally the node.js image thing is probably an easy fix, my understanding is that we are serving /shared as a static directory (https://github.com/hcrlab/stretch_web_interface/blob/save_poses/routes/index.js#L30), and we're correctly loading js files from there, so I'm not sure why images won't work.

[kavidey]

Communication between the operator and robot browsers is working now!

Most of it is handled in the new file request_response.js which facilitates data fetching between the two browsers, so either request info from the other.

Details on the request/response protocol:

Request Format

{
     type: 'request', // Message type
     id: "", // Unique id (for poses, this is the pose id)
     requestType: "", // The type of data that is being returned (For poses this is "jointState")
     responseHandler: "" // The response handler (for poses this is "poseManager"
}

Response Format

{
     type: "response", // Message type
     id: "", // Unique id (for poses, this is the pose id)
     responseHandler: "", // The response handler (for poses this is "poseManager"
     responseType: "", // The type of data that is being returned (For poses this is "jointState")
     data: {} // Response data
}

The id, combined with the responseHandler is used to match up a response with the request, so it can be easily handled in code. Ideally each id is unique, but as long as the messages were sent by different handlers (or the same handler at after the previous request with that id finished, they can be the same)

The response handler code checks that the responseType matches requestType and throws an error if it doesn't.

On the responding side, as soon as a request is received by respondToRequest, a response is generated and sent back.

On the requesting side, each request is made in the form of a promise, that is automatically fulfilled when the response is received (this is what the id and responseHandler take care of). Each time a new request is made, and the corresponding promise resolver is added to a pending_requests dictionary, were it can be triggered by the receiveResponse function.

Using promises means that the code that works with the response requests (in this case addPose) is super clean. Only one line,
let fullPose = await this.getRobotState(id); is needed to get the response, and everything else is handled in the background. In this case, fullPose can then be used as normal in the rest of the function (saving to firebase, creating a button, etc.)

Currently, all the promise code is handled within operator_pose.js, but this will create a lot of code duplication in the future if we ever want to use this protocol for more request/response communication. To solve this I think a ResponseHandler class should be created that takes care of creating/resolving promises in a consistent way. Then, the PoseManager class can just use an instance of that, rather than managing its own pending_requests dictionary.

---

Pose saving is handled with asynchronous functions getRobotState() (which returns the promise of the response) and addPose() (which saves the current robot state to firebase with the other necessary pose info) in operator_pose.js.

Next Steps:

• Create RequestResponseHandler class that creates and manages pending promises
• Implement deadman's switch
• Add dialogue to remove poses (we probably don't ever want to delete them from firebase, just mark them as disabled so that because its possible that previous demonstrations used those poses)
• Fix sign-in-with-google image