Who Stole My Pen?

The software setup was completed as part of Computer Setup.

The physical setup for this project requires the Trossen PincherX 100 and the Intel Realsense D435i. Here are some tips for how to locate the hardware.

1. The field of view of the RealSense should substantially overlap with part of the PincherX's workspace
2. Try to keep the PincherX away from walls and valuables; (you never know when it will go rogue!)
3. I've found the most compact setup is to offset the D435i 90 degrees from the front of the PincherX
4. Part of the challenge is to place the robot and sensor in appropriate locations. Depending on how you do this, the algorithm may have different parameters.
5. Remember to plug the power cable into the PincherX 100.
6. The RealSense has a tripod you can use to keep it in one location (also, the legs of the tripod extend, if needed).
   • An alternative is to place the RealSense on top of it's box.

1. Alignment is the process of placing two images, taken from different perspectives into the same reference frame
2. In this case, we collect two images: a depth map and a Red, Green, Blue (RGB) image
   • The Depth Map is an NxM array, where each "pixel" is a 16-bit integer proportional to the distance from the camera to the nearest object.
   • The RGB image is an NxMx3 array, where each NxM slice is a different R, G, or B color (be careful, OpenCV often uses BGR instead of RGB ordering for colors!)
3. This Alignment Example from Intel demonstrates how to do several tasks and is a good starting point for your project
   1. Stream depth and RGB images from the camera
   2. Convert the depth "pixel" value into meters
      • The example finds a scaling factor and manually converts. This scaling factor is useful if you want to, for example, convert the entire depth-image to meters.
      • The depth image also has a get_distance function that does the conversion for a single pixel.
   3. Remove objects that are farther than a distance threshold away
   4. Align both images
   5. Process the RGB image with OpenCV
   6. Display the results with OpenCV

• Being able to record data and play it back is useful when testing an image pipeline because it ensures that you are using the same data each time.
• To record data, use config.enable_record_to_file(filename)
• To play back the data, use the config.enable_device_from_file(filename)
• You may wish to provide command-line arguments to easily switch between recording and playback: the argparse library may be helpful.

1. We will use OpenCV to process the images from the Realsense.
2. A complete list of python tutorials is here.
3. It is useful during debug to visualize each step in an image processing pipeline
   • Create a new window using cv2.imshow("name", image) or cv2.newWindow()
   • First, get something reasonable, than worry about refining your object detection.
4. You can use any image processing methods that you want.
   • I've listed some techniques to get you started
   • The end goal of this pipeline is to output the centroid of the pen
5. The Depth map is calibrated by Intel and you can directly convert to real units (as the alignment example does)
   • You can use camera calibration and the known positions of the robot and RealSense to solve for the other dimensions.
   • The Realsense's built-in calibration is not great so for most projects you should do a calibration yourself: however, it is good enough for this project.
   • More information on calibration is under Useful Techniques
6. The output of the image pipeline should be the \((x,y,z)\) location, in meters, relative to the camera frame.
   • Be sure to display or print this information, as it will be needed later.

• See Specifications for a list of joint ids and their limits
• If a joint limit is exceeded, it will hit a hard-stop and the motor will turn off and flash red.
• If a motor is flashing red, you should end the ros2 launch and power-cycle the robot
• The default sleep position
• See Troubleshooting Notes
• Using the robot requires the interbotix workspace setup to be run in each terminal window you are using.

1. To start the arm, open a new terminal window and run the following commands (see Interbotix Setup Instructions for details).

   source ws/interbotix/install/setup.bash
   ros2 launch interbotix_xsarm_control xsarm_control.launch.py robot_model:=px100
   

2. An rviz window showing the location of the arm should open.
3. To Stop running the arm, press C-c in the terminal window.
   • When you stop the robot the motors loose power and the arm may crash to the ground.
   • To prevent damage, always return the robot to its sleep position prior to stopping it or hold the robot and guide it down gently
