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:
