Creating separate logic for controlling the drill via ROS Comm

[adnanmunawar]

The original implementation did not allow the control of the drill via the drill's ROS Comm. This is because the haptic device / keyboard took over the control. To allow the control via ROS comm, new logic has been added.

Use the keyboard shortcut CTRL + O to toggle between these two control modes a) Haptic Device / Keyboard. b) Drill's ROS Communication.

The picture below shows an info label that denotes what control mode the application is currently in.

Drill Control Mode (a)

Drill Control Mode (b)

[adnanmunawar]

@LauraConnolly @mli0603 @nnagururu This PR should now allow us to replay the drill pose data using Laura's scripts.

[mli0603]

Will check tomorrow after clearing a few items on my end.

[mli0603]

@adnanmunawar I tested with the command ./ambf_simulator --launch_file /home/maxli/git_ws/volumetric_drilling/launch.yaml -l 0,1,4,5

Two bugs I saw:

• there seems to be a bug that i cannot rotate the tool anymore in keyboard mode. It moves but jumpts back to identity.

• If I use the following move(objects, x=False, offset=-13, object_name='mastoidectomy_drill') in ambf_move.py on line 36, the pose data all become NaN

Any ideas?

PS: worth clairfying it is letter O not number 0

[adnanmunawar]

The new commit should fix the keyboard rotation issue. Need to check what is going on with the move method in ambf_move.py.

[mli0603]

I still saw weird behaviors of drilling, which I am not sure if you see with phantom omni. Two things I noticed

• the drill will flash back and forth, and
• sometimes would end up in position like the screenshot

I am not sure about the first one (could be keyboard control is fneaky). The second one looks like the shaft collision logic breaks.

[mli0603]

Here is the script I used to move the drill, essentially calling the set_pos function of the drill object. However, it still doesn't move. I checked Laura's script, and it looks like she is doing something very similar. I am not sure what I missed.

Should we combine these two PRs together so I can test Laura's script?

import time

import numpy as np

from ambf_client import Client

def move(objects, x=True, y=False, z=False, offset=10, object_name='main_camera'):
    # Moves the object in sinusoidally
    t = np.linspace(0.0, 6.28, num=100, endpoint=True)

    path_x = -np.sin(t) + offset
    path_y = -0.5 * np.sin(t * 2)
    path_z = -0.5 * np.sin(t / 2)

    obj = _client.get_obj_handle(object_name)
    for i in range(len(path_x)):
        temp_pos = [path_x[i], path_y[i], path_z[i]]
        print(temp_pos)
        obj.set_pos(temp_pos[0], temp_pos[1], temp_pos[2])
        time.sleep(0.2)

if __name__ == '__main__':
    _client = Client()
    _client.connect()

    move(_client, offset=-13, object_name='/ambf/env/mastoidectomy_drill')
    _client.clean_up()

[adnanmunawar]

I still saw weird behaviors of drilling, which I am not sure if you see with phantom omni. Two things I noticed

• the drill will flash back and forth, and
• sometimes would end up in position like the screenshot

I am not sure about the first one (could be keyboard control is fneaky). The second one looks like the shaft collision logic breaks.

Hmm, I don't see the issue here on my end, although, you are right about the second issue, i.e. when the shaft is colliding with the volume, drilling would stop, and then if you keep inserting the drill into the volume (using a keyboard), it would result in weird behavior. However, when using the haptic device, the force feedback prevents one from going too far in and thus adds a corrective feedback loop that avoids this situation.

For the first issue, does the drill jump back and forth even when not colliding? Can you turn on the visualization of tool cursors using CTRL + C and record a short video with the issue.

[adnanmunawar]

Regarding the script, thanks for sending an example. I think that the issue may be that you are initializing / setting just the position, and not the rotation which is represented via Quaternion. I think it defaults to (0, 0, 0, 0) if not set and it should be sufficient to just set the rotation only once.

I tweaked your example just a little bit, and added some comments. Notably, setting a small waiting period after getting the object handle.

import time

import numpy as np

from ambf_client import Client

def move(objects, x=True, y=False, z=False, offset=10, object_name='main_camera'):
    # Moves the object in sinusoidally
    t = np.linspace(0.0, 6.28, num=100, endpoint=True)

    path_x = -np.sin(t) + offset
    path_y = -0.5 * np.sin(t * 2)
    path_z = -0.5 * np.sin(t / 2)

    obj = _client.get_obj_handle(object_name)
    time.sleep(0.2) # Always good to put a small wait after getting an obj handle for initialization
    rpy = obj.get_rpy() # Get the current RPY
    obj.set_rpy(rpy[0], rpy[1], rpy[2]) # Set the RPY, can also directly set the Quaternion
    for i in range(len(path_x)):
        temp_pos = [path_x[i], path_y[i], path_z[i]]
        print(temp_pos)
        obj.set_pos(temp_pos[0], temp_pos[1], temp_pos[2])
        time.sleep(0.2)

[adnanmunawar]

The latest commit should fix the drill jumping issue.