Skip to content

Commit 147dc67

Browse files
yuvaltassacopybara-github
authored andcommitted
Allow anisotropic frame scales for meshes and aligned shapes and joints.
Mechanisms built from meshes, aligned boxes and ellipsoids, and joints along the scaling axes can be stretched non-uniformly. Positions are mapped exactly and bodies stay rigid, so each shape is stretched in its own coordinates; shapes which cannot represent that are compile errors, and uniform scales keep the exact scalar path, bitwise as before. - mjCGauge carries a 3x3 map, composed through the orientations of frames and bodies, with a uniform flag; Point, Stretch, Aligned, Size (by shape type and orientation) and an exact transform of the inertia tensor through its second moments. - Anisotropic scales require scaling 'geometry'; alignment is checked to within about 2 degrees. - Capsules stretch with hemispherical caps (documented); sphere sites and lengths without a direction take the geometric mean; sphere geoms suggest an ellipsoid. - Mesh and height field instances are keyed by the map in the geom frame; a map which is not diagonal transforms vertices and normals, and saving compiled writes such instances' vertices. - Skins require their bones to share one map in skin coordinates. - Mesh bounds are reset when a mesh is copied from its spec again, which recompiling a scaled mesh exposed. PiperOrigin-RevId: 998289545 Change-Id: Ide7d393164afeabc7666450351c074375189592c
1 parent 89d59b1 commit 147dc67

10 files changed

Lines changed: 1020 additions & 131 deletions

‎doc/XMLreference.rst‎

Lines changed: 26 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4395,13 +4395,36 @@ transformation they encode is accumulated in their direct children; frames are p
43954395
- ``massscale="k" scaling="geometry"``
43964396
* - a change of the units of length and mass
43974397
- ``scale="s" massscale="k"``
4398+
* - a body stretched along the frame's axes, see below
4399+
- ``scale="sx sy sz" scaling="geometry"``
43984400

43994401
Scales of nested frames compose by multiplication. A single value applies to all three axes. Values must be finite
4400-
and positive, and the three values must currently be equal: anisotropic scaling is not yet supported.
4402+
and positive.
4403+
4404+
Three different values stretch the contents along the frame's axes, which requires :at:`scaling` "geometry".
4405+
Positions are mapped exactly and orientations are kept, so each shape inside is stretched in its own coordinates,
4406+
which only some shapes can represent:
4407+
4408+
- meshes always, with an instance of the mesh for each distinct stretch;
4409+
- boxes, ellipsoids, planes and height fields aligned with the frame's axes;
4410+
- capsules and cylinders aligned with the frame's axes and stretched equally across their own axis. The caps of a
4411+
capsule stay hemispherical: with :math:`a` the factor across its axis and :math:`c` the factor along it, its
4412+
radius :math:`r` becomes :math:`a r` and its half-length :math:`h` becomes :math:`c h`, so that its total length
4413+
is :math:`2 (c h + a r)` rather than :math:`2 c (h + r)`;
4414+
- spheres only when scaled uniformly; use an ellipsoid otherwise.
4415+
4416+
Hinge and slide joints must also be aligned with the frame's axes. Anything else is a compile error; alignment is
4417+
checked to within about 2 degrees. Sites are never an error: a site box, ellipsoid, capsule or cylinder is stretched
4418+
by the factor along each of its own axes, with the radius of a capsule or cylinder taking the geometric mean of the
4419+
two factors across it, which is approximate when the site is not aligned. Explicit inertias transform exactly.
4420+
Sphere sites, and lengths without a direction like margins, contact widths and tendon widths, take the geometric mean
4421+
of the three factors.
44014422

