forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
65 lines
2.4 KiB
65 lines
2.4 KiB
2 years ago
|
#include "drake/geometry/meshcat_animation.h"
|
||
|
|
|
||
|
|
namespace drake {
|
||
|
|
namespace geometry {
|
||
|
|
|
||
|
|
MeshcatAnimation::MeshcatAnimation(double frames_per_second)
|
||
|
|
: frames_per_second_(frames_per_second) {}
|
||
|
|
|
||
|
|
MeshcatAnimation::~MeshcatAnimation() = default;
|
||
|
|
|
||
|
|
void MeshcatAnimation::SetTransform(int frame, const std::string& path,
|
||
|
|
const math::RigidTransformd& X_ParentPath) {
|
||
|
|
std::vector<double> position(3);
|
||
|
|
Eigen::Vector3d::Map(&position[0]) = X_ParentPath.translation();
|
||
|
|
const auto q = X_ParentPath.rotation().ToQuaternion();
|
||
|
|
std::vector<double> quaternion{q.x(), q.y(), q.z(), q.w()};
|
||
|
|
SetProperty(frame, path, "position", "vector3", position);
|
||
|
|
SetProperty(frame, path, "quaternion", "quaternion", quaternion);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MeshcatAnimation::SetProperty(int frame, const std::string& path,
|
||
|
|
const std::string& property, bool value) {
|
||
|
|
SetProperty(frame, path, property, "boolean", value);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MeshcatAnimation::SetProperty(int frame, const std::string& path,
|
||
|
|
const std::string& property, double value) {
|
||
|
|
SetProperty(frame, path, property, "number", value);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MeshcatAnimation::SetProperty(int frame, const std::string& path,
|
||
|
|
const std::string& property,
|
||
|
|
const std::vector<double>& value) {
|
||
|
|
SetProperty(frame, path, property, "vector", value);
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string MeshcatAnimation::get_javascript_type(
|
||
|
|
const std::string& path, const std::string& property) const {
|
||
|
|
if (path_tracks_.find(path) == path_tracks_.end() ||
|
||
|
|
path_tracks_.at(path).find(property) == path_tracks_.at(path).end()) {
|
||
|
|
return "";
|
||
|
|
}
|
||
|
|
return path_tracks_.at(path).at(property).js_type;
|
||
|
|
}
|
||
|
|
|
||
|
|
template <typename T>
|
||
|
|
void MeshcatAnimation::SetProperty(int frame, const std::string& path,
|
||
|
|
const std::string& property,
|
||
|
|
const std::string& js_type, const T& value) {
|
||
|
|
TypedTrack& tt = path_tracks_[path][property];
|
||
|
|
if (std::holds_alternative<std::monostate>(tt.track)) {
|
||
|
|
tt.track = Track<T>();
|
||
|
|
tt.js_type = js_type;
|
||
|
|
} else if (tt.js_type != js_type) {
|
||
|
|
throw std::runtime_error(fmt::format(
|
||
|
|
"{} property {} already has a track with javascript type {} != {}",
|
||
|
|
path, property, tt.js_type, js_type));
|
||
|
|
}
|
||
|
|
// get<T> will also throw bad_variant_access if the types don't match.
|
||
|
|
std::get<Track<T>>(tt.track)[frame] = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace geometry
|
||
|
|
} // namespace drake
|