4. Keep this terminal open until you are ready to stop using the robot. It must be open and running for you to run the robot
5. I have provided (very minimal!) sample code for using the interbotix python sdk.
   • See the comments in interbotix_xs_modules/arm.py and the official documentation
   • This code only works if the previous ros2 launch command is still running in the background
   • Especially when moving the robot for the first time, make sure it is clear of any potential obstacles
   • The code below lets you toggle between the sleep and home position

   • You must source ws/interbotix/install/setup.bash from any terminal where you want to run Interbotix code

     from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS
     from interbotix_common_modules.common_robot.robot import robot_shutdown, robot_startup
     # The robot object is what you use to control the robot
     robot = InterbotixManipulatorXS("px100", "arm", "gripper")
     
     robot_startup()
     mode = 'h'
     # Let the user select the position
     while mode != 'q':
         mode=input("[h]ome, [s]leep, [q]uit ")
         if mode == "h":
             robot.arm.go_to_home_pose()
         elif mode == "s":
             robot.arm.go_to_sleep_pose()
     
     robot_shutdown()
     

   • To see what other commands you can run, look at the documentation linked above.
   • Keep in mind, the robot has only 4 degrees of freedom (DOF). This means that not all end-effector poses are possible.
6. Dive-weight positioning
   • The dive weight helps stabilize the robot
   • However, it is a bit large and can interfere with the sleep position
   • Try to position the weight so that the arm can reach the sleep position and go back to the home position
   • Run the example code to move the arm between sleep and home several times.
     • If any of the motors flash red when going to the sleep position, this means your weight is interfering. you will need to power cycle the robot and try again
   • As the robot moves, you can watch it move in the RViz window as well

Here are some recommended steps to get the robot capturing the pen.

1. Start from my example code and add some testing modes to perform the following actions:
   1. Open and close the grippers.
   2. Move forward or backward by small distance (relative or absolute?)
   3. Move up and down by a small distance (relative or absolute?)
   4. Rotate arm about the base by a small distance (relative or absolute?)
2. Use these modes to get a feel for the workspace limitations of the robot.
   • There are many limitations to the reachable areas of the robot's workspace.
   • Singularities: where the robot geometry causes it to lose a degree of freedom, preventing it from moving in certain directions
   • Joint Limits: mechanical stops and limitations that prevent a joint from rotating more than a certain amount
   • Torque Limits: at some locations the motors may not have enough torque to move the weight of the robot.
     • At these locations, it is possible to command a move that causes a motor to over-torque and shutdown
     • The robot will then fall.
     • Industrial robots typically have more than enough power to lift themselves so this would not be an issue with them.
   • If the planner fails to compute the trajectory, it means that the algorithm could not find a feasible path.
     • A path might be infeasible due to geometry but it also might be infeasible due to time
     • Most movement functions have various parameters related to the timing of the action (for example, if commanding the robot to a position how long should it take for the robot to arrive).
     • Timing is important: large displacements require more time or the acceleration and velocity limits are exceeded. Small displacements can only be done so slowly due to friction and other losses in the system.
3. After each movement, the program should print out the end-effector pose and joint values.

1. robot.arm.set_ee_cartesian_trajectory
   • Move the end-effector along a straight-line path
2. robot.arm.get_joint_commands
   • Get the commanded joint positions of the robot
   • The robot.core.robot_get_joint_states function gets the actual state of the joints.
     • This is better information for our purposes, but it is also lower-level and not needed (get_joint_commands is sufficient)
     • It is listed here for reference.
3. robot.arm.set_joint_positions
   • Set joint positions of the robot
4. set_single_joint_position
   • Set the position of a single joint
   • The joint names (from base to end-effector) are waist, shoulder, elbow, wrist_angle
5. robot.arm.get_ee_pose() Get's the pose of the end effector
   • This is a transformation matrix of the end effector relative to the base of the robot
   • The last column contains the x, y, and z positions in rows 0-2 respectively

6. To compute the end-effector given the joint states, you can use the modern_robotics library directly:

   import modern_robotics as mr
   joints = [waist, shoulder, elbow, waist_angle] # pseudocode, use your own values
   T = mr.FKinSpace(robot.arm.robot_des.M, robot.arm.robot_des.Slist, joints)
   [R, p] = mr.TransToRp(T) # get the rotation matrix and the displacement
   

7. Some of the above commands have an option to make them blocking or non-blocking
   • When blocking, the function will wait until the action is complete before returning (this is the default)
   • When non-blocking, the function returns while the robot is still moving. Movement can then be updated by issuing another command

• The following are workarounds either to undocumented or incorrect behavior in the interbotix library
• Narrowing the problem down to a minimal example and reporting it on github can likely lead to a successful pull-request.
• robot.arm.set_trajectory_time(2, 0.3) resets the trajectory timing to the defaults
  • The current behavior of the library sometimes causes moves like robot.arm.go_to_home_pose() to use the settings from the last call to set_ee_cartesian_trajectory, which can lead to some unexpected motions
