Automatic base rotation (90 degrees CW/CCW)

[mayacakmak]

Problem description

The Stretch cannot move in the direction that its arm is extended. Because of that the user needs to drive towards a target then rotate 90 degrees towards the target to do manipulation. When the user fails to accurately position the base before rotating 90 degrees, they need to re-stow the arm, rotate back, drive closer, rotate again.. which is frustrating. This issue was observed in our user study and was mentioned by Henry Evans as a source of inefficiency and frustration (leading him to wish the Stretch has an omnidirectional base).

Possible ways to address

Some ways to reduce the inefficiency and burden on the user:

• Automate base positioning by adding two buttons for rotating the base by 90degrees CW and CCW (this issue)
• Automate arm stowing and arm preparation to manipulate
• Visualize the reach of the arm during drive mode

@kaviMD All three of these are nicely modular functionalities that you can get started on. I'll create to additional issues for the later two items and reference this issue for the problem description.

[mayacakmak]

With the navigation actions still being triggered through overlay regions (for the targeted version of 2.00) this is how I would recommend adding the 90 degree rotation actions to the interface.

Later if/when we switch to a Beam-like driving interface we can re-think.

[kavidey]

I've added the buttons and front-end code for rotating 90deg in both directions.

For the backend, it looks like we are currently only sending angular velocity goals to the robot. Using stretch's IMU, I can put together a PID loop that would achieve the 90 rotation, but that feels like a pretty clunky solution.

What branch should I be committing this too @mayacakmak? The camera following and sim code is all setup, so I think the best option would be to merge gazebo_sim into master or latest (looks like you already did that, thanks!) create a branch for the three tasks in this issue off of that

[kavidey]

I implemented a function that turns the robot to by a specific angle offset, using a proportional control loop that generates repeated goals until the robot achieves the desired rotation. I'm getting rotation info from tf, which should work in the sim and on the robot.
https://github.com/hcrlab/stretch_web_interface/blob/%2311-decrease-manipulation-burden/robot/ros_connect.js#L356 (the p value should be tuned on the actual robot)

The loop stops automatically if the robot is translated or rotated via the web interface.

[mayacakmak]

Cool! I'm excited to test this on the robot.

[mayacakmak]

@kaviMD I'm testing this merge now. Nothing's happening when I trigger the 90 degree rotation, I'm getting this error that might be relevant. Let me know if you have any thoughts on what might this mean/how to fix.

[kavidey]

I think the issue is with what transform I was getting the robot orientation from. Gazebo publishes as the root transform odom, which is what I was using in simulation. (I used rosrun tf view_frames to find the root transform)

Updating odom this line: https://github.com/hcrlab/stretch_web_interface/blob/master/robot/ros_connect.js#L119, with whatever the non-sim root transform is should fix the problem. My guess is that its world.

[mayacakmak]

Yes! I also did rosun tf view_frames and tried to look for something like robot_base but couldn't find it. World is typically a fixed frame on the ground where the robot starts, not sure it's equivalent to odom

[kavidey]

I think the base of the robot is base_link, which is what I'm using as the fixed frame when I get all the transforms. In order to get absolute orientation, I think that base_link would need to be compared to something fixed that is off the robot, like world, I may also be misunderstanding something about how transforms work though.

[mayacakmak]

That is correct! But typically we use world as the fixed frame and that allows the TFs that we read to be relative to that. Anyway, I tried that and it didn't work :-) it says "world" passed to lookUpTransform does not exist.. Also looking at the TF tree I don't see world or something separate from the robot, so perhaps the estimated pose of the robot is just not included in the TF. Let's ask Charlie what to do here.

[mayacakmak]

@elliston-f Quick question for you, since you're a collaborator on the repo ;) Is there an equivalent of odom TF frame for the (real) Stretch robot? Things were working in simulation with odom but not on the robot. If not, how can we access the base pose estimated with odometry on the Stretch? Thanks!

[mayacakmak]

