Aligning Stretch parallel to the drawing surface

I am trying to draw a silhouette of an image given as input. I got the contour and can make stretch draw multiple figures. Until now, I ran the code with Stretch parallel to the vertical drawing surface.
But I want to program it so that it can travel a small distance after detecting a whiteboard and aligning itself. In order to do that I’m using funmap and cliff alignment to turn toward the surface, which is working fine so far. But we are unable to change the pan of the camera.

Our drawing surface is parallel to both the arm and the height as shown in the image.

I want to change the position of the camera so that it faces the whiteboard, moves towards it, and aligns itself parallel to the surface with the wrist touching the surface.

I believe I can take care of the last part but everything before that is giving us multiple issues. Right now we are stuck attempting to make the camera face in the direction of the surface (the camera aligns parallel to the arm) What we want:

  1. Make the camera turn towards the surface with funmap running (as shown in the image)
  2. Decide on an alignment, right now stretch is indecisive and moves a bit in an attempt to be aligned.
  3. Move towards the surface and stop at a certain constant distance.

I think the idea needs more details, I can clarify any topic if need be. Any tips or resources would be helpful. Thanks in advance.

Hi @Vi7n, thanks for detailed explanation of what you’re trying to do. If you already have a node to call the align to nearest cliff service, you can access a method called “move_to_pose”. Your node can extend the HelloNode class (example here) and use

self.move_to_pose({ 'joint_head_pan': 0.2, 'joint_head_tilt': -0.2 })

to send position commands the head’s pan and tilt joints.

However, based on the image, I believe the closest cliff to the robot is not the whiteboard, but instead the table. If you take a look at this video, you’ll see a few examples of what the service deems to be the closest cliff.

Lastly, whenever the align to nearest cliff service is called, the cliff detected by the algorithm is logged as a debug image to the ‘~/stretch_user/debug/align_with_nearest_cliff/’ directory. This may help you better interpret what the service is thinking and why Stretch moved the way it did.

Hi, @bshah Thank you for your swift response.