44024423
A mesh or height field used at several scales compiles to an instance per scale; the first keeps its name and the
4403-
others are named ``name@1``, ``name@2``, ... The bones of a skin must have the same scale, and elements with
4404-
plugins and tactile sensors cannot be scaled.
4424+
others are named ``name@1``, ``name@2``, ... A model saved compiled writes a mesh instance which is not stretched
4425+
along its own axes as vertices and faces; its texture coordinates are kept only if they are given per vertex, and its
4426+
normals are recomputed. The bones of a skin must have the same scale, and elements with plugins and tactile sensors
4427+
cannot be scaled.
44054428

44064429
.. _frame-scaling:
44074430

‎doc/changelog.rst‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -10,13 +10,10 @@ Significant new features
1010
- Frames can now resize their contents at compile time, without modifying the spec, using the new
1111
:ref:`scale<frame-scale>`, :ref:`scaling<frame-scaling>` and :ref:`massscale<frame-massscale>` attributes. Under the
1212
default ``similar`` policy a scaled model moves like the original in proportionally scaled gravity; under ``geometry``
13-
only shapes and inertias change. Masses follow volumes unless ``massscale`` sets their factor. Elements outside the
14-
kinematic tree, like actuators, tendons, equalities, contact pairs and sensors, take the scale of the elements they
15-
refer to, and keyframes are scaled like the joints, bodies and actuators they set. :ref:`mj_copyBack` and the
16-
operations which write compiled inertials to the spec write values in its units. A mesh or height field which is used
17-
at several scales compiles to an instance per scale, named after it (``name@1``, ``name@2``, ...). A flex takes the
18-
scale of the lowest frame or body containing its bodies, with its vertices scaled by their own bodies; a skin scales
19-
with its bones, which must all have the same scale. See :ref:`frame/scale<frame-scale>` for details.
13+
only shapes and inertias change, and the scale may be anisotropic. Masses follow volumes unless ``massscale`` sets
14+
their factor. Everything which refers to the contents of a frame follows its scale: actuators, tendons, equalities,
15+
contact pairs, sensors, keyframes, meshes, height fields, flexes and skins. :ref:`mj_copyBack` and the operations
16+
which write compiled inertials to the spec write values in its units. See :ref:`frame/scale<frame-scale>` for details.
2017
- Models can now be saved as written. With the new :ref:`savecompiled<compiler-savecompiled>` compiler attribute set to
2118
"false", a model is saved as it is in the :ref:`mjSpec`, with its default classes, frames and exact numbers, and reads
2219
back as the same spec. With :ref:`savecanonical<compiler-savecanonical>` "false" it also keeps the notation it was

‎src/user/user_flexcomp.cc‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -950,12 +950,12 @@ bool mjCFlexcomp::MakeMesh(
950950
return false;
951951
}
952952