Hmm, just noticed this line in the launch file:
<param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
...will try changing it to true and go back to using odom.

[mayacakmak]

Okay, that resolved the issue of not having the odom frame! The feature not working yet though (multiple issues, will describe soon).

[hello-ck]

Hi everyone!

I have a couple of suggestions.

@kaviMD, it's great that you've been working on feedback control for the mobile base, since it could help the robot better achieve its goals on some floors. I recently wrote a relevant forum post that I recommend you read. Among other things, my post points to example code that uses a scan matcher with Stretch's laser range finder to provide improved odometry while controlling the mobile base.

That said, for the web interface, I recommend starting with a simple open-loop rotation command to the robot. It won't be perfect, but it will do pretty well on most surfaces, since Stretch's low-level control uses wheel odometry to achieve the commanded angle. Also, the human operator will be able to make corrections after the autonomous attempt.

For the real robot, a mobile base rotation command will rotate the robot by the commanded angle in radians. So, if you want to rotate Stretch by 90 degrees using wheel odometry, you won't need to consider reference frames. The web interface provides an example. The robot's browser executes rotation commands using the baseTurn function in ros_connect.js, which you could use to send a rotation command for ±π/2 radians.

baseTurn uses the generatePoseGoal function in ros_connect.js to move Stretch via the FollowJointTrajectory action server provided by stretch_ros. If you're ever curious about the details, you can look at joint_trajectory_server.py.

Outside of JavaScript, keyboard_teleop provides a Python example that uses the FollowJointTrajectory action server to command the mobile base. It first defines a command based on the user's keyboard input and then sends the command via the FollowJointTrajectory client in the HelloNode class.

I hope this helps!

Best wishes,
Charlie

[kavidey]

Thank you! The forum post was very helpful in understanding the different ways to get rotation data.

For some reason I thought that generatePoseGoal was setting velocity targets for rotation and translation rather than position targets. I can definitely implement sending a command for much ±π/2 radians like you suggested, that seems like a much cleaner solution.

[mayacakmak]

That's super useful @hello-ck, thank you!

@kaviMD I was part way debugging the feedback control with odom fixed in the master branch yesterday. I can get back to it and fix this and test now if you like, but let me know if you're already working on it (just be sure to merge with master first to avoid conflicts), I can look into other stuff in the meantime. thanks!

[mayacakmak]

Indeed generatePoseGoal already using odometry greatly simplifies this feature.

[kavidey]

I'm working on implementing the two necessary functions right now, if its okay I'll just push them to master, this should be a pretty small fix.

[mayacakmak]

Sounds good, let me know when it's ready to test.

[kavidey]

Sorry for the delay, I just modified commands.js to use baseTurn instead of the turnAngleOffset. It doesn't turn exactly 90 degrees, but that may be a simulation artifact or require some on-robot tuning.

I'm in the process of removing all the code attached to turnAngleOffset and will push a commit for that in a bit.

[hello-ck]

I'm glad things are going well and you're close to an initial implementation to test on the robot! Three issues worth considering in the future follow:

1. Rotational Speed Hazard: The default rotational speed for the mobile base might be too high for regular use. You might want to send a rotational command with a lower speed.
2. Extended Arm Hazard: If the telescoping arm is extended, rotating the mobile base can be hazardous. One way to handle this would be to refuse to autonomously rotate the mobile base if the arm is extended. Another way to handle this might be to reduce the rotational speed of the mobile base. For example, the linear speed of the farthest point (i.e., wrist or fingertips) might be kept constant.
3. Autonomous Motion Hazard: If the operator detects an issue, it would be good for them to be able to stop the autonomous motion. Maya and I discussed the possibility of a dead man's switch approach. For example, autonomous motion might require the operator to hold down a mouse button while the mouse cursor is over a web interface button for the motion to continue. If the operator releases the mouse button, moves the mouse cursor off of the web interface button, or activates some sort of stop command, then the robot would cease its autonomous motion. For the mobile base, I believe sending a rotation command for 0 radians will stop the mobile base from rotating. stretch_driver also provides a stop_the_robot service. It would also be good for the robot to stop if the operator's browser disconnects.