• robot_startup() starts a ROS 2 node in a separate thread to process updates to the joint positions. As far as I can tell, the need to use this function is not documented anywhere, but without it the robot's position is only updated when calling some (but not all) movement commands.

Regardless of the method you choose, understanding the assumptions you are making and having a good picture of the coordinate frames in question is essential for debugging.

Here I provide practical guide for the calibration math. This section is based on the notes here: [1])

1. Assume we have a set of \(n\) points:
   • \(P_i \in \mathbb{R}^3, i = 1 \text{ to } n\) are the points in the camera frame
   • \(Q_i \in \mathbb{R}^3\) i = 1 \text{ to } n$ are the same points in the robot frame
2. Find the centroid of the points:
   • \(\bar{P} = \frac{1}{n}\sum^n_i P_i\)
   • \(\bar{Q} = \frac{1}{n}\sum^n_i Q_i\)
3. Subtract the centroid from each point:
   • \(\bar{P}_i = P_i - \bar{P}\)
   • \(\bar{Q}_i = Q_i - \bar{Q}\)
   • The centroid of both point sets are at the same location in space.
4. If the points in one set undergo a pure rotation about their centroid, they should line up with the points in the other set exactly (except for noise).
   • The immediate is to find a rotation matrix \(R \in \mathbb{R}^{3\times3}\) such that \(\bar{Q}_i = R \bar{P}_i \text{ for } i = 1 \text { to } n\).
   • An algorithm to find \(R\) is the Kabsch Algorithm, which is implemented in scipy as scipy.spatial.transform.Rotation.align_vectors
   • The algorithm takes two sets of points and finds the rotation between them (assuming they only differ by a rotation and some noise).
5. After finding \(R\) the goal is to find the translation \(t\) between the point-sets:
   • Converting from a point \(P\) in the camera frame to a point \(Q\) in the robot frame is done with \(Q = RP + t\).
   • Once \(R\) is known then \(t\) can be found using the conversion formula and the centroids (or any two corresponding points)

Write a script that performs the calibration procedure:

1. The script should be run while the robot is holding a pen
2. The robot should move to a few pre-set positions to gather the necessary data
3. The script then computes and outputs the calibration parameters
4. The robot moves to a few test locations and the camera coordinates are converted to the robot coordinates and compared

When rotating (this assumes that both the arm and pen's locations are expressed as cylindrical coordinates relative to the robot base):

1. Measure the pen location
2. Measure the robot's location
3. Move based on the locations (e.g., proportionally to the error between the pen and robot angle)

1. Given the pen's location, compute the waist angle and move the arm to that angle
2. Then move forward based on the computed distance.

1. There may be a few modes of operation as you control the robot (e.g., moving to the pen, closing grippers, etc). It makes sense to track these modes by assigning a state to each of them. You can use a python enum to label and track states
2. If the pen's location is presented as cylindrical coordinates in the robot frame, the computations can be significantly simplified:
   • It may be helpful to refer to your drawing of the coordinate system
3. It may be easier to command locations in joint coordinates rather than needing to go through the IK process
   • You can use other tools you have develo

• You may have noticed (depending on how you implemented your control loop) that when putting the control together, the image data from the camera slows down.
• There are a few contributing factors to this slowness
  1. Blocking functions: some interbotix functions do not have the option of being non-blocking: thus, while using these functions to move the robot, the OpenCV window is not updated and images are not read while the Realsense is moving.
     • It is technically possible to complete this task without using any functions that require blocking, but that would amount to essentially re-writing some of the interbotix library
  2. When robot_startup is called, the interbotix library runs a thread in the background to read the joint information from the robot
     • Due to the concurrency model of python, only one thread can execute at a time, thus while the joints are updated, your code does not run
     • It is possible to run code without robot_startup and manually trigger updates to the robot state, however, accomplishing this would require a deep dive into the interbotix library implementation.
• Overall, I believe it is possible to get smooth movement and image processing in a single thread, but not without essentially rewriting parts of the interbotix library and interacting with the robot on a lower-level than what is provided by the Python API
  • While not ideal, the target of this API is for beginner users: when using the robot via ROS 2 you would not have this problem.
• There is, however, a workaround:
  1. Use the python multiprocessing library to run the image updating and movement code in separate processes at the same time.
  2. While there are many ways of implementing this, one of the less error prone is to use a multiprocessing.Queue to pass information between the processes.