Spaces:
Sleeping
Sleeping
File size: 2,666 Bytes
79ee8ac ac54616 79ee8ac ac54616 79ee8ac 7a6243f 79ee8ac 7a6243f ac54616 79ee8ac f22d17e 79ee8ac f22d17e ac54616 f22d17e 79ee8ac f22d17e 79ee8ac f22d17e 79ee8ac ac54616 79ee8ac f22d17e 79ee8ac ac54616 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 |
import numpy as np
import example_robot_data as erd
import pinocchio as pin
import meshcat
import meshcat.geometry as g
from pinocchio.visualize import MeshcatVisualizer
import time
def _actuated_dof_indices(model):
idxs, labels, bounds = [], [], []
for j in range(1, len(model.joints)):
J = model.joints[j]
if J.shortname() == "JointModelFreeFlyer":
continue
if J.nq == 1:
i0 = J.idx_q
lo = float(model.lowerPositionLimit[i0])
hi = float(model.upperPositionLimit[i0])
if not np.isfinite(lo): lo = -3.14
if not np.isfinite(hi): hi = 3.14
idxs.append(i0)
labels.append(model.names[j])
bounds.append((lo, hi))
return idxs, labels, bounds
class LiveRobot:
"""
Starts a local MeshCat ZMQ+Web server and keeps a persistent connection.
The web UI is proxied at /meshcat/ by nginx (see nginx.conf).
"""
def __init__(self, robot_name="talos"):
self.vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6001")
print(f"Zmq URL: {self.vis.window.zmq_url}, web URL: {self.vis.window.web_url}")
self.rdata = erd.load(robot_name)
self.model = self.rdata.model
self.data = self.model.createData()
self.vis.delete()
time.sleep(0.5) # wait for the robot to appear
self.viz = MeshcatVisualizer(
self.model, self.rdata.collision_model, self.rdata.visual_model
)
self.viz.initViewer(self.vis)
self.viz.clean()
time.sleep(0.5) # wait for the robot to appear
self.viz.loadViewerModel(rootNodeName="robot")
self.q = pin.neutral(self.model)
try:
self.viz.display(self.q)
except Exception as e:
print(f"Error setting robot joints: {e}")
self.idxs, self.labels, self.bounds = _actuated_dof_indices(self.model)
def set_joints(self, vals):
q = self.q.copy()
for v, qi in zip(vals, self.idxs):
q[qi] = float(v)
try:
self.viz.display(q)
self.q = q
except Exception as e:
print(f"Error setting robot joints: {e}")
def neutral(self):
self.q = pin.neutral(self.model)
self.viz.display(self.q)
return [float(self.q[i]) for i in self.idxs]
def iframe(self, width="100%", height=640):
# nginx proxies the MeshCat web app at /meshcat/
return f'<iframe src="/static/" style="width:{width};height:{height}px;border:0"></iframe>'
|