Best wishes,
Charlie

[mayacakmak]

@kaviMD Let me know if we have something ready to test for tomorrow.

Super important to think about the hazards that Charlie pointed out.

• For 1: The speed from the earlier tests looked okay
• For 2: Let's add a warning pop up if the arm is extended when a rotation is triggered, with the options (a) Stow arm then rotate; (b) rotate anyways (e.g. if there's nothing around the robot the user might want to take the risk), (c) cancel rotation.
• For 3: We should definitely have a way of cancelling the rotation, I think you were already anticipating this from what I saw in the code. Keep holding the button until rotated 90 degrees might be a bit tiring and awkward (e.g. when you release it's unclear if you press again whether it does another new 90degrees or continue the previous one). But perhaps we can make it so that any click anywhere will stop the rotation. (Non critical, ideas for later) Would be nice to let the user know about that in an intuitive way how to stop the rotation (e.g. the cursor turns into a stop sign while the robot is moving and back to normal once it stops). Also would be nice to have a counter to visualize the robot's progress towards 90degrees while it is rotating.

[kavidey]

The turn fix should already be committed to master (these two commits)

I can work on adding 2 and 3. I did have something in place to cancel the rotation. Previously only base rotations or translations would interrupt the movement, but that is easy to extend to any event. (I temporarily removed the interrupt functionality because it caused some errors with the new movement system, but I will add it back once its working)

[mayacakmak]

I just tested this and it works, yay!

• Like you observed in simulation, it seems to rotate a bit less than 90degrees but pretty consistently; e.g. it gets back to its original rotation after one cw+ccw turn. So, for now, we can try to fix this by commanding a larger value I guess.
• It does not feel very fast but it is a little jumpy, it feels like it's moving in steps rather than a smooth continuous motion, so there could be some tuning of step size, velocity, frequency to smooth that.
• Clicking somewhere else on the interface stops it and executes the other command instead. This is fine but would be better to just stop and not execute another random command that the user happens to click on when they're trying to stop the continuous rotation.

For 2, it would be useful to first address Issue #13 (automatic stowing), add to that some way of tracking if the arm has moved away from its stowed pose and then implement this warning. Let me know if/when you want to work on that--I have a few minor fixes on other things but can get to is shortly.

[hello-ck]

Excellent! The following forum post seems relevant to the accuracy of the rotation:

https://forum.hello-robot.com/t/inaccurate-base-rotations

[kavidey]

@nickswalker These two files contain most of the 90 degree rotation code

stretch_web_interface/shared/commands.js

Lines 530 to 539 in 928573a

	"turn_ccw": function(size) {
	console.log('drive: turn_ccw command received...executing');
	baseTurn(driveRotMedDist, Math.PI/2);
	},
	"turn_cw": function(size) {
	console.log('drive: turn_cw command received...executing');
	baseTurn(driveRotMedDist, -Math.PI/2);
	}

I think that the variable vel is poorly named, from my testing that actually controls the position.

stretch_web_interface/robot/ros_connect.js

Lines 422 to 435 in 928573a

	function baseTurn(ang_deg, vel) {
	// angle in degrees
	// velocity in centimeter / second (linear wheel velocity - same as BaseTranslate)
	console.log('sending baseTurn command')
	if (ang_deg > 0.0){
	var baseTurnLeftPoseGoal = generatePoseGoal({'rotate_mobile_base': -vel})
	baseTurnLeftPoseGoal.send()
	} else if (ang_deg < 0.0) {
	var baseTurnRightPoseGoal = generatePoseGoal({'rotate_mobile_base': vel})
	baseTurnRightPoseGoal.send()
	}
	//sendCommandBody({type: "base",action:"turn", ang:ang_deg, vel:vel});
	}