Add Signed Distance Field to collision geometries.
PiperOrigin-RevId: 557507088 Change-Id: I358a642407aee1ba8dfc9d405eb9a4f609435fe9
This commit is contained in:
Alessio Quaglino
committed by
Copybara-Service
Copybara-Service
parent
6245edae28
commit
fdb041580c
@@ -0,0 +1,51 @@
|
||||
# Copyright 2022 DeepMind Technologies Limited
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# https://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
set(MUJOCO_SDF_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/../..
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../../src
|
||||
)
|
||||
|
||||
set(MUJOCO_SDF_SRCS
|
||||
sdf.cc
|
||||
sdf.h
|
||||
bolt.cc
|
||||
bolt.h
|
||||
bowl.cc
|
||||
bowl.h
|
||||
gear.cc
|
||||
gear.h
|
||||
register.cc
|
||||
nut.cc
|
||||
nut.h
|
||||
torus.cc
|
||||
torus.h
|
||||
)
|
||||
|
||||
add_library(sdf SHARED)
|
||||
target_sources(sdf PRIVATE ${MUJOCO_SDF_SRCS})
|
||||
target_include_directories(sdf PRIVATE ${MUJOCO_SDF_INCLUDE})
|
||||
target_link_libraries(sdf PRIVATE mujoco)
|
||||
target_compile_options(
|
||||
sdf
|
||||
PRIVATE ${AVX_COMPILE_OPTIONS}
|
||||
${MUJOCO_MACOS_COMPILE_OPTIONS}
|
||||
${EXTRA_COMPILE_OPTIONS}
|
||||
${MUJOCO_CXX_FLAGS}
|
||||
)
|
||||
target_link_options(
|
||||
sdf
|
||||
PRIVATE
|
||||
${MUJOCO_MACOS_LINK_OPTIONS}
|
||||
${EXTRA_LINK_OPTIONS}
|
||||
)
|
||||
@@ -0,0 +1,184 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "sdf.h"
|
||||
#include "bolt.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
namespace {
|
||||
|
||||
static mjtNum distance(const mjtNum p[3], const mjtNum attributes[1]) {
|
||||
// see https://www.shadertoy.com/view/XtffzX
|
||||
mjtNum screw = 12;
|
||||
mjtNum radius = mju_sqrt(p[0]*p[0]+p[1]*p[1]) - attributes[0];
|
||||
mjtNum sqrt12 = mju_sqrt(2.)/2.;
|
||||
|
||||
// a triangle wave spun around Oy, offset by the angle between x and z
|
||||
mjtNum azimuth = mju_atan2(p[1], p[0]);
|
||||
mjtNum triangle = abs(Fract(p[2] * screw - azimuth / mjPI / 2.) - .5);
|
||||
mjtNum thread = (radius - triangle / screw) * sqrt12;
|
||||
|
||||
// clip the top and bottom
|
||||
mjtNum bolt = Subtraction(thread, .5 - abs(p[2] + .5));
|
||||
mjtNum cone = (p[2] - radius) * sqrt12;
|
||||
|
||||
// add a diagonal clipping for more realism
|
||||
bolt = Subtraction(bolt, cone + 1. * sqrt12);
|
||||
|
||||
// create the hexagonal geometry for the head
|
||||
mjtNum point2D[2] = {p[0], p[1]};
|
||||
mjtNum res[2];
|
||||
mjtNum k = 6. / mjPI / 2.;
|
||||
mjtNum angle = -floor((mju_atan2(point2D[1], point2D[0])) * k + .5) / k;
|
||||
mjtNum s[2] = {mju_sin(angle), mju_sin(angle + mjPI * .5)};
|
||||
mjtNum mat[4] = {s[1], -s[0], s[0], s[1]};
|
||||
mju_mulMatVec(res, mat, point2D, 2, 2);
|
||||
mjtNum point3D[3] = {res[0], res[1], p[2]};
|
||||
mjtNum head = point3D[0] - .5;
|
||||
|
||||
// the top is also rounded down with a cone
|
||||
head = Intersection(head, abs(point3D[2] + .25) - .25);
|
||||
head = Intersection(head, (point3D[2] + radius - .22) * sqrt12);
|
||||
return Union(bolt, head);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Bolt> Bolt::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("radius", m, instance)) {
|
||||
return Bolt(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in Bolt plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Bolt::Bolt(const mjModel* m, mjData* d, int instance) {
|
||||
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
|
||||
}
|
||||
|
||||
// add new element in the vector storing iteration counts
|
||||
void Bolt::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
visualizer_.Next();
|
||||
}
|
||||
|
||||
// reset visualization counter
|
||||
void Bolt::Reset() {
|
||||
visualizer_.Reset();
|
||||
}
|
||||
|
||||
// plugin visualization
|
||||
void Bolt::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
visualizer_.Visualize(m, d, opt, scn, instance);
|
||||
}
|
||||
|
||||
// sdf
|
||||
mjtNum Bolt::Distance(const mjtNum point[3]) const {
|
||||
return distance(point, &radius);
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void Bolt::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||
mjtNum eps = 1e-8;
|
||||
mjtNum dist0 = distance(point, &radius);
|
||||
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
|
||||
mjtNum distX = distance(pointX, &radius);
|
||||
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
|
||||
mjtNum distY = distance(pointY, &radius);
|
||||
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
|
||||
mjtNum distZ = distance(pointZ, &radius);
|
||||
|
||||
grad[0] = (distX - dist0) / eps;
|
||||
grad[1] = (distY - dist0) / eps;
|
||||
grad[2] = (distZ - dist0) / eps;
|
||||
}
|
||||
|
||||
// plugin registration
|
||||
void Bolt::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.sdf.bolt";
|
||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||
|
||||
const char* attributes[] = {"radius"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto sdf_or_null = Bolt::Create(m, d, instance);
|
||||
if (!sdf_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Bolt(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Bolt*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
|
||||
int instance) {
|
||||
auto sdf = reinterpret_cast<Bolt*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bolt*>(d->plugin_data[instance]);
|
||||
sdf->Visualize(m, d, opt, scn, instance);
|
||||
};
|
||||
plugin.compute =
|
||||
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
|
||||
auto* sdf = reinterpret_cast<Bolt*>(d->plugin_data[instance]);
|
||||
sdf->Compute(m, d, instance);
|
||||
};
|
||||
plugin.sdf_distance =
|
||||
+[](const mjtNum point[3], const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bolt*>(d->plugin_data[instance]);
|
||||
sdf->visualizer_.AddPoint(point);
|
||||
return sdf->Distance(point);
|
||||
};
|
||||
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
|
||||
const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bolt*>(d->plugin_data[instance]);
|
||||
sdf->Gradient(gradient, point);
|
||||
};
|
||||
plugin.sdf_staticdistance =
|
||||
+[](const mjtNum point[3], const mjtNum* attributes) {
|
||||
return distance(point, attributes);
|
||||
};
|
||||
plugin.sdf_aabb =
|
||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||
aabb[3] = aabb[4] = .6;
|
||||
aabb[5] = 1;
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_BOLT_H_
|
||||
#define MUJOCO_PLUGIN_SDF_BOLT_H_
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
// this plugin implements a modification of the signed distance function
|
||||
// from https://www.shadertoy.com/view/XtffzX of a bolt with a hexagonal head
|
||||
|
||||
class Bolt {
|
||||
public:
|
||||
// Creates a new Bolt instance (allocated with `new`) or
|
||||
// returns null on failure.
|
||||
static std::optional<Bolt> Create(const mjModel* m, mjData* d, int instance);
|
||||
Bolt(Bolt&&) = default;
|
||||
~Bolt() = default;
|
||||
|
||||
void Reset();
|
||||
void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance);
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
mjtNum Distance(const mjtNum point[3]) const;
|
||||
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
mjtNum radius;
|
||||
|
||||
private:
|
||||
Bolt(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
SdfVisualizer visualizer_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_BOLT_H_
|
||||
@@ -0,0 +1,184 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "bowl.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
namespace {
|
||||
|
||||
static mjtNum distance(const mjtNum point[3], const mjtNum attributes[3]) {
|
||||
mjtNum height = attributes[0];
|
||||
mjtNum radius = attributes[1];
|
||||
mjtNum thick = attributes[2];
|
||||
mjtNum width = mju_sqrt(radius*radius - height*height);
|
||||
// see https://iquilezles.org/articles/distfunctions/
|
||||
mjtNum q[2] = { mju_norm(point, 2), point[2] };
|
||||
mjtNum qdiff[2] = { q[0] - width, q[1] - height };
|
||||
return ((height*q[0] < width*q[1]) ? mju_norm(qdiff, 2)
|
||||
: mju_abs(mju_norm(q, 2)-radius))-thick;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Bowl> Bowl::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("radius", m, instance) && CheckAttr("height", m, instance) &&
|
||||
CheckAttr("thickness", m, instance)) {
|
||||
return Bowl(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in Bowl plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Bowl::Bowl(const mjModel* m, mjData* d, int instance) {
|
||||
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
|
||||
height = strtod(mj_getPluginConfig(m, instance, "height"), nullptr);
|
||||
thick = strtod(mj_getPluginConfig(m, instance, "thickness"), nullptr);
|
||||
width = mju_sqrt(radius*radius - height*height);
|
||||
}
|
||||
|
||||
// add new element in the vector storing iteration counts
|
||||
void Bowl::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
visualizer_.Next();
|
||||
}
|
||||
|
||||
// reset visualization counter
|
||||
void Bowl::Reset() {
|
||||
visualizer_.Reset();
|
||||
}
|
||||
|
||||
// plugin visualization
|
||||
void Bowl::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, mjvScene* scn,
|
||||
int instance) {
|
||||
visualizer_.Visualize(m, d, opt, scn, instance);
|
||||
}
|
||||
|
||||
// sdf
|
||||
mjtNum Bowl::Distance(const mjtNum point[3]) const {
|
||||
mjtNum attributes[3]= {height, radius, thick};
|
||||
return distance(point, attributes);
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void Bowl::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||
// mjtNum q[2] = { mju_norm(point, 2), point[2] };
|
||||
// if (height*q[0] < width*q[1]) {
|
||||
// mjtNum qdiff[2] = { q[0] - width, q[1] - height };
|
||||
// mjtNum qdiffnorm = mju_norm(qdiff, 2);
|
||||
// mjtNum grad_qdiff[3] = {qdiff[0] * point[0] / q[0],
|
||||
// qdiff[0] * point[1] / q[0],
|
||||
// qdiff[1]};
|
||||
// grad[0] = - grad_qdiff[0] / qdiffnorm;
|
||||
// grad[1] = - grad_qdiff[1] / qdiffnorm;
|
||||
// grad[2] = - grad_qdiff[2] / qdiffnorm;
|
||||
// } else {
|
||||
// mjtNum pnorm = mju_norm3(point);
|
||||
// mjtNum grad_dist = (pnorm - radius) / mju_abs(pnorm - radius);
|
||||
// grad[0] = - grad_dist * point[0] / pnorm;
|
||||
// grad[1] = - grad_dist * point[1] / pnorm;
|
||||
// grad[2] = - grad_dist * point[2] / pnorm;
|
||||
// }
|
||||
mjtNum attributes[3]= {height, radius, thick};
|
||||
mjtNum eps = 1e-8;
|
||||
mjtNum dist0 = distance(point, attributes);
|
||||
|
||||
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
|
||||
mjtNum distX = distance(pointX, attributes);
|
||||
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
|
||||
mjtNum distY = distance(pointY, attributes);
|
||||
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
|
||||
mjtNum distZ = distance(pointZ, attributes);
|
||||
|
||||
grad[0] = (distX - dist0) / eps;
|
||||
grad[1] = (distY - dist0) / eps;
|
||||
grad[2] = (distZ - dist0) / eps;
|
||||
}
|
||||
|
||||
// plugin registration
|
||||
void Bowl::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.sdf.bowl";
|
||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||
|
||||
const char* attributes[] = {"radius", "height", "thickness"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto sdf_or_null = Bowl::Create(m, d, instance);
|
||||
if (!sdf_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Bowl(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Bowl*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
|
||||
int instance) {
|
||||
auto sdf = reinterpret_cast<Bowl*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bowl*>(d->plugin_data[instance]);
|
||||
sdf->Visualize(m, d, opt, scn, instance);
|
||||
};
|
||||
plugin.compute =
|
||||
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
|
||||
auto* sdf = reinterpret_cast<Bowl*>(d->plugin_data[instance]);
|
||||
sdf->Compute(m, d, instance);
|
||||
};
|
||||
plugin.sdf_distance =
|
||||
+[](const mjtNum point[3], const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bowl*>(d->plugin_data[instance]);
|
||||
sdf->visualizer_.AddPoint(point);
|
||||
return sdf->Distance(point);
|
||||
};
|
||||
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
|
||||
const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Bowl*>(d->plugin_data[instance]);
|
||||
sdf->Gradient(gradient, point);
|
||||
};
|
||||
plugin.sdf_staticdistance =
|
||||
+[](const mjtNum point[3], const mjtNum* attributes) {
|
||||
return distance(point, attributes);
|
||||
};
|
||||
plugin.sdf_aabb =
|
||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||
mjtNum radius = attributes[1];
|
||||
mjtNum thick = attributes[2];
|
||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||
aabb[3] = aabb[4] = aabb[5] = radius + thick;
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_BOWL_H_
|
||||
#define MUJOCO_PLUGIN_SDF_BOWL_H_
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
class Bowl {
|
||||
public:
|
||||
// Creates a new Bowl instance (allocated with `new`) or
|
||||
// returns null on failure.
|
||||
static std::optional<Bowl> Create(const mjModel* m, mjData* d, int instance);
|
||||
Bowl(Bowl&&) = default;
|
||||
~Bowl() = default;
|
||||
|
||||
void Reset();
|
||||
void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance);
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
mjtNum Distance(const mjtNum point[3]) const;
|
||||
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
mjtNum radius;
|
||||
mjtNum height;
|
||||
mjtNum thick;
|
||||
mjtNum width;
|
||||
|
||||
private:
|
||||
Bowl(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
SdfVisualizer visualizer_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_BOWL_H_
|
||||
@@ -0,0 +1,263 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "gear.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
namespace {
|
||||
|
||||
static mjtNum circle(mjtNum rho, mjtNum r) {
|
||||
return rho - r;
|
||||
}
|
||||
|
||||
static mjtNum smoothUnion(mjtNum a, mjtNum b, mjtNum k) {
|
||||
mjtNum h = mju_clip(0.5 + 0.5*(b - a) / k, 0.0, 1.0);
|
||||
return b * (1. - h) + a * h - k * h * (1. - h);
|
||||
}
|
||||
|
||||
static mjtNum smoothIntersection(mjtNum a, mjtNum b, mjtNum k) {
|
||||
return Subtraction(
|
||||
Intersection(a, b),
|
||||
smoothUnion(Subtraction(a, b), Subtraction(b, a), k));
|
||||
}
|
||||
|
||||
static mjtNum extrusion(const mjtNum p[3], mjtNum sdf_2d, mjtNum h) {
|
||||
mjtNum w[2] = { sdf_2d, abs(p[2]) - h };
|
||||
mjtNum w_abs[2] = { mju_max(w[0], 0), mju_max(w[1], 0) };
|
||||
return mju_min(mju_max(w[0], w[1]), 0.) + mju_norm(w_abs, 2);
|
||||
}
|
||||
|
||||
static mjtNum mod(mjtNum x, mjtNum y) {
|
||||
return x - y * floor(x/y);
|
||||
}
|
||||
|
||||
static mjtNum distance2D(const mjtNum p[3], const mjtNum attributes[3]) {
|
||||
// see https://www.shadertoy.com/view/3lG3WR
|
||||
mjtNum D = 2.8; // should be an attribute
|
||||
mjtNum N = 25; // should be an attribute
|
||||
mjtNum psi = 3.096e-5 * N * N -6.557e-3 * N + 0.551; // pressure angle
|
||||
mjtNum alpha = attributes[0];
|
||||
|
||||
mjtNum R = D / 2.0;
|
||||
/* The Pitch Circle Diameter is the diameter of a circle which by a pure
|
||||
* rolling action would transmit the same motion as the actual gear wheel. It
|
||||
* should be noted that in the case of wheels which connect non-parallel
|
||||
* shafts, the pitch circle diameter is different for each cross section of
|
||||
* the wheel normal to the axis of rotation.
|
||||
*/
|
||||
|
||||
mjtNum rho = mju_norm(p, 2);
|
||||
mjtNum Pd = N / D; // Diametral Pitch: teeth per unit length of diameter
|
||||
mjtNum P =
|
||||
mjPI / Pd; // Circular Pitch: the length of arc round the pitch circle
|
||||
// between corresponding points on adjacent teeth.
|
||||
mjtNum a = 1.0 / Pd; // Addendum: radial length of a tooth from the pitch
|
||||
// circle to the tip of the tooth.
|
||||
|
||||
mjtNum Do = D + 2.0 * a; // Outside Diameter
|
||||
mjtNum Ro = Do / 2.0;
|
||||
|
||||
mjtNum h = 2.2 / Pd;
|
||||
|
||||
mjtNum innerR = Ro - h - 0.4;
|
||||
|
||||
// Early exit
|
||||
if (innerR - rho > 0.0)
|
||||
return innerR - rho;
|
||||
|
||||
// Early exit
|
||||
if (Ro - rho < -0.2)
|
||||
return rho - Ro;
|
||||
|
||||
mjtNum Db = D * mju_cos(psi); // Base Diameter
|
||||
mjtNum Rb = Db / 2.0;
|
||||
|
||||
mjtNum fi = mju_atan2(p[1], p[0]) + alpha;
|
||||
mjtNum alphaStride = P / R;
|
||||
|
||||
mjtNum invAlpha = mju_acos(Rb / R);
|
||||
mjtNum invPhi = mju_tan(invAlpha) - invAlpha;
|
||||
|
||||
mjtNum shift = alphaStride / 2.0 - 2.0 * invPhi;
|
||||
|
||||
mjtNum fia = mod(fi + shift / 2.0, alphaStride) - shift / 2.0;
|
||||
mjtNum fib = mod(-fi - shift + shift / 2.0, alphaStride) - shift / 2.0;
|
||||
|
||||
mjtNum dista = -1.0e6;
|
||||
mjtNum distb = -1.0e6;
|
||||
|
||||
if (Rb < rho) {
|
||||
mjtNum acos_rbRho = mju_acos(Rb/rho);
|
||||
|
||||
mjtNum thetaa = fia + acos_rbRho;
|
||||
mjtNum thetab = fib + acos_rbRho;
|
||||
|
||||
mjtNum ta = mju_sqrt(rho * rho - Rb * Rb);
|
||||
|
||||
// https://math.stackexchange.com/questions/1266689/distance-from-a-point-to-the-involute-of-a-circle
|
||||
dista = ta - Rb * thetaa;
|
||||
distb = ta - Rb * thetab;
|
||||
}
|
||||
|
||||
mjtNum gearOuter = circle(rho, Ro);
|
||||
mjtNum gearLowBase = circle(rho, Ro - h);
|
||||
mjtNum crownBase = circle(rho, innerR);
|
||||
mjtNum cogs = Intersection(dista, distb);
|
||||
mjtNum baseWalls = Intersection(fia - (alphaStride - shift),
|
||||
fib - (alphaStride - shift));
|
||||
|
||||
cogs = Intersection(baseWalls, cogs);
|
||||
cogs = smoothIntersection(gearOuter, cogs, 0.01);
|
||||
cogs = smoothUnion(gearLowBase, cogs, Rb - Ro + h);
|
||||
cogs = Subtraction(cogs, crownBase);
|
||||
|
||||
return extrusion(p, cogs, .1);
|
||||
}
|
||||
|
||||
static mjtNum distance(const mjtNum p[3], const mjtNum attributes[3]) {
|
||||
return extrusion(p, distance2D(p, attributes), .1) - .005;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Gear> Gear::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("alpha", m, instance)) {
|
||||
return Gear(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in Gear plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Gear::Gear(const mjModel* m, mjData* d, int instance) {
|
||||
alpha = strtod(mj_getPluginConfig(m, instance, "alpha"), nullptr);
|
||||
}
|
||||
|
||||
// plugin computation
|
||||
void Gear::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
visualizer_.Next();
|
||||
}
|
||||
|
||||
// plugin reset
|
||||
void Gear::Reset() {
|
||||
visualizer_.Reset();
|
||||
}
|
||||
|
||||
// plugin visualization
|
||||
void Gear::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
visualizer_.Visualize(m, d, opt, scn, instance);
|
||||
}
|
||||
|
||||
// sdf
|
||||
mjtNum Gear::Distance(const mjtNum point[3]) const {
|
||||
return distance(point, &alpha);
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void Gear::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||
mjtNum attributes[1]= {alpha};
|
||||
mjtNum eps = 1e-8;
|
||||
mjtNum dist0 = distance(point, attributes);
|
||||
|
||||
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
|
||||
mjtNum distX = distance(pointX, attributes);
|
||||
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
|
||||
mjtNum distY = distance(pointY, attributes);
|
||||
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
|
||||
mjtNum distZ = distance(pointZ, attributes);
|
||||
|
||||
grad[0] = (distX - dist0) / eps;
|
||||
grad[1] = (distY - dist0) / eps;
|
||||
grad[2] = (distZ - dist0) / eps;
|
||||
}
|
||||
|
||||
// plugin registration
|
||||
void Gear::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.sdf.gear";
|
||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||
|
||||
const char* attributes[] = {"alpha"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto sdf_or_null = Gear::Create(m, d, instance);
|
||||
if (!sdf_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Gear(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Gear*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
|
||||
int instance) {
|
||||
auto sdf = reinterpret_cast<Gear*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Gear*>(d->plugin_data[instance]);
|
||||
sdf->Visualize(m, d, opt, scn, instance);
|
||||
};
|
||||
plugin.compute =
|
||||
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
|
||||
auto* sdf = reinterpret_cast<Gear*>(d->plugin_data[instance]);
|
||||
sdf->Compute(m, d, instance);
|
||||
};
|
||||
plugin.sdf_distance =
|
||||
+[](const mjtNum point[3], const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Gear*>(d->plugin_data[instance]);
|
||||
sdf->visualizer_.AddPoint(point);
|
||||
return sdf->Distance(point);
|
||||
};
|
||||
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
|
||||
const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Gear*>(d->plugin_data[instance]);
|
||||
sdf->Gradient(gradient, point);
|
||||
};
|
||||
plugin.sdf_staticdistance =
|
||||
+[](const mjtNum point[3], const mjtNum* attributes) {
|
||||
return distance(point, attributes);
|
||||
};
|
||||
plugin.sdf_aabb =
|
||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||
aabb[3] = aabb[4] = 1.7;
|
||||
aabb[5] = .11;
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,56 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_GEAR_H_
|
||||
#define MUJOCO_PLUGIN_SDF_GEAR_H_
|
||||
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
class Gear {
|
||||
public:
|
||||
// Creates a new Gear instance (allocated with `new`) or
|
||||
// returns null on failure.
|
||||
static std::optional<Gear> Create(const mjModel* m, mjData* d, int instance);
|
||||
Gear(Gear&&) = default;
|
||||
~Gear() = default;
|
||||
|
||||
void Reset();
|
||||
void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance);
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
mjtNum Distance(const mjtNum point[3]) const;
|
||||
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
mjtNum alpha;
|
||||
|
||||
private:
|
||||
Gear(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
SdfVisualizer visualizer_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_GEAR_H_
|
||||
@@ -0,0 +1,183 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "nut.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
namespace {
|
||||
|
||||
static mjtNum distance(const mjtNum p[3], const mjtNum attributes[1]) {
|
||||
// see https://www.shadertoy.com/view/XtffzX
|
||||
mjtNum screw = 12;
|
||||
mjtNum radius2 = mju_sqrt(p[0]*p[0]+p[1]*p[1]) - attributes[0];
|
||||
mjtNum sqrt12 = mju_sqrt(2.)/2.;
|
||||
|
||||
// a triangle wave spun around Oy, offset by the angle between x and z
|
||||
mjtNum azimuth = mju_atan2(p[1], p[0]);
|
||||
mjtNum triangle = abs(Fract(p[2] * screw - azimuth / mjPI / 2.) - .5);
|
||||
mjtNum thread2 = (radius2 - triangle / screw) * sqrt12;
|
||||
|
||||
// clip the top
|
||||
mjtNum cone2 = (p[2] - radius2) * sqrt12;
|
||||
|
||||
// the hole is the same thing, but substracted from the whole thing
|
||||
mjtNum hole = Subtraction(thread2, cone2 + .5 * sqrt12);
|
||||
hole = Union(hole, -cone2 - .05 * sqrt12);
|
||||
|
||||
// create the hexagonal geometry for the head
|
||||
mjtNum point2D[2] = {p[0], p[1]};
|
||||
mjtNum res[2];
|
||||
mjtNum k = 6. / mjPI / 2.;
|
||||
mjtNum angle = -floor((mju_atan2(point2D[1], point2D[0])) * k + .5) / k;
|
||||
mjtNum s[2] = {mju_sin(angle), mju_sin(angle + mjPI * .5)};
|
||||
mjtNum mat[4] = {s[1], -s[0], s[0], s[1]};
|
||||
mju_mulMatVec(res, mat, point2D, 2, 2);
|
||||
mjtNum point3D[3] = {res[0], res[1], p[2]};
|
||||
mjtNum head = point3D[0] - .5;
|
||||
|
||||
// the top is also rounded down with a cone
|
||||
head = Intersection(head, abs(point3D[2] + .25) - .25);
|
||||
head = Intersection(head, (point3D[2] + radius2 - .22) * sqrt12);
|
||||
return Subtraction(head, hole);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Nut> Nut::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("radius", m, instance)) {
|
||||
return Nut(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid parameter specification in Nut plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Nut::Nut(const mjModel* m, mjData* d, int instance) {
|
||||
radius = strtod(mj_getPluginConfig(m, instance, "radius"), nullptr);
|
||||
}
|
||||
|
||||
// plugin computation
|
||||
void Nut::Compute(const mjModel* m, mjData* d, int instance) {
|
||||
visualizer_.Next();
|
||||
}
|
||||
|
||||
// plugin reset
|
||||
void Nut::Reset() {
|
||||
visualizer_.Reset();
|
||||
}
|
||||
|
||||
// plugin visualization
|
||||
void Nut::Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
visualizer_.Visualize(m, d, opt, scn, instance);
|
||||
}
|
||||
|
||||
// sdf
|
||||
mjtNum Nut::Distance(const mjtNum point[3]) const {
|
||||
return distance(point, &radius);
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void Nut::Gradient(mjtNum grad[3], const mjtNum point[3]) const {
|
||||
mjtNum eps = 1e-8;
|
||||
mjtNum dist0 = distance(point, &radius);
|
||||
mjtNum pointX[3] = {point[0]+eps, point[1], point[2]};
|
||||
mjtNum distX = distance(pointX, &radius);
|
||||
mjtNum pointY[3] = {point[0], point[1]+eps, point[2]};
|
||||
mjtNum distY = distance(pointY, &radius);
|
||||
mjtNum pointZ[3] = {point[0], point[1], point[2]+eps};
|
||||
mjtNum distZ = distance(pointZ, &radius);
|
||||
|
||||
grad[0] = (distX - dist0) / eps;
|
||||
grad[1] = (distY - dist0) / eps;
|
||||
grad[2] = (distZ - dist0) / eps;
|
||||
}
|
||||
|
||||
// plugin registration
|
||||
void Nut::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.sdf.nut";
|
||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||
|
||||
const char* attributes[] = {"radius"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto sdf_or_null = Nut::Create(m, d, instance);
|
||||
if (!sdf_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Nut(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Nut*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
|
||||
int instance) {
|
||||
auto sdf = reinterpret_cast<Nut*>(plugin_data);
|
||||
sdf->Reset();
|
||||
};
|
||||
plugin.visualize = +[](const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance) {
|
||||
auto* sdf = reinterpret_cast<Nut*>(d->plugin_data[instance]);
|
||||
sdf->Visualize(m, d, opt, scn, instance);
|
||||
};
|
||||
plugin.compute =
|
||||
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
|
||||
auto* sdf = reinterpret_cast<Nut*>(d->plugin_data[instance]);
|
||||
sdf->Compute(m, d, instance);
|
||||
};
|
||||
plugin.sdf_distance =
|
||||
+[](const mjtNum point[3], const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Nut*>(d->plugin_data[instance]);
|
||||
sdf->visualizer_.AddPoint(point);
|
||||
return sdf->Distance(point);
|
||||
};
|
||||
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
|
||||
const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Nut*>(d->plugin_data[instance]);
|
||||
sdf->Gradient(gradient, point);
|
||||
};
|
||||
plugin.sdf_staticdistance =
|
||||
+[](const mjtNum point[3], const mjtNum* attributes) {
|
||||
return distance(point, attributes);
|
||||
};
|
||||
plugin.sdf_aabb =
|
||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||
aabb[3] = aabb[4] = .6;
|
||||
aabb[5] = 1;
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_NUT_H_
|
||||
#define MUJOCO_PLUGIN_SDF_NUT_H_
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mjvisualize.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
// this plugin implements a modification of the signed distance function
|
||||
// from https://www.shadertoy.com/view/XtffzX of hexagonal nut
|
||||
|
||||
class Nut {
|
||||
public:
|
||||
// Creates a new Nut instance (allocated with `new`) or
|
||||
// returns null on failure.
|
||||
static std::optional<Nut> Create(const mjModel* m, mjData* d, int instance);
|
||||
Nut(Nut&&) = default;
|
||||
~Nut() = default;
|
||||
|
||||
void Reset();
|
||||
void Visualize(const mjModel* m, mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance);
|
||||
void Compute(const mjModel* m, mjData* d, int instance);
|
||||
mjtNum Distance(const mjtNum point[3]) const;
|
||||
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
mjtNum radius;
|
||||
|
||||
private:
|
||||
Nut(const mjModel* m, mjData* d, int instance);
|
||||
|
||||
SdfVisualizer visualizer_;
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_NUT_H_
|
||||
@@ -0,0 +1,31 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "bolt.h"
|
||||
#include "bowl.h"
|
||||
#include "gear.h"
|
||||
#include "nut.h"
|
||||
#include "torus.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
mjPLUGIN_LIB_INIT {
|
||||
Bolt::RegisterPlugin();
|
||||
Bowl::RegisterPlugin();
|
||||
Gear::RegisterPlugin();
|
||||
Nut::RegisterPlugin();
|
||||
Torus::RegisterPlugin();
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,128 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
bool CheckAttr(const char* name, const mjModel* m, int instance) {
|
||||
char *end;
|
||||
std::string value = mj_getPluginConfig(m, instance, name);
|
||||
value.erase(std::remove_if(value.begin(), value.end(), isspace), value.end());
|
||||
strtod(value.c_str(), &end);
|
||||
return end == value.data() + value.size();
|
||||
}
|
||||
|
||||
SdfVisualizer::SdfVisualizer() {
|
||||
points_.assign(10*(mjMAXCONPAIR+1)*(mjMAXCONPAIR+1)*3, 0);
|
||||
npoints_.clear();
|
||||
}
|
||||
|
||||
void SdfVisualizer::AddPoint(const mjtNum point[3]) {
|
||||
if (!npoints_.empty()) {
|
||||
points_[3*npoints_.back()+0] = point[0];
|
||||
points_[3*npoints_.back()+1] = point[1];
|
||||
points_[3*npoints_.back()+2] = point[2];
|
||||
npoints_.back()++;
|
||||
}
|
||||
}
|
||||
|
||||
void SdfVisualizer::Next() {
|
||||
npoints_.push_back(npoints_.empty() ? 0 : npoints_.back());
|
||||
}
|
||||
|
||||
void SdfVisualizer::Reset() {
|
||||
npoints_.clear();
|
||||
}
|
||||
|
||||
void SdfVisualizer::Visualize(const mjModel* m, const mjData* d,
|
||||
const mjvOption* opt, mjvScene* scn,
|
||||
int instance) {
|
||||
if (!opt->flags[mjVIS_SDFITER]) {
|
||||
return;
|
||||
}
|
||||
if (npoints_.empty()) {
|
||||
return;
|
||||
}
|
||||
int tot = 0, n = 0, g = 0;
|
||||
for (int i = 0; i < m->ngeom; i++) {
|
||||
if (m->geom_plugin[i] == instance) {
|
||||
g = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
mjtNum* points = points_.data();
|
||||
int* npoints = npoints_.data();
|
||||
int niter = npoints_.size();
|
||||
mjtNum geom_mat[9], offset[3], rotation[9], from[3], to[3];
|
||||
mjtNum* geom_xpos = d->geom_xpos + 3*g;
|
||||
mjtNum* geom_xmat = d->geom_xmat + 9*g;
|
||||
mjtNum* geom_pos = m->geom_pos + 3*g;
|
||||
mjtNum* geom_quat = m->geom_quat + 4*g;
|
||||
mju_quat2Mat(geom_mat, geom_quat);
|
||||
mju_mulMatMatT(rotation, geom_xmat, geom_mat, 3, 3, 3);
|
||||
mju_rotVecMat(offset, geom_pos, rotation);
|
||||
mju_sub3(offset, geom_xpos, offset);
|
||||
|
||||
for (int i = 0; i < niter; i++) {
|
||||
n = npoints[i] - tot;
|
||||
if (!n) {
|
||||
continue;
|
||||
}
|
||||
for (int k = 0; k < 2; k++) {
|
||||
for (int j = 0; j < (k == 0 ? 2 : n-1); j++) {
|
||||
if (scn->ngeom >= scn->maxgeom) {
|
||||
mj_warning((mjData*)d, mjWARN_VGEOMFULL, scn->maxgeom);
|
||||
return;
|
||||
}
|
||||
mjvGeom* thisgeom = scn->geoms + scn->ngeom;
|
||||
mjtNum* p1 = points + (tot + (k == 0 ? (n-1) * j : j))*3;
|
||||
mjtNum* p2 = points + (tot + j + 1)*3;
|
||||
mju_rotVecMat(from, p1, rotation);
|
||||
mju_addTo3(from, offset);
|
||||
mju_rotVecMat(to, p2, rotation);
|
||||
mju_addTo3(to, offset);
|
||||
if (k == 0) {
|
||||
float rgba[4] = {static_cast<float>(j > 0), 0,
|
||||
static_cast<float>(j == 0), 1};
|
||||
mjtNum size[] = {.2*m->stat.meansize};
|
||||
mjv_initGeom(thisgeom, mjGEOM_SPHERE, size, from, geom_xmat, rgba);
|
||||
} else {
|
||||
mjv_initGeom(thisgeom, mjGEOM_NONE, NULL, NULL, NULL, NULL);
|
||||
thisgeom->objtype = mjOBJ_UNKNOWN;
|
||||
thisgeom->objid = i;
|
||||
thisgeom->category = mjCAT_DECOR;
|
||||
thisgeom->segid = scn->ngeom;
|
||||
to[0] = from[0] + .95*(to[0]-from[0]);
|
||||
to[1] = from[1] + .95*(to[1]-from[1]);
|
||||
to[2] = from[2] + .95*(to[2]-from[2]);
|
||||
mjv_connector(thisgeom, mjGEOM_LINE, 2, from, to);
|
||||
thisgeom->rgba[0] = (j+1.)/(n-1.);
|
||||
thisgeom->rgba[1] = 0;
|
||||
thisgeom->rgba[2] = 1 - (j+1.)/(n-1.);
|
||||
thisgeom->rgba[3] = 1;
|
||||
}
|
||||
scn->ngeom++;
|
||||
}
|
||||
}
|
||||
tot += n;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,61 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_SDF_H_
|
||||
#define MUJOCO_PLUGIN_SDF_SDF_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mujoco.h>
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
inline mjtNum Union(mjtNum a, mjtNum b) {
|
||||
return mju_min(a, b);
|
||||
}
|
||||
|
||||
inline mjtNum Intersection(mjtNum a, mjtNum b) {
|
||||
return mju_max(a, b);
|
||||
}
|
||||
|
||||
inline mjtNum Subtraction(mjtNum a, mjtNum b) {
|
||||
return mju_max(a, -b);
|
||||
}
|
||||
|
||||
inline mjtNum Fract(mjtNum x) {
|
||||
return x - floor(x);
|
||||
}
|
||||
|
||||
// reads numeric attributes
|
||||
bool CheckAttr(const char* name, const mjModel* m, int instance);
|
||||
|
||||
class SdfVisualizer {
|
||||
public:
|
||||
SdfVisualizer();
|
||||
|
||||
void Visualize(const mjModel* m, const mjData* d, const mjvOption* opt,
|
||||
mjvScene* scn, int instance);
|
||||
|
||||
void AddPoint(const mjtNum point[3]);
|
||||
void Reset();
|
||||
void Next(); // adds a new gradient descent trajectory to be visualized
|
||||
|
||||
private:
|
||||
std::vector<mjtNum> points_; // query points
|
||||
std::vector<int> npoints_; // number of iterations from the starting point
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_SDF_H_
|
||||
@@ -0,0 +1,124 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <sstream>
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjplugin.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "torus.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
namespace {
|
||||
|
||||
static mjtNum distance(const mjtNum p[3], const mjtNum radius[2]) {
|
||||
mjtNum q = mju_sqrt(p[0]*p[0] + p[1]*p[1]) - radius[0];
|
||||
return mju_sqrt(q*q + p[2]*p[2]) - radius[1];
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// factory function
|
||||
std::optional<Torus> Torus::Create(
|
||||
const mjModel* m, mjData* d, int instance) {
|
||||
if (CheckAttr("radius1", m, instance) && CheckAttr("radius2", m, instance)) {
|
||||
return Torus(m, d, instance);
|
||||
} else {
|
||||
mju_warning("Invalid radius1 or radius2 parameters in Torus plugin");
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
// plugin constructor
|
||||
Torus::Torus(const mjModel* m, mjData* d, int instance) {
|
||||
radius[0] = strtod(mj_getPluginConfig(m, instance, "radius1"), nullptr);
|
||||
radius[1] = strtod(mj_getPluginConfig(m, instance, "radius2"), nullptr);
|
||||
}
|
||||
|
||||
// sdf
|
||||
mjtNum Torus::Distance(const mjtNum point[3]) const {
|
||||
return distance(point, radius);
|
||||
}
|
||||
|
||||
// gradient of sdf
|
||||
void Torus::Gradient(mjtNum grad[3], const mjtNum p[3]) const {
|
||||
mjtNum len_xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]);
|
||||
mjtNum q = len_xy - radius[0];
|
||||
mjtNum grad_q[2] = { p[0] / len_xy, p[1] / len_xy };
|
||||
mjtNum len_qz = mju_sqrt(q*q + p[2]*p[2]);
|
||||
grad[0] = q*grad_q[0] / mjMAX(len_qz, mjMINVAL);
|
||||
grad[1] = q*grad_q[1] / mjMAX(len_qz, mjMINVAL);
|
||||
grad[2] = p[2] / mjMAX(len_qz, mjMINVAL);
|
||||
}
|
||||
|
||||
// plugin registration
|
||||
void Torus::RegisterPlugin() {
|
||||
mjpPlugin plugin;
|
||||
mjp_defaultPlugin(&plugin);
|
||||
|
||||
plugin.name = "mujoco.sdf.torus";
|
||||
plugin.capabilityflags |= mjPLUGIN_SDF;
|
||||
|
||||
const char* attributes[] = {"radius1", "radius2", "axis"};
|
||||
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
|
||||
plugin.attributes = attributes;
|
||||
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
|
||||
|
||||
plugin.init = +[](const mjModel* m, mjData* d, int instance) {
|
||||
auto sdf_or_null = Torus::Create(m, d, instance);
|
||||
if (!sdf_or_null.has_value()) {
|
||||
return -1;
|
||||
}
|
||||
d->plugin_data[instance] = reinterpret_cast<uintptr_t>(
|
||||
new Torus(std::move(*sdf_or_null)));
|
||||
return 0;
|
||||
};
|
||||
plugin.destroy = +[](mjData* d, int instance) {
|
||||
delete reinterpret_cast<Torus*>(d->plugin_data[instance]);
|
||||
d->plugin_data[instance] = 0;
|
||||
};
|
||||
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
|
||||
int instance) {
|
||||
// do nothing
|
||||
};
|
||||
plugin.compute =
|
||||
+[](const mjModel* m, mjData* d, int instance, int capability_bit) {
|
||||
// do nothing;
|
||||
};
|
||||
plugin.sdf_distance =
|
||||
+[](const mjtNum point[3], const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Torus*>(d->plugin_data[instance]);
|
||||
return sdf->Distance(point);
|
||||
};
|
||||
plugin.sdf_gradient = +[](mjtNum gradient[3], const mjtNum point[3],
|
||||
const mjData* d, int instance) {
|
||||
auto* sdf = reinterpret_cast<Torus*>(d->plugin_data[instance]);
|
||||
sdf->Gradient(gradient, point);
|
||||
};
|
||||
plugin.sdf_staticdistance =
|
||||
+[](const mjtNum point[3], const mjtNum* attributes) {
|
||||
return distance(point, attributes);
|
||||
};
|
||||
plugin.sdf_aabb =
|
||||
+[](mjtNum aabb[6], const mjtNum* attributes) {
|
||||
aabb[0] = aabb[1] = aabb[2] = 0;
|
||||
aabb[3] = aabb[4] = attributes[0] + attributes[1];
|
||||
aabb[5] = attributes[1];
|
||||
};
|
||||
|
||||
mjp_registerPlugin(&plugin);
|
||||
}
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright 2022 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_PLUGIN_SDF_TORUS_H_
|
||||
#define MUJOCO_PLUGIN_SDF_TORUS_H_
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include <mujoco/mjtnum.h>
|
||||
#include "sdf.h"
|
||||
|
||||
namespace mujoco::plugin::sdf {
|
||||
|
||||
class Torus {
|
||||
public:
|
||||
// Creates a new Torus instance or returns null on failure.
|
||||
static std::optional<Torus> Create(const mjModel* m, mjData* d, int instance);
|
||||
Torus(Torus&&) = default;
|
||||
~Torus() = default;
|
||||
|
||||
mjtNum Distance(const mjtNum point[3]) const;
|
||||
void Gradient(mjtNum grad[3], const mjtNum point[3]) const;
|
||||
|
||||
static void RegisterPlugin();
|
||||
|
||||
mjtNum radius[2];
|
||||
|
||||
private:
|
||||
Torus(const mjModel* m, mjData* d, int instance);
|
||||
};
|
||||
|
||||
} // namespace mujoco::plugin::sdf
|
||||
|
||||
#endif // MUJOCO_PLUGIN_SDF_TORUS_H_
|
||||
Reference in New Issue
Block a user