42 inline std::vector<double> FrameToVector(
const KDL::Frame& frame,
double scale_x = 1.0,
double scale_y = 1.0,
double scale_z = 1.0)
44 std::vector<double> ret(16);
45 ret[0] = frame.
M.
data[0] * scale_x;
46 ret[1] = frame.
M.
data[3];
47 ret[2] = frame.
M.
data[6];
49 ret[4] = frame.
M.
data[1];
50 ret[5] = frame.
M.
data[4] * scale_y;
51 ret[6] = frame.
M.
data[7];
53 ret[8] = frame.
M.
data[2];
54 ret[9] = frame.
M.
data[5];
55 ret[10] = frame.
M.
data[8] * scale_z;
57 ret[12] = frame.
p.
data[0];
58 ret[13] = frame.
p.
data[1];
59 ret[14] = frame.
p.
data[2];
64 inline std::vector<double> PositionToVector(
const KDL::Frame& frame)
66 std::vector<double> ret(3);
67 ret[0] = frame.
p.
data[0];
68 ret[1] = frame.
p.
data[1];
69 ret[2] = frame.
p.
data[2];
73 inline std::vector<double> QuaternionToVector(
const KDL::Frame& frame)
75 std::vector<double> ret(4);
83 Initialize(use_mesh_materials);
92 web_url_ = RequestWebURL();
95 std::regex url_regex(
"(.*):(?:\\d+)(?:\\/static\\/)");
97 if (std::regex_search(web_url_, match, url_regex) && match.size() > 1)
99 file_url_ = match.str(1) +
":9000/files/";
103 if (web_url_.size() > 7) file_url_ = web_url_.substr(0, web_url_.size() - 7) +
"files/";
105 path_prefix_ =
"/exotica/" + scene_->GetName() +
"/";
110 socket_ = std::unique_ptr<zmq::socket_t>(
new zmq::socket_t(context_, ZMQ_REQ));
111 socket_->setsockopt(ZMQ_RCVTIMEO, TIMEOUT);
112 socket_->connect(zmq_url_);
117 zmq::message_t request(data.size());
118 std::memcpy(request.data(), data.c_str(), data.size());
119 socket_->send(request);
125 std::memset(buffer, 0, 2048);
126 socket_->recv(&buffer, 2048);
127 return std::string(buffer);
146 template <
typename T>
149 msgpack::sbuffer sbuf;
150 msgpack::pack(sbuf, msg);
151 socket_->send(msg.type.data(), msg.type.size(), ZMQ_SNDMORE);
152 socket_->send(msg.path.data(), msg.path.size(), ZMQ_SNDMORE);
153 socket_->send(sbuf.data(), sbuf.size());
159 const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
161 for (std::weak_ptr<KinematicElement> weak_element : elements)
163 std::shared_ptr<KinematicElement> element = weak_element.lock();
164 if (element->visual.size() == 0)
continue;
165 for (
auto visual : element->visual)
167 if (!visual.shape)
continue;
168 switch (visual.shape->type)
172 std::shared_ptr<shapes::Sphere> sphere = std::static_pointer_cast<
shapes::Sphere>(
ToStdPtr(visual.shape));
176 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
177 object.object.object.matrix = FrameToVector(visual.frame);
179 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
184 std::shared_ptr<shapes::Box> box = std::static_pointer_cast<
shapes::Box>(
ToStdPtr(visual.shape));
188 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
189 object.object.object.matrix = FrameToVector(visual.frame);
191 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
200 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
204 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
209 if (!visual.shape_resource_path.empty())
211 auto mesh = visualization::GeometryMesh(visual.shape_resource_path, file_url_ + visual.shape_resource_path);
213 if (!use_mesh_materials || mesh.format ==
"stl")
216 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
217 object.object.object.matrix =
218 FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
220 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
226 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
227 object.object.object.matrix =
228 FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
230 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
237 std::shared_ptr<shapes::Mesh> shape = std::static_pointer_cast<
shapes::Mesh>(visual.shape);
238 auto mesh = visualization::GeometryMeshBuffer(visual.shape);
240 visualization::Material(
visualization::RGB(visual.color(0), visual.color(1), visual.color(2)), visual.color(3))));
241 object.object.object.matrix =
242 FrameToVector(visual.frame, visual.scale(0), visual.scale(1), visual.scale(2));
244 SendMsg(visualization::SetTransform(
object.path, FrameToVector(element->frame)));
248 HIGHLIGHT(
"Unsupported shape! " << element->segment.getName());
260 const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
261 scene_->Update(state, t);
263 for (std::weak_ptr<KinematicElement> weak_element : elements)
265 std::shared_ptr<KinematicElement> element = weak_element.lock();
266 if (element->visual.size() == 0)
continue;
267 for (
auto visual : element->visual)
269 SendMsg(visualization::SetTransform(path_prefix_ + visual.name, FrameToVector(element->frame)));
276 auto set_animation = visualization::SetAnimation();
278 std::string clip_name =
"default";
279 double fps = 1.0 / dt;
281 const std::vector<std::weak_ptr<KinematicElement>>& elements = scene_->GetKinematicTree().GetTree();
282 for (std::weak_ptr<KinematicElement> weak_element : elements)
284 std::shared_ptr<KinematicElement> element = weak_element.lock();
285 if (element->visual.size() == 0)
continue;
286 for (
auto visual : element->visual)
288 visualization::Animation anim(path_prefix_ + visual.name);
289 anim.clip = visualization::Clip(fps, clip_name);
290 anim.clip.tracks.resize(2);
291 anim.clip.tracks[0] = visualization::Track(
".position",
"vector3");
292 anim.clip.tracks[1] = visualization::Track(
".quaternion",
"quaternion");
293 anim.clip.tracks[0].keys.reserve(trajectory.rows());
294 anim.clip.tracks[1].keys.reserve(trajectory.rows());
295 set_animation.animations.push_back(anim);
299 for (
int t = 0; t < trajectory.rows(); ++t)
301 double time =
static_cast<double>(t) * dt;
302 scene_->Update(trajectory.row(t), time);
305 for (std::weak_ptr<KinematicElement> weak_element : elements)
307 std::shared_ptr<KinematicElement> element = weak_element.lock();
308 if (element->visual.size() == 0)
continue;
309 for (
auto visual : element->visual)
311 set_animation.animations[i].clip.tracks[0].keys.push_back(visualization::Key(time, PositionToVector(element->frame)));
312 set_animation.animations[i].clip.tracks[1].keys.push_back(visualization::Key(time, QuaternionToVector(element->frame)));
318 SendMsg(set_animation);
323 SendMsg(visualization::Delete(
"/exotica/" + path));
328 SendMsg(visualization::Property<double>(path, property, value));
333 SendMsg(visualization::Property<std::string>(path, property, value));
338 SendMsg(visualization::Property<bool>(path, property, value));
343 std::vector<double> val(3);
347 SendMsg(visualization::Property<std::vector<double>>(path, property, val));
352 std::vector<double> val(4);
357 SendMsg(visualization::Property<std::vector<double>>(path, property, val));
360 #endif // MSGPACK_FOUND
void DisplayScene(bool use_mesh_materials=true)
virtual ~VisualizationMeshcat()
static Rotation RotX(double angle)
void Initialize(bool use_mesh_materials)
long RGB(double R, double G, double B)
std::shared_ptr< Scene > ScenePtr
VisualizationMeshcat(ScenePtr scene, const std::string &url, bool use_mesh_materials=true, const std::string &file_url="")
void GetQuaternion(double &x, double &y, double &z, double &w) const
const Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
void DisplayState(Eigen::VectorXdRefConst state, double t=0.0)
SetObjectType< T > SetObject(const std::string &path_in, const T &object_in)
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Convenience wrapper for storing references to sub-matrices/vectors.
void SendZMQ(const std::string &data)
std::string RequestWebURL()
#define HIGHLIGHT_NAMED(name, x)
void Delete(const std::string &path="")
Object< T > CreateGeometryObject(const T &geometry_in, const Material &material_in=Material(), const std::string &uuid_in="")
void SetProperty(const std::string &path, const std::string &property, const double &value)
MeshObject< T > CreateMeshObject(const T &geometry_in, const Material &material_in=Material(), const std::string &uuid_in="")
void DisplayTrajectory(Eigen::MatrixXdRefConst trajectory, double dt=1.0)