AMBF client not updating the pose of cameras and Camera frame

[jabarragann]

Hi Adnan,

To automatically collect data for a machine learning algorithm I was trying to use the AMBF clients to query the position of the cameras and camera frames while moving the virtual ECM with the GUI scripts. In this script, I am just instantiating the clients and then querying the camera's position every 1 second.

From the script, I was expecting to see changes in the cameraFrame's pose when moving the ECM sliders. Also, I was expecting to see changes in the cameraL/R when using my mouse to manually move them around. But, neither of these two things are happening. The 4x4 transformation matrices I get from the clients are always the same no matter how I interface with the simulator and they will only change every time I restart the script.

Could you advise me on how to get the correct poses from the clients?

from surgical_robotics_challenge.scene import Scene
from surgical_robotics_challenge.camera import Camera
from ambf_client import Client
import rospy
import time
import numpy as np
import tf_conversions.posemath as pm

np.set_printoptions(precision=3, suppress=True, sign=" ")

rospy.init_node("dataset_collection")
c = Client("juanclient")
c.connect()

# Create clients
scene = Scene(c)
ambf_cam_l = Camera(c, "cameraL")
ambf_cam_r = Camera(c, "cameraR")
ambf_cam_frame = Camera(c, "CameraFrame")

#Query camera pose.
counter = 0
flag = True
try:
    while flag:
        T_FC = pm.toMatrix(ambf_cam_l.get_T_c_w())
        T_WF = pm.toMatrix(ambf_cam_frame.get_T_c_w())
        print(f"pose at step {counter}")
        print(f"camera to frame\n {T_FC}")
        print(f"frame to world\n {T_WF}")
        print("\n\n\n")
        counter += 1
        time.sleep(1.5)
        if counter > 20:
            break

except Exception as e:
    print("Exit code")

[adnanmunawar]

Hi Juan,

Thanks for sending the example script. This may be a bug, resulting from some optimization logic. Can you try this updated script and check if the values are updated?

Also, the pose of cameraL and cameraR should not change if you only move CameraFrame, as they are parented to the CameraFrame, and thus their reported pose should always be relative to the parent. However, if you move them individually, then the pose should change.

from surgical_robotics_challenge.scene import Scene
from surgical_robotics_challenge.camera import Camera
from ambf_client import Client
import rospy
import time
import numpy as np
import tf_conversions.posemath as pm

np.set_printoptions(precision=3, suppress=True, sign=" ")

rospy.init_node("dataset_collection")
c = Client("juanclient")
c.connect()

# Create clients
scene = Scene(c)
ambf_cam_l = Camera(c, "cameraL")
ambf_cam_r = Camera(c, "cameraR")
ambf_cam_frame = Camera(c, "CameraFrame")

#Query camera pose.
counter = 0
flag = True
try:
    while flag:
        ambf_cam_l.set_pose_changed() # Addition
        ambf_cam_frame.set_pose_changed() # Addition
        T_FC = pm.toMatrix(ambf_cam_l.get_T_c_w())
        T_WF = pm.toMatrix(ambf_cam_frame.get_T_c_w())
        print(f"pose at step {counter}")
        print(f"camera to frame\n {T_FC}")
        print(f"frame to world\n {T_WF}")
        print("\n\n\n")
        counter += 1
        time.sleep(1.5)
        if counter > 20:
            break

except Exception as e:
    print("Exit code")

[jabarragann]

Hi Adnan! Thanks that fixed the issue! Will you be adding the set_pose_changed() call inside the get_T_c_w function? Or should I add the set_pose_changed() to my scripts?

[adnanmunawar]

Hi Juan, glad that we found what the issue is. I think we should fix it properly rather than requiring to add the set_pose_changed method. I shall try to create a fix today.

[adnanmunawar]

Closing this as the above commit should fix the issue.

[jabarragann]

Hi Adnan, just wanted to confirm that the issue was solved!