May 1, Thursday
This week we focus on robot manipulation.
Lab 12: Go over three basic tutorials on TF/TF2 and transform arithmetic:
Lab 13: Next you will develop a "Programming by Demonstration" tool for Stretch.
Here are the specifications for the tool:
The tool should allow you to save a robot end-effector pose (which you can get the robot to by tele-operating it) and later command the robot to move to a saved pose (which it can achieve using motion planning).
You should be able to name a pose when you first save it and see a list of poses to which you can send the robot to.
You should be able to set the coordinate frame of a pose to be one of: (i) robot's start pose (i.e., world coordinates), (ii) robot's base, or (iii) an external landmark (i.e., an ArUco marker detected in the environment).
You should be able to command the robot with a sequence of poses, where it will go to each pose in order.
You can make a command line tool or a web-based tool.
This is the most open-ended lab so far and it requires bringing together a few important concepts learned in past labs and lectures. Here are a few hints for anyone having trouble getting started.
Saving poses
Pay close attention to the Stretch tf2 Static Listener code example from Lab 12. It shows you how to use the tf2 TransformListener and Buffer classes to keep track of the transforms that Stretch is publishing.
Your code for Lab 13 can similarly use tf_buffer.lookup_transform to lookup a transform between link_grasp_center and your chosen reference frame -- this can be base_link (if you want to save end effector positions relative to the robot) or a detected ArUco marker frame (if you want to save positions relative to an object in the environment) and save it for later replay (e.g., in a JSON file with position, orientation, and the frame it was saved in).
Instead of running on a timer, your code should be triggered by subscribing to a message topic (you can reference the tutorial back in Lab 1 to see how). This allows you to save poses by publishing a message on the command line or via a web interface.
You might also find this Stretch Forum discussion useful.
Replaying poses
When you are replaying poses, if the pose is not already in base_link, you can use tf_buffer.transform(pose, target_frame) to reproject saved poses into the current base_link frame. The function expects the pose to be formatted as a PoseStamped message (use from tf2_geometry_msgs import PoseStamped)
To convert a desired end effector position into motion commands for Stretch, we learned in previous lectures this can be done using inverse kinematics (IK). But because the Stretch joints are relatively simple, you could just use a heuristic approach (e.g. use the pose's x to move the base, z to move the lift, and y to extend the arm).
If you’re curious, you can implement full inverse kinematics instead — Hello Robot has a tutorial notebook showing how.
Once you have computed desired commands for the relevant joints, you can send motion commands following this example from Lab 5.