953-
if (cmesh->instancescale_ != 1) {
954-
unscaled = std::make_unique<mjCMesh>(*cmesh);
955-
unscaled->model = cmesh->model;
956-
unscaled->compiler = cmesh->compiler;
957-
unscaled->instancescale_ = 1;
958-
cmesh = unscaled.get();
953+
if (cmesh->instancemap_ != decltype(cmesh->instancemap_){1, 0, 0, 0, 1, 0, 0, 0, 1}) {
954+
unscaled = std::make_unique<mjCMesh>(*cmesh);
955+
unscaled->model = cmesh->model;
956+
unscaled->compiler = cmesh->compiler;
957+
unscaled->instancemap_ = {1, 0, 0, 0, 1, 0, 0, 0, 1};
958+
cmesh = unscaled.get();
959959
}
960960

961961
try {

‎src/user/user_mesh.cc‎

Lines changed: 64 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -296,7 +296,9 @@ void mjCMesh::NameSpace(const mjCModel* m) {
296296

297297
void mjCMesh::CopyFromSpec() {
298298
*static_cast<mjsMesh*>(this) = spec;
299-
for (int i = 0; i < 3; i++) { scale[i] *= instancescale_; }
299+
if (InstanceDiagonal()) {
300+
for (int i = 0; i < 3; i++) { scale[i] *= instancemap_[4 * i]; }
301+
}
300302

301303
file_ = spec_file_;
302304
content_type_ = spec_content_type_;
@@ -324,6 +326,8 @@ void mjCMesh::CopyFromSpec() {
324326
tree_ = mjCBoundingVolumeHierarchy();
325327
face_aabb_.clear();
326328
octree_.Clear();
329+
mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
330+
mjuu_setvec(aamm_ + 3, -1e10, -1e10, -1e10);
327331
}
328332

329333

@@ -347,7 +351,7 @@ void mjCMesh::LoadSDF() {
347351
id);
348352
}
349353

350-
if (instancescale_ != 1) {
354+
if (instancemap_ != decltype(instancemap_){1, 0, 0, 0, 1, 0, 0, 0, 1}) {
351355
throw mjCError(this, "SDF mesh '%s' cannot be scaled by a frame", name.c_str());
352356
}
353357
if (scale[0] != 1 || scale[1] != 1 || scale[2] != 1) {
@@ -515,10 +519,27 @@ void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) {
515519
// id of the mesh in the asset cache: that of its file, and its factor for an instance at another
516520
// scale
517521
std::string mjCMesh::CacheId(const mjResource* resource) const {
518-
if (instancescale_ == 1) { return resource->name; }
519-
char factor[32];
520-
std::snprintf(factor, sizeof(factor), "@%a", instancescale_);
521-
return std::string(resource->name) + factor;
522+
const auto& m = instancemap_;
523+
if (InstanceDiagonal() && m[0] == m[4] && m[4] == m[8]) {
524+
if (m[0] == 1) { return resource->name; }
525+
char factor[32];
526+
std::snprintf(factor, sizeof(factor), "@%a", m[0]);
527+
return std::string(resource->name) + factor;
528+
}
529+
std::string id = std::string(resource->name) + "@";
530+
for (double x : m) {
531+
char factor[32];
532+
std::snprintf(factor, sizeof(factor), "%a,", x);
533+
id += factor;
534+
}
535+
return id;
536+
}
537+
538+
539+
// true if the map of this instance is diagonal, which then multiplies the scale of the mesh
540+
bool mjCMesh::InstanceDiagonal() const {
541+
const auto& m = instancemap_;
542+
return !m[1] && !m[2] && !m[3] && !m[5] && !m[6] && !m[7];
522543
}
523544

524545
namespace {
@@ -1278,6 +1299,26 @@ void mjCMesh::ApplyTransformations(double* dvert) {
12781299
}
12791300
}
12801301

1302+
// the map of an instance which does not multiply the scale; normals by the inverse transpose
1303+
if (!InstanceDiagonal()) {
1304+
const double* m = instancemap_.data();
1305+
for (int i = 0; i < nvert(); i++) { mjuu_mulvecmat(&dvert[3 * i], &dvert[3 * i], m); }
1306+
double cof[9] = {m[4] * m[8] - m[5] * m[7],
1307+
m[5] * m[6] - m[3] * m[8],
1308+
m[3] * m[7] - m[4] * m[6],
1309+
m[2] * m[7] - m[1] * m[8],
1310+
m[0] * m[8] - m[2] * m[6],
1311+
m[1] * m[6] - m[0] * m[7],
1312+
m[1] * m[5] - m[2] * m[4],
1313+
m[2] * m[3] - m[0] * m[5],
1314+
m[0] * m[4] - m[1] * m[3]};
1315+
for (int i = 0; i < nnormal(); i++) {
1316+
double n[3] = {normal_[3 * i], normal_[3 * i + 1], normal_[3 * i + 2]};
1317+
mjuu_mulvecmat(n, n, cof);
1318+
for (int k = 0; k < 3; k++) { normal_[3 * i + k] = (float)n[k]; }
1319+
}
1320+
}
1321+
12811322
// normalize normals
12821323
for (int i = 0; i < nnormal(); i++) {
12831324
// compute length
@@ -3520,17 +3561,26 @@ void mjCSkin::ResolveReferences(const mjCModel* m) {
35203561

35213562

35223563
// scale the compiled copy by the gauge of the bones: the vertices and the bind poses are given in
3523-
// the same coordinates, in which the offsets of the vertices from the bones scale with the bones
3564+
// the same coordinates, in which the offsets of the vertices from the bones scale with the bones,
3565+
// whose maps, expressed there through their bind orientations, must agree
35243566
void mjCSkin::ApplyGauge() {
3525-
scale_ = 1;
3567+
scaled_ = false;
35263568
if (bodyid.empty()) { return; }
3527-
const mjCGauge g = model->Bodies()[bodyid[0]]->ContentGauge();
3528-
for (int b : bodyid) {
3529-
if (model->Bodies()[b]->ContentGauge().shape != g.shape) {
3530-
throw mjCError(this, "the bones of a skin must have the same scale");
3569+
std::vector<mjCGauge> gauges;
3570+
for (int i = 0; i < bodyid.size(); i++) {
3571+
const float* q = bindquat_.data() + 4 * i;
3572+
double conj[4] = {q[0], -q[1], -q[2], -q[3]};
3573+
gauges.push_back(model->Bodies()[bodyid[i]]->ContentGauge().Rotated(conj));
3574+
}
3575+
const mjCGauge& g = gauges[0];
3576+
for (const mjCGauge& b : gauges) {
3577+
bool same = b.uniform == g.uniform && b.shape == g.shape;
3578+
for (int k = 0; k < 9 && same && !g.uniform; k++) {
3579+
same = std::abs(b.map[k] - g.map[k]) <= 1e-6 * g.shape;
35313580
}
3581+
if (!same) { throw mjCError(this, "the bones of a skin must have the same scale"); }
35323582
}
3533-
if (g.shape == 1) { return; }
3583+
if (g.uniform && g.shape == 1) { return; }
35343584
auto points = [&g](std::vector<float>& pos) {
35353585
for (int i = 0; i < pos.size() / 3; i++) {
35363586
double x[3] = {pos[3 * i], pos[3 * i + 1], pos[3 * i + 2]};
@@ -3541,7 +3591,7 @@ void mjCSkin::ApplyGauge() {
35413591
points(vert_);
35423592
points(bindpos_);
35433593
inflate *= g.Shape(1);
3544-
scale_ = g.shape;
3594+
scaled_ = true;
35453595
}
35463596

35473597

‎src/user/user_model.cc‎

Lines changed: 28 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -2050,33 +2050,35 @@ std::vector<mjCHField*> mjCModel::CompiledHFields() const {
20502050

20512051
// give each use of an asset the instance of the asset at the scale of the use: the asset itself at
20522052
// the scale of its first use, and instances named after it at the others
2053+
using InstanceMap = std::array<double, 9>;
2054+
20532055
template <class T>
2054-
static void Instantiate(const mjCModel* model,
2055-
mjtObj type,
2056-
std::vector<T*>& assets,
2057-
std::vector<T*>& instances,
2058-
const std::vector<std::pair<T**, double>>& uses,
2059-
bool reuse) {
2056+
static void Instantiate(const mjCModel* model,
2057+
mjtObj type,
2058+
std::vector<T*>& assets,
2059+
std::vector<T*>& instances,
2060+
const std::vector<std::pair<T**, InstanceMap>>& uses,
2061+
bool reuse) {
20602062
if (!reuse) {
20612063
for (T* instance : instances) { delete instance; }
20622064
instances.clear();
2063-
std::unordered_map<const T*, double> first;
2065+
std::unordered_map<const T*, InstanceMap> first;
20642066
for (const auto& [use, scale] : uses) { first.emplace(*use, scale); }
20652067
for (T* asset : assets) {
2066-
auto it = first.find(asset);
2067-
double scale = it == first.end() ? 1 : it->second;
2068-
if (asset->instancescale_ != scale) {
2069-
asset->instancescale_ = scale;
2068+
auto it = first.find(asset);
2069+
InstanceMap scale = it == first.end() ? InstanceMap{1, 0, 0, 0, 1, 0, 0, 0, 1} : it->second;
2070+
if (asset->instancemap_ != scale) {
2071+
asset->instancemap_ = scale;
20702072
asset->CopyFromSpec();
20712073
}
20722074
}
20732075
}
20742076

20752077
for (const auto& [use, scale] : uses) {
20762078
T* asset = *use;
2077-
if (asset->instancescale_ == scale) { continue; }
2078-
auto same = [asset, scale](const T* instance) {
2079-
return instance->primary_ == asset && instance->instancescale_ == scale;
2079+
if (asset->instancemap_ == scale) { continue; }
2080+
auto same = [asset, &scale](const T* instance) {
2081+
return instance->primary_ == asset && instance->instancemap_ == scale;
20802082
};
20812083
auto it = std::find_if(instances.begin(), instances.end(), same);
20822084
if (it == instances.end()) {
@@ -2088,13 +2090,13 @@ static void Instantiate(const mjCModel* model,
20882090
if (model->FindObject(type, name)) {
20892091
throw mjCError(asset, "the name '%s' of an instance of this asset is taken", name.c_str());
20902092
}
2091-
T* instance = new T(*asset);
2092-
instance->model = asset->model;
2093-
instance->compiler = asset->compiler;
2094-
instance->name = name;
2095-
instance->primary_ = asset;
2096-
instance->instancescale_ = scale;
2097-
instance->id = assets.size() + instances.size();
2093+
T* instance = new T(*asset);
2094+
instance->model = asset->model;
2095+
instance->compiler = asset->compiler;
2096+
instance->name = name;
2097+
instance->primary_ = asset;
2098+
instance->instancemap_ = scale;
2099+
instance->id = assets.size() + instances.size();
20982100
instance->CopyFromSpec();
20992101
instances.push_back(instance);
21002102
it = instances.end() - 1;
@@ -2122,14 +2124,14 @@ static void Instantiate(const mjCModel* model,
21222124
// give the geoms and sites which use a mesh or a height field the instance of the asset at the
21232125
// scale of their use, in the order of their ids, made again unless the assets are reused
21242126
void mjCModel::MakeInstances(bool reuse) {
2125-
std::vector<std::pair<mjCMesh**, double>> meshuses;
2126-
std::vector<std::pair<mjCHField**, double>> hfielduses;
2127+
std::vector<std::pair<mjCMesh**, InstanceMap>> meshuses;
2128+
std::vector<std::pair<mjCHField**, InstanceMap>> hfielduses;
21272129
for (mjCGeom* geom : geoms_) {
2128-
if (geom->mesh) { meshuses.emplace_back(&geom->mesh, geom->Gauge().shape); }
2129-
if (geom->hfield) { hfielduses.emplace_back(&geom->hfield, geom->Gauge().shape); }
2130+
if (geom->mesh) { meshuses.emplace_back(&geom->mesh, geom->ShapeMap()); }
2131+
if (geom->hfield) { hfielduses.emplace_back(&geom->hfield, geom->ShapeMap()); }
21302132
}
21312133
for (mjCSite* site : sites_) {
2132-
if (site->mesh) { meshuses.emplace_back(&site->mesh, site->Gauge().shape); }
2134+
if (site->mesh) { meshuses.emplace_back(&site->mesh, site->ShapeMap()); }
21332135
}
21342136
Instantiate(this, mjOBJ_MESH, meshes_, meshinstances_, meshuses, reuse);
21352137
Instantiate(this, mjOBJ_HFIELD, hfields_, hfieldinstances_, hfielduses, reuse);

0 commit comments

Comments
 (0)
⌂ 换网址