def main(): # Define the first pose. T_1 = SE3((SO3.from_roll_pitch_yaw(np.pi / 4, 0, np.pi / 2), np.array([[1, 1, 1]]).T)) # Define the second pose. T_2 = SE3((SO3.from_roll_pitch_yaw(-np.pi / 6, np.pi / 4, np.pi / 2), np.array([[1, 4, 2]]).T)) # Plot the interpolation. # Use Qt 5 backend in visualisation. matplotlib.use('qt5agg') # Create figure and axis. fig = plt.figure() ax = plt.axes(projection='3d') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # Plot the poses. vg.plot_pose(ax, T_1.to_tuple()) vg.plot_pose(ax, T_2.to_tuple()) # Plot the interpolated poses. for alpha in np.linspace(0, 1, 20): T = interpolate_lie_element(alpha, T_1, T_2) vg.plot_pose(ax, T.to_tuple(), alpha=0.1) # Show figure. vg.plot.axis_equal(ax) plt.show()
def test_oplus_with_ominus_diff(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) xi_vec_diff = Y.ominus(X) Y_from_X = X.oplus(xi_vec_diff) np.testing.assert_almost_equal(Y_from_X.to_matrix(), Y.to_matrix(), 14)
def test_jacobian_Y_ominus_X_wrt_Y(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_ominus_Y = Y.jac_Y_ominus_X_wrt_Y(X) # Should be J_r_inv. np.testing.assert_equal(J_ominus_Y, SE3.jac_right_inverse(Y - X)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = Y.oplus(delta).ominus(X) - (Y.ominus(X) + (J_ominus_Y @ delta)) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 6)
def test_jacobian_composition_XY_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_comp_X = X.jac_composition_XY_wrt_X(Y) # Jacobian should be Y.inverse().adjoint() np.testing.assert_almost_equal(J_comp_X, Y.inverse().adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).compose(Y) - X.compose(Y).oplus(J_comp_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_jacobian_composition_XY_wrt_Y(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) Y = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_comp_Y = X.jac_composition_XY_wrt_Y() # Jacobian should be identity np.testing.assert_array_equal(J_comp_Y, np.identity(6)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.compose(Y.oplus(delta)) - X.compose(Y).oplus(J_comp_Y @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_exp_of_log(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) xi_vec = X.Log() np.testing.assert_almost_equal( SE3.Exp(xi_vec).to_matrix(), X.to_matrix(), 14)
def test_to_tuple(): so3 = SO3.from_roll_pitch_yaw(np.pi / 3, -np.pi / 2, -3 * np.pi / 2) t = np.array([[-1, 1, 3]]).T se3 = SE3((so3, t)) pose_tuple = se3.to_tuple() np.testing.assert_equal(pose_tuple[0], so3.matrix) np.testing.assert_equal(pose_tuple[1], t)
def test_composition_with_identity(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) comp_with_identity = X.compose(SE3()) comp_from_identity = SE3().compose(X) np.testing.assert_almost_equal(comp_with_identity.to_matrix(), X.to_matrix(), 14) np.testing.assert_almost_equal(comp_from_identity.to_matrix(), X.to_matrix(), 14)
def test_to_matrix(): so3 = SO3.from_roll_pitch_yaw(np.pi / 4, np.pi / 2, 3 * np.pi / 2) t = np.array([[3, 2, 1]]).T se3 = SE3((so3, t)) T = se3.to_matrix() np.testing.assert_equal(T[0:3, 0:3], so3.matrix) np.testing.assert_equal(T[0:3, 3:], t) np.testing.assert_equal(T[3:, :], np.array([[0, 0, 0, 1]]))
def test_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 4, np.pi / 2, 3 * np.pi / 2), np.array([[1, -2, 3]]).T)) X_inv = X.inverse() np.testing.assert_equal(X_inv.rotation.matrix, X.rotation.inverse().matrix) np.testing.assert_equal(X_inv.translation, -(X.rotation.inverse() * X.translation)) np.testing.assert_almost_equal((X @ X_inv).to_matrix(), np.identity(4))
def test_construct_with_matrix(): so3 = SO3.from_roll_pitch_yaw(np.pi / 10, np.pi / 4, -np.pi / 2) t = np.array([[1, 2, 3]]).T T = np.block([[so3.matrix, t], [0, 0, 0, 1]]) se3 = SE3.from_matrix(T) np.testing.assert_almost_equal(se3.rotation.matrix, so3.matrix, 14) np.testing.assert_equal(se3.translation, t)
def test_construct_with_roll_pitch_yaw(): np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(np.pi / 2, 0, 0).matrix, SO3.rot_x(np.pi / 2).matrix, 14) np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(0, np.pi / 2, 0).matrix, SO3.rot_y(np.pi / 2).matrix, 14) np.testing.assert_almost_equal( SO3.from_roll_pitch_yaw(0, 0, np.pi / 2).matrix, SO3.rot_z(np.pi / 2).matrix, 14) roll = np.pi pitch = np.pi / 2 yaw = np.pi so3 = SO3.from_roll_pitch_yaw(roll, pitch, yaw) expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) np.testing.assert_almost_equal(so3.matrix, expected, 14)
def test_ominus_with_oplus_diff(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) rho_vec = np.array([[4, 3, 2]]).T theta_vec = 2 * np.pi / 3 * np.array([[2, 1, 1]]).T / np.sqrt(6) xi_vec = np.vstack((rho_vec, theta_vec)) Y = X.oplus(xi_vec) xi_vec_diff = Y.ominus(X) np.testing.assert_almost_equal(xi_vec_diff, xi_vec, 14)
def test_ominus_with_oplus_diff_with_operators(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) rho_vec = np.array([[1, 2, 3]]).T theta_vec = 2 * np.pi / 3 * np.array([[-2, 1, 1]]).T / np.sqrt(6) xi_vec = np.vstack((rho_vec, theta_vec)) Y = X + xi_vec xi_vec_diff = Y - X np.testing.assert_almost_equal(xi_vec_diff, xi_vec, 14)
def test_jacobian_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 7, np.pi / 3, 4 * np.pi / 6), np.array([[2, 1, 0]]).T)) J_inv = X.jac_inverse_X_wrt_X() # Jacobian should be -X.adjoint(). np.testing.assert_equal(J_inv, -X.adjoint()) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).inverse() - X.inverse().oplus(J_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_jacobian_action_Xx_wrt_x(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) x = np.array([[1, 2, 3]]).T J_action_x = X.jac_action_Xx_wrt_x() # Jacobian should be R. np.testing.assert_array_equal(J_action_x, X.rotation.matrix) # Test the Jacobian numerically. delta = 1e-3 * np.ones((3, 1)) taylor_diff = X.action(x + delta) - (X.action(x) + J_action_x @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 14)
def test_jacobian_action_Xx_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) x = np.array([[1, 2, 3]]).T J_action_X = X.jac_action_Xx_wrt_X(x) # Jacobian should be [R, - R*SO3.hat(x)]. np.testing.assert_array_equal(J_action_X, np.block([[X.rotation.matrix, -(X.rotation.matrix @ SO3.hat(x))]])) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).action(x) - (X.action(x) + J_action_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((3, 1)), 5)
def left_right_perturbations(): # Define the pose of a camera. T_w_c = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0, np.pi / 2), np.array([[2, 2, 2]]).T)) # Perturbation. xsi_vec = np.array([[2, 0, 0, 0, 0, 0]]).T # Perform the perturbation on the right (locally). T_r = T_w_c @ SE3.Exp(xsi_vec) # Perform the perturbation on the left (globally). T_l = SE3.Exp(xsi_vec) @ T_w_c # We can transform the tangent vector between local and global frames using the adjoint matrix. xsi_vec_w = T_w_c.adjoint( ) @ xsi_vec # When xsi_vec is used on the right side. xsi_vec_c = T_w_c.inverse().adjoint( ) @ xsi_vec # When xsi_vec is used on the left side. print('xsi_vec_w: ', xsi_vec_w.flatten()) print('xsi_vec_c: ', xsi_vec_c.flatten()) # Visualise the perturbations: # Use Qt 5 backend in visualisation, use latex. matplotlib.use('qt5agg') # Create figure and axis. fig = plt.figure() ax = plt.axes(projection='3d') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # Plot frames. to_right = np.hstack((T_w_c.translation, T_r.translation)) ax.plot(to_right[0, :], to_right[1, :], to_right[2, :], 'k:') to_left = np.hstack((T_w_c.translation, T_l.translation)) ax.plot(to_left[0, :], to_left[1, :], to_left[2, :], 'k:') vg.plot_pose(ax, SE3().to_tuple(), text='$\mathcal{F}_w$') vg.plot_pose(ax, T_w_c.to_tuple(), text='$\mathcal{F}_c$') vg.plot_pose(ax, T_r.to_tuple(), text=r'$\mathbf{T}_{wc} \circ \mathrm{Exp}(\mathbf{\xi})$') vg.plot_pose(ax, T_l.to_tuple(), text=r'$\mathrm{Exp}(\mathbf{\xi}) \circ \mathbf{T}_{wc}$') # Show figure. vg.plot.axis_equal(ax) plt.show()
def test_jacobian_right_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) xi_vec = X.Log() J_r_inv = SE3.jac_right_inverse(xi_vec) # Should have J_l * J_r_inv = Exp(xi_vec).adjoint(). J_l = SE3.jac_left(xi_vec) np.testing.assert_almost_equal(J_l @ J_r_inv, SE3.Exp(xi_vec).adjoint(), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).Log() - (X.Log() + J_r_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def test_jacobian_X_oplus_tau_wrt_tau(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) rho_vec = np.array([[2, 1, 2]]).T theta_vec = np.pi / 4 * np.array([[-1, -1, -1]]).T / np.sqrt(3) xi_vec = np.vstack((rho_vec, theta_vec)) J_oplus_tau = X.jac_X_oplus_tau_wrt_tau(xi_vec) # Should be J_r. np.testing.assert_equal(J_oplus_tau, X.jac_right(xi_vec)) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(xi_vec + delta) - X.oplus(xi_vec).oplus(J_oplus_tau @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 6)
def test_jacobian_X_oplus_tau_wrt_X(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 10, -np.pi / 3, 3 * np.pi / 4), np.array([[3, 2, 1]]).T)) rho_vec = np.array([[2, 1, 2]]).T theta_vec = np.pi / 4 * np.array([[-1, -1, -1]]).T / np.sqrt(3) xi_vec = np.vstack((rho_vec, theta_vec)) J_oplus_X = X.jac_X_oplus_tau_wrt_X(xi_vec) # Should be Exp(tau).adjoint().inverse() np.testing.assert_almost_equal(J_oplus_X, np.linalg.inv(SE3.Exp(xi_vec).adjoint()), 14) # Test the Jacobian numerically. delta = 1e-3 * np.ones((6, 1)) taylor_diff = X.oplus(delta).oplus(xi_vec) - X.oplus(xi_vec).oplus(J_oplus_X @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 14)
def test_jacobian_left_inverse(): X = SE3((SO3.from_roll_pitch_yaw(np.pi / 8, np.pi / 2, 5 * np.pi / 6), np.array([[2, 1, 1]]).T)) xi_vec = X.Log() J_l_inv = SE3.jac_left_inverse(xi_vec) # Should have that left and right are block transposes of each other. J_r_inv = SE3.jac_right_inverse(xi_vec) np.testing.assert_almost_equal(J_l_inv[:3, :3], J_r_inv[:3, :3].T, 14) np.testing.assert_almost_equal(J_l_inv[:3, 3:], J_r_inv[:3, 3:].T, 14) np.testing.assert_almost_equal(J_l_inv[3:, 3:], J_r_inv[3:, 3:].T, 14) # Test the Jacobian numerically (using Exps and Logs, since left oplus and ominus have not been defined). delta = 1e-3 * np.ones((6, 1)) taylor_diff = (SE3.Exp(delta) @ X).Log() - (X.Log() + J_l_inv @ delta) np.testing.assert_almost_equal(taylor_diff, np.zeros((6, 1)), 5)
def vis_perturbations(): # Define the fixed frame "a" relative to the world frame "w". T_w_a = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0, np.pi / 2), np.array([[2, 2, 2]]).T)) # The vector xi represents a perturbation on the tangent vector space. # We change the elements in this vector by using the sliders. xi_vec = np.zeros([6, 1]) # We can choose to draw an oriented box around the perturbed pose, # and we can draw the trajectory along the manifold (by interpolation). draw_options = {'Draw box': False, 'Draw manifold\ntrajectory': True} # Use Qt 5 backend in visualisation. matplotlib.use('qt5agg') # Create figure and axis. fig, ax = plt.subplots(subplot_kw={'projection': '3d'}) plt.subplots_adjust(left=0.25) ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') ax.margins(x=0) # Add the widgets. widget_color = 'lightgoldenrodyellow' xi_sliders = [ Slider(plt.axes([0.05, 0.9 - 0.05 * i, 0.20, 0.03], facecolor=widget_color), r'$\xi_' + str(i + 1) + '$', -4.0, 4.0, valinit=xi_vec[i].item(), valstep=0.01) for i in range(6) ] button = Button(plt.axes([0.1, 0.55, 0.1, 0.04]), 'Reset', color=widget_color, hovercolor='0.975') check = CheckButtons( plt.axes([0.025, 0.10, 0.24, 0.15], facecolor=widget_color), draw_options.keys(), draw_options.values()) radio = RadioButtons( plt.axes([0.025, 0.3, 0.24, 0.2], facecolor=widget_color), (r'1. $\mathrm{Exp}(\mathbf{\xi})$', r'2. $\mathbf{T}_{wc} \circ \mathrm{Exp}(\mathbf{\xi})$', r'3. $\mathrm{Exp}(\mathbf{\xi}) \circ \mathbf{T}_{wc}$'), active=0) # Setup the update callback, which is called by the sliders and the radio buttons. def update(val): ax.clear() for i, slider in enumerate(xi_sliders): xi_vec[i] = slider.val if radio.value_selected[0] == '1': draw_exp(ax, xi_vec, draw_options) elif radio.value_selected[0] == '2': draw_right_perturbation(ax, T_w_a, xi_vec, draw_options) else: draw_left_perturbation(ax, T_w_a, xi_vec, draw_options) ax.set_xlim(-4, 4) ax.set_ylim(-4, 4) ax.set_zlim(-4, 4) vg.plot.axis_equal(ax) fig.canvas.draw_idle() for slider in xi_sliders: slider.on_changed(update) radio.on_clicked(update) # Setup the check buttons to update the "draw options". def update_draw_options(label): draw_options[label] = not draw_options[label] update([]) check.on_clicked(update_draw_options) # Setup the reset callback, used by the reset button. def reset(event): for slider in xi_sliders: slider.reset() button.on_clicked(reset) # Start with first update. update([]) plt.show()
import numpy as np import visgeom as vg import matplotlib import matplotlib.pyplot as plt from pylie import SO3, SE3 # Define the pose of a camera. T_w_c = SE3((SO3.from_roll_pitch_yaw(5 * np.pi / 4, 0, np.pi / 2), np.array([[2, 2, 2]]).T)) # Perturbation. xsi_vec = np.array([[2, 0, 0, 0, 0, 0]]).T # Perform the perturbation on the right (locally). T_r = T_w_c @ SE3.Exp(xsi_vec) # Perform the perturbation on the left (globally). T_l = SE3.Exp(xsi_vec) @ T_w_c # We can transform the tangent vector between local and global frames using the adjoint matrix. xsi_vec_w = T_w_c.adjoint( ) @ xsi_vec # When xsi_vec is used on the right side. xsi_vec_c = T_w_c.inverse().adjoint( ) @ xsi_vec # When xsi_vec is used on the left side. print('xsi_vec_w: ', xsi_vec_w.flatten()) print('xsi_vec_c: ', xsi_vec_c.flatten()) # Visualise the perturbations: # Use Qt 5 backend in visualisation, use latex. matplotlib.use('qt5agg')
def poses_and_cameras(): # Use Qt 5 backend in visualisation. matplotlib.use('qt5agg') # Create figure and axis. fig = plt.figure() ax = plt.axes(projection='3d') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # Plot the pose of the world North-East-Down (NED) frame (relative to the world frame). T_w_w = SE3() vg.plot_pose(ax, T_w_w.to_tuple(), scale=3, text='$\mathcal{F}_w$') # Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame). roll = np.radians(-10) pitch = np.radians(0) yaw = np.radians(135) t_w_b = np.array([[-10, -10, -2]]).T T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b)) vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$') # Plot the camera frame. # The camera is placed 2 m directly above the body origin. # Its optical axis points to the left (in opposite direction of the y-axis in F_b). # Its y-axis points downwards along the z-axis of F_b. R_b_c = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) t_b_c = np.array([[0, 0, -2]]).T T_b_c = SE3((SO3(R_b_c), t_b_c)) T_w_c = T_w_b @ T_b_c vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$') # Plot obstacle frame. # The cube is placed at (North: 10 m, East: 10 m, Down: -1 m). # Its top points upwards, and its front points south. R_w_o = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]]) t_w_o = np.array([[10, 10, -1]]).T T_w_o = SE3((SO3(R_w_o), t_w_o)) vg.plot_pose(ax, T_w_o.to_tuple(), scale=3, text='$\mathcal{F}_o$') # Plot the cube with sides 3 meters. points_o = vg.utils.generate_box(scale=3) points_w = T_w_o * points_o vg.utils.plot_as_box(ax, points_w) # Plot the image plane. img_plane_scale = 3 K = np.array([[50, 0, 40], [0, 50, 30], [0, 0, 1]]) vg.plot_camera_image_plane(ax, K, T_w_c.to_tuple(), scale=img_plane_scale) # Project the box onto the normalised image plane (at z=img_plane_scale). points_c = T_w_c.inverse() @ T_w_o * points_o xn = points_c / points_c[2, :] xn_w = T_w_c * (img_plane_scale * xn) vg.utils.plot_as_box(ax, xn_w) # Show figure. vg.plot.axis_equal(ax) ax.invert_zaxis() ax.invert_yaxis() plt.show()
fig = plt.figure() ax = plt.axes(projection='3d') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') # Plot the pose of the world North-East-Down (NED) frame (relative to the world frame). T_w_w = SE3() vg.plot_pose(ax, T_w_w.to_tuple(), scale=3, text='$\mathcal{F}_w$') # Plot the body frame (a body-fixed Forward-Right-Down (FRD) frame). roll = np.radians(-10) pitch = np.radians(0) yaw = np.radians(135) t_w_b = np.array([[-10, -10, -2]]).T T_w_b = SE3((SO3.from_roll_pitch_yaw(roll, pitch, yaw), t_w_b)) vg.plot_pose(ax, T_w_b.to_tuple(), scale=3, text='$\mathcal{F}_b$') # Plot the camera frame. # The camera is placed 2 m directly above the body origin. # Its optical axis points to the left (in opposite direction of the y-axis in F_b). # Its y-axis points downwards along the z-axis of F_b. R_b_c = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) t_b_c = np.array([[0, 0, -2]]).T T_b_c = SE3((SO3(R_b_c), t_b_c)) T_w_c = T_w_b @ T_b_c vg.plot_pose(ax, T_w_c.to_tuple(), scale=3, text='$\mathcal{F}_c$') # Plot obstacle frame. # The cube is placed at (North: 10 m, East: 10 m, Down: -1 m). # Its top points upwards, and its front points south.