Hide the cubes,
Note cube pose.
Pick up the robot, put it down, note pose and origin_id.
Put cube in front of robot.
Note cube pose.
Turn robot 90 deg. and note poses.
Pick up robot and put it down again facing same way.
Note robot pose, cube pose.
Now turn robot -90 deg so it sees cube; not poses.

It looks like the cube will only force the robot back to
its old origin_id if the cube hasn't been moved.

====

Write "show kine" to display the tree

Create a kinematic frame for the back axle.


Create kinematic frames for left_hook and right_hook with z pointing up

Given a point p 100 mm in front of the base frame, so p =
transform.point(0,0,100).  We can find its coordinates relative to the
shoulder frame by writing: robot.kine.base_to_joint('shoulder').dot(p)

Suppose we have an object that is 50 mm in front of the camera's focal
plane, and we want to know how far forward of the front axle it lies.
If the head is tilted up, the object will not be as far forward as if
the head is pointing straight horizontal.  Write a function to
calculate the answer.  Then write a program to move the head through a
range of head angles (-10, 0, +10, +20, and +30 degrees) and print the
answer for each head angle