Stheta_ddot = np.dot(inertia_inv, wrench_in_cylinder) Stheta_dot += dt * Stheta_ddot cylinder2world = transform_from_exponential_coordinates( dt * Stheta_dot).dot(prev_cylinder2world) # Update visualization cylinder_frame.set_data(cylinder2world) cylinder.set_data(cylinder2world) prev_cylinder2world[:, :] = cylinder2world return cylinder_frame, cylinder fig = pv.figure() # Definition of cylinder mass = 1.0 length = 0.5 radius = 0.1 inertia_inv = np.linalg.inv( spatial_inertia_of_cylinder(mass=mass, length=length, radius=radius)) # State of cylinder cylinder2world = np.eye(4) twist = np.zeros(6) cylinder = fig.plot_cylinder(length=length, radius=radius, c=[1, 0.5, 0]) cylinder_frame = fig.plot_transform(A2B=cylinder2world, s=0.5)
<child link="link6"/> <axis xyz="0 0 1.0"/> </joint> <joint name="jointtcp" type="fixed"> <origin xyz="0 0 0.05" rpy="0 0 0"/> <parent link="link6"/> <child link="tcp"/> </joint> </robot> """ tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) joint_names = ["joint%d" % i for i in range(1, 7)] joint_angles = [0, 0.5, 0.5, 0, 0.5, 0] for name, angle in zip(joint_names, joint_angles): tm.set_joint(name, angle) fig = pv.figure("URDF") fig.plot_graph(tm, "compi", show_frames=True, show_connections=True, whitelist=["link%d" % d for d in range(1, 7)], s=0.05) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg")
Animates a rotation about the x-axis. """ print(__doc__) import numpy as np import pytransform3d.visualizer as pv from pytransform3d import rotations as pr def animation_callback(step, n_frames, frame): angle = 2.0 * np.pi * (step + 1) / n_frames R = pr.matrix_from_angle(0, angle) A2B = np.eye(4) A2B[:3, :3] = R frame.set_data(A2B) return frame fig = pv.figure(width=500, height=500) frame = fig.plot_basis(R=np.eye(3), s=0.5) fig.view_init() n_frames = 100 if "__file__" in globals(): fig.animate( animation_callback, n_frames, fargs=(n_frames, frame), loop=True) fig.show() else: fig.save_image("__open3d_rendered_image.jpg")
return graph BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while (not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d"): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(data_dir + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=data_dir) fig = pv.figure("URDF with meshes") graph = fig.plot_graph(tm, "lower_cone", s=0.1, show_connections=True, show_visuals=True) fig.view_init() fig.set_zoom(1.2) n_frames = 100 if "__file__" in globals(): fig.animate(animation_callback, n_frames, loop=True, fargs=(n_frames, tm, graph)) fig.show() else: