Alessio Quaglino
9d5063799f
Previously, it was assumed that attaching was the last operation performed on the mjSpec, so keyframes resulted in the incorrect size. In order to prevent information loss, we now only allow the resize function to expand the keyframe array, since attach should never remove a degree of freedom. PiperOrigin-RevId: 804917828 Change-Id: Icfacd207ced1c6aac600d514f8d9065ceff2d347
7825 lines
203 KiB
C++
7825 lines
203 KiB
C++
// Copyright 2021 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 "user/user_objects.h"
|
||
|
||
#include <algorithm>
|
||
#include <array>
|
||
#include <cmath>
|
||
#include <cstddef>
|
||
#include <cstdint>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <functional>
|
||
#include <limits>
|
||
#include <memory>
|
||
#include <new>
|
||
#include <optional>
|
||
#include <random>
|
||
#include <sstream>
|
||
#include <string>
|
||
#include <string_view>
|
||
#include <unordered_map>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
#include "lodepng.h"
|
||
#include "cc/array_safety.h"
|
||
#include "engine/engine_passive.h"
|
||
#include "engine/engine_support.h"
|
||
#include <mujoco/mjspec.h>
|
||
#include <mujoco/mujoco.h>
|
||
#include "user/user_api.h"
|
||
#include "user/user_cache.h"
|
||
#include "user/user_model.h"
|
||
#include "user/user_resource.h"
|
||
#include "user/user_util.h"
|
||
|
||
namespace {
|
||
namespace mju = ::mujoco::util;
|
||
using mujoco::user::FilePath;
|
||
|
||
class PNGImage {
|
||
public:
|
||
static PNGImage Load(const mjCBase* obj, mjResource* resource,
|
||
LodePNGColorType color_type);
|
||
int Width() const { return width_; }
|
||
int Height() const { return height_; }
|
||
bool IsSRGB() const { return is_srgb_; }
|
||
|
||
uint8_t operator[] (int i) const { return data_[i]; }
|
||
std::vector<unsigned char>& MoveData() { return data_; }
|
||
|
||
private:
|
||
std::size_t Size() const {
|
||
return data_.size() + (3 * sizeof(int));
|
||
}
|
||
|
||
int width_;
|
||
int height_;
|
||
bool is_srgb_;
|
||
LodePNGColorType color_type_;
|
||
std::vector<uint8_t> data_;
|
||
};
|
||
|
||
PNGImage PNGImage::Load(const mjCBase* obj, mjResource* resource,
|
||
LodePNGColorType color_type) {
|
||
PNGImage image;
|
||
image.color_type_ = color_type;
|
||
mjCCache *cache = reinterpret_cast<mjCCache*>(mj_getCache()->impl_);
|
||
|
||
// cache callback
|
||
auto callback = [&image](const void* data) {
|
||
const PNGImage *cached_image = static_cast<const PNGImage*>(data);
|
||
if (cached_image->color_type_ == image.color_type_) {
|
||
image = *cached_image;
|
||
return true;
|
||
}
|
||
return false;
|
||
};
|
||
|
||
// try loading from cache
|
||
if (cache && cache->PopulateData(resource, callback)) {
|
||
return image;
|
||
}
|
||
|
||
// open PNG resource
|
||
const unsigned char* buffer;
|
||
int nbuffer = mju_readResource(resource, (const void**) &buffer);
|
||
|
||
if (nbuffer < 0) {
|
||
throw mjCError(obj, "could not read PNG file '%s'", resource->name);
|
||
}
|
||
|
||
if (!nbuffer) {
|
||
throw mjCError(obj, "empty PNG file '%s'", resource->name);
|
||
}
|
||
|
||
// decode PNG from buffer
|
||
unsigned int w, h;
|
||
|
||
lodepng::State state;
|
||
state.info_raw.colortype = image.color_type_;
|
||
state.info_raw.bitdepth = 8;
|
||
unsigned err = lodepng::decode(image.data_, w, h, state, buffer, nbuffer);
|
||
|
||
// check for errors
|
||
if (err) {
|
||
std::stringstream ss;
|
||
ss << "error decoding PNG file '" << resource->name << "': " << lodepng_error_text(err);
|
||
throw mjCError(obj, "%s", ss.str().c_str());
|
||
}
|
||
|
||
image.width_ = w;
|
||
image.height_ = h;
|
||
image.is_srgb_ = (state.info_png.srgb_defined == 1);
|
||
|
||
if (image.width_ <= 0 || image.height_ < 0) {
|
||
std::stringstream ss;
|
||
ss << "error decoding PNG file '" << resource->name << "': " << "dimensions are invalid";
|
||
throw mjCError(obj, "%s", ss.str().c_str());
|
||
}
|
||
|
||
// insert raw image data into cache
|
||
if (cache) {
|
||
PNGImage *cached_image = new PNGImage(image);;
|
||
std::size_t size = image.Size();
|
||
std::shared_ptr<const void> cached_data(cached_image, +[] (const void* data) {
|
||
delete static_cast<const PNGImage*>(data);
|
||
});
|
||
cache->Insert("", resource, cached_data, size);
|
||
}
|
||
|
||
return image;
|
||
}
|
||
|
||
// associate all child list elements with a frame and copy them to parent list, clear child list
|
||
template <typename T>
|
||
void MapFrame(std::vector<T*>& parent, std::vector<T*>& child,
|
||
mjCFrame* frame, mjCBody* parent_body) {
|
||
std::for_each(child.begin(), child.end(), [frame, parent_body](T* element) {
|
||
element->SetFrame(frame);
|
||
element->SetParent(parent_body);
|
||
});
|
||
parent.insert(parent.end(), child.begin(), child.end());
|
||
child.clear();
|
||
}
|
||
|
||
} // namespace
|
||
|
||
|
||
// utiility function for checking size parameters
|
||
static void checksize(double* size, mjtGeom type, mjCBase* object, const char* name, int id) {
|
||
// plane: handle infinite
|
||
if (type == mjGEOM_PLANE) {
|
||
if (size[2] <= 0) {
|
||
throw mjCError(object, "plane size(3) must be positive");
|
||
}
|
||
}
|
||
|
||
// regular geom
|
||
else {
|
||
for (int i=0; i < mjGEOMINFO[type]; i++) {
|
||
if (size[i] <= 0) {
|
||
throw mjCError(object, "size %d must be positive in geom", nullptr, i);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// error message for missing "limited" attribute
|
||
static void checklimited(
|
||
const mjCBase* obj,
|
||
bool autolimits, const char* entity, const char* attr, int limited, bool hasrange) {
|
||
if (!autolimits && limited == 2 && hasrange) {
|
||
std::stringstream ss;
|
||
ss << entity << " has `" << attr << "range` but not `" << attr << "limited`. "
|
||
<< "set the autolimits=\"true\" compiler option, specify `" << attr << "limited` "
|
||
<< "explicitly (\"true\" or \"false\"), or remove the `" << attr << "range` attribute.";
|
||
throw mjCError(obj, "%s", ss.str().c_str());
|
||
}
|
||
}
|
||
|
||
// returns true if limits should be active
|
||
static bool islimited(int limited, const double range[2]) {
|
||
if (limited == mjLIMITED_TRUE || (limited == mjLIMITED_AUTO && range[0] < range[1])) {
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
//------------------------- class mjCError implementation ------------------------------------------
|
||
|
||
// constructor
|
||
mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos1, int pos2) {
|
||
char temp[600];
|
||
|
||
// init
|
||
warning = false;
|
||
if (obj || msg) {
|
||
mju::sprintf_arr(message, "Error");
|
||
} else {
|
||
message[0] = 0;
|
||
}
|
||
|
||
// construct error message
|
||
if (msg) {
|
||
if (str) {
|
||
mju::sprintf_arr(temp, msg, str, pos1, pos2);
|
||
} else {
|
||
mju::sprintf_arr(temp, msg, pos1, pos2);
|
||
}
|
||
|
||
mju::strcat_arr(message, ": ");
|
||
mju::strcat_arr(message, temp);
|
||
}
|
||
|
||
// append info from mjCBase element
|
||
if (obj) {
|
||
// with or without xml position
|
||
if (!obj->info.empty()) {
|
||
mju::sprintf_arr(temp, "Element name '%s', id %d, %s",
|
||
obj->name.c_str(), obj->id, obj->info.c_str());
|
||
} else {
|
||
mju::sprintf_arr(temp, "Element name '%s', id %d", obj->name.c_str(), obj->id);
|
||
}
|
||
|
||
// append to message
|
||
mju::strcat_arr(message, "\n");
|
||
mju::strcat_arr(message, temp);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ alternative orientation implementation ----------------------------------------
|
||
|
||
// compute frame orientation given alternative specifications
|
||
// used for geom, site, body and camera frames
|
||
const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
|
||
const mjsOrientation& orient) {
|
||
double axisangle[4];
|
||
double xyaxes[6];
|
||
double zaxis[3];
|
||
double euler[3];
|
||
|
||
mjuu_copyvec(axisangle, orient.axisangle, 4);
|
||
mjuu_copyvec(xyaxes, orient.xyaxes, 6);
|
||
mjuu_copyvec(zaxis, orient.zaxis, 3);
|
||
mjuu_copyvec(euler, orient.euler, 3);
|
||
|
||
// set quat using axisangle
|
||
if (orient.type == mjORIENTATION_AXISANGLE) {
|
||
// convert to radians if necessary, normalize axis
|
||
if (degree) {
|
||
axisangle[3] = axisangle[3] / 180.0 * mjPI;
|
||
}
|
||
if (mjuu_normvec(axisangle, 3) < mjEPS) {
|
||
return "axisangle too small";
|
||
}
|
||
|
||
// construct quaternion
|
||
double ang2 = axisangle[3]/2;
|
||
quat[0] = cos(ang2);
|
||
quat[1] = sin(ang2)*axisangle[0];
|
||
quat[2] = sin(ang2)*axisangle[1];
|
||
quat[3] = sin(ang2)*axisangle[2];
|
||
}
|
||
|
||
// set quat using xyaxes
|
||
if (orient.type == mjORIENTATION_XYAXES) {
|
||
// normalize x axis
|
||
if (mjuu_normvec(xyaxes, 3) < mjEPS) {
|
||
return "xaxis too small";
|
||
}
|
||
|
||
// make y axis orthogonal to x axis, normalize
|
||
double d = mjuu_dot3(xyaxes, xyaxes+3);
|
||
xyaxes[3] -= xyaxes[0]*d;
|
||
xyaxes[4] -= xyaxes[1]*d;
|
||
xyaxes[5] -= xyaxes[2]*d;
|
||
if (mjuu_normvec(xyaxes+3, 3) < mjEPS) {
|
||
return "yaxis too small";
|
||
}
|
||
|
||
// compute and normalize z axis
|
||
double z[3];
|
||
mjuu_crossvec(z, xyaxes, xyaxes+3);
|
||
if (mjuu_normvec(z, 3) < mjEPS) {
|
||
return "cross(xaxis, yaxis) too small";
|
||
}
|
||
|
||
// convert frame into quaternion
|
||
mjuu_frame2quat(quat, xyaxes, xyaxes+3, z);
|
||
}
|
||
|
||
// set quat using zaxis
|
||
if (orient.type == mjORIENTATION_ZAXIS) {
|
||
if (mjuu_normvec(zaxis, 3) < mjEPS) {
|
||
return "zaxis too small";
|
||
}
|
||
mjuu_z2quat(quat, zaxis);
|
||
}
|
||
|
||
|
||
// handle euler
|
||
if (orient.type == mjORIENTATION_EULER) {
|
||
// convert to radians if necessary
|
||
if (degree) {
|
||
for (int i=0; i < 3; i++) {
|
||
euler[i] = euler[i] / 180.0 * mjPI;
|
||
}
|
||
}
|
||
|
||
// init
|
||
mjuu_setvec(quat, 1, 0, 0, 0);
|
||
|
||
// loop over euler angles, accumulate rotations
|
||
for (int i=0; i < 3; i++) {
|
||
double tmp[4], qrot[4] = {cos(euler[i]/2), 0, 0, 0};
|
||
double sa = sin(euler[i]/2);
|
||
|
||
// construct quaternion rotation
|
||
if (sequence[i] == 'x' || sequence[i] == 'X') {
|
||
qrot[1] = sa;
|
||
} else if (sequence[i] == 'y' || sequence[i] == 'Y') {
|
||
qrot[2] = sa;
|
||
} else if (sequence[i] == 'z' || sequence[i] == 'Z') {
|
||
qrot[3] = sa;
|
||
} else {
|
||
return "euler sequence can only contain x, y, z, X, Y, Z";
|
||
}
|
||
|
||
// accumulate rotation
|
||
if (sequence[i] == 'x' || sequence[i] == 'y' || sequence[i] == 'z') {
|
||
mjuu_mulquat(tmp, quat, qrot); // moving axes: post-multiply
|
||
} else {
|
||
mjuu_mulquat(tmp, qrot, quat); // fixed axes: pre-multiply
|
||
}
|
||
mjuu_copyvec(quat, tmp, 4);
|
||
}
|
||
|
||
// normalize, just in case
|
||
mjuu_normvec(quat, 4);
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
//------------------------- class mjCBoundingVolumeHierarchy implementation ------------------------
|
||
|
||
|
||
// assign position and orientation
|
||
void mjCBoundingVolumeHierarchy::Set(double ipos_element[3], double iquat_element[4]) {
|
||
mjuu_copyvec(ipos_, ipos_element, 3);
|
||
mjuu_copyvec(iquat_, iquat_element, 4);
|
||
}
|
||
|
||
|
||
|
||
void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
|
||
nbvh_ = 0;
|
||
bvh_.clear();
|
||
child_.clear();
|
||
nodeid_.clear();
|
||
level_.clear();
|
||
bvleaf_.clear();
|
||
bvleaf_.reserve(nleaf);
|
||
}
|
||
|
||
|
||
void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) {
|
||
bvleaf_.erase(bvleaf_.begin() + nmax, bvleaf_.end());
|
||
}
|
||
|
||
const mjCBoundingVolume*
|
||
mjCBoundingVolumeHierarchy::AddBoundingVolume(int id, int contype, int conaffinity,
|
||
const double* pos, const double* quat,
|
||
const double* aabb) {
|
||
bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb);
|
||
return &bvleaf_.back();
|
||
}
|
||
|
||
|
||
const mjCBoundingVolume*
|
||
mjCBoundingVolumeHierarchy::AddBoundingVolume(const int* id, int contype, int conaffinity,
|
||
const double* pos, const double* quat,
|
||
const double* aabb) {
|
||
bvleaf_.emplace_back(id, contype, conaffinity, pos, quat, aabb);
|
||
return &bvleaf_.back();
|
||
}
|
||
|
||
|
||
// create bounding volume hierarchy
|
||
void mjCBoundingVolumeHierarchy::CreateBVH() {
|
||
std::vector<BVElement> elements;
|
||
Make(elements);
|
||
MakeBVH(elements.begin(), elements.end());
|
||
}
|
||
|
||
|
||
void mjCBoundingVolumeHierarchy::Make(std::vector<BVElement>& elements) {
|
||
// precompute the positions of each element in the hierarchy's axes, and drop
|
||
// visual-only elements.
|
||
elements.reserve(bvleaf_.size());
|
||
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
|
||
for (int i = 0; i < bvleaf_.size(); i++) {
|
||
if (bvleaf_[i].Conaffinity() || bvleaf_[i].Contype()) {
|
||
BVElement element;
|
||
element.e = &bvleaf_[i];
|
||
double vert[3] = {element.e->Pos(0) - ipos_[0],
|
||
element.e->Pos(1) - ipos_[1],
|
||
element.e->Pos(2) - ipos_[2]};
|
||
mjuu_rotVecQuat(element.lpos, vert, qinv);
|
||
elements.push_back(std::move(element));
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
// compute bounding volume hierarchy
|
||
int mjCBoundingVolumeHierarchy::MakeBVH(
|
||
std::vector<BVElement>::iterator elements_begin,
|
||
std::vector<BVElement>::iterator elements_end, int lev) {
|
||
int nelements = elements_end - elements_begin;
|
||
if (nelements == 0) {
|
||
return -1;
|
||
}
|
||
constexpr double kMaxVal = std::numeric_limits<double>::max();
|
||
double AAMM[6] = {kMaxVal, kMaxVal, kMaxVal, -kMaxVal, -kMaxVal, -kMaxVal};
|
||
|
||
// inverse transformation
|
||
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
|
||
|
||
// accumulate AAMM over elements
|
||
for (auto element = elements_begin; element != elements_end; ++element) {
|
||
// transform element aabb to aamm format
|
||
double aamm[6] = {element->e->AABB(0) - element->e->AABB(3),
|
||
element->e->AABB(1) - element->e->AABB(4),
|
||
element->e->AABB(2) - element->e->AABB(5),
|
||
element->e->AABB(0) + element->e->AABB(3),
|
||
element->e->AABB(1) + element->e->AABB(4),
|
||
element->e->AABB(2) + element->e->AABB(5)};
|
||
|
||
// update node AAMM
|
||
for (int v=0; v < 8; v++) {
|
||
double vert[3], box[3];
|
||
vert[0] = (v&1 ? aamm[3] : aamm[0]);
|
||
vert[1] = (v&2 ? aamm[4] : aamm[1]);
|
||
vert[2] = (v&4 ? aamm[5] : aamm[2]);
|
||
|
||
// rotate to the body inertial frame if specified
|
||
if (element->e->Quat()) {
|
||
mjuu_rotVecQuat(box, vert, element->e->Quat());
|
||
box[0] += element->e->Pos(0) - ipos_[0];
|
||
box[1] += element->e->Pos(1) - ipos_[1];
|
||
box[2] += element->e->Pos(2) - ipos_[2];
|
||
mjuu_rotVecQuat(vert, box, qinv);
|
||
}
|
||
|
||
AAMM[0] = std::min(AAMM[0], vert[0]);
|
||
AAMM[1] = std::min(AAMM[1], vert[1]);
|
||
AAMM[2] = std::min(AAMM[2], vert[2]);
|
||
AAMM[3] = std::max(AAMM[3], vert[0]);
|
||
AAMM[4] = std::max(AAMM[4], vert[1]);
|
||
AAMM[5] = std::max(AAMM[5], vert[2]);
|
||
}
|
||
}
|
||
|
||
// inflate flat AABBs
|
||
for (int i = 0; i < 3; i++) {
|
||
if (std::abs(AAMM[i] - AAMM[i+3]) < mjEPS) {
|
||
AAMM[i + 0] -= mjEPS;
|
||
AAMM[i + 3] += mjEPS;
|
||
}
|
||
}
|
||
|
||
// store current index
|
||
int index = nbvh_++;
|
||
child_.push_back(-1);
|
||
child_.push_back(-1);
|
||
nodeid_.push_back(-1);
|
||
nodeidptr_.push_back(nullptr);
|
||
level_.push_back(lev);
|
||
|
||
// store bounding box of the current node
|
||
bvh_.push_back((AAMM[3] + AAMM[0]) / 2);
|
||
bvh_.push_back((AAMM[4] + AAMM[1]) / 2);
|
||
bvh_.push_back((AAMM[5] + AAMM[2]) / 2);
|
||
bvh_.push_back((AAMM[3] - AAMM[0]) / 2);
|
||
bvh_.push_back((AAMM[4] - AAMM[1]) / 2);
|
||
bvh_.push_back((AAMM[5] - AAMM[2]) / 2);
|
||
|
||
// leaf node, return
|
||
if (nelements == 1) {
|
||
child_[2*index + 0] = -1;
|
||
child_[2*index + 1] = -1;
|
||
nodeid_[index] = *elements_begin->e->Id();
|
||
nodeidptr_[index] = (int*)elements_begin->e->Id();
|
||
return index;
|
||
}
|
||
|
||
// find longest axis, by a margin of at least mjEPS, default to 0
|
||
int axis = 0;
|
||
double edges[3] = {AAMM[3] - AAMM[0], AAMM[4] - AAMM[1], AAMM[5] - AAMM[2]};
|
||
if (edges[1] >= edges[0] + mjEPS) axis = 1;
|
||
if (edges[2] >= edges[axis] + mjEPS) axis = 2;
|
||
|
||
// find median along the axis
|
||
auto compare = [&](const BVElement& e1, const BVElement& e2) {
|
||
if (std::abs(e1.lpos[axis] - e2.lpos[axis]) > mjEPS) {
|
||
return e1.lpos[axis] < e2.lpos[axis];
|
||
}
|
||
// comparing pointers gives a stable sort, because they both come from the same array
|
||
return e1.e < e2.e;
|
||
};
|
||
|
||
// note: nth_element performs a partial sort of elements
|
||
int m = nelements / 2;
|
||
std::nth_element(elements_begin, elements_begin + m, elements_end, compare);
|
||
|
||
// recursive calls
|
||
if (m > 0) {
|
||
child_[2*index + 0] = MakeBVH(elements_begin, elements_begin + m, lev + 1);
|
||
}
|
||
|
||
if (m != nelements) {
|
||
child_[2*index + 1] = MakeBVH(elements_begin + m, elements_end, lev + 1);
|
||
}
|
||
|
||
// SHOULD NOT OCCUR
|
||
if (child_[2*index + 0] == -1 && child_[2*index + 1] == -1) {
|
||
mju_error("this should have been a leaf, body=%s nelements=%d",
|
||
name_.c_str(), nelements);
|
||
}
|
||
|
||
if (lev > mjMAXTREEDEPTH) {
|
||
mju_warning("max tree depth exceeded in body=%s", name_.c_str());
|
||
}
|
||
|
||
return index;
|
||
}
|
||
|
||
|
||
|
||
//------------------------- class mjCOctree implementation --------------------------------------------
|
||
|
||
void mjCOctree::CopyLevel(int* level) const {
|
||
for (int i = 0; i < node_.size(); ++i) {
|
||
level[i] = node_[i].level;
|
||
}
|
||
}
|
||
|
||
void mjCOctree::CopyChild(int* child) const {
|
||
for (int i = 0; i < node_.size(); ++i) {
|
||
for (int j = 0; j < 8; ++j) {
|
||
child[i * 8 + j] = node_[i].child[j];
|
||
}
|
||
}
|
||
}
|
||
|
||
void mjCOctree::CopyAabb(mjtNum* aabb) const {
|
||
for (int i = 0; i < node_.size(); ++i) {
|
||
for (int j = 0; j < 6; ++j) {
|
||
aabb[i * 6 + j] = node_[i].aabb[j];
|
||
}
|
||
}
|
||
}
|
||
|
||
void mjCOctree::CopyCoeff(mjtNum* coeff) const {
|
||
for (int i = 0; i < node_.size(); ++i) {
|
||
for (int j = 0; j < 8; ++j) {
|
||
coeff[i * 8 + j] = node_[i].coeff[j];
|
||
}
|
||
}
|
||
}
|
||
|
||
void mjCOctree::SetFace(const std::vector<double>& vert, const std::vector<int>& face) {
|
||
for (int i = 0; i < face.size(); i += 3) {
|
||
std::array<double, 3> v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]};
|
||
std::array<double, 3> v1 = {vert[3*face[i+1]], vert[3*face[i+1]+1], vert[3*face[i+1]+2]};
|
||
std::array<double, 3> v2 = {vert[3*face[i+2]], vert[3*face[i+2]+1], vert[3*face[i+2]+2]};
|
||
face_.push_back({v0, v1, v2});
|
||
}
|
||
}
|
||
|
||
|
||
// TODO: use the same code as mjCBoundingVolumeHierarchy::Make()
|
||
void mjCOctree::Make(std::vector<Triangle>& elements) {
|
||
// rotate triangles to the body inertial frame
|
||
elements.assign(face_.size(), {{{0}}});
|
||
double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
|
||
for (int i = 0; i < face_.size(); i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
double vert[3] = {face_[i][j][0] - ipos_[0],
|
||
face_[i][j][1] - ipos_[1],
|
||
face_[i][j][2] - ipos_[2]};
|
||
mjuu_rotVecQuat(elements[i][j].data(), vert, qinv);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
void mjCOctree::CreateOctree(const double aamm[6]) {
|
||
double aabb[6] = {(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2, (aamm[2] + aamm[5]) / 2,
|
||
(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2};
|
||
double box[6] = {aabb[0] - 1.1 * aabb[3], aabb[1] - 1.1 * aabb[4], aabb[2] - 1.1 * aabb[5],
|
||
aabb[0] + 1.1 * aabb[3], aabb[1] + 1.1 * aabb[4], aabb[2] + 1.1 * aabb[5]};
|
||
std::vector<Triangle> elements;
|
||
Make(elements);
|
||
std::vector<Triangle*> elements_ptrs(elements.size());
|
||
std::transform(elements.begin(), elements.end(), elements_ptrs.begin(),
|
||
[](Triangle& triangle) { return ▵ });
|
||
std::unordered_map<Point, int> vert_map;
|
||
MakeOctree(elements_ptrs, box, 0, vert_map);
|
||
}
|
||
|
||
|
||
static double dot2(const double* a, const double* b) {
|
||
return a[0] * b[0] + a[1] * b[1];
|
||
}
|
||
|
||
|
||
// From M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs".
|
||
static bool boxTriangle(const Triangle& v, const double aamm[6]) {
|
||
// bounding box tests
|
||
for (int i = 0; i < 3; i++) {
|
||
if (v[0][i] < aamm[i] && v[1][i] < aamm[i] && v[2][i] < aamm[i]) {
|
||
return false;
|
||
}
|
||
int j = i + 3;
|
||
if (v[0][i] > aamm[j] && v[1][i] > aamm[j] && v[2][i] > aamm[j]) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// test for triangle plane and box overlap
|
||
double n[3];
|
||
double e[3][3];
|
||
for (int i = 0; i < 3; i++) {
|
||
for (int j = 0; j < 3; j++) {
|
||
e[i][j] = v[(i+1)%3][j] - v[i][j];
|
||
}
|
||
}
|
||
|
||
mjuu_crossvec(n, e[0], e[1]);
|
||
double size[3] = {aamm[3] - aamm[0], aamm[4] - aamm[1], aamm[5] - aamm[2]};
|
||
double c[3] = {n[0] > 0 ? size[0] : 0, n[1] > 0 ? size[1] : 0, n[2] > 0 ? size[2] : 0};
|
||
double c1[3] = {c[0] - v[0][0], c[1] - v[0][1], c[2] - v[0][2]};
|
||
double c2[3] = {size[0] - c[0] - v[0][0], size[1] - c[1] - v[0][1], size[2] - c[2] - v[0][2]};
|
||
|
||
if ((mjuu_dot3(n, aamm) + mjuu_dot3(n, c1)) * (mjuu_dot3(n, aamm) + mjuu_dot3(n, c2)) > 0) {
|
||
return false;
|
||
}
|
||
|
||
// test projection overlap
|
||
for (int a = 0; a < 3; a++) {
|
||
int b = (a + 1) % 3;
|
||
int c = (a + 2) % 3;
|
||
for (int i = 0; i < 3; i++) {
|
||
double sign = n[a] >= 0 ? 1 : -1;
|
||
double ne[2] = {-e[i][c] * sign, e[i][b] * sign};
|
||
double vi[2] = {v[i][b], v[i][c]};
|
||
double d = -dot2(ne, vi) + mju_max(0, size[b]*ne[0]) + mju_max(0, size[c]*ne[1]);
|
||
double p[2] = {aamm[b], aamm[c]};
|
||
if (dot2(ne, p) + d < 0) {
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
int mjCOctree::MakeOctree(const std::vector<Triangle*>& elements, const double aamm[6], int lev,
|
||
std::unordered_map<Point, int>& vert_map) {
|
||
node_.push_back(OctNode());
|
||
OctNode& node = node_.back();
|
||
node.level = lev;
|
||
|
||
// create a new node
|
||
int index = nnode_++;
|
||
std::array<double, 6> aabb = {
|
||
(aamm[0] + aamm[3]) / 2, (aamm[1] + aamm[4]) / 2,
|
||
(aamm[2] + aamm[5]) / 2, (aamm[3] - aamm[0]) / 2,
|
||
(aamm[4] - aamm[1]) / 2, (aamm[5] - aamm[2]) / 2};
|
||
node.aabb = aabb;
|
||
for (int i = 0; i < 8; i++) {
|
||
Point v = {{(i & 1) ? aamm[3] : aamm[0],
|
||
(i & 2) ? aamm[4] : aamm[1],
|
||
(i & 4) ? aamm[5] : aamm[2]}};
|
||
auto it = vert_map.find(v);
|
||
if (it != vert_map.end()) {
|
||
node.vertid[i] = it->second;
|
||
} else {
|
||
node.vertid[i] = nvert_;
|
||
vert_map[v] = nvert_++;
|
||
vert_.push_back(v);
|
||
}
|
||
node.child[i] = -1;
|
||
}
|
||
|
||
// find all triangles that intersect the current box
|
||
std::vector<Triangle*> colliding;
|
||
for (auto* element : elements) {
|
||
if (boxTriangle(*element, aamm)) {
|
||
colliding.push_back(element);
|
||
}
|
||
}
|
||
|
||
// return if the box is empty
|
||
if (colliding.empty() || lev >= 6) {
|
||
return index;
|
||
}
|
||
|
||
// split the box into 8 sub-boxes
|
||
double new_aamm[8][6];
|
||
for (int i = 0; i < 8; i++) {
|
||
new_aamm[i][0] = aabb[0] + aabb[3] * (i & 1 ? -1 : 0);
|
||
new_aamm[i][1] = aabb[1] + aabb[4] * (i & 2 ? -1 : 0);
|
||
new_aamm[i][2] = aabb[2] + aabb[5] * (i & 4 ? -1 : 0);
|
||
new_aamm[i][3] = aabb[0] + aabb[3] * (i & 1 ? 0 : 1);
|
||
new_aamm[i][4] = aabb[1] + aabb[4] * (i & 2 ? 0 : 1);
|
||
new_aamm[i][5] = aabb[2] + aabb[5] * (i & 4 ? 0 : 1);
|
||
}
|
||
|
||
// recursive calls to create sub-boxes
|
||
for (int i = 0; i < 8; i++) {
|
||
node_[index].child[i] = MakeOctree(colliding, new_aamm[i], lev + 1, vert_map);
|
||
}
|
||
|
||
return index;
|
||
}
|
||
|
||
//------------------------- class mjCDef implementation --------------------------------------------
|
||
|
||
// constructor
|
||
mjCDef::mjCDef() {
|
||
name.clear();
|
||
id = 0;
|
||
parent = nullptr;
|
||
model = 0;
|
||
child.clear();
|
||
elemtype = mjOBJ_DEFAULT;
|
||
mjs_defaultJoint(&joint_.spec);
|
||
mjs_defaultGeom(&geom_.spec);
|
||
mjs_defaultSite(&site_.spec);
|
||
mjs_defaultCamera(&camera_.spec);
|
||
mjs_defaultLight(&light_.spec);
|
||
mjs_defaultFlex(&flex_.spec);
|
||
mjs_defaultMesh(&mesh_.spec);
|
||
mjs_defaultMaterial(&material_.spec);
|
||
mjs_defaultPair(&pair_.spec);
|
||
mjs_defaultEquality(&equality_.spec);
|
||
mjs_defaultTendon(&tendon_.spec);
|
||
mjs_defaultActuator(&actuator_.spec);
|
||
|
||
// make sure all the pointers are local
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
// constructor with model
|
||
mjCDef::mjCDef(mjCModel* _model) : mjCDef() {
|
||
model = _model;
|
||
}
|
||
|
||
|
||
|
||
// copy constructor
|
||
mjCDef::mjCDef(const mjCDef& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCDef::Compile(const mjCModel* model) {
|
||
CopyFromSpec();
|
||
|
||
// enforce length of all default userdata arrays
|
||
joint_.userdata_.resize(model->nuser_jnt);
|
||
geom_.userdata_.resize(model->nuser_geom);
|
||
site_.userdata_.resize(model->nuser_site);
|
||
camera_.userdata_.resize(model->nuser_cam);
|
||
tendon_.userdata_.resize(model->nuser_tendon);
|
||
actuator_.userdata_.resize(model->nuser_actuator);
|
||
}
|
||
|
||
|
||
|
||
// assignment operator
|
||
mjCDef& mjCDef::operator=(const mjCDef& other) {
|
||
if (this != &other) {
|
||
CopyWithoutChildren(other);
|
||
|
||
// copy the rest of the default tree
|
||
*this += other;
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
mjCDef& mjCDef::operator+=(const mjCDef& other) {
|
||
for (unsigned int i=0; i < other.child.size(); i++) {
|
||
child.push_back(new mjCDef(*other.child[i])); // triggers recursive call
|
||
child.back()->parent = this;
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCDef::NameSpace(const mjCModel* m) {
|
||
if (!name.empty()) {
|
||
name = m->prefix + name + m->suffix;
|
||
}
|
||
for (auto c : child) {
|
||
c->NameSpace(m);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCDef::CopyWithoutChildren(const mjCDef& other) {
|
||
name = other.name;
|
||
elemtype = other.elemtype;
|
||
parent = nullptr;
|
||
child.clear();
|
||
joint_ = other.joint_;
|
||
geom_ = other.geom_;
|
||
site_ = other.site_;
|
||
camera_ = other.camera_;
|
||
light_ = other.light_;
|
||
flex_ = other.flex_;
|
||
mesh_ = other.mesh_;
|
||
material_ = other.material_;
|
||
pair_ = other.pair_;
|
||
equality_ = other.equality_;
|
||
tendon_ = other.tendon_;
|
||
actuator_ = other.actuator_;
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
void mjCDef::PointToLocal() {
|
||
joint_.PointToLocal();
|
||
geom_.PointToLocal();
|
||
site_.PointToLocal();
|
||
camera_.PointToLocal();
|
||
light_.PointToLocal();
|
||
flex_.PointToLocal();
|
||
mesh_.PointToLocal();
|
||
material_.PointToLocal();
|
||
pair_.PointToLocal();
|
||
equality_.PointToLocal();
|
||
tendon_.PointToLocal();
|
||
actuator_.PointToLocal();
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.joint = &joint_.spec;
|
||
spec.geom = &geom_.spec;
|
||
spec.site = &site_.spec;
|
||
spec.camera = &camera_.spec;
|
||
spec.light = &light_.spec;
|
||
spec.flex = &flex_.spec;
|
||
spec.mesh = &mesh_.spec;
|
||
spec.material = &material_.spec;
|
||
spec.pair = &pair_.spec;
|
||
spec.equality = &equality_.spec;
|
||
spec.tendon = &tendon_.spec;
|
||
spec.actuator = &actuator_.spec;
|
||
}
|
||
|
||
|
||
|
||
void mjCDef::CopyFromSpec() {
|
||
joint_.CopyFromSpec();
|
||
geom_.CopyFromSpec();
|
||
site_.CopyFromSpec();
|
||
camera_.CopyFromSpec();
|
||
light_.CopyFromSpec();
|
||
flex_.CopyFromSpec();
|
||
mesh_.CopyFromSpec();
|
||
material_.CopyFromSpec();
|
||
pair_.CopyFromSpec();
|
||
equality_.CopyFromSpec();
|
||
tendon_.CopyFromSpec();
|
||
actuator_.CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
//------------------------- class mjCBase implementation -------------------------------------------
|
||
|
||
// constructor
|
||
mjCBase::mjCBase() {
|
||
name.clear();
|
||
classname.clear();
|
||
id = -1;
|
||
info = "";
|
||
model = 0;
|
||
frame = nullptr;
|
||
}
|
||
|
||
|
||
|
||
mjCBase::mjCBase(const mjCBase& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCBase& mjCBase::operator=(const mjCBase& other) {
|
||
if (this != &other) {
|
||
*static_cast<mjCBase_*>(this) = static_cast<const mjCBase_&>(other);
|
||
}
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCBase::NameSpace(const mjCModel* m) {
|
||
if (!name.empty()) {
|
||
name = m->prefix + name + m->suffix;
|
||
}
|
||
if (!classname.empty() && classname != "main" && m != model) {
|
||
classname = m->prefix + classname + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// load resource if found (fallback to OS filesystem)
|
||
mjResource* mjCBase::LoadResource(const std::string& modelfiledir,
|
||
const std::string& filename,
|
||
const mjVFS* vfs) {
|
||
// try reading from provided VFS or fallback to OS filesystem
|
||
std::array<char, 1024> error;
|
||
mjResource* resource = mju_openResource(modelfiledir.c_str(), filename.c_str(), vfs,
|
||
error.data(), error.size());
|
||
if (!resource) {
|
||
throw mjCError(nullptr, "%s", error.data());
|
||
}
|
||
return resource;
|
||
}
|
||
|
||
|
||
// Get and sanitize content type from raw_text if not empty, otherwise parse
|
||
// content type from resource_name; throw error on failure
|
||
std::string mjCBase::GetAssetContentType(std::string_view resource_name,
|
||
std::string_view raw_text) {
|
||
if (!raw_text.empty()) {
|
||
auto type = mjuu_parseContentTypeAttrType(raw_text);
|
||
auto subtype = mjuu_parseContentTypeAttrSubtype(raw_text);
|
||
if (!type.has_value() || !subtype.has_value()) {
|
||
return "";
|
||
}
|
||
return std::string(*type) + "/" + std::string(*subtype);
|
||
} else {
|
||
return mjuu_extToContentType(resource_name);
|
||
}
|
||
}
|
||
|
||
|
||
void mjCBase::SetFrame(mjCFrame* _frame) {
|
||
if (!_frame) {
|
||
return;
|
||
}
|
||
if (_frame->body && GetParent() != _frame->body) {
|
||
throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str());
|
||
}
|
||
frame = _frame;
|
||
}
|
||
|
||
void mjCBase::SetUserValue(std::string_view key, const void* data,
|
||
void (*cleanup)(const void*)) {
|
||
user_payload_[std::string(key)] = UserValue(data, cleanup);
|
||
}
|
||
|
||
const void* mjCBase::GetUserValue(std::string_view key) {
|
||
auto found = user_payload_.find(std::string(key));
|
||
return found != user_payload_.end() ? found->second.value : nullptr;
|
||
}
|
||
|
||
|
||
void mjCBase::DeleteUserValue(std::string_view key) {
|
||
user_payload_.erase(std::string(key));
|
||
}
|
||
|
||
|
||
//------------------ class mjCBody implementation --------------------------------------------------
|
||
|
||
// constructor
|
||
mjCBody::mjCBody(mjCModel* _model) {
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
refcount = 1;
|
||
mjs_defaultBody(&spec);
|
||
elemtype = mjOBJ_BODY;
|
||
parent = nullptr;
|
||
weldid = -1;
|
||
dofnum = 0;
|
||
lastdof = -1;
|
||
subtreedofs = 0;
|
||
contype = 0;
|
||
conaffinity = 0;
|
||
margin = 0;
|
||
mjuu_zerovec(xpos0, 3);
|
||
mjuu_setvec(xquat0, 1, 0, 0, 0);
|
||
last_attached = nullptr;
|
||
mocapid = -1;
|
||
|
||
// clear object lists
|
||
bodies.clear();
|
||
geoms.clear();
|
||
frames.clear();
|
||
joints.clear();
|
||
sites.clear();
|
||
cameras.clear();
|
||
lights.clear();
|
||
spec_userdata_.clear();
|
||
|
||
// in case this body is not compiled
|
||
CopyFromSpec();
|
||
|
||
// point to local (needs to be after defaults)
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
mjCBody::mjCBody(const mjCBody& other, mjCModel* _model) {
|
||
model = _model;
|
||
mjSpec* origin = model->FindSpec(other.compiler);
|
||
compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
*this = other;
|
||
CopyPlugin();
|
||
}
|
||
|
||
|
||
|
||
mjCBody& mjCBody::operator=(const mjCBody& other) {
|
||
if (this != &other) {
|
||
spec = other.spec;
|
||
*static_cast<mjCBody_*>(this) = static_cast<const mjCBody_&>(other);
|
||
*static_cast<mjsBody*>(this) = static_cast<const mjsBody&>(other);
|
||
bodies.clear();
|
||
frames.clear();
|
||
geoms.clear();
|
||
joints.clear();
|
||
sites.clear();
|
||
cameras.clear();
|
||
lights.clear();
|
||
id = -1;
|
||
subtreedofs = 0;
|
||
|
||
// add elements to lists
|
||
*this += other;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// copy children of other body into body
|
||
mjCBody& mjCBody::operator+=(const mjCBody& other) {
|
||
// map other frames to indices
|
||
std::map<mjCFrame*, int> fmap;
|
||
for (int i=0; i < other.frames.size(); i++) {
|
||
fmap[other.frames[i]] = i + frames.size();
|
||
}
|
||
|
||
// copy frames, needs to happen first
|
||
CopyList(frames, other.frames, fmap);
|
||
|
||
// copy all children
|
||
CopyList(geoms, other.geoms, fmap);
|
||
CopyList(joints, other.joints, fmap);
|
||
CopyList(sites, other.sites, fmap);
|
||
CopyList(cameras, other.cameras, fmap);
|
||
CopyList(lights, other.lights, fmap);
|
||
|
||
for (int i=0; i < other.bodies.size(); i++) {
|
||
bodies.push_back(new mjCBody(*other.bodies[i], model)); // triggers recursive call
|
||
bodies.back()->parent = this;
|
||
bodies.back()->frame = nullptr;
|
||
if (other.bodies[i]->frame) {
|
||
if (fmap.find(other.bodies[i]->frame) != fmap.end()) {
|
||
bodies.back()->frame = frames[fmap[other.bodies[i]->frame]];
|
||
} else {
|
||
throw mjCError(this, "Frame '%s' not found in other body",
|
||
other.bodies[i]->frame->name.c_str());
|
||
}
|
||
if (bodies.back()->frame && bodies.back()->frame->body != this) {
|
||
throw mjCError(this, "Frame and body '%s' have mismatched parents", name.c_str());
|
||
}
|
||
}
|
||
}
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// attach frame to body
|
||
mjCBody& mjCBody::operator+=(const mjCFrame& other) {
|
||
// append a copy of the attached spec
|
||
if (other.model != model && !model->FindSpec(&other.model->spec.compiler)) {
|
||
model->AppendSpec(mj_copySpec(&other.model->spec), &other.model->spec.compiler);
|
||
}
|
||
|
||
// create a copy of the subtree that contains the frame
|
||
mjCBody* subtree = other.body;
|
||
other.model->prefix = other.prefix;
|
||
other.model->suffix = other.suffix;
|
||
other.model->StoreKeyframes(model);
|
||
mjCModel* other_model = other.model;
|
||
|
||
// attach defaults
|
||
if (other_model != model) {
|
||
mjCDef* subdef = new mjCDef(*other_model->Default());
|
||
subdef->NameSpace(other_model);
|
||
*model += *subdef;
|
||
}
|
||
|
||
// copy input frame
|
||
mjSpec* origin = model->FindSpec(other.compiler);
|
||
mjCFrame* newframe(model->deepcopy_ ? new mjCFrame(other) : (mjCFrame*)&other);
|
||
frames.push_back(newframe);
|
||
frames.back()->body = this;
|
||
frames.back()->model = model;
|
||
frames.back()->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
frames.back()->frame = other.frame;
|
||
if (model->deepcopy_) {
|
||
frames.back()->NameSpace(other_model);
|
||
} else {
|
||
frames.back()->AddRef();
|
||
}
|
||
int i = frames.size();
|
||
last_attached = &frames.back()->spec;
|
||
|
||
// map input frames to index in this->frames
|
||
std::map<mjCFrame*, int> fmap;
|
||
for (auto frame : subtree->frames) {
|
||
if (frame == static_cast<const mjCFrame*>(&other)) {
|
||
fmap[frame] = frames.size() - 1;
|
||
} else if (other.IsAncestor(frame)) {
|
||
fmap[frame] = i++;
|
||
}
|
||
}
|
||
|
||
// copy children that are inside the input frame
|
||
CopyList(frames, subtree->frames, fmap, &other); // needs to be done first
|
||
CopyList(geoms, subtree->geoms, fmap, &other);
|
||
CopyList(joints, subtree->joints, fmap, &other);
|
||
CopyList(sites, subtree->sites, fmap, &other);
|
||
CopyList(cameras, subtree->cameras, fmap, &other);
|
||
CopyList(lights, subtree->lights, fmap, &other);
|
||
|
||
if (!model->deepcopy_) {
|
||
std::string name = subtree->name;
|
||
subtree->SetModel(model);
|
||
subtree->NameSpace(other_model);
|
||
subtree->name = name;
|
||
}
|
||
|
||
int nbodies = (int)subtree->bodies.size();
|
||
for (int i=0; i < nbodies; i++) {
|
||
if (!other.IsAncestor(subtree->bodies[i]->frame)) {
|
||
continue;
|
||
}
|
||
if (model->deepcopy_) {
|
||
mjCBody* newbody(new mjCBody(*subtree->bodies[i], model)); // triggers recursive call
|
||
bodies.push_back(newbody);
|
||
subtree->bodies[i]->ForgetKeyframes();
|
||
bodies.back()->NameSpace_(other_model, /*propagate=*/ false);
|
||
} else {
|
||
bodies.push_back(subtree->bodies[i]);
|
||
bodies.back()->SetModel(model);
|
||
bodies.back()->ResetId();
|
||
bodies.back()->AddRef();
|
||
}
|
||
bodies.back()->parent = this;
|
||
bodies.back()->frame =
|
||
subtree->bodies[i]->frame ? frames[fmap[subtree->bodies[i]->frame]] : nullptr;
|
||
}
|
||
|
||
// attach referencing elements
|
||
other_model->SetAttached(model->deepcopy_);
|
||
*model += *other_model;
|
||
|
||
// leave the source model in a clean state
|
||
if (other_model != model) {
|
||
other_model->key_pending_.clear();
|
||
}
|
||
|
||
// clear namespace and return body
|
||
other_model->prefix.clear();
|
||
other_model->suffix.clear();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// copy src list of elements into dst; set body, model and frame
|
||
template <typename T>
|
||
void mjCBody::CopyList(std::vector<T*>& dst, const std::vector<T*>& src,
|
||
std::map<mjCFrame*, int>& fmap, const mjCFrame* pframe) {
|
||
int nsrc = (int)src.size();
|
||
int ndst = (int)dst.size();
|
||
for (int i=0; i < nsrc; i++) {
|
||
if (pframe && !pframe->IsAncestor(src[i]->frame)) {
|
||
continue; // skip if the element is not inside pframe
|
||
}
|
||
mjSpec* origin = model->FindSpec(src[i]->compiler);
|
||
T* new_obj = model->deepcopy_ ? new T(*src[i]) : src[i];
|
||
dst.push_back(new_obj);
|
||
dst.back()->body = this;
|
||
dst.back()->model = model;
|
||
dst.back()->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
dst.back()->id = -1;
|
||
dst.back()->CopyPlugin();
|
||
dst.back()->classname = src[i]->classname;
|
||
|
||
// increment refcount if shallow copy is made
|
||
if (!model->deepcopy_) {
|
||
dst.back()->AddRef();
|
||
}
|
||
|
||
// set namespace
|
||
dst.back()->NameSpace(src[i]->model);
|
||
}
|
||
|
||
// assign dst frame to src frame
|
||
// needs to be done after the copy in case T is an mjCFrame
|
||
int j = 0;
|
||
for (int i = 0; i < src.size(); i++) {
|
||
if (pframe && !pframe->IsAncestor(src[i]->frame)) {
|
||
continue; // skip if the element is not inside pframe
|
||
}
|
||
dst[ndst + j++]->frame = src[i]->frame ? frames[fmap[src[i]->frame]] : nullptr;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// find and remove subtree
|
||
mjCBody& mjCBody::operator-=(const mjCBody& subtree) {
|
||
for (int i=0; i < bodies.size(); i++) {
|
||
if (bodies[i] == &subtree) {
|
||
bodies.erase(bodies.begin() + i);
|
||
break;
|
||
}
|
||
*bodies[i] -= subtree;
|
||
}
|
||
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// set model of this body and its subtree
|
||
void mjCBody::SetModel(mjCModel* _model) {
|
||
model = _model;
|
||
mjSpec* origin = _model->FindSpec(compiler);
|
||
compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
|
||
for (auto& body : bodies) {
|
||
body->SetModel(_model);
|
||
}
|
||
for (auto& frame : frames) {
|
||
origin = _model->FindSpec(frame->compiler);
|
||
frame->model = _model;
|
||
frame->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
for (auto& geom : geoms) {
|
||
origin = _model->FindSpec(geom->compiler);
|
||
geom->model = _model;
|
||
geom->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
for (auto& joint : joints) {
|
||
origin = _model->FindSpec(joint->compiler);
|
||
joint->model = _model;
|
||
joint->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
for (auto& site : sites) {
|
||
origin = _model->FindSpec(site->compiler);
|
||
site->model = _model;
|
||
site->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
for (auto& camera : cameras) {
|
||
origin = _model->FindSpec(camera->compiler);
|
||
camera->model = _model;
|
||
camera->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
for (auto& light : lights) {
|
||
origin = _model->FindSpec(light->compiler);
|
||
light->model = _model;
|
||
light->compiler = origin ? &origin->compiler : &model->spec.compiler;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// reset ids of all objects in this body
|
||
void mjCBody::ResetId() {
|
||
id = -1;
|
||
for (auto& body : bodies) {
|
||
body->ResetId();
|
||
}
|
||
for (auto& frame : frames) {
|
||
frame->id = -1;
|
||
}
|
||
for (auto& geom : geoms) {
|
||
geom->id = -1;
|
||
}
|
||
for (auto& joint : joints) {
|
||
joint->id = -1;
|
||
joint->qposadr_ = -1;
|
||
joint->dofadr_ = -1;
|
||
}
|
||
for (auto& site : sites) {
|
||
site->id = -1;
|
||
}
|
||
for (auto& camera : cameras) {
|
||
camera->id = -1;
|
||
}
|
||
for (auto& light : lights) {
|
||
light->id = -1;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCBody::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.childclass = &classname;
|
||
spec.userdata = &spec_userdata_;
|
||
spec.plugin.plugin_name = &plugin_name;
|
||
spec.plugin.name = (&plugin_instance_name);
|
||
spec.info = &info;
|
||
userdata = nullptr;
|
||
}
|
||
|
||
|
||
void mjCBody::CopyFromSpec() {
|
||
*static_cast<mjsBody*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
plugin.active = spec.plugin.active;
|
||
plugin.element = spec.plugin.element;
|
||
plugin.plugin_name = spec.plugin.plugin_name;
|
||
plugin.name = spec.plugin.name;
|
||
}
|
||
|
||
|
||
|
||
void mjCBody::CopyPlugin() {
|
||
model->CopyExplicitPlugin(this);
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCBody::~mjCBody() {
|
||
for (int i=0; i < bodies.size(); i++) bodies[i]->Release();
|
||
for (int i=0; i < geoms.size(); i++) geoms[i]->Release();
|
||
for (int i=0; i < frames.size(); i++) frames[i]->Release();
|
||
for (int i=0; i < joints.size(); i++) joints[i]->Release();
|
||
for (int i=0; i < sites.size(); i++) sites[i]->Release();
|
||
for (int i=0; i < cameras.size(); i++) cameras[i]->Release();
|
||
for (int i=0; i < lights.size(); i++) lights[i]->Release();
|
||
}
|
||
|
||
|
||
|
||
// apply prefix and suffix, propagate to children
|
||
void mjCBody::NameSpace(const mjCModel* m) {
|
||
NameSpace_(m, true);
|
||
}
|
||
|
||
|
||
|
||
// apply prefix and suffix, propagate to all descendants or only to child bodies
|
||
void mjCBody::NameSpace_(const mjCModel* m, bool propagate) {
|
||
mjCBase::NameSpace(m);
|
||
if (!plugin_instance_name.empty()) {
|
||
plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
|
||
}
|
||
|
||
for (auto& body : bodies) {
|
||
body->prefix = m->prefix;
|
||
body->suffix = m->suffix;
|
||
body->NameSpace_(m, propagate);
|
||
}
|
||
|
||
if (!propagate) {
|
||
return;
|
||
}
|
||
|
||
for (auto& joint : joints) {
|
||
joint->NameSpace(m);
|
||
}
|
||
|
||
for (auto& geom : geoms) {
|
||
geom->NameSpace(m);
|
||
}
|
||
|
||
for (auto& site : sites) {
|
||
site->NameSpace(m);
|
||
}
|
||
|
||
for (auto& camera : cameras) {
|
||
camera->NameSpace(m);
|
||
}
|
||
|
||
for (auto& light : lights) {
|
||
light->NameSpace(m);
|
||
}
|
||
|
||
for (auto& frame : frames) {
|
||
frame->NameSpace(m);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// create child body and add it to body
|
||
mjCBody* mjCBody::AddBody(mjCDef* _def) {
|
||
// create body
|
||
mjCBody* obj = new mjCBody(model);
|
||
|
||
// handle def recursion (i.e. childclass)
|
||
obj->classname = _def ? _def->name : classname;
|
||
|
||
bodies.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
obj->parent = this;
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new frame and add it to body
|
||
mjCFrame* mjCBody::AddFrame(mjCFrame* _frame) {
|
||
mjCFrame* obj = new mjCFrame(model, _frame ? _frame : NULL);
|
||
frames.push_back(obj);
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new free joint (no default inheritance) and add it to body
|
||
mjCJoint* mjCBody::AddFreeJoint() {
|
||
// create free joint, don't inherit from defaults
|
||
mjCJoint* obj = new mjCJoint(model, NULL);
|
||
obj->spec.type = mjJNT_FREE;
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
joints.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new joint and add it to body
|
||
mjCJoint* mjCBody::AddJoint(mjCDef* _def) {
|
||
// create joint
|
||
mjCJoint* obj = new mjCJoint(model, _def ? _def : model->def_map[classname]);
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
joints.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new geom and add it to body
|
||
mjCGeom* mjCBody::AddGeom(mjCDef* _def) {
|
||
// create geom
|
||
mjCGeom* obj = new mjCGeom(model, _def ? _def : model->def_map[classname]);
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
geoms.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new site and add it to body
|
||
mjCSite* mjCBody::AddSite(mjCDef* _def) {
|
||
// create site
|
||
mjCSite* obj = new mjCSite(model, _def ? _def : model->def_map[classname]);
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
sites.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new camera and add it to body
|
||
mjCCamera* mjCBody::AddCamera(mjCDef* _def) {
|
||
// create camera
|
||
mjCCamera* obj = new mjCCamera(model, _def ? _def : model->def_map[classname]);
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
cameras.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create new light and add it to body
|
||
mjCLight* mjCBody::AddLight(mjCDef* _def) {
|
||
// create light
|
||
mjCLight* obj = new mjCLight(model, _def ? _def : model->def_map[classname]);
|
||
|
||
// set body pointer, add
|
||
obj->body = this;
|
||
|
||
lights.push_back(obj);
|
||
|
||
// recompute lists
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
|
||
// update signature
|
||
model->spec.element->signature = model->Signature();
|
||
return obj;
|
||
}
|
||
|
||
|
||
|
||
// create a frame in the parent body and move all contents of this body into it
|
||
mjCFrame* mjCBody::ToFrame() {
|
||
mjCFrame* newframe = parent->AddFrame(frame);
|
||
mjuu_copyvec(newframe->spec.pos, spec.pos, 3);
|
||
mjuu_copyvec(newframe->spec.quat, spec.quat, 4);
|
||
if (parent->name != "world" && mass >= mjMINVAL) {
|
||
if (!parent->explicitinertial) {
|
||
parent->MakeInertialExplicit();
|
||
mjuu_zerovec(parent->spec.ipos, 3);
|
||
mjuu_zerovec(parent->spec.iquat, 4);
|
||
mjuu_zerovec(parent->spec.inertia, 3);
|
||
}
|
||
parent->AccumulateInertia(&this->spec, &parent->spec);
|
||
}
|
||
MapFrame(parent->bodies, bodies, newframe, parent);
|
||
MapFrame(parent->geoms, geoms, newframe, parent);
|
||
MapFrame(parent->joints, joints, newframe, parent);
|
||
MapFrame(parent->sites, sites, newframe, parent);
|
||
MapFrame(parent->cameras, cameras, newframe, parent);
|
||
MapFrame(parent->lights, lights, newframe, parent);
|
||
MapFrame(parent->frames, frames, newframe, parent);
|
||
parent->bodies.erase(
|
||
std::remove_if(parent->bodies.begin(), parent->bodies.end(),
|
||
[this](mjCBody* body) { return body == this; }),
|
||
parent->bodies.end());
|
||
model->ResetTreeLists();
|
||
model->MakeTreeLists();
|
||
model->spec.element->signature = model->Signature();
|
||
return newframe;
|
||
}
|
||
|
||
|
||
|
||
// get number of objects of specified type
|
||
int mjCBody::NumObjects(mjtObj type) {
|
||
switch (type) {
|
||
case mjOBJ_BODY:
|
||
case mjOBJ_XBODY:
|
||
return (int)bodies.size();
|
||
case mjOBJ_JOINT:
|
||
return (int)joints.size();
|
||
case mjOBJ_GEOM:
|
||
return (int)geoms.size();
|
||
case mjOBJ_SITE:
|
||
return (int)sites.size();
|
||
case mjOBJ_CAMERA:
|
||
return (int)cameras.size();
|
||
case mjOBJ_LIGHT:
|
||
return (int)lights.size();
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// get poiner to specified object
|
||
mjCBase* mjCBody::GetObject(mjtObj type, int i) {
|
||
if (i >= 0 && i < NumObjects(type)) {
|
||
switch (type) {
|
||
case mjOBJ_BODY:
|
||
case mjOBJ_XBODY:
|
||
return bodies[i];
|
||
case mjOBJ_JOINT:
|
||
return joints[i];
|
||
case mjOBJ_GEOM:
|
||
return geoms[i];
|
||
case mjOBJ_SITE:
|
||
return sites[i];
|
||
case mjOBJ_CAMERA:
|
||
return cameras[i];
|
||
case mjOBJ_LIGHT:
|
||
return lights[i];
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
// find object by name in given list
|
||
template <class T>
|
||
static T* findobject(std::string name, std::vector<T*>& list) {
|
||
for (unsigned int i=0; i < list.size(); i++) {
|
||
if (list[i]->name == name) {
|
||
return list[i];
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
// recursive find by name
|
||
mjCBase* mjCBody::FindObject(mjtObj type, std::string _name, bool recursive) {
|
||
mjCBase* res = 0;
|
||
|
||
// check self: just in case
|
||
if (name == _name) {
|
||
return this;
|
||
}
|
||
|
||
// search elements of this body
|
||
if (type == mjOBJ_BODY || type == mjOBJ_XBODY) {
|
||
res = findobject(_name, bodies);
|
||
} else if (type == mjOBJ_JOINT) {
|
||
res = findobject(_name, joints);
|
||
} else if (type == mjOBJ_GEOM) {
|
||
res = findobject(_name, geoms);
|
||
} else if (type == mjOBJ_SITE) {
|
||
res = findobject(_name, sites);
|
||
} else if (type == mjOBJ_CAMERA) {
|
||
res = findobject(_name, cameras);
|
||
} else if (type == mjOBJ_LIGHT) {
|
||
res = findobject(_name, lights);
|
||
}
|
||
|
||
// found
|
||
if (res) {
|
||
return res;
|
||
}
|
||
|
||
// search children
|
||
if (recursive) {
|
||
for (int i=0; i < (int)bodies.size(); i++) {
|
||
if ((res = bodies[i]->FindObject(type, _name, true))) {
|
||
return res;
|
||
}
|
||
}
|
||
}
|
||
|
||
// not found
|
||
return res;
|
||
}
|
||
|
||
|
||
|
||
// get list of a given type
|
||
template<>
|
||
const std::vector<mjCBody*>& mjCBody::GetList<mjCBody>() const {
|
||
return bodies;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCJoint*>& mjCBody::GetList<mjCJoint>() const {
|
||
return joints;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCGeom*>& mjCBody::GetList<mjCGeom>() const {
|
||
return geoms;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCSite*>& mjCBody::GetList<mjCSite>() const {
|
||
return sites;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCCamera*>& mjCBody::GetList<mjCCamera>() const {
|
||
return cameras;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCLight*>& mjCBody::GetList<mjCLight>() const {
|
||
return lights;
|
||
}
|
||
|
||
template<>
|
||
const std::vector<mjCFrame*>& mjCBody::GetList<mjCFrame>() const {
|
||
return frames;
|
||
}
|
||
|
||
|
||
|
||
// gets next child of the same type, recursively depth first if requested
|
||
template <class T>
|
||
static mjsElement* GetNext(const mjCBody* body, const mjsElement* child,
|
||
bool* found, bool recursive) {
|
||
std::vector<T*> list = body->GetList<T>();
|
||
|
||
for (unsigned int i = 0; i < list.size(); i++) {
|
||
if (*found) {
|
||
return list[i]->spec.element;
|
||
}
|
||
|
||
if (list[i]->spec.element == child) {
|
||
*found = true;
|
||
}
|
||
}
|
||
|
||
if (!recursive) {
|
||
return nullptr;
|
||
}
|
||
|
||
for (auto& other : body->Bodies()) {
|
||
mjsElement* candidate = GetNext<T>(other, child, found, true);
|
||
if (candidate) {
|
||
return candidate;
|
||
}
|
||
}
|
||
|
||
return nullptr;
|
||
}
|
||
|
||
|
||
|
||
// get next body depth first
|
||
static mjsElement* GetNextBody(const mjCBody* body, const mjsElement* child,
|
||
bool* found, bool recursive) {
|
||
for (auto& other : body->Bodies()) {
|
||
if (*found) {
|
||
return other->spec.element;
|
||
}
|
||
|
||
if (other->spec.element == child) {
|
||
*found = true;
|
||
}
|
||
|
||
if (!recursive) {
|
||
continue;
|
||
}
|
||
|
||
mjsElement* candidate = GetNextBody(other, child, found, true);
|
||
if (candidate) {
|
||
return candidate;
|
||
}
|
||
}
|
||
|
||
return nullptr;
|
||
}
|
||
|
||
|
||
|
||
|
||
// get next child of given type
|
||
mjsElement* mjCBody::NextChild(const mjsElement* child, mjtObj type, bool recursive) {
|
||
if (type == mjOBJ_UNKNOWN) {
|
||
if (!child) {
|
||
throw mjCError(this, "child type must be specified if no child element is given");
|
||
} else {
|
||
type = child->elemtype;
|
||
}
|
||
} else if (child && child->elemtype != type) {
|
||
throw mjCError(this, "child element is not of requested type");
|
||
}
|
||
|
||
mjsElement* candidate = nullptr;
|
||
bool found = child == nullptr;
|
||
switch (type) {
|
||
case mjOBJ_BODY:
|
||
case mjOBJ_XBODY:
|
||
candidate = GetNextBody(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_JOINT:
|
||
candidate = GetNext<mjCJoint>(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_GEOM:
|
||
candidate = GetNext<mjCGeom>(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_SITE:
|
||
candidate = GetNext<mjCSite>(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_CAMERA:
|
||
candidate = GetNext<mjCCamera>(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_LIGHT:
|
||
candidate = GetNext<mjCLight>(this, child, &found, recursive);
|
||
break;
|
||
case mjOBJ_FRAME:
|
||
candidate = GetNext<mjCFrame>(this, child, &found, recursive);
|
||
break;
|
||
default:
|
||
throw mjCError(this,
|
||
"Body.NextChild supports the types: body, frame, geom, "
|
||
"site, light, camera");
|
||
break;
|
||
}
|
||
|
||
return candidate;
|
||
}
|
||
|
||
|
||
|
||
// compute geom inertial frame: ipos, iquat, mass, inertia
|
||
void mjCBody::InertiaFromGeom(void) {
|
||
int sz;
|
||
double com[3] = {0, 0, 0};
|
||
double toti[6] = {0, 0, 0, 0, 0, 0};
|
||
std::vector<mjCGeom*> sel;
|
||
|
||
// select geoms based on group, ignore tiny masses
|
||
sel.clear();
|
||
for (int i=0; i < geoms.size(); i++) {
|
||
if (geoms[i]->group >= compiler->inertiagrouprange[0] &&
|
||
geoms[i]->group <= compiler->inertiagrouprange[1] &&
|
||
geoms[i]->mass_ > mjEPS) {
|
||
sel.push_back(geoms[i]);
|
||
}
|
||
}
|
||
sz = sel.size();
|
||
|
||
// single geom: copy
|
||
if (sz == 1) {
|
||
mjuu_copyvec(ipos, sel[0]->pos, 3);
|
||
mjuu_copyvec(iquat, sel[0]->quat, 4);
|
||
mass = sel[0]->mass_;
|
||
mjuu_copyvec(inertia, sel[0]->inertia, 3);
|
||
}
|
||
|
||
// multiple geoms
|
||
else if (sz > 1) {
|
||
// compute total mass and center of mass
|
||
mass = 0;
|
||
for (int i=0; i < sz; i++) {
|
||
mass += sel[i]->mass_;
|
||
com[0] += sel[i]->mass_ * sel[i]->pos[0];
|
||
com[1] += sel[i]->mass_ * sel[i]->pos[1];
|
||
com[2] += sel[i]->mass_ * sel[i]->pos[2];
|
||
}
|
||
|
||
// check for small mass
|
||
if (mass < mjEPS) {
|
||
throw mjCError(this, "body mass is too small, cannot compute center of mass");
|
||
}
|
||
|
||
// ipos = geom com
|
||
ipos[0] = com[0]/mass;
|
||
ipos[1] = com[1]/mass;
|
||
ipos[2] = com[2]/mass;
|
||
|
||
// add geom inertias
|
||
for (int i=0; i < sz; i++) {
|
||
double inert0[6], inert1[6];
|
||
double dpos[3] = {
|
||
sel[i]->pos[0] - ipos[0],
|
||
sel[i]->pos[1] - ipos[1],
|
||
sel[i]->pos[2] - ipos[2]
|
||
};
|
||
|
||
mjuu_globalinertia(inert0, sel[i]->inertia, sel[i]->quat);
|
||
mjuu_offcenter(inert1, sel[i]->mass_, dpos);
|
||
for (int j=0; j < 6; j++) {
|
||
toti[j] = toti[j] + inert0[j] + inert1[j];
|
||
}
|
||
}
|
||
|
||
// compute principal axes of inertia
|
||
mjuu_copyvec(fullinertia, toti, 6);
|
||
const char* errq = mjuu_fullInertia(iquat, inertia, fullinertia);
|
||
if (errq) {
|
||
throw mjCError(this, "error '%s' in alternative for principal axes", errq);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// set explicitinertial to true
|
||
void mjCBody::MakeInertialExplicit() {
|
||
spec.explicitinertial = true;
|
||
}
|
||
|
||
|
||
|
||
// accumulate inertia of another body into this body
|
||
void mjCBody::AccumulateInertia(const mjsBody* other, mjsBody* result) {
|
||
if (!result) {
|
||
result = this; // use the private mjsBody
|
||
}
|
||
|
||
// body_ipose = body_pose * body_ipose
|
||
double other_ipos[3];
|
||
double other_iquat[4];
|
||
mjuu_copyvec(other_ipos, other->ipos, 3);
|
||
mjuu_copyvec(other_iquat, other->iquat, 4);
|
||
mjuu_frameaccum(other_ipos, other_iquat, other->pos, other->quat);
|
||
|
||
// organize data
|
||
double mass[2] = {
|
||
result->mass,
|
||
other->mass
|
||
};
|
||
double inertia[2][3] = {
|
||
{result->inertia[0], result->inertia[1], result->inertia[2]},
|
||
{other->inertia[0], other->inertia[1], other->inertia[2]}
|
||
};
|
||
double ipos[2][3] = {
|
||
{result->ipos[0], result->ipos[1], result->ipos[2]},
|
||
{other_ipos[0], other_ipos[1], other_ipos[2]}
|
||
};
|
||
double iquat[2][4] = {
|
||
{result->iquat[0], result->iquat[1], result->iquat[2], result->iquat[3]},
|
||
{other->iquat[0], other->iquat[1], other->iquat[2], other->iquat[3]}
|
||
};
|
||
|
||
// compute total mass
|
||
result->mass = 0;
|
||
mjuu_setvec(result->ipos, 0, 0, 0);
|
||
for (int j=0; j < 2; j++) {
|
||
result->mass += mass[j];
|
||
result->ipos[0] += mass[j]*ipos[j][0];
|
||
result->ipos[1] += mass[j]*ipos[j][1];
|
||
result->ipos[2] += mass[j]*ipos[j][2];
|
||
}
|
||
|
||
// small mass: allow for now, check for errors later
|
||
if (result->mass < mjMINVAL) {
|
||
result->mass = 0;
|
||
mjuu_setvec(result->inertia, 0, 0, 0);
|
||
mjuu_setvec(result->ipos, 0, 0, 0);
|
||
mjuu_setvec(result->iquat, 1, 0, 0, 0);
|
||
}
|
||
|
||
// proceed with regular computation
|
||
else {
|
||
// locipos = center-of-mass
|
||
result->ipos[0] /= result->mass;
|
||
result->ipos[1] /= result->mass;
|
||
result->ipos[2] /= result->mass;
|
||
|
||
// add inertias
|
||
double toti[6] = {0, 0, 0, 0, 0, 0};
|
||
for (int j=0; j < 2; j++) {
|
||
double inertA[6], inertB[6];
|
||
double dpos[3] = {
|
||
ipos[j][0] - result->ipos[0],
|
||
ipos[j][1] - result->ipos[1],
|
||
ipos[j][2] - result->ipos[2]
|
||
};
|
||
|
||
mjuu_globalinertia(inertA, inertia[j], iquat[j]);
|
||
mjuu_offcenter(inertB, mass[j], dpos);
|
||
for (int k=0; k < 6; k++) {
|
||
toti[k] += inertA[k] + inertB[k];
|
||
}
|
||
}
|
||
|
||
// compute principal axes of inertia
|
||
mjuu_copyvec(result->fullinertia, toti, 6);
|
||
const char* err1 = mjuu_fullInertia(result->iquat, result->inertia, result->fullinertia);
|
||
if (err1) {
|
||
throw mjCError(nullptr, "error '%s' in fusing static body inertias", err1);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compute bounding volume hierarchy
|
||
void mjCBody::ComputeBVH() {
|
||
if (geoms.empty()) {
|
||
return;
|
||
}
|
||
|
||
tree.Set(ipos, iquat);
|
||
tree.AllocateBoundingVolumes(geoms.size());
|
||
for (const mjCGeom* geom : geoms) {
|
||
tree.AddBoundingVolume(&geom->id, geom->contype, geom->conaffinity,
|
||
geom->pos, geom->quat, geom->aabb);
|
||
}
|
||
tree.CreateBVH();
|
||
}
|
||
|
||
|
||
|
||
// reset keyframe references for allowing self-attach
|
||
void mjCBody::ForgetKeyframes() const {
|
||
for (auto joint : joints) {
|
||
joint->qpos_.clear();
|
||
joint->qvel_.clear();
|
||
}
|
||
((mjCBody*)this)->mpos_.clear();
|
||
((mjCBody*)this)->mquat_.clear();
|
||
for (auto body : bodies) {
|
||
body->ForgetKeyframes();
|
||
}
|
||
}
|
||
|
||
|
||
|
||
mjtNum* mjCBody::mpos(const std::string& state_name) {
|
||
if (mpos_.find(state_name) == mpos_.end()) {
|
||
mpos_[state_name] = {mjNAN, 0, 0};
|
||
}
|
||
return mpos_.at(state_name).data();
|
||
}
|
||
|
||
|
||
|
||
mjtNum* mjCBody::mquat(const std::string& state_name) {
|
||
if (mquat_.find(state_name) == mquat_.end()) {
|
||
mquat_[state_name] = {mjNAN, 0, 0, 0};
|
||
}
|
||
return mquat_.at(state_name).data();
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCBody::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// compile all frames
|
||
for (int i=0; i < frames.size(); i++) {
|
||
frames[i]->Compile();
|
||
}
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_body) {
|
||
throw mjCError(this, "user has more values than nuser_body in body");
|
||
}
|
||
userdata_.resize(model->nuser_body);
|
||
|
||
// normalize user-defined quaternions
|
||
mjuu_normvec(quat, 4);
|
||
mjuu_normvec(iquat, 4);
|
||
|
||
// set parentid and weldid of children
|
||
for (int i=0; i < bodies.size(); i++) {
|
||
bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid);
|
||
}
|
||
|
||
// check and process orientation alternatives for body
|
||
if (alt.type != mjORIENTATION_QUAT) {
|
||
const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt);
|
||
if (err) {
|
||
throw mjCError(this, "error '%s' in frame alternative", err);
|
||
}
|
||
}
|
||
|
||
// check orientation alternatives for inertia
|
||
if (mjuu_defined(fullinertia[0]) && ialt.type != mjORIENTATION_QUAT) {
|
||
throw mjCError(this, "fullinertia and inertial orientation cannot both be specified");
|
||
}
|
||
if (mjuu_defined(fullinertia[0]) && (inertia[0] || inertia[1] || inertia[2])) {
|
||
throw mjCError(this, "fullinertia and diagonal inertia cannot both be specified");
|
||
}
|
||
|
||
// process orientation alternatives for inertia
|
||
if (mjuu_defined(fullinertia[0])) {
|
||
const char* err = mjuu_fullInertia(iquat, inertia, this->fullinertia);
|
||
if (err) {
|
||
throw mjCError(this, "error '%s' in fullinertia", err);
|
||
}
|
||
}
|
||
|
||
if (ialt.type != mjORIENTATION_QUAT) {
|
||
const char* err = ResolveOrientation(iquat, compiler->degree, compiler->eulerseq, ialt);
|
||
if (err) {
|
||
throw mjCError(this, "error '%s' in inertia alternative", err);
|
||
}
|
||
}
|
||
|
||
// compile all geoms
|
||
for (int i=0; i < geoms.size(); i++) {
|
||
geoms[i]->inferinertia = id > 0 &&
|
||
(!explicitinertial || compiler->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
|
||
geoms[i]->spec.group >= compiler->inertiagrouprange[0] &&
|
||
geoms[i]->spec.group <= compiler->inertiagrouprange[1];
|
||
geoms[i]->Compile();
|
||
}
|
||
|
||
// set inertial frame from geoms if necessary
|
||
if (id > 0 && (compiler->inertiafromgeom == mjINERTIAFROMGEOM_TRUE ||
|
||
(!mjuu_defined(ipos[0]) && compiler->inertiafromgeom == mjINERTIAFROMGEOM_AUTO))) {
|
||
InertiaFromGeom();
|
||
}
|
||
|
||
// ipos undefined: copy body frame into inertial
|
||
if (!mjuu_defined(ipos[0])) {
|
||
mjuu_copyvec(ipos, pos, 3);
|
||
mjuu_copyvec(iquat, quat, 4);
|
||
}
|
||
|
||
// check and correct mass and inertia
|
||
if (id > 0) {
|
||
// fix minimum
|
||
mass = std::max(mass, compiler->boundmass);
|
||
inertia[0] = std::max(inertia[0], compiler->boundinertia);
|
||
inertia[1] = std::max(inertia[1], compiler->boundinertia);
|
||
inertia[2] = std::max(inertia[2], compiler->boundinertia);
|
||
|
||
// check for negative values
|
||
if (mass < 0 || inertia[0] < 0 || inertia[1] < 0 ||inertia[2] < 0) {
|
||
throw mjCError(this, "mass and inertia cannot be negative");
|
||
}
|
||
|
||
// check for non-physical inertia
|
||
if (inertia[0] + inertia[1] < inertia[2] ||
|
||
inertia[0] + inertia[2] < inertia[1] ||
|
||
inertia[1] + inertia[2] < inertia[0]) {
|
||
if (compiler->balanceinertia) {
|
||
inertia[0] = inertia[1] = inertia[2] = (inertia[0] + inertia[1] + inertia[2])/3.0;
|
||
} else {
|
||
throw mjCError(this, "inertia must satisfy A + B >= C; use 'balanceinertia' to fix");
|
||
}
|
||
}
|
||
}
|
||
|
||
// frame
|
||
if (frame) {
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
|
||
}
|
||
|
||
// accumulate rbound, contype, conaffinity over geoms
|
||
contype = conaffinity = 0;
|
||
margin = 0;
|
||
for (int i=0; i < geoms.size(); i++) {
|
||
contype |= geoms[i]->contype;
|
||
conaffinity |= geoms[i]->conaffinity;
|
||
margin = std::max(margin, geoms[i]->margin);
|
||
}
|
||
|
||
// check conditions for free-joint alignment
|
||
bool align_free = (joints.size() == 1 && // only one joint AND
|
||
joints[0]->spec.type == mjJNT_FREE && // it is a free joint AND
|
||
bodies.empty() && // no child bodies AND
|
||
(joints[0]->spec.align == 1 || // either joint.align="true"
|
||
(joints[0]->spec.align == 2 && // or joint.align="auto"
|
||
compiler->alignfree))); // and compiler->align="true"
|
||
|
||
// free-joint alignment, phase 1 (this body + child geoms)
|
||
double ipos_inverse[3], iquat_inverse[4];
|
||
if (align_free) {
|
||
// accumulate iframe transformation to body frame
|
||
mjuu_frameaccum(pos, quat, ipos, iquat);
|
||
|
||
// compute inverse iframe transformation
|
||
mjuu_frameinvert(ipos_inverse, iquat_inverse, ipos, iquat);
|
||
|
||
// save iframe, set it to null
|
||
mjuu_setvec(ipos, 0, 0, 0);
|
||
mjuu_setvec(iquat, 1, 0, 0, 0);
|
||
|
||
// apply inverse iframe transformation to all child geoms
|
||
for (int i=0; i < geoms.size(); i++) {
|
||
mjuu_frameaccumChild(ipos_inverse, iquat_inverse, geoms[i]->pos, geoms[i]->quat);
|
||
}
|
||
}
|
||
|
||
// compute bounding volume hierarchy
|
||
ComputeBVH();
|
||
|
||
// compile all joints, count dofs
|
||
dofnum = 0;
|
||
for (int i=0; i < joints.size(); i++) {
|
||
dofnum += joints[i]->Compile();
|
||
}
|
||
|
||
// check for excessive number of dofs
|
||
if (dofnum > 6) {
|
||
throw mjCError(this, "more than 6 dofs in body '%s'", name.c_str());
|
||
}
|
||
|
||
// check for rotation dof after ball joint
|
||
bool hasball = false;
|
||
for (int i=0; i < joints.size(); i++) {
|
||
if ((joints[i]->type == mjJNT_BALL || joints[i]->type == mjJNT_HINGE) && hasball) {
|
||
throw mjCError(this, "ball followed by rotation in body '%s'", name.c_str());
|
||
}
|
||
if (joints[i]->type == mjJNT_BALL) {
|
||
hasball = true;
|
||
}
|
||
}
|
||
|
||
// make sure mocap body is fixed child of world
|
||
if (mocap && (dofnum || (parent && parent->name != "world"))) {
|
||
throw mjCError(this, "mocap body '%s' is not a fixed child of world", name.c_str());
|
||
}
|
||
|
||
// compute body global pose (no joint transformations in qpos0)
|
||
if (id > 0) {
|
||
mjuu_rotVecQuat(xpos0, pos, parent->xquat0);
|
||
mjuu_addtovec(xpos0, parent->xpos0, 3);
|
||
mjuu_mulquat(xquat0, parent->xquat0, quat);
|
||
}
|
||
|
||
// compile all sites
|
||
for (int i=0; i < sites.size(); i++)sites[i]->Compile();
|
||
|
||
// compile all cameras
|
||
for (int i=0; i < cameras.size(); i++)cameras[i]->Compile();
|
||
|
||
// compile all lights
|
||
for (int i=0; i < lights.size(); i++)lights[i]->Compile();
|
||
|
||
// plugin
|
||
if (plugin.active) {
|
||
if (plugin_name.empty() && plugin_instance_name.empty()) {
|
||
throw mjCError(
|
||
this, "neither 'plugin' nor 'instance' is specified for body '%s', (id = %d)",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.element);
|
||
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
|
||
plugin.element = plugin_instance;
|
||
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
|
||
if (!(pplugin->capabilityflags & mjPLUGIN_PASSIVE)) {
|
||
throw mjCError(this, "plugin '%s' does not support passive forces", pplugin->name);
|
||
}
|
||
}
|
||
|
||
// free joint alignment, phase 2 (transform sites, cameras and lights)
|
||
if (align_free) {
|
||
// frames have already been compiled and applied to children
|
||
|
||
// sites
|
||
for (int i=0; i < sites.size(); i++) {
|
||
mjuu_frameaccumChild(ipos_inverse, iquat_inverse, sites[i]->pos, sites[i]->quat);
|
||
}
|
||
|
||
// cameras
|
||
for (int i=0; i < cameras.size(); i++) {
|
||
mjuu_frameaccumChild(ipos_inverse, iquat_inverse, cameras[i]->pos, cameras[i]->quat);
|
||
}
|
||
|
||
// lights
|
||
for (int i=0; i < lights.size(); i++) {
|
||
double qunit[4]= {1, 0, 0, 0};
|
||
mjuu_frameaccumChild(ipos_inverse, iquat_inverse, lights[i]->pos, qunit);
|
||
mjuu_rotVecQuat(lights[i]->dir, lights[i]->dir, iquat_inverse);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCFrame implementation -------------------------------------------------
|
||
|
||
// initialize frame
|
||
mjCFrame::mjCFrame(mjCModel* _model, mjCFrame* _frame) {
|
||
mjs_defaultFrame(&spec);
|
||
elemtype = mjOBJ_FRAME;
|
||
compiled = false;
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
body = NULL;
|
||
frame = _frame ? _frame : NULL;
|
||
last_attached = nullptr;
|
||
PointToLocal();
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCFrame::mjCFrame(const mjCFrame& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCFrame& mjCFrame::operator=(const mjCFrame& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCFrame_*>(this) = static_cast<const mjCFrame_&>(other);
|
||
*static_cast<mjsFrame*>(this) = static_cast<const mjsFrame&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// attach body to frame
|
||
mjCFrame& mjCFrame::operator+=(const mjCBody& other) {
|
||
// append a copy of the attached spec
|
||
if (other.model != model && !model->FindSpec(&other.model->spec.compiler)) {
|
||
model->AppendSpec(mj_copySpec(&other.model->spec), &other.model->spec.compiler);
|
||
}
|
||
|
||
// apply namespace and store keyframes in the source model
|
||
other.model->prefix = other.prefix;
|
||
other.model->suffix = other.suffix;
|
||
other.model->StoreKeyframes(model);
|
||
other.model->prefix = "";
|
||
other.model->suffix = "";
|
||
mjCModel* other_model = other.model;
|
||
|
||
// attach or copy the subtree
|
||
mjCBody* subtree = model->deepcopy_ ? new mjCBody(other, model) : (mjCBody*)&other;
|
||
if (model->deepcopy_) {
|
||
other.ForgetKeyframes();
|
||
} else {
|
||
subtree->SetModel(model);
|
||
subtree->ResetId();
|
||
subtree->AddRef();
|
||
}
|
||
other_model->prefix = subtree->prefix;
|
||
other_model->suffix = subtree->suffix;
|
||
subtree->SetParent(body);
|
||
subtree->SetFrame(this);
|
||
subtree->NameSpace(other_model);
|
||
|
||
// attach defaults
|
||
if (other_model != model) {
|
||
mjCDef* subdef = new mjCDef(*other_model->Default());
|
||
subdef->NameSpace(other_model);
|
||
*model += *subdef;
|
||
}
|
||
|
||
// add to body children
|
||
body->bodies.push_back(subtree);
|
||
last_attached = &body->bodies.back()->spec;
|
||
|
||
// attach referencing elements
|
||
other_model->SetAttached(model->deepcopy_);
|
||
*model += *other_model;
|
||
|
||
// leave the source model in a clean state
|
||
if (other_model != model) {
|
||
other_model->key_pending_.clear();
|
||
}
|
||
|
||
// clear suffixes and return
|
||
other_model->suffix.clear();
|
||
other_model->prefix.clear();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// return true if child is descendent of this frame
|
||
bool mjCFrame::IsAncestor(const mjCFrame* child) const {
|
||
if (!child) {
|
||
return false;
|
||
}
|
||
|
||
if (child == this) {
|
||
return true;
|
||
}
|
||
|
||
return IsAncestor(child->frame);
|
||
}
|
||
|
||
|
||
|
||
void mjCFrame::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.childclass = &classname;
|
||
spec.info = &info;
|
||
}
|
||
|
||
|
||
|
||
void mjCFrame::CopyFromSpec() {
|
||
*static_cast<mjsFrame*>(this) = spec;
|
||
mjuu_copyvec(pos, spec.pos, 3);
|
||
mjuu_copyvec(quat, spec.quat, 4);
|
||
}
|
||
|
||
|
||
|
||
void mjCFrame::Compile() {
|
||
if (compiled) {
|
||
return;
|
||
}
|
||
|
||
CopyFromSpec();
|
||
const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt);
|
||
if (err) {
|
||
throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
|
||
}
|
||
|
||
// compile parents and accumulate result
|
||
if (frame) {
|
||
frame->Compile();
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
|
||
}
|
||
|
||
mjuu_normvec(quat, 4);
|
||
compiled = true;
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCJoint implementation -------------------------------------------------
|
||
|
||
// initialize default joint
|
||
mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultJoint(&spec);
|
||
elemtype = mjOBJ_JOINT;
|
||
|
||
// clear internal variables
|
||
spec_userdata_.clear();
|
||
body = 0;
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Joint();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this joint is not compiled
|
||
CopyFromSpec();
|
||
|
||
// no previous state when a joint is created
|
||
qposadr_ = -1;
|
||
dofadr_ = -1;
|
||
}
|
||
|
||
|
||
|
||
mjCJoint::mjCJoint(const mjCJoint& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCJoint& mjCJoint::operator=(const mjCJoint& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCJoint_*>(this) = static_cast<const mjCJoint_&>(other);
|
||
*static_cast<mjsJoint*>(this) = static_cast<const mjsJoint&>(other);
|
||
qposadr_ = -1;
|
||
dofadr_ = -1;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
bool mjCJoint::is_limited() const {
|
||
return islimited(limited, range);
|
||
}
|
||
bool mjCJoint::is_actfrclimited() const {
|
||
return islimited(actfrclimited, actfrcrange);
|
||
}
|
||
|
||
|
||
|
||
int mjCJoint::nq(mjtJoint joint_type) {
|
||
switch (joint_type) {
|
||
case mjJNT_FREE:
|
||
return 7;
|
||
case mjJNT_BALL:
|
||
return 4;
|
||
case mjJNT_SLIDE:
|
||
case mjJNT_HINGE:
|
||
return 1;
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
|
||
int mjCJoint::nv(mjtJoint joint_type) {
|
||
switch (joint_type) {
|
||
case mjJNT_FREE:
|
||
return 6;
|
||
case mjJNT_BALL:
|
||
return 3;
|
||
case mjJNT_SLIDE:
|
||
case mjJNT_HINGE:
|
||
return 1;
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
|
||
mjtNum* mjCJoint::qpos(const std::string& state_name) {
|
||
if (qpos_.find(state_name) == qpos_.end()) {
|
||
qpos_[state_name] = {mjNAN, 0, 0, 0, 0, 0, 0};
|
||
}
|
||
return qpos_.at(state_name).data();
|
||
}
|
||
|
||
|
||
|
||
mjtNum* mjCJoint::qvel(const std::string& state_name) {
|
||
if (qvel_.find(state_name) == qvel_.end()) {
|
||
qvel_[state_name] = {mjNAN, 0, 0, 0, 0, 0};
|
||
}
|
||
return qvel_.at(state_name).data();
|
||
}
|
||
|
||
|
||
|
||
void mjCJoint::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.userdata = &spec_userdata_;
|
||
spec.info = &info;
|
||
userdata = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCJoint::CopyFromSpec() {
|
||
*static_cast<mjsJoint*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
int mjCJoint::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_jnt) {
|
||
throw mjCError(this, "user has more values than nuser_jnt in joint");
|
||
}
|
||
userdata_.resize(model->nuser_jnt);
|
||
|
||
// check springdamper
|
||
if (springdamper[0] || springdamper[1]) {
|
||
if (springdamper[0] <= 0 || springdamper[1] <= 0) {
|
||
throw mjCError(this, "when defined, springdamper values must be positive in joint");
|
||
}
|
||
}
|
||
|
||
// free joints cannot be limited
|
||
if (type == mjJNT_FREE) {
|
||
limited = mjLIMITED_FALSE;
|
||
}
|
||
|
||
// otherwise if limited is auto, check consistency wrt auto-limits
|
||
else if (limited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(range[0] == 0 && range[1] == 0);
|
||
checklimited(this, compiler->autolimits, "joint", "", limited, hasrange);
|
||
}
|
||
|
||
// resolve limits
|
||
if (is_limited()) {
|
||
// check data
|
||
if (range[0] >= range[1] && type != mjJNT_BALL) {
|
||
throw mjCError(this, "range[0] should be smaller than range[1] in joint");
|
||
}
|
||
if (range[0] && type == mjJNT_BALL) {
|
||
throw mjCError(this, "range[0] should be 0 in ball joint");
|
||
}
|
||
|
||
// convert limits to radians
|
||
if (compiler->degree && (type == mjJNT_HINGE || type == mjJNT_BALL)) {
|
||
if (range[0]) {
|
||
range[0] *= mjPI/180.0;
|
||
}
|
||
if (range[1]) {
|
||
range[1] *= mjPI/180.0;
|
||
}
|
||
}
|
||
}
|
||
|
||
// actuator force range: none for free or ball joints
|
||
if (type == mjJNT_FREE || type == mjJNT_BALL) {
|
||
actfrclimited = mjLIMITED_FALSE;
|
||
}
|
||
|
||
// otherwise if actfrclimited is auto, check consistency wrt auto-limits
|
||
else if (actfrclimited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0);
|
||
checklimited(this, compiler->autolimits, "joint", "", actfrclimited, hasrange);
|
||
}
|
||
|
||
// resolve actuator force range limits
|
||
if (is_actfrclimited()) {
|
||
// check data
|
||
if (actfrcrange[0] >= actfrcrange[1]) {
|
||
throw mjCError(this, "actfrcrange[0] should be smaller than actfrcrange[1] in joint");
|
||
}
|
||
}
|
||
|
||
// axis: FREE or BALL are fixed to (0,0,1)
|
||
if (type == mjJNT_FREE || type == mjJNT_BALL) {
|
||
axis[0] = axis[1] = 0;
|
||
axis[2] = 1;
|
||
}
|
||
|
||
// otherwise accumulate frame rotation
|
||
else if (frame) {
|
||
mjuu_rotVecQuat(axis, axis, frame->quat);
|
||
}
|
||
|
||
// normalize axis, check norm
|
||
if (mjuu_normvec(axis, 3) < mjEPS) {
|
||
throw mjCError(this, "axis too small in joint");
|
||
}
|
||
|
||
// check data
|
||
if (type == mjJNT_FREE && limited == mjLIMITED_TRUE) {
|
||
throw mjCError(this, "limits should not be defined in free joint");
|
||
}
|
||
|
||
// pos: FREE is fixed to (0,0,0)
|
||
if (type == mjJNT_FREE) {
|
||
mjuu_zerovec(pos, 3);
|
||
}
|
||
|
||
// otherwise accumulate frame translation
|
||
else if (frame) {
|
||
double qunit[4] = {1, 0, 0, 0};
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, qunit);
|
||
}
|
||
|
||
// convert reference angles to radians for hinge joints
|
||
if (type == mjJNT_HINGE && compiler->degree) {
|
||
ref *= mjPI/180.0;
|
||
springref *= mjPI/180.0;
|
||
}
|
||
|
||
// return dofnum
|
||
if (type == mjJNT_FREE) {
|
||
return 6;
|
||
} else if (type == mjJNT_BALL) {
|
||
return 3;
|
||
} else {
|
||
return 1;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCGeom implementation --------------------------------------------------
|
||
|
||
// initialize default geom
|
||
mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultGeom(&spec);
|
||
elemtype = mjOBJ_GEOM;
|
||
|
||
mass_ = 0;
|
||
body = 0;
|
||
matid = -1;
|
||
mesh = nullptr;
|
||
hfield = nullptr;
|
||
visual_ = false;
|
||
mjuu_setvec(inertia, 0, 0, 0);
|
||
inferinertia = true;
|
||
spec_material_.clear();
|
||
spec_userdata_.clear();
|
||
spec_meshname_.clear();
|
||
spec_hfieldname_.clear();
|
||
spec_userdata_.clear();
|
||
|
||
for (int i = 0; i < mjNFLUID; i++){
|
||
fluid[i] = 0;
|
||
}
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Geom();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this geom is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCGeom::mjCGeom(const mjCGeom& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCGeom& mjCGeom::operator=(const mjCGeom& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCGeom_*>(this) = static_cast<const mjCGeom_&>(other);
|
||
*static_cast<mjsGeom*>(this) = static_cast<const mjsGeom&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
// to be called after any default copy constructor
|
||
void mjCGeom::PointToLocal(void) {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.info = &info;
|
||
spec.userdata = &spec_userdata_;
|
||
spec.material = &spec_material_;
|
||
spec.meshname = &spec_meshname_;
|
||
spec.hfieldname = &spec_hfieldname_;
|
||
spec.plugin.plugin_name = &plugin_name;
|
||
spec.plugin.name = &plugin_instance_name;
|
||
userdata = nullptr;
|
||
hfieldname = nullptr;
|
||
meshname = nullptr;
|
||
material = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCGeom::CopyFromSpec() {
|
||
*static_cast<mjsGeom*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
hfieldname_ = spec_hfieldname_;
|
||
meshname_ = spec_meshname_;
|
||
material_ = spec_material_;
|
||
plugin.active = spec.plugin.active;
|
||
plugin.element = spec.plugin.element;
|
||
plugin.plugin_name = spec.plugin.plugin_name;
|
||
plugin.name = spec.plugin.name;
|
||
}
|
||
|
||
|
||
|
||
void mjCGeom::CopyPlugin() {
|
||
model->CopyExplicitPlugin(this);
|
||
}
|
||
|
||
|
||
|
||
void mjCGeom::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!spec_material_.empty() && model != m) {
|
||
spec_material_ = m->prefix + spec_material_ + m->suffix;
|
||
}
|
||
if (!spec_hfieldname_.empty() && model != m) {
|
||
spec_hfieldname_ = m->prefix + spec_hfieldname_ + m->suffix;
|
||
}
|
||
if (!spec_meshname_.empty() && model != m) {
|
||
spec_meshname_ = m->prefix + spec_meshname_ + m->suffix;
|
||
}
|
||
if (!plugin_instance_name.empty()) {
|
||
plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compute geom volume / surface area
|
||
double mjCGeom::GetVolume() const {
|
||
// get from mesh
|
||
if (type == mjGEOM_MESH || type == mjGEOM_SDF) {
|
||
if (mesh->id < 0 || !((std::size_t) mesh->id <= model->Meshes().size())) {
|
||
throw mjCError(this, "invalid mesh id in mesh geom");
|
||
}
|
||
|
||
return mesh->GetVolumeRef();
|
||
}
|
||
|
||
// compute from geom shape (type) and inertia type (typeinertia)
|
||
switch (type) {
|
||
case mjGEOM_SPHERE: {
|
||
double radius = size[0];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
return 4 * mjPI * radius * radius * radius / 3;
|
||
case mjINERTIA_SHELL:
|
||
return 4 * mjPI * radius * radius;
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_CAPSULE: {
|
||
double height = 2 * size[1];
|
||
double radius = size[0];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
return mjPI * (radius * radius * height + 4 * radius * radius * radius / 3);
|
||
case mjINERTIA_SHELL:
|
||
return 4 * mjPI * radius * radius + 2 * mjPI * radius * height;
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_CYLINDER: {
|
||
double height = 2 * size[1];
|
||
double radius = size[0];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
return mjPI * radius * radius * height;
|
||
case mjINERTIA_SHELL:
|
||
return 2 * mjPI * radius * radius + 2 * mjPI * radius * height;
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_ELLIPSOID: {
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
return 4 * mjPI * size[0] * size[1] * size[2] / 3;
|
||
case mjINERTIA_SHELL: {
|
||
// Thomsen approximation
|
||
// https://www.numericana.com/answer/ellipsoid.htm#thomsen
|
||
double p = 1.6075;
|
||
double tmp = std::pow(size[0] * size[1], p) +
|
||
std::pow(size[1] * size[2], p) +
|
||
std::pow(size[2] * size[0], p);
|
||
return 4 * mjPI * std::pow(tmp / 3, 1 / p);
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_HFIELD:
|
||
case mjGEOM_BOX: {
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
return size[0] * size[1] * size[2] * 8;
|
||
case mjINERTIA_SHELL:
|
||
return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]);
|
||
}
|
||
break;
|
||
}
|
||
default:
|
||
break;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
// set geom diagonal inertia given density
|
||
void mjCGeom::SetInertia(void) {
|
||
// get from mesh
|
||
if (type == mjGEOM_MESH || type == mjGEOM_SDF) {
|
||
if (mesh->id < 0 || !((std::size_t)mesh->id <= model->Meshes().size())) {
|
||
throw mjCError(this, "invalid mesh id in mesh geom");
|
||
}
|
||
|
||
double* boxsz = mesh->GetInertiaBoxPtr();
|
||
inertia[0] = mass_ * (boxsz[1] * boxsz[1] + boxsz[2] * boxsz[2]) / 3;
|
||
inertia[1] = mass_ * (boxsz[0] * boxsz[0] + boxsz[2] * boxsz[2]) / 3;
|
||
inertia[2] = mass_ * (boxsz[0] * boxsz[0] + boxsz[1] * boxsz[1]) / 3;
|
||
|
||
return;
|
||
}
|
||
|
||
// compute from geom shape (type) and inertia type (typeinertia)
|
||
switch (type) {
|
||
case mjGEOM_SPHERE: {
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 5;
|
||
return;
|
||
case mjINERTIA_SHELL:
|
||
inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3;
|
||
return;
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_CAPSULE: {
|
||
double halfheight = size[1];
|
||
double height = 2 * size[1];
|
||
double radius = size[0];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME: {
|
||
double sphere_mass =
|
||
mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol)
|
||
double cylinder_mass = mass_ - sphere_mass;
|
||
|
||
// cylinder part
|
||
inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12;
|
||
inertia[2] = cylinder_mass * radius * radius / 2;
|
||
|
||
// add two hemispheres, displace along third axis
|
||
double sphere_inertia = 2 * sphere_mass * radius * radius / 5;
|
||
inertia[0] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
|
||
inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8;
|
||
inertia[2] += sphere_inertia;
|
||
return;
|
||
}
|
||
case mjINERTIA_SHELL: {
|
||
// surface area
|
||
double Asphere = 4 * mjPI * radius * radius;
|
||
double Acylinder = 2 * mjPI * radius * height;
|
||
double Atotal = Asphere + Acylinder;
|
||
|
||
// mass
|
||
double sphere_mass = mass_ * Asphere / Atotal; // mass*(sphere_area/total_area)
|
||
double cylinder_mass = mass_ - sphere_mass;
|
||
|
||
// cylinder part
|
||
inertia[0] = inertia[1] = cylinder_mass * (6 * radius * radius + height * height) / 12;
|
||
inertia[2] = cylinder_mass * radius * radius;
|
||
|
||
// add two hemispheres, displace along third axis
|
||
double sphere_inertia = 2 * sphere_mass * radius * radius / 3;
|
||
double hs_com = radius / 2; // hemisphere center of mass
|
||
double hs_pos = halfheight + hs_com; // hemisphere position
|
||
inertia[0] += sphere_inertia + sphere_mass * (hs_pos * hs_pos - hs_com * hs_com);
|
||
inertia[1] += sphere_inertia + sphere_mass * (hs_pos * hs_pos - hs_com * hs_com);
|
||
inertia[2] += sphere_inertia;
|
||
return;
|
||
}
|
||
break;
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_CYLINDER: {
|
||
double halfheight = size[1];
|
||
double height = 2 * halfheight;
|
||
double radius = size[0];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME:
|
||
|
||
inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12;
|
||
inertia[2] = mass_ * radius * radius / 2;
|
||
return;
|
||
case mjINERTIA_SHELL: {
|
||
// surface area
|
||
double Adisk = mjPI * radius * radius;
|
||
double Acylinder = 2 * mjPI * radius * height;
|
||
double Atotal = 2 * Adisk + Acylinder;
|
||
|
||
// mass
|
||
double mass_disk = mass_ * Adisk / Atotal;
|
||
double mass_cylinder = mass_ - 2 * mass_disk;
|
||
|
||
// cylinder contribution
|
||
inertia[0] = inertia[1] = mass_cylinder * (6 * radius * radius + height * height) / 12;
|
||
inertia[2] = mass_cylinder * radius * radius;
|
||
|
||
// disk inertia
|
||
double inertia_disk_x = mass_disk * radius * radius / 4 +
|
||
mass_disk * halfheight * halfheight;
|
||
double inertia_disk_z = mass_disk * radius * radius / 2;
|
||
|
||
// top and bottom disk contributions
|
||
inertia[0] += 2 * inertia_disk_x;
|
||
inertia[1] += 2 * inertia_disk_x;
|
||
inertia[2] += 2 * inertia_disk_z;
|
||
return;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_ELLIPSOID: {
|
||
double s00 = size[0] * size[0];
|
||
double s11 = size[1] * size[1];
|
||
double s22 = size[2] * size[2];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME: {
|
||
inertia[0] = mass_ * (s11 + s22) / 5;
|
||
inertia[1] = mass_ * (s00 + s22) / 5;
|
||
inertia[2] = mass_ * (s00 + s11) / 5;
|
||
return;
|
||
}
|
||
case mjINERTIA_SHELL: {
|
||
// approximate shell inertia by subtracting ellipsoid from expanded ellipsoid
|
||
double eps = 1e-6;
|
||
|
||
// solid volume (a)
|
||
double Va = 4 * mjPI * size[0] * size[1] * size[2] / 3;
|
||
|
||
// expanded volume (b)
|
||
double ae = size[0] + eps;
|
||
double be = size[1] + eps;
|
||
double ce = size[2] + eps;
|
||
double Vb = 4 * mjPI * ae * be * ce / 3;
|
||
|
||
// density
|
||
double density = mass_ / (Vb - Va);
|
||
|
||
// inertia
|
||
double mass_a = Va * density;
|
||
double inertia_a[3];
|
||
inertia_a[0] = mass_a * (s11 + s22) / 5;
|
||
inertia_a[1] = mass_a * (s00 + s22) / 5;
|
||
inertia_a[2] = mass_a * (s00 + s11) / 5;
|
||
|
||
double mass_b = Vb * density;
|
||
double inertia_b[3];
|
||
inertia_b[0] = mass_b * (be * be + ce * ce) / 5;
|
||
inertia_b[1] = mass_b * (ae * ae + ce * ce) / 5;
|
||
inertia_b[2] = mass_b * (ae * ae + be * be) / 5;
|
||
|
||
// shell inertia
|
||
inertia[0] = inertia_b[0] - inertia_a[0];
|
||
inertia[1] = inertia_b[1] - inertia_a[1];
|
||
inertia[2] = inertia_b[2] - inertia_a[2];
|
||
return;
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
case mjGEOM_HFIELD:
|
||
case mjGEOM_BOX: {
|
||
double s00 = size[0] * size[0];
|
||
double s11 = size[1] * size[1];
|
||
double s22 = size[2] * size[2];
|
||
switch (typeinertia) {
|
||
case mjINERTIA_VOLUME: {
|
||
inertia[0] = mass_ * (s11 + s22) / 3;
|
||
inertia[1] = mass_ * (s00 + s22) / 3;
|
||
inertia[2] = mass_ * (s00 + s11) / 3;
|
||
return;
|
||
}
|
||
case mjINERTIA_SHELL: {
|
||
// length
|
||
double lx = 2 * size[0]; // side 0
|
||
double ly = 2 * size[1]; // side 1
|
||
double lz = 2 * size[2]; // side 2
|
||
|
||
// surface area
|
||
double A0 = lx * ly; // side 0
|
||
double A1 = ly * lz; // side 1
|
||
double A2 = lz * lx; // side 2
|
||
double Atotal = 2 * (A0 + A1 + A2);
|
||
|
||
// side 0
|
||
double mass0 = mass_ * A0 / Atotal;
|
||
double Ix0 = mass0 * ly * ly / 12;
|
||
double Iy0 = mass0 * lx * lx / 12;
|
||
double Iz0 = mass0 * (lx * lx + ly * ly) / 12;
|
||
|
||
// side 1
|
||
double mass1 = mass_ * A1 / Atotal;
|
||
double Ix1 = mass1 * (ly * ly + lz * lz) / 12;
|
||
double Iy1 = mass1 * lz * lz / 12;
|
||
double Iz1 = mass1 * ly * ly / 12;
|
||
|
||
// side 3
|
||
double mass2 = mass_ * A2 / Atotal;
|
||
double Ix2 = mass2 * lz * lz / 12;
|
||
double Iy2 = mass2 * (lx * lx + lz * lz) / 12;
|
||
double Iz2 = mass2 * lx * lx / 12;
|
||
|
||
// total inertia
|
||
inertia[0] = 2 * (mass0 * s22 + mass2 * s11 + Ix0 + Ix1 + Ix2);
|
||
inertia[1] = 2 * (mass0 * s22 + mass1 * s00 + Iy0 + Iy1 + Iy2);
|
||
inertia[2] = 2 * (mass1 * s00 + mass2 * s11 + Iz0 + Iz1 + Iz2);
|
||
return;
|
||
}
|
||
break;
|
||
}
|
||
break;
|
||
}
|
||
default:
|
||
inertia[0] = inertia[1] = inertia[2] = 0;
|
||
return;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compute radius of bounding sphere
|
||
double mjCGeom::GetRBound(void) {
|
||
const double *aamm, *hsize;
|
||
double haabb[3] = {0};
|
||
|
||
switch (type) {
|
||
case mjGEOM_HFIELD:
|
||
hsize = hfield->size;
|
||
return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] +
|
||
std::max(hsize[2]*hsize[2], hsize[3]*hsize[3]));
|
||
|
||
case mjGEOM_SPHERE:
|
||
return size[0];
|
||
|
||
case mjGEOM_CAPSULE:
|
||
return size[0]+size[1];
|
||
|
||
case mjGEOM_CYLINDER:
|
||
return sqrt(size[0]*size[0]+size[1]*size[1]);
|
||
|
||
case mjGEOM_ELLIPSOID:
|
||
return std::max(std::max(size[0], size[1]), size[2]);
|
||
|
||
case mjGEOM_BOX:
|
||
return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]);
|
||
|
||
case mjGEOM_MESH:
|
||
case mjGEOM_SDF:
|
||
aamm = mesh->aamm();
|
||
haabb[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3]));
|
||
haabb[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4]));
|
||
haabb[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5]));
|
||
return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]);
|
||
|
||
default:
|
||
return 0;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// Compute the coefficients of the added inertia due to the surrounding fluid.
|
||
double mjCGeom::GetAddedMassKappa(double dx, double dy, double dz) {
|
||
// Integration by Gauss–Kronrod quadrature on interval l in [0, infinity] of
|
||
// f(l) = dx*dy*dz / np.sqrt((dx*dx+ l)**3 * (dy*dy+ l) * (dz*dz+ l))
|
||
// 15-point Gauss–Kronrod quadrature (K15) points x in [0, 1].
|
||
|
||
// static constexpr mjtNum kronrod_x[15] = [ // unused, left in comment for completeness
|
||
// 0.00427231, 0.02544604, 0.06756779, 0.12923441, 0.20695638,
|
||
// 0.29707742, 0.39610752, 0.50000000, 0.60389248, 0.70292258,
|
||
// 0.79304362, 0.87076559, 0.93243221, 0.97455396, 0.99572769];
|
||
// 15-point Gauss–Kronrod quadrature (K15) weights.
|
||
static constexpr double kronrod_w[15] = {
|
||
0.01146766, 0.03154605, 0.05239501, 0.07032663, 0.08450236,
|
||
0.09517529, 0.10221647, 0.10474107, 0.10221647, 0.09517529,
|
||
0.08450236, 0.07032663, 0.05239501, 0.03154605, 0.01146766};
|
||
// Integrate from 0 to inf by change of variables:
|
||
// l = x^3 / (1-x)^2. Exponents 3 and 2 found to minimize error.
|
||
static constexpr double kronrod_l[15] = {
|
||
7.865151709349917e-08, 1.7347976913907274e-05, 0.0003548008144506193,
|
||
0.002846636252924549, 0.014094260903596077, 0.053063261727396636,
|
||
0.17041978741317773, 0.5, 1.4036301548686991, 3.9353484827022642,
|
||
11.644841677041734, 39.53187807410903, 177.5711362220801,
|
||
1429.4772912937397, 54087.416549217705};
|
||
// dl = dl/dx dx. The following are dl/dx(x).
|
||
static constexpr double kronrod_d[15] = {
|
||
5.538677720489877e-05, 0.002080868285293228, 0.016514126520723166,
|
||
0.07261900344370877, 0.23985243401862602, 0.6868318249020725,
|
||
1.8551129519182894, 5.0, 14.060031152313941, 43.28941239611009,
|
||
156.58546376397112, 747.9826085305024, 5827.4042950027115,
|
||
116754.0197944512, 25482945.327264845};
|
||
|
||
const double invdx2 = 1.0 / (dx * dx);
|
||
const double invdy2 = 1.0 / (dy * dy);
|
||
const double invdz2 = 1.0 / (dz * dz);
|
||
|
||
// for added numerical stability we non-dimensionalize x by scale
|
||
// because 1 + l/d^2 in denom, l should be scaled by d^2
|
||
const double scale = std::pow(dx*dx*dx * dy * dz, 0.4); // ** (2/5)
|
||
double kappa = 0.0;
|
||
for (int i = 0; i < 15; ++i) {
|
||
const double lambda = scale * kronrod_l[i];
|
||
const double denom = (1 + lambda*invdx2) * std::sqrt(
|
||
(1 + lambda*invdx2) * (1 + lambda*invdy2) * (1 + lambda*invdz2));
|
||
kappa += scale * kronrod_d[i] / denom * kronrod_w[i];
|
||
}
|
||
return kappa * invdx2;
|
||
}
|
||
|
||
|
||
|
||
// Compute the kappa coefs of the added inertia due to the surrounding fluid.
|
||
void mjCGeom::SetFluidCoefs(void) {
|
||
double dx, dy, dz;
|
||
|
||
// get semiaxes
|
||
switch (type) {
|
||
case mjGEOM_SPHERE:
|
||
dx = size[0];
|
||
dy = size[0];
|
||
dz = size[0];
|
||
break;
|
||
|
||
case mjGEOM_CAPSULE:
|
||
dx = size[0];
|
||
dy = size[0];
|
||
dz = size[1] + size[0];
|
||
break;
|
||
|
||
case mjGEOM_CYLINDER:
|
||
dx = size[0];
|
||
dy = size[0];
|
||
dz = size[1];
|
||
break;
|
||
|
||
default:
|
||
dx = size[0];
|
||
dy = size[1];
|
||
dz = size[2];
|
||
}
|
||
|
||
// volume of equivalent ellipsoid
|
||
const double volume = 4.0 / 3.0 * mjPI * dx * dy * dz;
|
||
|
||
// GetAddedMassKappa is invariant to permutation of last two arguments
|
||
const double kx = GetAddedMassKappa(dx, dy, dz);
|
||
const double ky = GetAddedMassKappa(dy, dz, dx);
|
||
const double kz = GetAddedMassKappa(dz, dx, dy);
|
||
|
||
// coefficients of virtual moment of inertia. Note: if (kz-ky) in numerator
|
||
// is negative, also the denom is negative. Abs both and clip to MINVAL
|
||
const auto pow2 = [](const double val) { return val * val; };
|
||
const double Ixfac = pow2(dy*dy - dz*dz) * std::abs(kz - ky) / std::max(
|
||
mjEPS, std::abs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz)));
|
||
const double Iyfac = pow2(dz*dz - dx*dx) * std::abs(kx - kz) / std::max(
|
||
mjEPS, std::abs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx)));
|
||
const double Izfac = pow2(dx*dx - dy*dy) * std::abs(ky - kx) / std::max(
|
||
mjEPS, std::abs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky)));
|
||
|
||
mjtNum virtual_mass[3];
|
||
virtual_mass[0] = volume * kx / std::max(mjEPS, 2-kx);
|
||
virtual_mass[1] = volume * ky / std::max(mjEPS, 2-ky);
|
||
virtual_mass[2] = volume * kz / std::max(mjEPS, 2-kz);
|
||
mjtNum virtual_inertia[3];
|
||
virtual_inertia[0] = volume*Ixfac/5;
|
||
virtual_inertia[1] = volume*Iyfac/5;
|
||
virtual_inertia[2] = volume*Izfac/5;
|
||
|
||
writeFluidGeomInteraction(fluid, &fluid_ellipsoid, &fluid_coefs[0],
|
||
&fluid_coefs[1], &fluid_coefs[2],
|
||
&fluid_coefs[3], &fluid_coefs[4],
|
||
virtual_mass, virtual_inertia);
|
||
}
|
||
|
||
|
||
// compute bounding box
|
||
void mjCGeom::ComputeAABB(void) {
|
||
double aamm[6]; // axis-aligned bounding box in (min, max) format
|
||
switch (type) {
|
||
case mjGEOM_HFIELD:
|
||
aamm[0] = -hfield->size[0];
|
||
aamm[1] = -hfield->size[1];
|
||
aamm[2] = -hfield->size[3];
|
||
aamm[3] = hfield->size[0];
|
||
aamm[4] = hfield->size[1];
|
||
aamm[5] = hfield->size[2];
|
||
break;
|
||
|
||
case mjGEOM_SPHERE:
|
||
aamm[3] = aamm[4] = aamm[5] = size[0];
|
||
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
|
||
break;
|
||
|
||
case mjGEOM_CAPSULE:
|
||
aamm[3] = aamm[4] = size[0];
|
||
aamm[5] = size[0] + size[1];
|
||
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
|
||
break;
|
||
|
||
case mjGEOM_CYLINDER:
|
||
aamm[3] = aamm[4] = size[0];
|
||
aamm[5] = size[1];
|
||
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
|
||
break;
|
||
|
||
case mjGEOM_MESH:
|
||
case mjGEOM_SDF:
|
||
mjuu_copyvec(aamm, mesh->aamm(), 6);
|
||
break;
|
||
|
||
case mjGEOM_PLANE:
|
||
aamm[0] = aamm[1] = aamm[2] = -mjMAXVAL;
|
||
aamm[3] = aamm[4] = mjMAXVAL;
|
||
aamm[5] = 0;
|
||
break;
|
||
|
||
default:
|
||
mjuu_copyvec(aamm+3, size, 3);
|
||
mjuu_setvec(aamm, -size[0], -size[1], -size[2]);
|
||
break;
|
||
}
|
||
|
||
// convert aamm to aabb (center, size) format
|
||
double pos[] = {(aamm[3] + aamm[0]) / 2, (aamm[4] + aamm[1]) / 2,
|
||
(aamm[5] + aamm[2]) / 2};
|
||
double size[] = {(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2,
|
||
(aamm[5] - aamm[2]) / 2};
|
||
mjuu_copyvec(aabb, pos, 3);
|
||
mjuu_copyvec(aabb+3, size, 3);
|
||
}
|
||
|
||
const std::string& mjCGeom::get_material() const {
|
||
if (mesh && spec_material_.empty()) {
|
||
return mesh->Material();
|
||
}
|
||
return spec_material_;
|
||
}
|
||
|
||
// compiler
|
||
void mjCGeom::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_geom) {
|
||
throw mjCError(this, "user has more values than nuser_geom in geom '%s' (id = %d)",
|
||
name.c_str(), id);
|
||
}
|
||
userdata_.resize(model->nuser_geom);
|
||
|
||
// check type
|
||
if (type < 0 || type >= mjNGEOMTYPES) {
|
||
throw mjCError(this, "invalid type in geom");
|
||
}
|
||
|
||
// check condim
|
||
if (condim != 1 && condim != 3 && condim != 4 && condim != 6) {
|
||
throw mjCError(this, "invalid condim in geom");
|
||
}
|
||
|
||
// check mesh
|
||
if ((type == mjGEOM_MESH || type == mjGEOM_SDF) && !mesh) {
|
||
throw mjCError(this, "mesh geom '%s' (id = %d) must have valid meshid", name.c_str(), id);
|
||
}
|
||
|
||
// check hfield
|
||
if ((type == mjGEOM_HFIELD && !hfield) || (type != mjGEOM_HFIELD && hfield)) {
|
||
throw mjCError(this, "hfield geom '%s' (id = %d) must have valid hfieldid", name.c_str(), id);
|
||
}
|
||
|
||
// plane only allowed in static bodies
|
||
if (type == mjGEOM_PLANE && body->weldid != 0) {
|
||
throw mjCError(this, "plane only allowed in static bodies");
|
||
}
|
||
|
||
// check if can collide
|
||
visual_ = !contype && !conaffinity;
|
||
|
||
// normalize quaternion
|
||
mjuu_normvec(quat, 4);
|
||
|
||
// 'fromto': compute pos, quat, size
|
||
if (mjuu_defined(fromto[0])) {
|
||
// check type
|
||
if (type != mjGEOM_CAPSULE &&
|
||
type != mjGEOM_CYLINDER &&
|
||
type != mjGEOM_ELLIPSOID &&
|
||
type != mjGEOM_BOX) {
|
||
throw mjCError(this, "fromto requires capsule, cylinder, box or ellipsoid in geom");
|
||
}
|
||
|
||
// make sure pos is not defined; cannot use mjuu_defined because default is (0,0,0)
|
||
if (pos[0] || pos[1] || pos[2]) {
|
||
throw mjCError(this, "both pos and fromto defined in geom");
|
||
}
|
||
|
||
// size[1] = length (for capsule and cylinder)
|
||
double vec[3] = {
|
||
fromto[0]-fromto[3],
|
||
fromto[1]-fromto[4],
|
||
fromto[2]-fromto[5]
|
||
};
|
||
size[1] = mjuu_normvec(vec, 3)/2;
|
||
if (size[1] < mjEPS) {
|
||
throw mjCError(this, "fromto points too close in geom");
|
||
}
|
||
|
||
// adjust size for ellipsoid and box
|
||
if (type == mjGEOM_ELLIPSOID || type == mjGEOM_BOX) {
|
||
size[2] = size[1];
|
||
size[1] = size[0];
|
||
}
|
||
|
||
// compute position
|
||
pos[0] = (fromto[0]+fromto[3])/2;
|
||
pos[1] = (fromto[1]+fromto[4])/2;
|
||
pos[2] = (fromto[2]+fromto[5])/2;
|
||
|
||
// compute orientation
|
||
mjuu_z2quat(quat, vec);
|
||
}
|
||
|
||
// not 'fromto': try alternative
|
||
else {
|
||
const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt);
|
||
if (err) {
|
||
throw mjCError(this, "orientation specification error '%s' in geom %d", err, id);
|
||
}
|
||
}
|
||
|
||
// mesh: accumulate frame, fit geom if needed
|
||
if (mesh) {
|
||
// check for inapplicable fromto
|
||
if (mjuu_defined(fromto[0])) {
|
||
throw mjCError(this, "fromto cannot be used with mesh geom");
|
||
}
|
||
|
||
// save reference in case this is not an mjGEOM_MESH
|
||
mjCMesh* pmesh = mesh;
|
||
|
||
// fit geom if type is not mjGEOM_MESH
|
||
if (type != mjGEOM_MESH && type != mjGEOM_SDF) {
|
||
double meshpos[3];
|
||
mesh->FitGeom(this, meshpos);
|
||
|
||
// remove reference to mesh
|
||
meshname_.clear();
|
||
mesh = nullptr;
|
||
mjuu_copyvec(pmesh->GetPosPtr(), meshpos, 3);
|
||
} else if (typeinertia == mjINERTIA_SHELL) {
|
||
throw mjCError(this, "for mesh geoms, inertia should be specified in the mesh asset");
|
||
}
|
||
|
||
// apply geom pos/quat as offset
|
||
mjuu_frameaccum(pos, quat, pmesh->GetPosPtr(), pmesh->GetQuatPtr());
|
||
}
|
||
|
||
// check size parameters
|
||
checksize(size, type, this, name.c_str(), id);
|
||
|
||
// set hfield sizes in geom.size
|
||
if (type == mjGEOM_HFIELD) {
|
||
size[0] = hfield->size[0];
|
||
size[1] = hfield->size[1];
|
||
size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3];
|
||
} else if (type == mjGEOM_MESH || type == mjGEOM_SDF) {
|
||
const double* aamm = mesh->aamm();
|
||
size[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3]));
|
||
size[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4]));
|
||
size[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5]));
|
||
}
|
||
|
||
for (double s : size) {
|
||
if (std::isnan(s)) {
|
||
throw mjCError(this, "nan size in geom");
|
||
}
|
||
}
|
||
// compute aabb
|
||
ComputeAABB();
|
||
|
||
// compute geom mass and inertia
|
||
if (inferinertia) {
|
||
// mass is defined
|
||
if (mjuu_defined(mass)) {
|
||
if (mass == 0) {
|
||
mass_ = 0;
|
||
density = 0;
|
||
} else if (GetVolume() > mjEPS) {
|
||
mass_ = mass;
|
||
density = mass / GetVolume();
|
||
SetInertia();
|
||
}
|
||
}
|
||
|
||
// mass is not defined
|
||
else {
|
||
if (density == 0) {
|
||
mass_ = 0;
|
||
} else {
|
||
mass_ = density * GetVolume();
|
||
SetInertia();
|
||
}
|
||
}
|
||
|
||
|
||
// check for negative values
|
||
if (mass_ < 0 || inertia[0] < 0 || inertia[1] < 0 || inertia[2] < 0 || density < 0)
|
||
throw mjCError(this, "mass, inertia or density are negative in geom");
|
||
}
|
||
|
||
// fluid-interaction coefficients, requires computed inertia and mass
|
||
if (fluid_ellipsoid > 0) {
|
||
SetFluidCoefs();
|
||
}
|
||
|
||
// plugin
|
||
if (plugin.active) {
|
||
if (plugin_name.empty() && plugin_instance_name.empty()) {
|
||
throw mjCError(
|
||
this, "neither 'plugin' nor 'instance' is specified for geom");
|
||
}
|
||
|
||
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.element);
|
||
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
|
||
plugin.element = plugin_instance;
|
||
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
|
||
if (!(pplugin->capabilityflags & mjPLUGIN_SDF)) {
|
||
throw mjCError(this, "plugin '%s' does not support sign distance fields", pplugin->name);
|
||
}
|
||
}
|
||
|
||
// frame
|
||
if (frame) {
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCSite implementation --------------------------------------------------
|
||
|
||
// initialize default site
|
||
mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultSite(&spec);
|
||
elemtype = mjOBJ_SITE;
|
||
|
||
// clear internal variables
|
||
body = 0;
|
||
matid = -1;
|
||
spec_material_.clear();
|
||
spec_userdata_.clear();
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Site();
|
||
}
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this site is not compiled
|
||
CopyFromSpec();
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
}
|
||
|
||
|
||
|
||
mjCSite::mjCSite(const mjCSite& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCSite& mjCSite::operator=(const mjCSite& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCSite_*>(this) = static_cast<const mjCSite_&>(other);
|
||
*static_cast<mjsSite*>(this) = static_cast<const mjsSite&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCSite::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.info = &info;
|
||
spec.material = &spec_material_;
|
||
spec.userdata = &spec_userdata_;
|
||
userdata = nullptr;
|
||
material = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCSite::CopyFromSpec() {
|
||
*static_cast<mjsSite*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
material_ = spec_material_;
|
||
}
|
||
|
||
|
||
|
||
void mjCSite::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!spec_material_.empty() && model != m) {
|
||
spec_material_ = m->prefix + spec_material_ + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCSite::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_site) {
|
||
throw mjCError(this, "user has more values than nuser_site in site");
|
||
}
|
||
userdata_.resize(model->nuser_site);
|
||
|
||
// check type
|
||
if (type < 0 || type >= mjNGEOMTYPES) {
|
||
throw mjCError(this, "invalid type in site");
|
||
}
|
||
|
||
// do not allow meshes, hfields and planes
|
||
if (type == mjGEOM_MESH || type == mjGEOM_HFIELD || type == mjGEOM_PLANE) {
|
||
throw mjCError(this, "meshes, hfields and planes not allowed in site");
|
||
}
|
||
|
||
// 'fromto': compute pos, quat, size
|
||
if (mjuu_defined(fromto[0])) {
|
||
// check type
|
||
if (type != mjGEOM_CAPSULE &&
|
||
type != mjGEOM_CYLINDER &&
|
||
type != mjGEOM_ELLIPSOID &&
|
||
type != mjGEOM_BOX) {
|
||
throw mjCError(this, "fromto requires capsule, cylinder, box or ellipsoid in geom");
|
||
}
|
||
|
||
// make sure pos is not defined; cannot use mjuu_defined because default is (0,0,0)
|
||
if (pos[0] || pos[1] || pos[2]) {
|
||
throw mjCError(this, "both pos and fromto defined in geom");
|
||
}
|
||
|
||
// size[1] = length (for capsule and cylinder)
|
||
double vec[3] = {fromto[0]-fromto[3], fromto[1]-fromto[4], fromto[2]-fromto[5]};
|
||
size[1] = mjuu_normvec(vec, 3)/2;
|
||
if (size[1] < mjEPS) {
|
||
throw mjCError(this, "fromto points too close in geom");
|
||
}
|
||
|
||
// adjust size for ellipsoid and box
|
||
if (type == mjGEOM_ELLIPSOID || type == mjGEOM_BOX) {
|
||
size[2] = size[1];
|
||
size[1] = size[0];
|
||
}
|
||
|
||
// compute position
|
||
pos[0] = (fromto[0]+fromto[3])/2;
|
||
pos[1] = (fromto[1]+fromto[4])/2;
|
||
pos[2] = (fromto[2]+fromto[5])/2;
|
||
|
||
// compute orientation
|
||
mjuu_z2quat(quat, vec);
|
||
}
|
||
|
||
// alternative orientation
|
||
else {
|
||
const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt);
|
||
if (err) {
|
||
throw mjCError(this, "orientation specification error '%s' in site %d", err, id);
|
||
}
|
||
}
|
||
|
||
// frame
|
||
if (frame) {
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
|
||
}
|
||
|
||
// normalize quaternion
|
||
mjuu_normvec(quat, 4);
|
||
|
||
// check size parameters
|
||
checksize(size, type, this, name.c_str(), id);
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCCamera implementation ------------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultCamera(&spec);
|
||
elemtype = mjOBJ_CAMERA;
|
||
|
||
// clear private variables
|
||
body = 0;
|
||
targetbodyid = -1;
|
||
spec_targetbody_.clear();
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Camera();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this camera is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCCamera::mjCCamera(const mjCCamera& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCCamera& mjCCamera::operator=(const mjCCamera& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCCamera_*>(this) = static_cast<const mjCCamera_&>(other);
|
||
*static_cast<mjsCamera*>(this) = static_cast<const mjsCamera&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCCamera::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.userdata = &spec_userdata_;
|
||
spec.targetbody = &spec_targetbody_;
|
||
spec.info = &info;
|
||
userdata = nullptr;
|
||
targetbody = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCCamera::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!spec_targetbody_.empty()) {
|
||
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCCamera::CopyFromSpec() {
|
||
*static_cast<mjsCamera*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
targetbody_ = spec_targetbody_;
|
||
}
|
||
|
||
|
||
|
||
void mjCCamera::ResolveReferences(const mjCModel* m) {
|
||
if (!targetbody_.empty()) {
|
||
mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_);
|
||
if (tb) {
|
||
targetbodyid = tb->id;
|
||
} else {
|
||
throw mjCError(this, "unknown target body in camera");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCCamera::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_cam) {
|
||
throw mjCError(this, "user has more values than nuser_cam in camera");
|
||
}
|
||
userdata_.resize(model->nuser_cam);
|
||
|
||
// process orientation specifications
|
||
const char* err = ResolveOrientation(quat, compiler->degree, compiler->eulerseq, alt);
|
||
if (err) {
|
||
throw mjCError(this, "orientation specification error '%s' in camera %d", err, id);
|
||
}
|
||
|
||
// frame
|
||
if (frame) {
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, quat);
|
||
}
|
||
|
||
// normalize quaternion
|
||
mjuu_normvec(quat, 4);
|
||
|
||
// get targetbodyid
|
||
ResolveReferences(model);
|
||
|
||
// make sure the image size is finite
|
||
if (fovy >= 180) {
|
||
throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
|
||
name.c_str(), id, fovy);
|
||
}
|
||
|
||
// check that specs are not duplicated
|
||
if ((principal_length[0] && principal_pixel[0]) ||
|
||
(principal_length[1] && principal_pixel[1])) {
|
||
throw mjCError(this, "principal length duplicated in camera");
|
||
}
|
||
|
||
if ((focal_length[0] && focal_pixel[0]) ||
|
||
(focal_length[1] && focal_pixel[1])) {
|
||
throw mjCError(this, "focal length duplicated in camera");
|
||
}
|
||
|
||
// compute number of pixels per unit length
|
||
if (sensor_size[0] > 0 && sensor_size[1] > 0) {
|
||
float pixel_density[2] = {
|
||
(float)resolution[0] / sensor_size[0],
|
||
(float)resolution[1] / sensor_size[1],
|
||
};
|
||
|
||
// defaults are zero, so only one term in each sum is nonzero
|
||
intrinsic[0] = focal_pixel[0] / pixel_density[0] + focal_length[0];
|
||
intrinsic[1] = focal_pixel[1] / pixel_density[1] + focal_length[1];
|
||
intrinsic[2] = principal_pixel[0] / pixel_density[0] + principal_length[0];
|
||
intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1];
|
||
|
||
// fovy with principal point at (0, 0)
|
||
fovy = std::atan2(sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI;
|
||
} else {
|
||
intrinsic[0] = model->visual.map.znear;
|
||
intrinsic[1] = model->visual.map.znear;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCLight implementation -------------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultLight(&spec);
|
||
elemtype = mjOBJ_LIGHT;
|
||
|
||
// clear private variables
|
||
body = 0;
|
||
targetbodyid = -1;
|
||
texid = -1;
|
||
spec_targetbody_.clear();
|
||
spec_texture_.clear();
|
||
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Light();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
PointToLocal();
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCLight::mjCLight(const mjCLight& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCLight& mjCLight::operator=(const mjCLight& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCLight_*>(this) = static_cast<const mjCLight_&>(other);
|
||
*static_cast<mjsLight*>(this) = static_cast<const mjsLight&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCLight::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.targetbody = &spec_targetbody_;
|
||
spec.texture = &spec_texture_;
|
||
spec.info = &info;
|
||
targetbody = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCLight::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!spec_targetbody_.empty()) {
|
||
spec_targetbody_ = m->prefix + spec_targetbody_ + m->suffix;
|
||
}
|
||
if (!spec_texture_.empty()) {
|
||
spec_texture_ = m->prefix + spec_texture_ + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCLight::CopyFromSpec() {
|
||
*static_cast<mjsLight*>(this) = spec;
|
||
targetbody_ = spec_targetbody_;
|
||
texture_ = spec_texture_;
|
||
}
|
||
|
||
|
||
|
||
void mjCLight::ResolveReferences(const mjCModel* m) {
|
||
if (!targetbody_.empty()) {
|
||
mjCBody* tb = (mjCBody*)m->FindObject(mjOBJ_BODY, targetbody_);
|
||
if (tb) {
|
||
targetbodyid = tb->id;
|
||
} else {
|
||
throw mjCError(this, "unknown target body in light");
|
||
}
|
||
}
|
||
if (!texture_.empty()) {
|
||
mjCTexture* tex = (mjCTexture*)m->FindObject(mjOBJ_TEXTURE, texture_);
|
||
if (tex) {
|
||
texid = tex->id;
|
||
} else {
|
||
throw mjCError(this, "unknown texture in light");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCLight::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// frame
|
||
if (frame) {
|
||
// apply frame transform to pos, qunit is unused
|
||
double qunit[4]= {1, 0, 0, 0};
|
||
mjuu_frameaccumChild(frame->pos, frame->quat, pos, qunit);
|
||
|
||
// rotate dir
|
||
mjuu_rotVecQuat(dir, dir, frame->quat);
|
||
}
|
||
|
||
// normalize direction, make sure it is not zero
|
||
if (mjuu_normvec(dir, 3) < mjEPS) {
|
||
throw mjCError(this, "zero direction in light");
|
||
}
|
||
|
||
// get targetbodyid and texid
|
||
ResolveReferences(model);
|
||
}
|
||
|
||
|
||
|
||
//------------------------- class mjCHField --------------------------------------------------------
|
||
|
||
// constructor
|
||
mjCHField::mjCHField(mjCModel* _model) {
|
||
mjs_defaultHField(&spec);
|
||
elemtype = mjOBJ_HFIELD;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear variables
|
||
data.clear();
|
||
spec_file_.clear();
|
||
spec_userdata_.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// copy from spec
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCHField::mjCHField(const mjCHField& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCHField& mjCHField::operator=(const mjCHField& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCHField_*>(this) = static_cast<const mjCHField_&>(other);
|
||
*static_cast<mjsHField*>(this) = static_cast<const mjsHField&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCHField::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.file = &spec_file_;
|
||
spec.content_type = &spec_content_type_;
|
||
spec.userdata = &spec_userdata_;
|
||
spec.info = &info;
|
||
file = nullptr;
|
||
content_type = nullptr;
|
||
userdata = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCHField::CopyFromSpec() {
|
||
*static_cast<mjsHField*>(this) = spec;
|
||
file_ = spec_file_;
|
||
content_type_ = spec_content_type_;
|
||
userdata_ = spec_userdata_;
|
||
|
||
// clear precompiled asset. TODO: use asset cache
|
||
data.clear();
|
||
if (!file_.empty()) {
|
||
nrow = 0;
|
||
ncol = 0;
|
||
}
|
||
|
||
// use filename if name is missing
|
||
if (name.empty()) {
|
||
std::string stripped = mjuu_strippath(file_);
|
||
name = mjuu_stripext(stripped);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCHField::NameSpace(const mjCModel* m) {
|
||
// use filename if name is missing
|
||
if (name.empty()) {
|
||
std::string stripped = mjuu_strippath(spec_file_);
|
||
name = mjuu_stripext(stripped);
|
||
}
|
||
mjCBase::NameSpace(m);
|
||
if (modelfiledir_.empty()) {
|
||
modelfiledir_ = FilePath(m->spec_modelfiledir_);
|
||
}
|
||
if (meshdir_.empty()) {
|
||
meshdir_ = FilePath(m->spec_meshdir_);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCHField::~mjCHField() {
|
||
data.clear();
|
||
userdata_.clear();
|
||
spec_userdata_.clear();
|
||
}
|
||
|
||
|
||
|
||
// load elevation data from custom format
|
||
void mjCHField::LoadCustom(mjResource* resource) {
|
||
// get file data in buffer
|
||
const void* buffer = 0;
|
||
int buffer_sz = mju_readResource(resource, &buffer);
|
||
|
||
if (buffer_sz < 1) {
|
||
throw mjCError(this, "could not read hfield file '%s'", resource->name);
|
||
} else if (!buffer_sz) {
|
||
throw mjCError(this, "empty hfield file '%s'", resource->name);
|
||
}
|
||
|
||
|
||
if (buffer_sz < 2*sizeof(int)) {
|
||
throw mjCError(this, "hfield missing header '%s'", resource->name);
|
||
}
|
||
|
||
// read dimensions
|
||
int* pint = (int*)buffer;
|
||
nrow = pint[0];
|
||
ncol = pint[1];
|
||
|
||
// check dimensions
|
||
if (nrow < 1 || ncol < 1) {
|
||
throw mjCError(this, "non-positive hfield dimensions in file '%s'", resource->name);
|
||
}
|
||
|
||
// check buffer size
|
||
if (buffer_sz != nrow*ncol*sizeof(float)+8) {
|
||
throw mjCError(this, "unexpected file size in file '%s'", resource->name);
|
||
}
|
||
|
||
// allocate
|
||
data.assign(nrow*ncol, 0);
|
||
if (data.empty()) {
|
||
throw mjCError(this, "could not allocate buffers in hfield");
|
||
}
|
||
|
||
// copy data
|
||
memcpy(data.data(), (void*)(pint+2), nrow*ncol*sizeof(float));
|
||
}
|
||
|
||
|
||
|
||
// load elevation data from PNG format
|
||
void mjCHField::LoadPNG(mjResource* resource) {
|
||
PNGImage image = PNGImage::Load(this, resource, LCT_GREY);
|
||
|
||
ncol = image.Width();
|
||
nrow = image.Height();
|
||
|
||
// copy image data over with rows reversed
|
||
data.reserve(nrow * ncol);
|
||
for (int r = 0; r < nrow; r++) {
|
||
for (int c = 0; c < ncol; c++) {
|
||
data.push_back((float) image[c + (nrow - 1 - r)*ncol]);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCHField::Compile(const mjVFS* vfs) {
|
||
CopyFromSpec();
|
||
|
||
// copy userdata into data
|
||
if (!userdata_.empty()) {
|
||
if (nrow*ncol != userdata_.size()) {
|
||
throw mjCError(this, "elevation data length must match nrow*ncol");
|
||
}
|
||
data.assign(nrow*ncol, 0);
|
||
if (data.empty()) {
|
||
throw mjCError(this, "could not allocate buffers in hfield");
|
||
}
|
||
memcpy(data.data(), userdata_.data(), nrow*ncol*sizeof(float));
|
||
}
|
||
|
||
// check size parameters
|
||
for (int i=0; i < 4; i++)
|
||
if (size[i] <= 0)
|
||
throw mjCError(this, "size parameter is not positive in hfield");
|
||
|
||
// remove path from file if necessary
|
||
if (model->strippath) {
|
||
file_ = mjuu_strippath(file_);
|
||
}
|
||
|
||
// load from file if specified
|
||
if (!file_.empty()) {
|
||
// make sure hfield was not already specified manually
|
||
if (nrow || ncol || !data.empty()) {
|
||
throw mjCError(this, "hfield specified from file and manually");
|
||
}
|
||
|
||
std::string asset_type = GetAssetContentType(file_, content_type_);
|
||
|
||
// fallback to custom
|
||
if (asset_type.empty()) {
|
||
asset_type = "image/vnd.mujoco.hfield";
|
||
}
|
||
|
||
if (asset_type != "image/png" && asset_type != "image/vnd.mujoco.hfield") {
|
||
throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
|
||
}
|
||
|
||
// copy paths from model if not already defined
|
||
if (modelfiledir_.empty()) {
|
||
modelfiledir_ = FilePath(model->modelfiledir_);
|
||
}
|
||
if (meshdir_.empty()) {
|
||
meshdir_ = FilePath(model->meshdir_);
|
||
}
|
||
|
||
FilePath filename = meshdir_ + FilePath(file_);
|
||
mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs);
|
||
|
||
try {
|
||
if (asset_type == "image/png") {
|
||
LoadPNG(resource);
|
||
} else {
|
||
LoadCustom(resource);
|
||
}
|
||
mju_closeResource(resource);
|
||
} catch(mjCError err) {
|
||
mju_closeResource(resource);
|
||
throw err;
|
||
}
|
||
}
|
||
|
||
// make sure hfield was specified (from file or manually)
|
||
if (nrow < 1 || ncol < 1 || data.empty()) {
|
||
throw mjCError(this, "hfield not specified");
|
||
}
|
||
|
||
// set elevation data to [0-1] range
|
||
float emin = 1E+10, emax = -1E+10;
|
||
for (int i = 0; i < nrow*ncol; i++) {
|
||
emin = std::min(emin, data[i]);
|
||
emax = std::max(emax, data[i]);
|
||
}
|
||
if (emin > emax) {
|
||
throw mjCError(this, "invalid data range in hfield '%s'", file_.c_str());
|
||
}
|
||
for (int i=0; i < nrow*ncol; i++) {
|
||
data[i] -= emin;
|
||
if (emax-emin > mjEPS) {
|
||
data[i] /= (emax - emin);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCTexture implementation -----------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCTexture::mjCTexture(mjCModel* _model) {
|
||
mjs_defaultTexture(&spec);
|
||
elemtype = mjOBJ_TEXTURE;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear user settings: single file
|
||
spec_file_.clear();
|
||
spec_content_type_.clear();
|
||
|
||
// clear user settings: separate file
|
||
spec_cubefiles_.assign(6, "");
|
||
|
||
// clear internal variables
|
||
data_.clear();
|
||
clear_data_ = false;
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this texture is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCTexture::mjCTexture(const mjCTexture& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCTexture& mjCTexture::operator=(const mjCTexture& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCTexture_*>(this) = static_cast<const mjCTexture_&>(other);
|
||
clear_data_ = other.clear_data_;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCTexture::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.file = &spec_file_;
|
||
spec.data = &data_;
|
||
spec.content_type = &spec_content_type_;
|
||
spec.cubefiles = &spec_cubefiles_;
|
||
spec.info = &info;
|
||
file = nullptr;
|
||
content_type = nullptr;
|
||
cubefiles = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCTexture::CopyFromSpec() {
|
||
*static_cast<mjsTexture*>(this) = spec;
|
||
file_ = spec_file_;
|
||
content_type_ = spec_content_type_;
|
||
cubefiles_ = spec_cubefiles_;
|
||
|
||
if (clear_data_) {
|
||
// clear precompiled asset. TODO: use asset cache
|
||
data_.clear();
|
||
}
|
||
|
||
// use filename if name is missing
|
||
if (name.empty()) {
|
||
std::string stripped = mjuu_strippath(file_);
|
||
name = mjuu_stripext(stripped);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCTexture::NameSpace(const mjCModel* m) {
|
||
// use filename if name is missing
|
||
if (name.empty()) {
|
||
std::string stripped = mjuu_strippath(spec_file_);
|
||
name = mjuu_stripext(stripped);
|
||
}
|
||
mjCBase::NameSpace(m);
|
||
if (modelfiledir_.empty()) {
|
||
modelfiledir_ = FilePath(m->spec_modelfiledir_);
|
||
}
|
||
if (texturedir_.empty()) {
|
||
texturedir_ = FilePath(m->spec_texturedir_);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// free data storage allocated by lodepng
|
||
mjCTexture::~mjCTexture() {
|
||
data_.clear();
|
||
}
|
||
|
||
|
||
|
||
// insert random dots
|
||
static void randomdot(std::byte* rgb, const double* markrgb,
|
||
int width, int height, double probability) {
|
||
// make distribution using fixed seed
|
||
std::mt19937_64 rng;
|
||
rng.seed(42);
|
||
std::uniform_real_distribution<double> dist(0, 1);
|
||
|
||
// sample
|
||
for (int r=0; r < height; r++) {
|
||
for (int c=0; c < width; c++) {
|
||
if (dist(rng) < probability) {
|
||
for (int j=0; j < 3; j++) {
|
||
rgb[3*(r*width+c)+j] = (std::byte)(255*markrgb[j]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// interpolate between colors based on value in (-1, +1)
|
||
static void interp(std::byte* rgb, const double* rgb1, const double* rgb2, double pos) {
|
||
const double correction = 1.0/sqrt(2);
|
||
double alpha = 0.5*(1 + pos/sqrt(1+pos*pos)/correction);
|
||
if (alpha < 0) {
|
||
alpha = 0;
|
||
} else if (alpha > 1) {
|
||
alpha = 1;
|
||
}
|
||
|
||
for (int j=0; j < 3; j++) {
|
||
rgb[j] = (std::byte)(255*(alpha*rgb1[j] + (1-alpha)*rgb2[j]));
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// make checker pattern for one side
|
||
static void checker(std::byte* rgb, const std::byte* RGB1, const std::byte* RGB2,
|
||
int width, int height) {
|
||
for (int r=0; r < height/2; r++) {
|
||
for (int c=0; c < width/2; c++) {
|
||
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
||
}
|
||
}
|
||
for (int r=height/2; r < height; r++) {
|
||
for (int c=width/2; c < width; c++) {
|
||
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
||
}
|
||
}
|
||
for (int r=0; r < height/2; r++) {
|
||
for (int c=width/2; c < width; c++) {
|
||
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
||
}
|
||
}
|
||
for (int r=height/2; r < height; r++) {
|
||
for (int c=0; c < width/2; c++) {
|
||
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// make builtin: 2D
|
||
void mjCTexture::Builtin2D(void) {
|
||
std::byte RGB1[3], RGB2[3], RGBm[3];
|
||
// convert fixed colors
|
||
for (int j=0; j < 3; j++) {
|
||
RGB1[j] = (std::byte)(255*rgb1[j]);
|
||
RGB2[j] = (std::byte)(255*rgb2[j]);
|
||
RGBm[j] = (std::byte)(255*markrgb[j]);
|
||
}
|
||
|
||
//------------------ face
|
||
|
||
// gradient
|
||
if (builtin == mjBUILTIN_GRADIENT) {
|
||
for (int r=0; r < height; r++) {
|
||
for (int c=0; c < width; c++) {
|
||
// compute normalized coordinates and radius
|
||
double x = 2*c/((double)(width-1)) - 1;
|
||
double y = 1 - 2*r/((double)(height-1));
|
||
double pos = 2*sqrt(x*x+y*y) - 1;
|
||
|
||
// interpolate through sigmoid
|
||
interp(data_.data() + 3*(r*width+c), rgb2, rgb1, pos);
|
||
}
|
||
}
|
||
}
|
||
|
||
// checker
|
||
else if (builtin == mjBUILTIN_CHECKER) {
|
||
checker(data_.data(), RGB1, RGB2, width, height);
|
||
}
|
||
|
||
// flat
|
||
else if (builtin == mjBUILTIN_FLAT) {
|
||
for (int r=0; r < height; r++) {
|
||
for (int c=0; c < width; c++) {
|
||
memcpy(data_.data()+3*(r*width+c), RGB1, 3);
|
||
}
|
||
}
|
||
}
|
||
|
||
//------------------ marks
|
||
|
||
// edge
|
||
if (mark == mjMARK_EDGE) {
|
||
for (int r=0; r < height; r++) {
|
||
memcpy(data_.data()+3*(r*width+0), RGBm, 3);
|
||
memcpy(data_.data()+3*(r*width+width-1), RGBm, 3);
|
||
}
|
||
for (int c=0; c < width; c++) {
|
||
memcpy(data_.data()+3*(0*width+c), RGBm, 3);
|
||
memcpy(data_.data()+3*((height-1)*width+c), RGBm, 3);
|
||
}
|
||
}
|
||
|
||
// cross
|
||
else if (mark == mjMARK_CROSS) {
|
||
for (int r=0; r < height; r++) {
|
||
memcpy(data_.data()+3*(r*width+width/2), RGBm, 3);
|
||
}
|
||
for (int c=0; c < width; c++) {
|
||
memcpy(data_.data()+3*(height/2*width+c), RGBm, 3);
|
||
}
|
||
}
|
||
|
||
// random dots
|
||
else if (mark == mjMARK_RANDOM && random > 0) {
|
||
randomdot(data_.data(), markrgb, width, height, random);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// make builtin: Cube
|
||
void mjCTexture::BuiltinCube(void) {
|
||
std::byte RGB1[3], RGB2[3], RGBm[3], RGBi[3];
|
||
int w = width;
|
||
if (w > std::numeric_limits<int>::max() / w) {
|
||
throw mjCError(this, "Cube texture width is too large.");
|
||
}
|
||
int ww = width*width;
|
||
|
||
// convert fixed colors
|
||
for (int j = 0; j < 3; j++) {
|
||
RGB1[j] = (std::byte)(255 * rgb1[j]);
|
||
RGB2[j] = (std::byte)(255 * rgb2[j]);
|
||
RGBm[j] = (std::byte)(255 * markrgb[j]);
|
||
}
|
||
|
||
//------------------ faces
|
||
|
||
// gradient
|
||
if (builtin == mjBUILTIN_GRADIENT) {
|
||
if (ww > std::numeric_limits<int>::max() / 18) {
|
||
throw mjCError(this, "Gradient texture width is too large.");
|
||
}
|
||
for (int r = 0; r < w; r++) {
|
||
for (int c = 0; c < w; c++) {
|
||
// compute normalized pixel coordinates
|
||
double x = 2 * c / ((double)(w - 1)) - 1;
|
||
double y = 1 - 2 * r / ((double)(w - 1));
|
||
|
||
// compute normalized elevation for sides and up/down
|
||
double elside = asin(y / sqrt(1 + x * x + y * y)) / (0.5 * mjPI);
|
||
double elup = 1 - acos(1.0 / sqrt(1 + x * x + y * y)) / (0.5 * mjPI);
|
||
|
||
// set sides
|
||
interp(RGBi, rgb1, rgb2, elside);
|
||
memcpy(data_.data() + 0 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 0: right
|
||
memcpy(data_.data() + 1 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 1: left
|
||
memcpy(data_.data() + 4 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 4: front
|
||
memcpy(data_.data() + 5 * 3 * ww + 3 * (r * w + c), RGBi, 3); // 5: back
|
||
|
||
// set up and down
|
||
interp(data_.data() + 2 * 3 * ww + 3 * (r * w + c), rgb1, rgb2, elup); // 2: up
|
||
interp(data_.data() + 3 * 3 * ww + 3 * (r * w + c), rgb1, rgb2, -elup); // 3: down
|
||
}
|
||
}
|
||
}
|
||
|
||
// checker
|
||
else if (builtin == mjBUILTIN_CHECKER) {
|
||
checker(data_.data() + 0 * 3 * ww, RGB1, RGB2, w, w);
|
||
checker(data_.data() + 1 * 3 * ww, RGB1, RGB2, w, w);
|
||
checker(data_.data() + 2 * 3 * ww, RGB1, RGB2, w, w);
|
||
checker(data_.data() + 3 * 3 * ww, RGB1, RGB2, w, w);
|
||
checker(data_.data() + 4 * 3 * ww, RGB2, RGB1, w, w);
|
||
checker(data_.data() + 5 * 3 * ww, RGB2, RGB1, w, w);
|
||
}
|
||
|
||
// flat
|
||
else if (builtin == mjBUILTIN_FLAT) {
|
||
for (int r = 0; r < w; r++) {
|
||
for (int c = 0; c < w; c++) {
|
||
// set sides and up
|
||
memcpy(data_.data() + 0 * 3 * ww + 3 * (r * w + c), RGB1, 3);
|
||
memcpy(data_.data() + 1 * 3 * ww + 3 * (r * w + c), RGB1, 3);
|
||
memcpy(data_.data() + 2 * 3 * ww + 3 * (r * w + c), RGB1, 3);
|
||
memcpy(data_.data() + 4 * 3 * ww + 3 * (r * w + c), RGB1, 3);
|
||
memcpy(data_.data() + 5 * 3 * ww + 3 * (r * w + c), RGB1, 3);
|
||
|
||
// set down
|
||
memcpy(data_.data() + 3 * 3 * ww + 3 * (r * w + c), RGB2, 3);
|
||
}
|
||
}
|
||
}
|
||
|
||
//------------------ marks
|
||
|
||
// edge
|
||
if (mark == mjMARK_EDGE) {
|
||
for (int j = 0; j < 6; j++) {
|
||
for (int r = 0; r < w; r++) {
|
||
memcpy(data_.data() + j * 3 * ww + 3 * (r * w + 0), RGBm, 3);
|
||
memcpy(data_.data() + j * 3 * ww + 3 * (r * w + w - 1), RGBm, 3);
|
||
}
|
||
for (int c = 0; c < w; c++) {
|
||
memcpy(data_.data() + j * 3 * ww + 3 * (0 * w + c), RGBm, 3);
|
||
memcpy(data_.data() + j * 3 * ww + 3 * ((w - 1) * w + c), RGBm, 3);
|
||
}
|
||
}
|
||
}
|
||
|
||
// cross
|
||
else if (mark == mjMARK_CROSS) {
|
||
for (int j = 0; j < 6; j++) {
|
||
for (int r = 0; r < w; r++) {
|
||
memcpy(data_.data() + j * 3 * ww + 3 * (r * w + w / 2), RGBm, 3);
|
||
}
|
||
for (int c = 0; c < w; c++) {
|
||
memcpy(data_.data() + j * 3 * ww + 3 * (w / 2 * w + c), RGBm, 3);
|
||
}
|
||
}
|
||
}
|
||
|
||
// random dots
|
||
else if (mark == mjMARK_RANDOM && random > 0) {
|
||
randomdot(data_.data(), markrgb, w, height, random);
|
||
}
|
||
}
|
||
|
||
// load PNG file
|
||
void mjCTexture::LoadPNG(mjResource* resource,
|
||
std::vector<unsigned char>& image,
|
||
unsigned int& w, unsigned int& h, bool& is_srgb) {
|
||
LodePNGColorType color_type;
|
||
if (nchannel == 4) {
|
||
color_type = LCT_RGBA;
|
||
} else if (nchannel == 3) {
|
||
color_type = LCT_RGB;
|
||
} else if (nchannel == 1) {
|
||
color_type = LCT_GREY;
|
||
} else {
|
||
throw mjCError(this, "Unsupported number of channels: %s",
|
||
std::to_string(nchannel).c_str());
|
||
}
|
||
PNGImage png_image = PNGImage::Load(this, resource, color_type);
|
||
w = png_image.Width();
|
||
h = png_image.Height();
|
||
is_srgb = png_image.IsSRGB();
|
||
image = png_image.MoveData();
|
||
}
|
||
|
||
// load KTX file
|
||
void mjCTexture::LoadKTX(mjResource* resource,
|
||
std::vector<unsigned char>& image, unsigned int& w,
|
||
unsigned int& h, bool& is_srgb) {
|
||
const void* buffer = 0;
|
||
int buffer_sz = mju_readResource(resource, &buffer);
|
||
|
||
// still not found
|
||
if (buffer_sz < 0) {
|
||
throw mjCError(this, "could not read texture file '%s'", resource->name);
|
||
} else if (!buffer_sz) {
|
||
throw mjCError(this, "texture file is empty: '%s'", resource->name);
|
||
}
|
||
|
||
w = buffer_sz;
|
||
h = 1;
|
||
is_srgb = false;
|
||
|
||
image.resize(buffer_sz);
|
||
memcpy(image.data(), buffer, buffer_sz);
|
||
}
|
||
|
||
// load custom file
|
||
void mjCTexture::LoadCustom(mjResource* resource,
|
||
std::vector<unsigned char>& image,
|
||
unsigned int& w, unsigned int& h, bool& is_srgb) {
|
||
const void* buffer = 0;
|
||
int buffer_sz = mju_readResource(resource, &buffer);
|
||
|
||
// still not found
|
||
if (buffer_sz < 0) {
|
||
throw mjCError(this, "could not read texture file '%s'", resource->name);
|
||
} else if (!buffer_sz) {
|
||
throw mjCError(this, "texture file is empty: '%s'", resource->name);
|
||
}
|
||
|
||
|
||
// read dimensions
|
||
int* pint = (int*)buffer;
|
||
w = pint[0];
|
||
h = pint[1];
|
||
|
||
// assume linear color space
|
||
is_srgb = false;
|
||
|
||
// check dimensions
|
||
if (w < 1 || h < 1) {
|
||
throw mjCError(this, "Non-PNG texture, assuming custom binary file format,\n"
|
||
"non-positive texture dimensions in file '%s'", resource->name);
|
||
}
|
||
|
||
// check buffer size
|
||
if (buffer_sz != 2*sizeof(int) + w*h*3*sizeof(char)) {
|
||
throw mjCError(this, "Non-PNG texture, assuming custom binary file format,\n"
|
||
"unexpected file size in file '%s'", resource->name);
|
||
}
|
||
|
||
// allocate and copy
|
||
image.resize(w*h*3);
|
||
memcpy(image.data(), (void*)(pint+2), w*h*3*sizeof(char));
|
||
}
|
||
|
||
|
||
|
||
// load from PNG or custom file, flip if specified
|
||
void mjCTexture::LoadFlip(std::string filename, const mjVFS* vfs,
|
||
std::vector<unsigned char>& image,
|
||
unsigned int& w, unsigned int& h, bool& is_srgb) {
|
||
std::string asset_type = GetAssetContentType(filename, content_type_);
|
||
|
||
// fallback to custom
|
||
if (asset_type.empty()) {
|
||
asset_type = "image/vnd.mujoco.texture";
|
||
}
|
||
|
||
if (asset_type != "image/png" && asset_type != "image/ktx" && asset_type != "image/vnd.mujoco.texture") {
|
||
throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str());
|
||
}
|
||
|
||
mjResource* resource = LoadResource(modelfiledir_.Str(), filename, vfs);
|
||
|
||
try {
|
||
if (asset_type == "image/png") {
|
||
LoadPNG(resource, image, w, h, is_srgb);
|
||
} else if (asset_type == "image/ktx") {
|
||
if (hflip || vflip) {
|
||
throw mjCError(this, "cannot flip KTX textures");
|
||
}
|
||
LoadKTX(resource, image, w, h, is_srgb);
|
||
} else {
|
||
LoadCustom(resource, image, w, h, is_srgb);
|
||
}
|
||
mju_closeResource(resource);
|
||
} catch(mjCError err) {
|
||
mju_closeResource(resource);
|
||
throw err;
|
||
}
|
||
|
||
// horizontal flip
|
||
if (hflip) {
|
||
if (nchannel != 3) {
|
||
throw mjCError(
|
||
this, "currently only 3-channel textures support horizontal flip");
|
||
}
|
||
for (int r=0; r < h; r++) {
|
||
for (int c=0; c < w/2; c++) {
|
||
int c1 = w-1-c;
|
||
unsigned char tmp[3] = {
|
||
image[3*(r*w+c)],
|
||
image[3*(r*w+c)+1],
|
||
image[3*(r*w+c)+2]
|
||
};
|
||
|
||
image[3*(r*w+c)] = image[3*(r*w+c1)];
|
||
image[3*(r*w+c)+1] = image[3*(r*w+c1)+1];
|
||
image[3*(r*w+c)+2] = image[3*(r*w+c1)+2];
|
||
|
||
image[3*(r*w+c1)] = tmp[0];
|
||
image[3*(r*w+c1)+1] = tmp[1];
|
||
image[3*(r*w+c1)+2] = tmp[2];
|
||
}
|
||
}
|
||
}
|
||
|
||
// vertical flip
|
||
if (vflip) {
|
||
if (nchannel != 3) {
|
||
throw mjCError(
|
||
this, "currently only 3-channel textures support vertical flip");
|
||
}
|
||
for (int r=0; r < h/2; r++) {
|
||
for (int c=0; c < w; c++) {
|
||
int r1 = h-1-r;
|
||
unsigned char tmp[3] = {
|
||
image[3*(r*w+c)],
|
||
image[3*(r*w+c)+1],
|
||
image[3*(r*w+c)+2]
|
||
};
|
||
|
||
image[3*(r*w+c)] = image[3*(r1*w+c)];
|
||
image[3*(r*w+c)+1] = image[3*(r1*w+c)+1];
|
||
image[3*(r*w+c)+2] = image[3*(r1*w+c)+2];
|
||
|
||
image[3*(r1*w+c)] = tmp[0];
|
||
image[3*(r1*w+c)+1] = tmp[1];
|
||
image[3*(r1*w+c)+2] = tmp[2];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// load 2D
|
||
void mjCTexture::Load2D(std::string filename, const mjVFS* vfs) {
|
||
// load PNG or custom
|
||
unsigned int w, h;
|
||
bool is_srgb;
|
||
std::vector<unsigned char> image;
|
||
LoadFlip(filename, vfs, image, w, h, is_srgb);
|
||
|
||
// assign size
|
||
width = w;
|
||
height = h;
|
||
if (colorspace == mjCOLORSPACE_AUTO) {
|
||
colorspace = is_srgb ? mjCOLORSPACE_SRGB : mjCOLORSPACE_LINEAR;
|
||
}
|
||
|
||
// allocate and copy data
|
||
std::int64_t size = static_cast<std::int64_t>(width)*height;
|
||
if (size >= std::numeric_limits<int>::max() / nchannel || size <= 0) {
|
||
throw mjCError(this, "Texture too large");
|
||
}
|
||
try {
|
||
data_.assign(nchannel*size, std::byte(0));
|
||
} catch (const std::bad_alloc& e) {
|
||
throw mjCError(this, "Could not allocate memory for texture '%s' (id %d)",
|
||
(const char*)file_.c_str(), id);
|
||
}
|
||
memcpy(data_.data(), image.data(), nchannel*size);
|
||
image.clear();
|
||
}
|
||
|
||
|
||
|
||
// load cube or skybox from single file (repeated or grid)
|
||
void mjCTexture::LoadCubeSingle(std::string filename, const mjVFS* vfs) {
|
||
// check gridsize
|
||
if (gridsize[0] < 1 || gridsize[1] < 1 || gridsize[0]*gridsize[1] > 12) {
|
||
throw mjCError(this, "gridsize must be non-zero and no more than 12 squares in texture");
|
||
}
|
||
|
||
// load PNG or custom
|
||
unsigned int w, h;
|
||
bool is_srgb;
|
||
std::vector<unsigned char> image;
|
||
LoadFlip(filename, vfs, image, w, h, is_srgb);
|
||
|
||
if (colorspace == mjCOLORSPACE_AUTO) {
|
||
colorspace = is_srgb ? mjCOLORSPACE_SRGB : mjCOLORSPACE_LINEAR;
|
||
}
|
||
|
||
// check gridsize for compatibility
|
||
if (w/gridsize[1] != h/gridsize[0] || (w%gridsize[1]) || (h%gridsize[0])) {
|
||
throw mjCError(this,
|
||
"PNG size must be integer multiple of gridsize in texture '%s' (id %d)",
|
||
(const char*)file_.c_str(), id);
|
||
}
|
||
|
||
// assign size: repeated or full
|
||
if (gridsize[0] == 1 && gridsize[1] == 1) {
|
||
width = height = w;
|
||
} else {
|
||
width = w/gridsize[1];
|
||
if (width >= std::numeric_limits<int>::max()/6) {
|
||
throw mjCError(this, "Invalid width of cube texture");
|
||
}
|
||
height = 6*width;
|
||
}
|
||
|
||
// allocate data
|
||
std::int64_t size = static_cast<std::int64_t>(width)*height;
|
||
if (size >= std::numeric_limits<int>::max() / 3 || size <= 0) {
|
||
throw mjCError(this, "Cube texture too large");
|
||
}
|
||
try {
|
||
data_.assign(3*size, std::byte(0));
|
||
} catch (const std::bad_alloc& e) {
|
||
throw mjCError(this,
|
||
"Could not allocate memory for texture '%s' (id %d)",
|
||
(const char*)file_.c_str(), id);
|
||
}
|
||
|
||
// copy: repeated
|
||
if (gridsize[0] == 1 && gridsize[1] == 1) {
|
||
memcpy(data_.data(), image.data(), 3*width*width);
|
||
}
|
||
|
||
// copy: grid
|
||
else {
|
||
// keep track of which faces were defined
|
||
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
||
|
||
// process grid
|
||
for (int k=0; k < gridsize[0]*gridsize[1]; k++) {
|
||
// decode face symbol
|
||
int i = -1;
|
||
if (gridlayout[k] == 'R') {
|
||
i = 0;
|
||
} else if (gridlayout[k] == 'L') {
|
||
i = 1;
|
||
} else if (gridlayout[k] == 'U') {
|
||
i = 2;
|
||
} else if (gridlayout[k] == 'D') {
|
||
i = 3;
|
||
} else if (gridlayout[k] == 'F') {
|
||
i = 4;
|
||
} else if (gridlayout[k] == 'B') {
|
||
i = 5;
|
||
} else if (gridlayout[k] != '.')
|
||
throw mjCError(this, "gridlayout symbol is not among '.RLUDFB' in texture");
|
||
|
||
// load if specified
|
||
if (i >= 0) {
|
||
// extract sub-image
|
||
int rstart = width*(k/gridsize[1]);
|
||
int cstart = width*(k%gridsize[1]);
|
||
for (int j=0; j < width; j++) {
|
||
memcpy(data_.data()+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
|
||
}
|
||
|
||
// mark as defined
|
||
loaded[i] = 1;
|
||
}
|
||
}
|
||
|
||
// set undefined faces to rgb1
|
||
for (int i=0; i < 6; i++) {
|
||
if (!loaded[i]) {
|
||
for (int k=0; k < width; k++) {
|
||
for (int s=0; s < width; s++) {
|
||
for (int j=0; j < 3; j++) {
|
||
data_[i*3*width*width + 3*(k*width+s) + j] = (std::byte)(255*rgb1[j]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
image.clear();
|
||
}
|
||
|
||
|
||
|
||
// load cube or skybox from separate file
|
||
void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
|
||
// keep track of which faces were defined
|
||
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
||
|
||
// process nonempty files
|
||
for (int i=0; i < 6; i++) {
|
||
if (!cubefiles_[i].empty()) {
|
||
// remove path from file if necessary
|
||
if (model->strippath) {
|
||
cubefiles_[i] = mjuu_strippath(cubefiles_[i]);
|
||
}
|
||
|
||
// make filename
|
||
FilePath filename = texturedir_ + FilePath(cubefiles_[i]);
|
||
|
||
// load PNG or custom
|
||
unsigned int w, h;
|
||
bool is_srgb;
|
||
std::vector<unsigned char> image;
|
||
LoadFlip(filename.Str(), vfs, image, w, h, is_srgb);
|
||
|
||
// assume all faces have the same colorspace
|
||
if (colorspace == mjCOLORSPACE_AUTO) {
|
||
colorspace = is_srgb ? mjCOLORSPACE_SRGB : mjCOLORSPACE_LINEAR;
|
||
}
|
||
|
||
// PNG must be square
|
||
if (w != h) {
|
||
throw mjCError(this,
|
||
"Non-square PNG file '%s' in cube or skybox id %d",
|
||
(const char*)cubefiles_[i].c_str(), id);
|
||
}
|
||
|
||
// first file: set size and allocate data
|
||
if (data_.empty()) {
|
||
width = w;
|
||
if (width >= std::numeric_limits<int>::max()/6) {
|
||
throw mjCError(this, "Invalid width of builtin texture");
|
||
}
|
||
height = 6*width;
|
||
std::int64_t size = static_cast<std::int64_t>(width)*height;
|
||
if (size >= std::numeric_limits<int>::max() / 3 || size <= 0) {
|
||
throw mjCError(this, "PNG texture too large");
|
||
}
|
||
try {
|
||
data_.assign(3*size, std::byte(0));
|
||
} catch (const std::bad_alloc& e) {
|
||
throw mjCError(this, "Could not allocate memory for texture");
|
||
}
|
||
}
|
||
|
||
// otherwise check size
|
||
else if (width != w) {
|
||
throw mjCError(this,
|
||
"PNG file '%s' has incompatible size in texture id %d",
|
||
(const char*)cubefiles_[i].c_str(), id);
|
||
}
|
||
|
||
// copy data
|
||
memcpy(data_.data()+i*3*width*width, image.data(), 3*width*width);
|
||
image.clear();
|
||
|
||
// mark as defined
|
||
loaded[i] = 1;
|
||
}
|
||
}
|
||
|
||
// set undefined faces to rgb1
|
||
for (int i=0; i < 6; i++) {
|
||
if (!loaded[i]) {
|
||
for (int k=0; k < width; k++) {
|
||
for (int s=0; s < width; s++) {
|
||
for (int j=0; j < 3; j++) {
|
||
data_[i*3*width*width + 3*(k*width+s) + j] = (std::byte)(255*rgb1[j]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCTexture::Compile(const mjVFS* vfs) {
|
||
CopyFromSpec();
|
||
|
||
// copy paths from model if not already defined
|
||
if (modelfiledir_.empty()) {
|
||
modelfiledir_ = FilePath(model->modelfiledir_);
|
||
}
|
||
if (texturedir_.empty()) {
|
||
texturedir_ = FilePath(model->texturedir_);
|
||
}
|
||
|
||
// buffer from user
|
||
if (!data_.empty()) {
|
||
if (data_.size() != nchannel*width*height) {
|
||
throw mjCError(this, "Texture buffer has incorrect size, given %d expected %d", nullptr,
|
||
data_.size(), nchannel * width * height);
|
||
}
|
||
return;
|
||
}
|
||
|
||
// builtin
|
||
else if (builtin != mjBUILTIN_NONE) {
|
||
// check width
|
||
if (width < 1) {
|
||
throw mjCError(this, "Invalid width of builtin texture");
|
||
}
|
||
|
||
// adjust height of cube texture
|
||
if (type != mjTEXTURE_2D) {
|
||
if (width >= std::numeric_limits<int>::max()/6) {
|
||
throw mjCError(this, "Invalid width of builtin texture");
|
||
}
|
||
height = 6*width;
|
||
} else {
|
||
if (height < 1) {
|
||
throw mjCError(this, "Invalid height of builtin texture");
|
||
}
|
||
}
|
||
|
||
std::int64_t size = static_cast<std::int64_t>(width)*height;
|
||
if (size >= std::numeric_limits<int>::max() / nchannel || size <= 0) {
|
||
throw mjCError(this, "Builtin texture too large");
|
||
}
|
||
// allocate data
|
||
try {
|
||
data_.assign(nchannel*size, std::byte(0));
|
||
} catch (const std::bad_alloc& e) {
|
||
throw mjCError(this, "Could not allocate memory for texture");
|
||
}
|
||
|
||
// dispatch
|
||
if (type == mjTEXTURE_2D) {
|
||
Builtin2D();
|
||
} else {
|
||
BuiltinCube();
|
||
}
|
||
}
|
||
|
||
// single file
|
||
else if (!file_.empty()) {
|
||
// remove path from file if necessary
|
||
if (model->strippath) {
|
||
file_ = mjuu_strippath(file_);
|
||
}
|
||
|
||
// make filename
|
||
FilePath filename = texturedir_ + FilePath(file_);
|
||
|
||
// dispatch
|
||
if (type == mjTEXTURE_2D) {
|
||
Load2D(filename.Str(), vfs);
|
||
} else {
|
||
LoadCubeSingle(filename.Str(), vfs);
|
||
}
|
||
}
|
||
|
||
// separate files
|
||
else {
|
||
// 2D not allowed
|
||
if (type == mjTEXTURE_2D) {
|
||
throw mjCError(this,
|
||
"Cannot load 2D texture from separate files, texture");
|
||
}
|
||
|
||
// at least one cubefile must be defined
|
||
bool defined = false;
|
||
for (int i=0; i < 6; i++) {
|
||
if (!cubefiles_[i].empty()) {
|
||
defined = true;
|
||
break;
|
||
}
|
||
}
|
||
if (!defined) {
|
||
throw mjCError(this,
|
||
"No cubefiles_ defined in cube or skybox texture");
|
||
}
|
||
|
||
// only cube and skybox
|
||
LoadCubeSeparate(vfs);
|
||
}
|
||
|
||
// make sure someone allocated data; SHOULD NOT OCCUR
|
||
if (data_.empty()) {
|
||
throw mjCError(this, "texture '%s' (id %d) was not specified", name.c_str(), id);
|
||
}
|
||
|
||
// if recompiled is called, clear data_ first
|
||
clear_data_ = true;
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCMaterial implementation ----------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCMaterial::mjCMaterial(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultMaterial(&spec);
|
||
elemtype = mjOBJ_MATERIAL;
|
||
textures_.assign(mjNTEXROLE, "");
|
||
spec_textures_.assign(mjNTEXROLE, "");
|
||
|
||
// clear internal
|
||
for (int i=0; i < mjNTEXROLE; i++) {
|
||
texid[i] = -1;
|
||
}
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Material();
|
||
}
|
||
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
PointToLocal();
|
||
|
||
// in case this material is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCMaterial::mjCMaterial(const mjCMaterial& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCMaterial& mjCMaterial::operator=(const mjCMaterial& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCMaterial_*>(this) = static_cast<const mjCMaterial_&>(other);
|
||
*static_cast<mjsMaterial*>(this) = static_cast<const mjsMaterial&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCMaterial::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.textures = &spec_textures_;
|
||
spec.info = &info;
|
||
textures = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCMaterial::CopyFromSpec() {
|
||
*static_cast<mjsMaterial*>(this) = spec;
|
||
textures_ = spec_textures_;
|
||
}
|
||
|
||
|
||
|
||
void mjCMaterial::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
for (int i=0; i < mjNTEXROLE; i++) {
|
||
if (!spec_textures_[i].empty()) {
|
||
spec_textures_[i] = m->prefix + spec_textures_[i] + m->suffix;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCMaterial::Compile(void) {
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCPair implementation --------------------------------------------------
|
||
|
||
// constructor
|
||
mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultPair(&spec);
|
||
elemtype = mjOBJ_PAIR;
|
||
|
||
// set defaults
|
||
spec_geomname1_.clear();
|
||
spec_geomname2_.clear();
|
||
|
||
// clear internal variables
|
||
geom1 = nullptr;
|
||
geom2 = nullptr;
|
||
signature = -1;
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Pair();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this camera is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCPair::mjCPair(const mjCPair& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCPair& mjCPair::operator=(const mjCPair& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCPair_*>(this) = static_cast<const mjCPair_&>(other);
|
||
*static_cast<mjsPair*>(this) = static_cast<const mjsPair&>(other);
|
||
this->geom1 = nullptr;
|
||
this->geom2 = nullptr;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCPair::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.geomname1 = &spec_geomname1_;
|
||
spec.geomname2 = &spec_geomname2_;
|
||
geomname1 = nullptr;
|
||
geomname2 = nullptr;
|
||
spec.info = &info;
|
||
}
|
||
|
||
|
||
|
||
void mjCPair::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
prefix = m->prefix;
|
||
suffix = m->suffix;
|
||
}
|
||
|
||
|
||
|
||
void mjCPair::CopyFromSpec() {
|
||
*static_cast<mjsPair*>(this) = spec;
|
||
geomname1_ = spec_geomname1_;
|
||
geomname2_ = spec_geomname2_;
|
||
}
|
||
|
||
|
||
|
||
void mjCPair::ResolveReferences(const mjCModel* m) {
|
||
geomname1_ = prefix + geomname1_ + suffix;
|
||
geomname2_ = prefix + geomname2_ + suffix;
|
||
geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_);
|
||
geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
|
||
|
||
if (!geom1 && geom2) {
|
||
geomname1_ = spec_geomname1_;
|
||
geom1 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname1_);
|
||
}
|
||
if (geom1 && !geom2) {
|
||
geomname2_ = spec_geomname2_;
|
||
geom2 = (mjCGeom*)m->FindObject(mjOBJ_GEOM, geomname2_);
|
||
}
|
||
|
||
if (!geom1) {
|
||
throw mjCError(this, "geom '%s' not found in collision %d", geomname1_.c_str(), id);
|
||
}
|
||
if (!geom2) {
|
||
throw mjCError(this, "geom '%s' not found in collision %d", geomname2_.c_str(), id);
|
||
}
|
||
|
||
spec_geomname1_ = geomname1_;
|
||
spec_geomname2_ = geomname2_;
|
||
prefix.clear();
|
||
suffix.clear();
|
||
|
||
// swap if body1 > body2
|
||
if (geom1->body->id > geom2->body->id) {
|
||
std::string nametmp = geomname1_;
|
||
geomname1_ = geomname2_;
|
||
geomname2_ = nametmp;
|
||
|
||
mjCGeom* geomtmp = geom1;
|
||
geom1 = geom2;
|
||
geom2 = geomtmp;
|
||
}
|
||
|
||
// get geom ids and body signature
|
||
signature = ((geom1->body->id)<<16) + geom2->body->id;
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCPair::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// check condim
|
||
if (condim != 1 && condim != 3 && condim != 4 && condim != 6) {
|
||
throw mjCError(this, "invalid condim in contact pair");
|
||
}
|
||
|
||
// find geoms
|
||
ResolveReferences(model);
|
||
|
||
// mark geoms as not visual
|
||
geom1->SetNotVisual();
|
||
geom2->SetNotVisual();
|
||
|
||
// set undefined margin: max
|
||
if (!mjuu_defined(margin)) {
|
||
margin = std::max(geom1->margin, geom2->margin);
|
||
}
|
||
|
||
// set undefined gap: max
|
||
if (!mjuu_defined(gap)) {
|
||
gap = std::max(geom1->gap, geom2->gap);
|
||
}
|
||
|
||
// set undefined condim, friction, solref, solimp: different priority
|
||
if (geom1->priority != geom2->priority) {
|
||
mjCGeom* pgh = (geom1->priority > geom2->priority ? geom1 : geom2);
|
||
|
||
// condim
|
||
if (condim < 0) {
|
||
condim = pgh->condim;
|
||
}
|
||
|
||
// friction
|
||
if (!mjuu_defined(friction[0])) {
|
||
friction[0] = friction[1] = pgh->friction[0];
|
||
friction[2] = pgh->friction[1];
|
||
friction[3] = friction[4] = pgh->friction[2];
|
||
}
|
||
|
||
// reference
|
||
if (!mjuu_defined(solref[0])) {
|
||
for (int i=0; i < mjNREF; i++) {
|
||
solref[i] = pgh->solref[i];
|
||
}
|
||
}
|
||
|
||
// impedance
|
||
if (!mjuu_defined(solimp[0])) {
|
||
for (int i=0; i < mjNIMP; i++) {
|
||
solimp[i] = pgh->solimp[i];
|
||
}
|
||
}
|
||
}
|
||
|
||
// set undefined condim, friction, solref, solimp: same priority
|
||
else {
|
||
// condim: max
|
||
if (condim < 0) {
|
||
condim = std::max(geom1->condim, geom2->condim);
|
||
}
|
||
|
||
// friction: max
|
||
if (!mjuu_defined(friction[0])) {
|
||
friction[0] = friction[1] = std::max(geom1->friction[0], geom2->friction[0]);
|
||
friction[2] = std::max(geom1->friction[1], geom2->friction[1]);
|
||
friction[3] = friction[4] = std::max(geom1->friction[2], geom2->friction[2]);
|
||
}
|
||
|
||
// solver mix factor
|
||
double mix;
|
||
if (geom1->solmix >= mjEPS && geom2->solmix >= mjEPS) {
|
||
mix = geom1->solmix / (geom1->solmix + geom2->solmix);
|
||
} else if (geom1->solmix < mjEPS && geom2->solmix < mjEPS) {
|
||
mix = 0.5;
|
||
} else if (geom1->solmix < mjEPS) {
|
||
mix = 0.0;
|
||
} else {
|
||
mix = 1.0;
|
||
}
|
||
|
||
// reference
|
||
if (!mjuu_defined(solref[0])) {
|
||
// standard: mix
|
||
if (solref[0] > 0) {
|
||
for (int i=0; i < mjNREF; i++) {
|
||
solref[i] = mix*geom1->solref[i] + (1-mix)*geom2->solref[i];
|
||
}
|
||
}
|
||
|
||
// direct: min
|
||
else {
|
||
for (int i=0; i < mjNREF; i++) {
|
||
solref[i] = std::min(geom1->solref[i], geom2->solref[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
// impedance
|
||
if (!mjuu_defined(solimp[0])) {
|
||
for (int i=0; i < mjNIMP; i++) {
|
||
solimp[i] = mix*geom1->solimp[i] + (1-mix)*geom2->solimp[i];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCBodyPair implementation ----------------------------------------------
|
||
|
||
// constructor
|
||
mjCBodyPair::mjCBodyPair(mjCModel* _model) {
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
elemtype = mjOBJ_EXCLUDE;
|
||
|
||
// set defaults
|
||
spec_bodyname1_.clear();
|
||
spec_bodyname2_.clear();
|
||
|
||
// clear internal variables
|
||
body1 = body2 = signature = -1;
|
||
|
||
PointToLocal();
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCBodyPair::mjCBodyPair(const mjCBodyPair& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCBodyPair& mjCBodyPair::operator=(const mjCBodyPair& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCBodyPair_*>(this) = static_cast<const mjCBodyPair_&>(other);
|
||
*static_cast<mjsExclude*>(this) = static_cast<const mjsExclude&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCBodyPair::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.bodyname1 = &spec_bodyname1_;
|
||
spec.bodyname2 = &spec_bodyname2_;
|
||
spec.info = &info;
|
||
bodyname1 = nullptr;
|
||
bodyname2 = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCBodyPair::NameSpace(const mjCModel* m) {
|
||
if (!name.empty()) {
|
||
name = m->prefix + name + m->suffix;
|
||
}
|
||
prefix = m->prefix;
|
||
suffix = m->suffix;
|
||
}
|
||
|
||
|
||
|
||
void mjCBodyPair::CopyFromSpec() {
|
||
*static_cast<mjsExclude*>(this) = spec;
|
||
bodyname1_ = spec_bodyname1_;
|
||
bodyname2_ = spec_bodyname2_;
|
||
}
|
||
|
||
|
||
|
||
void mjCBodyPair::ResolveReferences(const mjCModel* m) {
|
||
bodyname1_ = prefix + bodyname1_ + suffix;
|
||
bodyname2_ = prefix + bodyname2_ + suffix;
|
||
mjCBody* pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_);
|
||
mjCBody* pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
|
||
|
||
if (!pb1 && pb2) {
|
||
bodyname1_ = spec_bodyname1_;
|
||
pb1 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname1_);
|
||
}
|
||
if (pb1 && !pb2) {
|
||
bodyname2_ = spec_bodyname2_;
|
||
pb2 = (mjCBody*)m->FindObject(mjOBJ_BODY, bodyname2_);
|
||
}
|
||
|
||
if (!pb1) {
|
||
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname1_.c_str(), id);
|
||
}
|
||
if (!pb2) {
|
||
throw mjCError(this, "body '%s' not found in bodypair %d", bodyname2_.c_str(), id);
|
||
}
|
||
|
||
spec_bodyname1_ = bodyname1_;
|
||
spec_bodyname2_ = bodyname2_;
|
||
prefix.clear();
|
||
suffix.clear();
|
||
|
||
// swap if body1 > body2
|
||
if (pb1->id > pb2->id) {
|
||
std::string nametmp = bodyname1_;
|
||
bodyname1_ = bodyname2_;
|
||
bodyname2_ = nametmp;
|
||
|
||
mjCBody* bodytmp = pb1;
|
||
pb1 = pb2;
|
||
pb2 = bodytmp;
|
||
}
|
||
|
||
// get body ids and body signature
|
||
body1 = pb1->id;
|
||
body2 = pb2->id;
|
||
signature = (body1<<16) + body2;
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCBodyPair::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// find bodies
|
||
ResolveReferences(model);
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCEquality implementation ----------------------------------------------
|
||
|
||
// initialize default constraint
|
||
mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultEquality(&spec);
|
||
elemtype = mjOBJ_EQUALITY;
|
||
|
||
// clear internal variables
|
||
spec_name1_.clear();
|
||
spec_name2_.clear();
|
||
obj1id = obj2id = -1;
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Equality();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this camera is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCEquality::mjCEquality(const mjCEquality& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCEquality& mjCEquality::operator=(const mjCEquality& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCEquality_*>(this) = static_cast<const mjCEquality_&>(other);
|
||
*static_cast<mjsEquality*>(this) = static_cast<const mjsEquality&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCEquality::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.name1 = &spec_name1_;
|
||
spec.name2 = &spec_name2_;
|
||
spec.info = &info;
|
||
name1 = nullptr;
|
||
name2 = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCEquality::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!spec_name1_.empty()) {
|
||
spec_name1_ = m->prefix + spec_name1_ + m->suffix;
|
||
}
|
||
if (!spec_name2_.empty()) {
|
||
spec_name2_ = m->prefix + spec_name2_ + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCEquality::CopyFromSpec() {
|
||
*static_cast<mjsEquality*>(this) = spec;
|
||
name1_ = spec_name1_;
|
||
name2_ = spec_name2_;
|
||
}
|
||
|
||
|
||
|
||
void mjCEquality::ResolveReferences(const mjCModel* m) {
|
||
mjtObj object_type;
|
||
mjCBase *px1, *px2;
|
||
mjtJoint jt1, jt2;
|
||
|
||
// determine object type
|
||
if (type == mjEQ_WELD) {
|
||
if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
|
||
throw mjCError(this, "weld constraint supports only sites and bodies");
|
||
}
|
||
object_type = objtype;
|
||
} else if (type == mjEQ_CONNECT) {
|
||
if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
|
||
throw mjCError(this, "connect constraint supports only sites and bodies");
|
||
}
|
||
object_type = objtype;
|
||
} else if (type == mjEQ_JOINT) {
|
||
object_type = mjOBJ_JOINT;
|
||
} else if (type == mjEQ_TENDON) {
|
||
object_type = mjOBJ_TENDON;
|
||
} else if (type == mjEQ_FLEX) {
|
||
object_type = mjOBJ_FLEX;
|
||
} else {
|
||
throw mjCError(this, "invalid type in equality constraint");
|
||
}
|
||
|
||
// find object 1, get id
|
||
px1 = m->FindObject(object_type, name1_);
|
||
if (!px1) {
|
||
throw mjCError(this, "unknown element '%s' in equality constraint", name1_.c_str());
|
||
}
|
||
obj1id = px1->id;
|
||
|
||
// find object 2, get id
|
||
if (!name2_.empty()) {
|
||
px2 = m->FindObject(object_type, name2_);
|
||
if (!px2) {
|
||
throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id);
|
||
}
|
||
obj2id = px2->id;
|
||
} else {
|
||
// object 2 unspecified: set to -1
|
||
obj2id = -1;
|
||
px2 = nullptr;
|
||
}
|
||
|
||
// set missing body = world
|
||
if (object_type == mjOBJ_BODY && obj2id == -1) {
|
||
obj2id = 0;
|
||
}
|
||
|
||
// make sure the two objects are different
|
||
if (obj1id == obj2id) {
|
||
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id);
|
||
}
|
||
|
||
// make sure joints are scalar
|
||
if (type == mjEQ_JOINT) {
|
||
jt1 = ((mjCJoint*)px1)->type;
|
||
jt2 = (px2 ? ((mjCJoint*)px2)->type : mjJNT_HINGE);
|
||
if ((jt1 != mjJNT_HINGE && jt1 != mjJNT_SLIDE) ||
|
||
(jt2 != mjJNT_HINGE && jt2 != mjJNT_SLIDE)) {
|
||
throw mjCError(this, "only HINGE and SLIDE joint allowed in constraint");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCEquality::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// find objects
|
||
ResolveReferences(model);
|
||
|
||
// make sure flex is not rigid
|
||
if (type == mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
|
||
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCTendon implementation ------------------------------------------------
|
||
|
||
// constructor
|
||
mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultTendon(&spec);
|
||
elemtype = mjOBJ_TENDON;
|
||
|
||
// clear internal variables
|
||
spec_material_.clear();
|
||
spec_userdata_.clear();
|
||
path.clear();
|
||
matid = -1;
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Tendon();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this camera is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCTendon::mjCTendon(const mjCTendon& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCTendon& mjCTendon::operator=(const mjCTendon& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCTendon_*>(this) = static_cast<const mjCTendon_&>(other);
|
||
*static_cast<mjsTendon*>(this) = static_cast<const mjsTendon&>(other);
|
||
for (int i=0; i < other.path.size(); i++) {
|
||
path.push_back(new mjCWrap(*other.path[i]));
|
||
path.back()->tendon = this;
|
||
}
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
bool mjCTendon::is_limited() const {
|
||
return islimited(limited, range);
|
||
}
|
||
bool mjCTendon::is_actfrclimited() const {
|
||
return islimited(actfrclimited, actfrcrange);
|
||
}
|
||
|
||
void mjCTendon::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.material = &spec_material_;
|
||
spec.userdata = &spec_userdata_;
|
||
spec.info = &info;
|
||
material = nullptr;
|
||
userdata = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCTendon::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
prefix = m->prefix;
|
||
suffix = m->suffix;
|
||
}
|
||
|
||
|
||
|
||
void mjCTendon::CopyFromSpec() {
|
||
*static_cast<mjsTendon*>(this) = spec;
|
||
material_ = spec_material_;
|
||
userdata_ = spec_userdata_;
|
||
|
||
// clear precompiled
|
||
for (int i=0; i < path.size(); i++) {
|
||
if (path[i]->type == mjWRAP_CYLINDER) {
|
||
path[i]->type = mjWRAP_SPHERE;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// desctructor
|
||
mjCTendon::~mjCTendon() {
|
||
// delete objects allocated here
|
||
for (unsigned int i=0; i < path.size(); i++) {
|
||
delete path[i];
|
||
}
|
||
|
||
path.clear();
|
||
}
|
||
|
||
|
||
|
||
void mjCTendon::SetModel(mjCModel* _model) {
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
for (int i=0; i < path.size(); i++) {
|
||
path[i]->model = _model;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// add site as wrap object
|
||
void mjCTendon::WrapSite(std::string wrapname, std::string_view wrapinfo) {
|
||
// create wrap object
|
||
mjCWrap* wrap = new mjCWrap(model, this);
|
||
wrap->info = wrapinfo;
|
||
|
||
// set parameters, add to path
|
||
wrap->type = mjWRAP_SITE;
|
||
wrap->name = wrapname;
|
||
wrap->id = (int)path.size();
|
||
path.push_back(wrap);
|
||
}
|
||
|
||
|
||
|
||
// add geom (with side site) as wrap object
|
||
void mjCTendon::WrapGeom(std::string wrapname, std::string sidesite, std::string_view wrapinfo) {
|
||
// create wrap object
|
||
mjCWrap* wrap = new mjCWrap(model, this);
|
||
wrap->info = wrapinfo;
|
||
|
||
// set parameters, add to path
|
||
wrap->type = mjWRAP_SPHERE; // replace with cylinder later if needed
|
||
wrap->name = wrapname;
|
||
wrap->sidesite = sidesite;
|
||
wrap->id = (int)path.size();
|
||
path.push_back(wrap);
|
||
}
|
||
|
||
|
||
|
||
// add joint as wrap object
|
||
void mjCTendon::WrapJoint(std::string wrapname, double coef, std::string_view wrapinfo) {
|
||
// create wrap object
|
||
mjCWrap* wrap = new mjCWrap(model, this);
|
||
wrap->info = wrapinfo;
|
||
|
||
// set parameters, add to path
|
||
wrap->type = mjWRAP_JOINT;
|
||
wrap->name = wrapname;
|
||
wrap->prm = coef;
|
||
wrap->id = (int)path.size();
|
||
path.push_back(wrap);
|
||
}
|
||
|
||
|
||
|
||
// add pulley
|
||
void mjCTendon::WrapPulley(double divisor, std::string_view wrapinfo) {
|
||
// create wrap object
|
||
mjCWrap* wrap = new mjCWrap(model, this);
|
||
wrap->info = wrapinfo;
|
||
|
||
// set parameters, add to path
|
||
wrap->type = mjWRAP_PULLEY;
|
||
wrap->prm = divisor;
|
||
wrap->id = (int)path.size();
|
||
path.push_back(wrap);
|
||
}
|
||
|
||
|
||
|
||
// get number of wraps
|
||
int mjCTendon::NumWraps() const {
|
||
return (int)path.size();
|
||
}
|
||
|
||
|
||
|
||
// get pointer to specified wrap
|
||
const mjCWrap* mjCTendon::GetWrap(int i) const {
|
||
if (i >= 0 && i < (int)path.size()) {
|
||
return path[i];
|
||
}
|
||
return nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCTendon::ResolveReferences(const mjCModel* m) {
|
||
int nfailure = 0;
|
||
int npulley = 0;
|
||
for (int i=0; i < path.size(); i++) {
|
||
std::string pname = path[i]->name;
|
||
std::string psidesite = path[i]->sidesite;
|
||
if (path[i]->type == mjWRAP_PULLEY) {
|
||
npulley++;
|
||
}
|
||
try {
|
||
// look for wrapped element with namespace
|
||
path[i]->name = prefix + pname + suffix;
|
||
path[i]->sidesite = prefix + psidesite + suffix;
|
||
path[i]->ResolveReferences(m);
|
||
} catch(mjCError) {
|
||
// remove namespace from wrap names
|
||
path[i]->name = pname;
|
||
path[i]->sidesite = psidesite;
|
||
path[i]->ResolveReferences(m);
|
||
nfailure++;
|
||
}
|
||
}
|
||
if (nfailure == path.size()-npulley) {
|
||
throw mjCError(this, "tendon '%s' (id = %d): no attached reference found", name.c_str(), id);
|
||
}
|
||
prefix.clear();
|
||
suffix.clear();
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCTendon::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_tendon) {
|
||
throw mjCError(this, "user has more values than nuser_tendon in tendon");
|
||
}
|
||
userdata_.resize(model->nuser_tendon);
|
||
|
||
// check for empty path
|
||
int sz = (int)path.size();
|
||
if (!sz) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): path cannot be empty",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
// determine type
|
||
bool spatial = (path[0]->type != mjWRAP_JOINT);
|
||
|
||
// require at least two objects in spatial path
|
||
if (spatial && sz < 2) {
|
||
throw mjCError(this, "tendon '%s' (id = %d): spatial path must contain at least two objects",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
// require positive width
|
||
if (spatial && width <= 0) {
|
||
throw mjCError(this, "tendon '%s' (id = %d) must have positive width", name.c_str(), id);
|
||
}
|
||
|
||
// compile objects in path
|
||
ResolveReferences(model);
|
||
|
||
// check path
|
||
for (int i=0; i < sz; i++) {
|
||
// fixed
|
||
if (!spatial) {
|
||
// make sure all objects are joints
|
||
if (path[i]->type != mjWRAP_JOINT) {
|
||
throw mjCError(this, "tendon '%s' (id = %d): spatial object found in fixed path at pos %d",
|
||
name.c_str(), id, i);
|
||
}
|
||
}
|
||
|
||
// spatial path
|
||
else {
|
||
if (armature < 0) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): tendon armature cannot be negative",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
switch (path[i]->type) {
|
||
case mjWRAP_PULLEY:
|
||
// pulley should not follow other pulley
|
||
if (i > 0 && path[i-1]->type == mjWRAP_PULLEY) {
|
||
throw mjCError(this, "tendon '%s' (id = %d): consecutive pulleys (pos %d)",
|
||
name.c_str(), id, i);
|
||
}
|
||
|
||
// pulley should not be last
|
||
if (i == sz-1) {
|
||
throw mjCError(this, "tendon '%s' (id = %d): path ends with pulley", name.c_str(), id);
|
||
}
|
||
break;
|
||
|
||
case mjWRAP_SITE:
|
||
// site needs a neighbor that is not a pulley
|
||
if ((i == 0 || path[i-1]->type == mjWRAP_PULLEY) &&
|
||
(i == sz-1 || path[i+1]->type == mjWRAP_PULLEY)) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): site %d needs a neighbor that is not a pulley",
|
||
name.c_str(), id, i);
|
||
}
|
||
|
||
// site cannot be repeated
|
||
if (i < sz-1 && path[i+1]->type == mjWRAP_SITE && path[i]->obj->id == path[i+1]->obj->id) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): site %d is repeated",
|
||
name.c_str(), id, i);
|
||
}
|
||
|
||
break;
|
||
|
||
case mjWRAP_SPHERE:
|
||
case mjWRAP_CYLINDER:
|
||
// geom must be bracketed by sites
|
||
if (i == 0 || i == sz-1 || path[i-1]->type != mjWRAP_SITE || path[i+1]->type != mjWRAP_SITE) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): geom at pos %d not bracketed by sites",
|
||
name.c_str(), id, i);
|
||
}
|
||
|
||
if (armature > 0) {
|
||
throw mjCError(this,
|
||
"tendon '%s' (id = %d): geom wrapping not supported by tendon armature",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
// mark geoms as non visual
|
||
model->Geoms()[path[i]->obj->id]->SetNotVisual();
|
||
break;
|
||
|
||
case mjWRAP_JOINT:
|
||
throw mjCError(this,
|
||
"tendon '%s (id = %d)': joint wrap found in spatial path at pos %d",
|
||
name.c_str(), id, i);
|
||
|
||
default:
|
||
throw mjCError(this,
|
||
"tendon '%s (id = %d)': invalid wrap object at pos %d",
|
||
name.c_str(), id, i);
|
||
}
|
||
}
|
||
}
|
||
|
||
// if limited is auto, set to 1 if range is specified, otherwise unlimited
|
||
if (limited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(range[0] == 0 && range[1] == 0);
|
||
checklimited(this, compiler->autolimits, "tendon", "", limited, hasrange);
|
||
}
|
||
|
||
// check limits
|
||
if (range[0] >= range[1] && is_limited()) {
|
||
throw mjCError(this, "invalid limits in tendon");
|
||
}
|
||
|
||
// if limited is auto, set to 1 if range is specified, otherwise unlimited
|
||
if (actfrclimited == mjLIMITED_AUTO) {
|
||
bool hasactfrcrange = !(actfrcrange[0] == 0 && actfrcrange[1] == 0);
|
||
checklimited(this, compiler->autolimits, "tendon", "", actfrclimited,
|
||
hasactfrcrange);
|
||
}
|
||
|
||
// check actfrclimits
|
||
if (actfrcrange[0] >= actfrcrange[1] && is_actfrclimited()) {
|
||
throw mjCError(this, "invalid actuatorfrcrange in tendon");
|
||
}
|
||
if ((actfrcrange[0] > 0 || actfrcrange[1] < 0) && is_actfrclimited()) {
|
||
throw mjCError(this, "invalid actuatorfrcrange in tendon");
|
||
}
|
||
|
||
// check springlength
|
||
if (springlength[0] > springlength[1]) {
|
||
throw mjCError(this, "invalid springlength in tendon");
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCWrap implementation --------------------------------------------------
|
||
|
||
// constructor
|
||
mjCWrap::mjCWrap(mjCModel* _model, mjCTendon* _tendon) {
|
||
elemtype = mjOBJ_UNKNOWN;
|
||
|
||
// set model and tendon pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
tendon = _tendon;
|
||
|
||
// clear variables
|
||
type = mjWRAP_NONE;
|
||
obj = nullptr;
|
||
sideid = -1;
|
||
prm = 0;
|
||
sidesite.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
mjCWrap::mjCWrap(const mjCWrap& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCWrap& mjCWrap::operator=(const mjCWrap& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCWrap_*>(this) = static_cast<const mjCWrap_&>(other);
|
||
*static_cast<mjsWrap*>(this) = static_cast<const mjsWrap&>(other);
|
||
obj = nullptr;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCWrap::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.info = &info;
|
||
}
|
||
|
||
|
||
|
||
void mjCWrap::NameSpace(const mjCModel* m) {
|
||
name = m->prefix + name + m->suffix;
|
||
if (!sidesite.empty()) {
|
||
sidesite = m->prefix + sidesite + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCWrap::ResolveReferences(const mjCModel* m) {
|
||
mjCBase *pside;
|
||
|
||
// handle wrap object types
|
||
switch (type) {
|
||
case mjWRAP_JOINT: // joint
|
||
// find joint by name
|
||
obj = m->FindObject(mjOBJ_JOINT, name);
|
||
if (!obj) {
|
||
throw mjCError(this,
|
||
"joint '%s' not found in tendon %d, wrap %d",
|
||
name.c_str(), tendon->id, id);
|
||
}
|
||
|
||
break;
|
||
|
||
case mjWRAP_SPHERE: // geom (cylinder type set here)
|
||
// find geom by name
|
||
obj = m->FindObject(mjOBJ_GEOM, name);
|
||
if (!obj) {
|
||
throw mjCError(this,
|
||
"geom '%s' not found in tendon %d, wrap %d",
|
||
name.c_str(), tendon->id, id);
|
||
}
|
||
|
||
// set/check geom type
|
||
if (((mjCGeom*)obj)->type == mjGEOM_CYLINDER) {
|
||
type = mjWRAP_CYLINDER;
|
||
} else if (((mjCGeom*)obj)->type != mjGEOM_SPHERE) {
|
||
throw mjCError(this,
|
||
"geom '%s' in tendon %d, wrap %d is not sphere or cylinder",
|
||
name.c_str(), tendon->id, id);
|
||
}
|
||
|
||
// process side site
|
||
if (!sidesite.empty()) {
|
||
// find site by name
|
||
pside = m->FindObject(mjOBJ_SITE, sidesite);
|
||
if (!pside) {
|
||
throw mjCError(this,
|
||
"side site '%s' not found in tendon %d, wrap %d",
|
||
sidesite.c_str(), tendon->id, id);
|
||
}
|
||
|
||
// save side site id
|
||
sideid = pside->id;
|
||
}
|
||
break;
|
||
|
||
case mjWRAP_PULLEY: // pulley
|
||
// make sure divisor is non-negative
|
||
if (prm < 0) {
|
||
throw mjCError(this,
|
||
"pulley has negative divisor in tendon %d, wrap %d",
|
||
0, tendon->id, id);
|
||
}
|
||
|
||
break;
|
||
|
||
case mjWRAP_SITE: // site
|
||
// find site by name
|
||
obj = m->FindObject(mjOBJ_SITE, name);
|
||
if (!obj) {
|
||
throw mjCError(this, "site '%s' not found in wrap %d", name.c_str(), id);
|
||
}
|
||
break;
|
||
|
||
default: // SHOULD NOT OCCUR
|
||
throw mjCError(this, "unknown wrap type in tendon %d, wrap %d", 0, tendon->id, id);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCActuator implementation ----------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) {
|
||
mjs_defaultActuator(&spec);
|
||
elemtype = mjOBJ_ACTUATOR;
|
||
|
||
// clear private variables
|
||
ptarget = nullptr;
|
||
spec_target_.clear();
|
||
spec_slidersite_.clear();
|
||
spec_refsite_.clear();
|
||
spec_userdata_.clear();
|
||
trnid[0] = trnid[1] = -1;
|
||
|
||
// reset to default if given
|
||
if (_def) {
|
||
*this = _def->Actuator();
|
||
}
|
||
|
||
// set model, def
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
classname = _def ? _def->name : "main";
|
||
|
||
// in case this actuator is not compiled
|
||
CopyFromSpec();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// no previous state when an actuator is created
|
||
actadr_ = -1;
|
||
actdim_ = -1;
|
||
}
|
||
|
||
|
||
|
||
mjCActuator::mjCActuator(const mjCActuator& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCActuator& mjCActuator::operator=(const mjCActuator& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCActuator_*>(this) = static_cast<const mjCActuator_&>(other);
|
||
*static_cast<mjsActuator*>(this) = static_cast<const mjsActuator&>(other);
|
||
ptarget = nullptr;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::ForgetKeyframes() {
|
||
act_.clear();
|
||
ctrl_.clear();
|
||
}
|
||
|
||
|
||
|
||
bool mjCActuator::is_ctrllimited() const {
|
||
return islimited(ctrllimited, ctrlrange);
|
||
}
|
||
bool mjCActuator::is_forcelimited() const {
|
||
return islimited(forcelimited, forcerange);
|
||
}
|
||
bool mjCActuator::is_actlimited() const {
|
||
return islimited(actlimited, actrange);
|
||
}
|
||
|
||
|
||
|
||
std::vector<mjtNum>& mjCActuator::act(const std::string& state_name) {
|
||
if (act_.find(state_name) == act_.end()) {
|
||
act_[state_name] = std::vector<mjtNum>(model->nu, mjNAN);
|
||
}
|
||
return act_.at(state_name);
|
||
}
|
||
|
||
|
||
|
||
mjtNum& mjCActuator::ctrl(const std::string& state_name) {
|
||
if (ctrl_.find(state_name) == ctrl_.end()) {
|
||
ctrl_[state_name] = mjNAN;
|
||
}
|
||
return ctrl_.at(state_name);
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.userdata = &spec_userdata_;
|
||
spec.target = &spec_target_;
|
||
spec.refsite = &spec_refsite_;
|
||
spec.slidersite = &spec_slidersite_;
|
||
spec.plugin.plugin_name = &plugin_name;
|
||
spec.plugin.name = &plugin_instance_name;
|
||
spec.info = &info;
|
||
userdata = nullptr;
|
||
target = nullptr;
|
||
refsite = nullptr;
|
||
slidersite = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::NameSpace(const mjCModel* m) {
|
||
mjCBase::NameSpace(m);
|
||
if (!plugin_instance_name.empty()) {
|
||
plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
|
||
}
|
||
if (!spec_target_.empty()) {
|
||
spec_target_ = m->prefix + spec_target_ + m->suffix;
|
||
}
|
||
if (!spec_refsite_.empty()) {
|
||
spec_refsite_ = m->prefix + spec_refsite_ + m->suffix;
|
||
}
|
||
if (!spec_slidersite_.empty()) {
|
||
spec_slidersite_ = m->prefix + spec_slidersite_ + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::CopyFromSpec() {
|
||
*static_cast<mjsActuator*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
target_ = spec_target_;
|
||
refsite_ = spec_refsite_;
|
||
slidersite_ = spec_slidersite_;
|
||
plugin.active = spec.plugin.active;
|
||
plugin.element = spec.plugin.element;
|
||
plugin.plugin_name = spec.plugin.plugin_name;
|
||
plugin.name = spec.plugin.name;
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::CopyPlugin() {
|
||
model->CopyExplicitPlugin(this);
|
||
}
|
||
|
||
|
||
|
||
void mjCActuator::ResolveReferences(const mjCModel* m) {
|
||
switch (trntype) {
|
||
case mjTRN_JOINT:
|
||
case mjTRN_JOINTINPARENT:
|
||
// get joint
|
||
ptarget = m->FindObject(mjOBJ_JOINT, target_);
|
||
if (!ptarget) {
|
||
throw mjCError(this,
|
||
"unknown transmission target '%s' for actuator id = %d", target_.c_str(), id);
|
||
}
|
||
break;
|
||
|
||
case mjTRN_SLIDERCRANK:
|
||
// get slidersite, copy in trnid[1]
|
||
if (slidersite_.empty()) {
|
||
throw mjCError(this, "missing base site for slider-crank '%s' (id = %d)", name.c_str(), id);
|
||
}
|
||
ptarget = m->FindObject(mjOBJ_SITE, slidersite_);
|
||
if (!ptarget) {
|
||
throw mjCError(this, "base site '%s' not found for actuator %d", slidersite_.c_str(), id);
|
||
}
|
||
trnid[1] = ptarget->id;
|
||
|
||
// check cranklength
|
||
if (cranklength <= 0) {
|
||
throw mjCError(this,
|
||
"crank length must be positive in actuator '%s' (id = %d)", name.c_str(), id);
|
||
}
|
||
|
||
// proceed with regular target
|
||
ptarget = m->FindObject(mjOBJ_SITE, target_);
|
||
break;
|
||
|
||
case mjTRN_TENDON:
|
||
// get tendon
|
||
ptarget = m->FindObject(mjOBJ_TENDON, target_);
|
||
break;
|
||
|
||
case mjTRN_SITE:
|
||
// get refsite, copy into trnid[1]
|
||
if (!refsite_.empty()) {
|
||
ptarget = m->FindObject(mjOBJ_SITE, refsite_);
|
||
if (!ptarget) {
|
||
throw mjCError(this, "reference site '%s' not found for actuator %d", refsite_.c_str(), id);
|
||
}
|
||
trnid[1] = ptarget->id;
|
||
}
|
||
|
||
// proceed with regular site target
|
||
ptarget = m->FindObject(mjOBJ_SITE, target_);
|
||
break;
|
||
|
||
case mjTRN_BODY:
|
||
// get body
|
||
ptarget = m->FindObject(mjOBJ_BODY, target_);
|
||
break;
|
||
|
||
default:
|
||
throw mjCError(this, "invalid transmission type in actuator");
|
||
}
|
||
|
||
// assign and check
|
||
if (!ptarget) {
|
||
throw mjCError(this, "transmission target '%s' not found in actuator %d", target_.c_str(), id);
|
||
} else {
|
||
trnid[0] = ptarget->id;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCActuator::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_actuator) {
|
||
throw mjCError(this, "user has more values than nuser_actuator in actuator '%s' (id = %d)",
|
||
name.c_str(), id);
|
||
}
|
||
userdata_.resize(model->nuser_actuator);
|
||
|
||
// check for missing target name
|
||
if (target_.empty()) {
|
||
throw mjCError(this,
|
||
"missing transmission target for actuator");
|
||
}
|
||
|
||
// find transmission target in object arrays
|
||
ResolveReferences(model);
|
||
|
||
// handle inheritrange
|
||
if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE &&
|
||
gainprm[0] == -biasprm[1] && inheritrange > 0) {
|
||
// semantic of actuator is the same as transmission, inheritrange is applicable
|
||
double* range;
|
||
if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT) {
|
||
// position actuator
|
||
range = ctrlrange;
|
||
} else if (dyntype == mjDYN_INTEGRATOR) {
|
||
// intvelocity actuator
|
||
range = actrange;
|
||
} else {
|
||
throw mjCError(this, "inheritrange only available for position "
|
||
"and intvelocity actuators");
|
||
}
|
||
|
||
const double* target_range;
|
||
if (trntype == mjTRN_JOINT) {
|
||
mjCJoint* pjnt = (mjCJoint*) ptarget;
|
||
if (pjnt->spec.type != mjJNT_HINGE && pjnt->spec.type != mjJNT_SLIDE) {
|
||
throw mjCError(this, "inheritrange can only be used with hinge and slide joints, "
|
||
"actuator");
|
||
}
|
||
target_range = pjnt->get_range();
|
||
} else if (trntype == mjTRN_TENDON) {
|
||
mjCTendon* pten = (mjCTendon*) ptarget;
|
||
target_range = pten->get_range();
|
||
} else {
|
||
throw mjCError(this, "inheritrange can only be used with joint and tendon transmission, "
|
||
"actuator");
|
||
}
|
||
|
||
if (target_range[0] == target_range[1]) {
|
||
throw mjCError(this, "inheritrange used but target '%s' has no range defined in actuator %d",
|
||
target_.c_str(), id);
|
||
}
|
||
|
||
// set range automatically
|
||
double mean = 0.5*(target_range[1] + target_range[0]);
|
||
double radius = 0.5*(target_range[1] - target_range[0]) * inheritrange;
|
||
range[0] = mean - radius;
|
||
range[1] = mean + radius;
|
||
}
|
||
|
||
// if limited is auto, check for inconsistency wrt to autolimits
|
||
if (forcelimited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(forcerange[0] == 0 && forcerange[1] == 0);
|
||
checklimited(this, compiler->autolimits, "actuator", "force", forcelimited, hasrange);
|
||
}
|
||
if (ctrllimited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(ctrlrange[0] == 0 && ctrlrange[1] == 0);
|
||
checklimited(this, compiler->autolimits, "actuator", "ctrl", ctrllimited, hasrange);
|
||
}
|
||
if (actlimited == mjLIMITED_AUTO) {
|
||
bool hasrange = !(actrange[0] == 0 && actrange[1] == 0);
|
||
checklimited(this, compiler->autolimits, "actuator", "act", actlimited, hasrange);
|
||
}
|
||
|
||
// check limits
|
||
if (forcerange[0] >= forcerange[1] && is_forcelimited()) {
|
||
throw mjCError(this, "invalid force range for actuator");
|
||
}
|
||
if (ctrlrange[0] >= ctrlrange[1] && is_ctrllimited()) {
|
||
throw mjCError(this, "invalid control range for actuator");
|
||
}
|
||
if (actrange[0] >= actrange[1] && is_actlimited()) {
|
||
throw mjCError(this, "invalid actrange for actuator");
|
||
}
|
||
if (is_actlimited() && dyntype == mjDYN_NONE) {
|
||
throw mjCError(this, "actrange specified but dyntype is 'none' in actuator");
|
||
}
|
||
|
||
// check and set actdim
|
||
if (!plugin.active) {
|
||
if (actdim > 1 && dyntype != mjDYN_USER) {
|
||
throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator");
|
||
}
|
||
if (actdim == 1 && dyntype == mjDYN_NONE) {
|
||
throw mjCError(this, "invalid actdim 1 in stateless actuator");
|
||
}
|
||
if (actdim == 0 && dyntype != mjDYN_NONE) {
|
||
throw mjCError(this, "invalid actdim 0 in stateful actuator");
|
||
}
|
||
}
|
||
|
||
// set actdim
|
||
if (actdim < 0) {
|
||
actdim = (dyntype != mjDYN_NONE);
|
||
}
|
||
|
||
// check muscle parameters
|
||
for (int i=0; i < 2; i++) {
|
||
// select gain or bias
|
||
double* prm = NULL;
|
||
if (i == 0 && gaintype == mjGAIN_MUSCLE) {
|
||
prm = gainprm;
|
||
} else if (i == 1 && biastype == mjBIAS_MUSCLE) {
|
||
prm = biasprm;
|
||
}
|
||
|
||
// nothing to check
|
||
if (!prm) {
|
||
continue;
|
||
}
|
||
|
||
// range
|
||
if (prm[0] >= prm[1]) {
|
||
throw mjCError(this, "range[0]<range[1] required in muscle");
|
||
}
|
||
|
||
// lmin<1<lmax
|
||
if (prm[4] >= 1 || prm[5] <= 1) {
|
||
throw mjCError(this, "lmin<1<lmax required in muscle");
|
||
}
|
||
|
||
// scale, vmax, fpmax, fvmax>0
|
||
if (prm[3] <= 0 || prm[6] <= 0 || prm[7] <= 0 || prm[8] <= 0) {
|
||
throw mjCError(this,
|
||
"positive scale, vmax, fpmax, fvmax required in muscle '%s' (id = %d)",
|
||
name.c_str(), id);
|
||
}
|
||
}
|
||
|
||
// plugin
|
||
if (plugin.active) {
|
||
if (plugin_name.empty() && plugin_instance_name.empty()) {
|
||
throw mjCError(
|
||
this, "neither 'plugin' nor 'instance' is specified for actuator '%s', (id = %d)",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.element);
|
||
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
|
||
plugin.element = plugin_instance;
|
||
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
|
||
if (!(pplugin->capabilityflags & mjPLUGIN_ACTUATOR)) {
|
||
throw mjCError(this, "plugin '%s' does not support actuators", pplugin->name);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCSensor implementation ------------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCSensor::mjCSensor(mjCModel* _model) {
|
||
mjs_defaultSensor(&spec);
|
||
elemtype = mjOBJ_SENSOR;
|
||
|
||
// set model
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear private variables
|
||
spec_objname_.clear();
|
||
spec_refname_.clear();
|
||
spec_userdata_.clear();
|
||
obj = nullptr;
|
||
ref = nullptr;
|
||
|
||
// in case this sensor is not compiled
|
||
CopyFromSpec();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
mjCSensor::mjCSensor(const mjCSensor& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCSensor& mjCSensor::operator=(const mjCSensor& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCSensor_*>(this) = static_cast<const mjCSensor_&>(other);
|
||
*static_cast<mjsSensor*>(this) = static_cast<const mjsSensor&>(other);
|
||
obj = nullptr;
|
||
ref = nullptr;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCSensor::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.userdata = &spec_userdata_;
|
||
spec.objname = &spec_objname_;
|
||
spec.refname = &spec_refname_;
|
||
spec.plugin.plugin_name = &plugin_name;
|
||
spec.plugin.name = &plugin_instance_name;
|
||
spec.info = &info;
|
||
userdata = nullptr;
|
||
objname = nullptr;
|
||
refname = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCSensor::NameSpace(const mjCModel* m) {
|
||
if (!name.empty()) {
|
||
name = m->prefix + name + m->suffix;
|
||
}
|
||
if (!plugin_instance_name.empty()) {
|
||
plugin_instance_name = m->prefix + plugin_instance_name + m->suffix;
|
||
}
|
||
prefix = m->prefix;
|
||
suffix = m->suffix;
|
||
}
|
||
|
||
|
||
|
||
void mjCSensor::CopyFromSpec() {
|
||
*static_cast<mjsSensor*>(this) = spec;
|
||
userdata_ = spec_userdata_;
|
||
objname_ = spec_objname_;
|
||
refname_ = spec_refname_;
|
||
plugin.active = spec.plugin.active;
|
||
plugin.element = spec.plugin.element;
|
||
plugin.plugin_name = spec.plugin.plugin_name;
|
||
plugin.name = spec.plugin.name;
|
||
}
|
||
|
||
|
||
|
||
void mjCSensor::CopyPlugin() {
|
||
model->CopyExplicitPlugin(this);
|
||
}
|
||
|
||
|
||
|
||
void mjCSensor::ResolveReferences(const mjCModel* m) {
|
||
obj = nullptr;
|
||
ref = nullptr;
|
||
objname_ = prefix + objname_ + suffix;
|
||
refname_ = prefix + refname_ + suffix;
|
||
|
||
// get references using the namespace
|
||
if (objtype != mjOBJ_UNKNOWN) {
|
||
obj = m->FindObject(objtype, objname_);
|
||
}
|
||
if (reftype != mjOBJ_UNKNOWN) {
|
||
ref = m->FindObject(reftype, refname_);
|
||
}
|
||
|
||
// if failure and both were requested, use namespace only on one
|
||
if (objtype != mjOBJ_UNKNOWN && reftype != mjOBJ_UNKNOWN && !obj && ref) {
|
||
objname_ = spec_objname_;
|
||
obj = m->FindObject(objtype, objname_);
|
||
}
|
||
if (objtype != mjOBJ_UNKNOWN && reftype != mjOBJ_UNKNOWN && obj && !ref) {
|
||
refname_ = spec_refname_;
|
||
ref = m->FindObject(reftype, refname_);
|
||
}
|
||
|
||
// check object
|
||
if (objtype != mjOBJ_UNKNOWN) {
|
||
// check for missing object name
|
||
if (objname_.empty()) {
|
||
throw mjCError(this, "missing name of sensorized object in sensor");
|
||
}
|
||
|
||
// find name
|
||
if (!obj) {
|
||
throw mjCError(this, "unrecognized name '%s' of sensorized object", objname_.c_str());
|
||
}
|
||
|
||
// if geom or mesh, mark it as non visual
|
||
if (objtype == mjOBJ_GEOM) {
|
||
static_cast<mjCGeom*>(obj)->SetNotVisual();
|
||
}
|
||
if (objtype == mjOBJ_MESH) {
|
||
static_cast<mjCMesh*>(obj)->SetNotVisual();
|
||
}
|
||
|
||
} else if (type != mjSENS_E_POTENTIAL &&
|
||
type != mjSENS_E_KINETIC &&
|
||
type != mjSENS_CLOCK &&
|
||
type != mjSENS_PLUGIN &&
|
||
type != mjSENS_CONTACT &&
|
||
type != mjSENS_USER) {
|
||
throw mjCError(this, "invalid type in sensor");
|
||
}
|
||
|
||
// check reference object
|
||
if (reftype != mjOBJ_UNKNOWN) {
|
||
// check for missing object name
|
||
if (refname_.empty()) {
|
||
throw mjCError(this, "missing name of reference frame object in sensor");
|
||
}
|
||
|
||
// find name
|
||
if (!ref) {
|
||
throw mjCError(this, "unrecognized name '%s' of object", refname_.c_str());
|
||
}
|
||
|
||
// if geom or mesh, mark it as non visual
|
||
if (reftype == mjOBJ_GEOM) {
|
||
static_cast<mjCGeom*>(ref)->SetNotVisual();
|
||
}
|
||
if (reftype == mjOBJ_MESH) {
|
||
static_cast<mjCMesh*>(ref)->SetNotVisual();
|
||
}
|
||
|
||
// must be attached to object with spatial frame
|
||
if (reftype != mjOBJ_BODY && reftype != mjOBJ_XBODY &&
|
||
reftype != mjOBJ_GEOM && reftype != mjOBJ_SITE && reftype != mjOBJ_CAMERA) {
|
||
throw mjCError(this,
|
||
"reference frame object must be (x)body, geom, site or camera in sensor");
|
||
}
|
||
}
|
||
|
||
spec_objname_ = objname_;
|
||
spec_refname_ = refname_;
|
||
prefix.clear();
|
||
suffix.clear();
|
||
}
|
||
|
||
// return sensor datatype
|
||
mjtDataType sensorDatatype(mjtSensor type) {
|
||
switch (type) {
|
||
case mjSENS_TOUCH:
|
||
case mjSENS_RANGEFINDER:
|
||
case mjSENS_INSIDESITE:
|
||
return mjDATATYPE_POSITIVE;
|
||
|
||
case mjSENS_FRAMEXAXIS:
|
||
case mjSENS_FRAMEYAXIS:
|
||
case mjSENS_FRAMEZAXIS:
|
||
case mjSENS_GEOMNORMAL:
|
||
return mjDATATYPE_AXIS;
|
||
|
||
case mjSENS_BALLQUAT:
|
||
case mjSENS_FRAMEQUAT:
|
||
return mjDATATYPE_QUATERNION;
|
||
|
||
case mjSENS_ACCELEROMETER:
|
||
case mjSENS_VELOCIMETER:
|
||
case mjSENS_GYRO:
|
||
case mjSENS_FORCE:
|
||
case mjSENS_TORQUE:
|
||
case mjSENS_MAGNETOMETER:
|
||
case mjSENS_CAMPROJECTION:
|
||
case mjSENS_JOINTPOS:
|
||
case mjSENS_JOINTVEL:
|
||
case mjSENS_TENDONPOS:
|
||
case mjSENS_TENDONVEL:
|
||
case mjSENS_ACTUATORPOS:
|
||
case mjSENS_ACTUATORVEL:
|
||
case mjSENS_ACTUATORFRC:
|
||
case mjSENS_JOINTACTFRC:
|
||
case mjSENS_TENDONACTFRC:
|
||
case mjSENS_BALLANGVEL:
|
||
case mjSENS_JOINTLIMITPOS:
|
||
case mjSENS_JOINTLIMITVEL:
|
||
case mjSENS_JOINTLIMITFRC:
|
||
case mjSENS_TENDONLIMITPOS:
|
||
case mjSENS_TENDONLIMITVEL:
|
||
case mjSENS_TENDONLIMITFRC:
|
||
case mjSENS_FRAMEPOS:
|
||
case mjSENS_FRAMELINVEL:
|
||
case mjSENS_FRAMEANGVEL:
|
||
case mjSENS_FRAMELINACC:
|
||
case mjSENS_FRAMEANGACC:
|
||
case mjSENS_SUBTREECOM:
|
||
case mjSENS_SUBTREELINVEL:
|
||
case mjSENS_SUBTREEANGMOM:
|
||
case mjSENS_GEOMDIST:
|
||
case mjSENS_GEOMFROMTO:
|
||
case mjSENS_CONTACT:
|
||
case mjSENS_TACTILE:
|
||
case mjSENS_E_POTENTIAL:
|
||
case mjSENS_E_KINETIC:
|
||
case mjSENS_CLOCK:
|
||
case mjSENS_PLUGIN:
|
||
case mjSENS_USER:
|
||
return mjDATATYPE_REAL;
|
||
}
|
||
|
||
return mjDATATYPE_REAL; // all cases are covered but GCC is extra persnickety
|
||
}
|
||
|
||
// return sensor needstage
|
||
mjtStage sensorNeedstage(mjtSensor type) {
|
||
switch (type) {
|
||
case mjSENS_TOUCH:
|
||
case mjSENS_ACCELEROMETER:
|
||
case mjSENS_FORCE:
|
||
case mjSENS_TORQUE:
|
||
case mjSENS_ACTUATORFRC:
|
||
case mjSENS_JOINTACTFRC:
|
||
case mjSENS_TENDONACTFRC:
|
||
case mjSENS_JOINTLIMITFRC:
|
||
case mjSENS_TENDONLIMITFRC:
|
||
case mjSENS_FRAMELINACC:
|
||
case mjSENS_FRAMEANGACC:
|
||
case mjSENS_CONTACT:
|
||
case mjSENS_TACTILE:
|
||
return mjSTAGE_ACC;
|
||
|
||
case mjSENS_VELOCIMETER:
|
||
case mjSENS_GYRO:
|
||
case mjSENS_JOINTVEL:
|
||
case mjSENS_TENDONVEL:
|
||
case mjSENS_ACTUATORVEL:
|
||
case mjSENS_BALLANGVEL:
|
||
case mjSENS_JOINTLIMITVEL:
|
||
case mjSENS_TENDONLIMITVEL:
|
||
case mjSENS_FRAMELINVEL:
|
||
case mjSENS_FRAMEANGVEL:
|
||
case mjSENS_SUBTREELINVEL:
|
||
case mjSENS_SUBTREEANGMOM:
|
||
return mjSTAGE_VEL;
|
||
|
||
case mjSENS_MAGNETOMETER:
|
||
case mjSENS_RANGEFINDER:
|
||
case mjSENS_CAMPROJECTION:
|
||
case mjSENS_JOINTPOS:
|
||
case mjSENS_TENDONPOS:
|
||
case mjSENS_ACTUATORPOS:
|
||
case mjSENS_BALLQUAT:
|
||
case mjSENS_JOINTLIMITPOS:
|
||
case mjSENS_TENDONLIMITPOS:
|
||
case mjSENS_FRAMEPOS:
|
||
case mjSENS_FRAMEQUAT:
|
||
case mjSENS_FRAMEXAXIS:
|
||
case mjSENS_FRAMEYAXIS:
|
||
case mjSENS_FRAMEZAXIS:
|
||
case mjSENS_SUBTREECOM:
|
||
case mjSENS_INSIDESITE:
|
||
case mjSENS_GEOMDIST:
|
||
case mjSENS_GEOMNORMAL:
|
||
case mjSENS_GEOMFROMTO:
|
||
case mjSENS_E_POTENTIAL:
|
||
case mjSENS_E_KINETIC:
|
||
case mjSENS_CLOCK:
|
||
case mjSENS_PLUGIN:
|
||
case mjSENS_USER:
|
||
return mjSTAGE_POS;
|
||
}
|
||
|
||
return mjSTAGE_POS; // all cases are covered but GCC is extra persnickety
|
||
}
|
||
|
||
// compiler
|
||
void mjCSensor::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// resize userdata
|
||
if (userdata_.size() > model->nuser_sensor) {
|
||
throw mjCError(this, "user has more values than nuser_sensor in sensor");
|
||
}
|
||
userdata_.resize(model->nuser_sensor);
|
||
|
||
// require non-negative noise
|
||
if (noise < 0) {
|
||
throw mjCError(this, "negative noise in sensor");
|
||
}
|
||
|
||
// require non-negative cutoff
|
||
if (cutoff < 0) {
|
||
throw mjCError(this, "negative cutoff in sensor");
|
||
}
|
||
|
||
// Find referenced object
|
||
ResolveReferences(model);
|
||
|
||
// set datatype for non-user sensors
|
||
if (type != mjSENS_USER) {
|
||
datatype = sensorDatatype(type);
|
||
}
|
||
|
||
// set needstage for non-user and non-plugin sensors
|
||
if (type != mjSENS_USER && type != mjSENS_PLUGIN) {
|
||
needstage = sensorNeedstage(type);
|
||
}
|
||
|
||
// process according to sensor type
|
||
switch (type) {
|
||
case mjSENS_TOUCH:
|
||
case mjSENS_ACCELEROMETER:
|
||
case mjSENS_VELOCIMETER:
|
||
case mjSENS_GYRO:
|
||
case mjSENS_FORCE:
|
||
case mjSENS_TORQUE:
|
||
case mjSENS_MAGNETOMETER:
|
||
case mjSENS_RANGEFINDER:
|
||
case mjSENS_CAMPROJECTION:
|
||
// must be attached to site
|
||
if (objtype != mjOBJ_SITE) {
|
||
throw mjCError(this, "sensor must be attached to site");
|
||
}
|
||
|
||
// check for camera resolution for camera projection sensor
|
||
if (type == mjSENS_CAMPROJECTION) {
|
||
mjCCamera* camref = (mjCCamera*)ref;
|
||
if (!camref->resolution[0] || !camref->resolution[1]) {
|
||
throw mjCError(this, "camera projection sensor requires camera resolution");
|
||
}
|
||
}
|
||
break;
|
||
|
||
case mjSENS_JOINTPOS:
|
||
case mjSENS_JOINTVEL:
|
||
case mjSENS_JOINTACTFRC:
|
||
// must be attached to joint
|
||
if (objtype != mjOBJ_JOINT) {
|
||
throw mjCError(this, "sensor must be attached to joint");
|
||
}
|
||
|
||
// make sure joint is slide or hinge
|
||
if (((mjCJoint*)obj)->type != mjJNT_SLIDE && ((mjCJoint*)obj)->type != mjJNT_HINGE) {
|
||
throw mjCError(this, "joint must be slide or hinge in sensor");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_TENDONACTFRC:
|
||
// must be attached to tendon
|
||
if (objtype != mjOBJ_TENDON) {
|
||
throw mjCError(this, "sensor must be attached to tendon");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_TENDONPOS:
|
||
case mjSENS_TENDONVEL:
|
||
// must be attached to tendon
|
||
if (objtype != mjOBJ_TENDON) {
|
||
throw mjCError(this, "sensor must be attached to tendon");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_ACTUATORPOS:
|
||
case mjSENS_ACTUATORVEL:
|
||
case mjSENS_ACTUATORFRC:
|
||
// must be attached to actuator
|
||
if (objtype != mjOBJ_ACTUATOR) {
|
||
throw mjCError(this, "sensor must be attached to actuator");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_BALLQUAT:
|
||
case mjSENS_BALLANGVEL:
|
||
// must be attached to joint
|
||
if (objtype != mjOBJ_JOINT) {
|
||
throw mjCError(this, "sensor must be attached to joint");
|
||
}
|
||
|
||
// make sure joint is ball
|
||
if (((mjCJoint*)obj)->type != mjJNT_BALL) {
|
||
throw mjCError(this, "joint must be ball in sensor");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_JOINTLIMITPOS:
|
||
case mjSENS_JOINTLIMITVEL:
|
||
case mjSENS_JOINTLIMITFRC:
|
||
// must be attached to joint
|
||
if (objtype != mjOBJ_JOINT) {
|
||
throw mjCError(this, "sensor must be attached to joint");
|
||
}
|
||
|
||
// make sure joint has limit
|
||
if (!((mjCJoint*)obj)->is_limited()) {
|
||
throw mjCError(this, "joint must be limited in sensor");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_TENDONLIMITPOS:
|
||
case mjSENS_TENDONLIMITVEL:
|
||
case mjSENS_TENDONLIMITFRC:
|
||
// must be attached to tendon
|
||
if (objtype != mjOBJ_TENDON) {
|
||
throw mjCError(this, "sensor must be attached to tendon");
|
||
}
|
||
|
||
// make sure tendon has limit
|
||
if (!((mjCTendon*)obj)->is_limited()) {
|
||
throw mjCError(this, "tendon must be limited in sensor");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_FRAMEPOS:
|
||
case mjSENS_FRAMEQUAT:
|
||
case mjSENS_FRAMEXAXIS:
|
||
case mjSENS_FRAMEYAXIS:
|
||
case mjSENS_FRAMEZAXIS:
|
||
case mjSENS_FRAMELINVEL:
|
||
case mjSENS_FRAMEANGVEL:
|
||
case mjSENS_FRAMELINACC:
|
||
case mjSENS_FRAMEANGACC:
|
||
// must be attached to object with spatial frame
|
||
if (objtype != mjOBJ_BODY && objtype != mjOBJ_XBODY &&
|
||
objtype != mjOBJ_GEOM && objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
|
||
throw mjCError(this, "sensor must be attached to (x)body, geom, site or camera");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_SUBTREECOM:
|
||
case mjSENS_SUBTREELINVEL:
|
||
case mjSENS_SUBTREEANGMOM:
|
||
// must be attached to body
|
||
if (objtype != mjOBJ_BODY) {
|
||
throw mjCError(this, "sensor must be attached to body");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_INSIDESITE:
|
||
if (objtype != mjOBJ_BODY && objtype != mjOBJ_XBODY &&
|
||
objtype != mjOBJ_GEOM && objtype != mjOBJ_SITE && objtype != mjOBJ_CAMERA) {
|
||
throw mjCError(this, "sensor must be attached to (x)body, geom, site or camera");
|
||
}
|
||
if (reftype != mjOBJ_SITE) {
|
||
throw mjCError(this, "sensor must be associated with a site");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_GEOMDIST:
|
||
case mjSENS_GEOMNORMAL:
|
||
case mjSENS_GEOMFROMTO:
|
||
// must be attached to body or geom
|
||
if ((objtype != mjOBJ_BODY && objtype != mjOBJ_GEOM) ||
|
||
(reftype != mjOBJ_BODY && reftype != mjOBJ_GEOM)) {
|
||
throw mjCError(this, "sensor must be attached to body or geom");
|
||
}
|
||
|
||
// objects must be different
|
||
if (objtype == reftype && obj == ref) {
|
||
throw mjCError(this, "1st body/geom must be different from 2nd body/geom");
|
||
}
|
||
|
||
// height fields are not necessarily convex and are not yet supported
|
||
if ((objtype == mjOBJ_GEOM && static_cast<mjCGeom*>(obj)->Type() == mjGEOM_HFIELD) ||
|
||
(reftype == mjOBJ_GEOM && static_cast<mjCGeom*>(ref)->Type() == mjGEOM_HFIELD)) {
|
||
throw mjCError(this, "height fields are not supported in geom distance sensors");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_CONTACT:
|
||
{
|
||
// check first matching criterion
|
||
if (objtype != mjOBJ_SITE &&
|
||
objtype != mjOBJ_BODY &&
|
||
objtype != mjOBJ_XBODY &&
|
||
objtype != mjOBJ_GEOM &&
|
||
objtype != mjOBJ_UNKNOWN) {
|
||
throw mjCError(this, "first matching criterion: if set, must be (x)body, geom or site");
|
||
}
|
||
|
||
// check second matching criterion
|
||
if (reftype != mjOBJ_BODY &&
|
||
reftype != mjOBJ_XBODY &&
|
||
reftype != mjOBJ_GEOM &&
|
||
reftype != mjOBJ_UNKNOWN) {
|
||
throw mjCError(this, "second matching criterion: if set, must be (x)body or geom");
|
||
}
|
||
|
||
// check for dataspec correctness
|
||
int dataspec = intprm[0];
|
||
if (dataspec <= 0) {
|
||
throw mjCError(this, "data spec (intprm[0]) must be positive, got %d", nullptr, dataspec);
|
||
}
|
||
int mask = (1 << mjNCONDATA) - 1;
|
||
if (!(dataspec & mask)) {
|
||
throw mjCError(this, "data spec intprm[0]=%d must have at least one bit set of the first "
|
||
"mjNCONDATA bits", nullptr, dataspec);
|
||
}
|
||
if (dataspec & ~mask) {
|
||
throw mjCError(this, "data spec intprm[0]=%d has bits set beyond the first "
|
||
"mjNCONDATA bits", nullptr, dataspec);
|
||
}
|
||
|
||
// check for reduce correctness
|
||
int reduce = intprm[1];
|
||
if (reduce < 0 || reduce > 3) {
|
||
throw mjCError(this, "unknown reduction criterion. got %d, "
|
||
"expected one of {0, 1, 2, 3}", nullptr, reduce);
|
||
}
|
||
|
||
// check for non-positive num
|
||
if (intprm[2] <= 0) {
|
||
throw mjCError(this, "num (intprm[2]) must be positive in sensor, got %d", nullptr, dim);
|
||
}
|
||
}
|
||
break;
|
||
|
||
case mjSENS_E_POTENTIAL:
|
||
case mjSENS_E_KINETIC:
|
||
case mjSENS_CLOCK:
|
||
break;
|
||
|
||
case mjSENS_USER:
|
||
// check for negative dim
|
||
if (dim < 0) {
|
||
throw mjCError(this, "sensor dim must be non-negative in sensor");
|
||
}
|
||
|
||
// make sure dim is consistent with datatype
|
||
if (datatype == mjDATATYPE_AXIS && dim != 3) {
|
||
throw mjCError(this, "datatype AXIS requires dim=3 in sensor");
|
||
}
|
||
if (datatype == mjDATATYPE_QUATERNION && dim != 4) {
|
||
throw mjCError(this, "datatype QUATERNION requires dim=4 in sensor");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_TACTILE:
|
||
if (objtype != mjOBJ_MESH) {
|
||
throw mjCError(this, "sensor must be associated with a mesh");
|
||
}
|
||
if (reftype != mjOBJ_GEOM) {
|
||
throw mjCError(this, "sensor must be associated with a geom");
|
||
}
|
||
break;
|
||
|
||
case mjSENS_PLUGIN:
|
||
if (plugin_name.empty() && plugin_instance_name.empty()) {
|
||
throw mjCError(this, "neither 'plugin' nor 'instance' is specified for sensor");
|
||
}
|
||
|
||
// resolve plugin instance, or create one if using the "plugin" attribute shortcut
|
||
{
|
||
mjCPlugin* plugin_instance = static_cast<mjCPlugin*>(plugin.element);
|
||
model->ResolvePlugin(this, plugin_name, plugin_instance_name, &plugin_instance);
|
||
plugin.element = plugin_instance;
|
||
const mjpPlugin* pplugin = mjp_getPluginAtSlot(plugin_instance->plugin_slot);
|
||
if (!(pplugin->capabilityflags & mjPLUGIN_SENSOR)) {
|
||
throw mjCError(this, "plugin '%s' does not support sensors", pplugin->name);
|
||
}
|
||
needstage = static_cast<mjtStage>(pplugin->needstage);
|
||
}
|
||
|
||
break;
|
||
|
||
default:
|
||
throw mjCError(this, "invalid type in sensor '%s' (id = %d)", name.c_str(), id);
|
||
}
|
||
|
||
dim = mjs_sensorDim(this);
|
||
|
||
// check cutoff for incompatible data types
|
||
if (cutoff > 0 && (datatype == mjDATATYPE_QUATERNION ||
|
||
(datatype == mjDATATYPE_AXIS && type != mjSENS_GEOMNORMAL))) {
|
||
throw mjCError(this, "cutoff applied to axis or quaternion datatype in sensor");
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCNumeric implementation -----------------------------------------------
|
||
|
||
// constructor
|
||
mjCNumeric::mjCNumeric(mjCModel* _model) {
|
||
mjs_defaultNumeric(&spec);
|
||
elemtype = mjOBJ_NUMERIC;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear variables
|
||
spec_data_.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this numeric is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCNumeric::mjCNumeric(const mjCNumeric& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCNumeric& mjCNumeric::operator=(const mjCNumeric& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCNumeric_*>(this) = static_cast<const mjCNumeric_&>(other);
|
||
*static_cast<mjsNumeric*>(this) = static_cast<const mjsNumeric&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCNumeric::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.data = &spec_data_;
|
||
spec.info = &info;
|
||
data = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCNumeric::CopyFromSpec() {
|
||
*static_cast<mjsNumeric*>(this) = spec;
|
||
data_ = spec_data_;
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCNumeric::~mjCNumeric() {
|
||
spec_data_.clear();
|
||
data_.clear();
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCNumeric::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// check for size conflict
|
||
if (size && !data_.empty() && size < (int)data_.size()) {
|
||
throw mjCError(this,
|
||
"numeric '%s' (id = %d): specified size smaller than initialization array",
|
||
name.c_str(), id);
|
||
}
|
||
|
||
// set size if left unspecified
|
||
if (!size) {
|
||
size = (int)data_.size();
|
||
}
|
||
|
||
// size cannot be zero
|
||
if (!size) {
|
||
throw mjCError(this, "numeric '%s' (id = %d): size cannot be zero", name.c_str(), id);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCText implementation --------------------------------------------------
|
||
|
||
// constructor
|
||
mjCText::mjCText(mjCModel* _model) {
|
||
mjs_defaultText(&spec);
|
||
elemtype = mjOBJ_TEXT;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear variables
|
||
spec_data_.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this text is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCText::mjCText(const mjCText& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCText& mjCText::operator=(const mjCText& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCText_*>(this) = static_cast<const mjCText_&>(other);
|
||
*static_cast<mjsText*>(this) = static_cast<const mjsText&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCText::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.data = &spec_data_;
|
||
spec.info = &info;
|
||
data = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCText::CopyFromSpec() {
|
||
*static_cast<mjsText*>(this) = spec;
|
||
data_ = spec_data_;
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCText::~mjCText() {
|
||
data_.clear();
|
||
spec_data_.clear();
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCText::Compile(void) {
|
||
CopyFromSpec();
|
||
|
||
// size cannot be zero
|
||
if (data_.empty()) {
|
||
throw mjCError(this, "text '%s' (id = %d): size cannot be zero", name.c_str(), id);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCTuple implementation -------------------------------------------------
|
||
|
||
// constructor
|
||
mjCTuple::mjCTuple(mjCModel* _model) {
|
||
mjs_defaultTuple(&spec);
|
||
elemtype = mjOBJ_TUPLE;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear variables
|
||
spec_objtype_.clear();
|
||
spec_objname_.clear();
|
||
spec_objprm_.clear();
|
||
obj.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this tuple is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCTuple::mjCTuple(const mjCTuple& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCTuple& mjCTuple::operator=(const mjCTuple& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCTuple_*>(this) = static_cast<const mjCTuple_&>(other);
|
||
*static_cast<mjsTuple*>(this) = static_cast<const mjsTuple&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCTuple::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.objtype = (mjIntVec*)&spec_objtype_;
|
||
spec.objname = &spec_objname_;
|
||
spec.objprm = &spec_objprm_;
|
||
spec.info = &info;
|
||
objname = nullptr;
|
||
objprm = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCTuple::NameSpace(const mjCModel* m) {
|
||
if (!name.empty()) {
|
||
name = m->prefix + name + m->suffix;
|
||
}
|
||
for (int i=0; i < spec_objname_.size(); i++) {
|
||
spec_objname_[i] = m->prefix + spec_objname_[i] + m->suffix;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void mjCTuple::CopyFromSpec() {
|
||
*static_cast<mjsTuple*>(this) = spec;
|
||
objtype_ = spec_objtype_;
|
||
objname_ = spec_objname_;
|
||
objprm_ = spec_objprm_;
|
||
objtype = (mjIntVec*)&objtype_;
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCTuple::~mjCTuple() {
|
||
objtype_.clear();
|
||
objname_.clear();
|
||
objprm_.clear();
|
||
spec_objtype_.clear();
|
||
spec_objname_.clear();
|
||
spec_objprm_.clear();
|
||
obj.clear();
|
||
}
|
||
|
||
|
||
|
||
void mjCTuple::ResolveReferences(const mjCModel* m) {
|
||
// check for empty tuple
|
||
if (objtype_.empty()) {
|
||
throw mjCError(this, "tuple '%s' (id = %d) is empty", name.c_str(), id);
|
||
}
|
||
|
||
// check for size conflict
|
||
if (objtype_.size() != objname_.size() || objtype_.size() != objprm_.size()) {
|
||
throw mjCError(this,
|
||
"tuple '%s' (id = %d) has object arrays with different sizes", name.c_str(), id);
|
||
}
|
||
|
||
// resize objid to correct size
|
||
obj.resize(objtype_.size());
|
||
|
||
// find objects, fill in ids
|
||
for (int i=0; i < objtype_.size(); i++) {
|
||
// find object by type and name
|
||
mjCBase* res = m->FindObject(objtype_[i], objname_[i]);
|
||
if (!res) {
|
||
throw mjCError(this, "unrecognized object '%s' in tuple %d", objname_[i].c_str(), id);
|
||
}
|
||
|
||
// if geom mark it as non visual
|
||
if (objtype_[i] == mjOBJ_GEOM) {
|
||
((mjCGeom*)res)->SetNotVisual();
|
||
}
|
||
|
||
// assign id
|
||
obj[i] = res;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCTuple::Compile(void) {
|
||
CopyFromSpec();
|
||
ResolveReferences(model);
|
||
}
|
||
|
||
|
||
|
||
//------------------ class mjCKey implementation ---------------------------------------------------
|
||
|
||
// constructor
|
||
mjCKey::mjCKey(mjCModel* _model) {
|
||
mjs_defaultKey(&spec);
|
||
elemtype = mjOBJ_KEY;
|
||
|
||
// set model pointer
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
|
||
// clear variables
|
||
spec_qpos_.clear();
|
||
spec_qvel_.clear();
|
||
spec_act_.clear();
|
||
spec_mpos_.clear();
|
||
spec_mquat_.clear();
|
||
spec_ctrl_.clear();
|
||
|
||
// point to local
|
||
PointToLocal();
|
||
|
||
// in case this keyframe is not compiled
|
||
CopyFromSpec();
|
||
}
|
||
|
||
|
||
|
||
mjCKey::mjCKey(const mjCKey& other) {
|
||
*this = other;
|
||
}
|
||
|
||
|
||
|
||
mjCKey& mjCKey::operator=(const mjCKey& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCKey_*>(this) = static_cast<const mjCKey_&>(other);
|
||
*static_cast<mjsKey*>(this) = static_cast<const mjsKey&>(other);
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCKey::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.qpos = &spec_qpos_;
|
||
spec.qvel = &spec_qvel_;
|
||
spec.act = &spec_act_;
|
||
spec.mpos = &spec_mpos_;
|
||
spec.mquat = &spec_mquat_;
|
||
spec.ctrl = &spec_ctrl_;
|
||
spec.info = &info;
|
||
qpos = nullptr;
|
||
qvel = nullptr;
|
||
act = nullptr;
|
||
mpos = nullptr;
|
||
mquat = nullptr;
|
||
ctrl = nullptr;
|
||
}
|
||
|
||
|
||
|
||
void mjCKey::CopyFromSpec() {
|
||
*static_cast<mjsKey*>(this) = spec;
|
||
qpos_ = spec_qpos_;
|
||
qvel_ = spec_qvel_;
|
||
act_ = spec_act_;
|
||
mpos_ = spec_mpos_;
|
||
mquat_ = spec_mquat_;
|
||
ctrl_ = spec_ctrl_;
|
||
}
|
||
|
||
|
||
|
||
// destructor
|
||
mjCKey::~mjCKey() {
|
||
qpos_.clear();
|
||
qvel_.clear();
|
||
act_.clear();
|
||
mpos_.clear();
|
||
mquat_.clear();
|
||
ctrl_.clear();
|
||
spec_qpos_.clear();
|
||
spec_qvel_.clear();
|
||
spec_act_.clear();
|
||
spec_mpos_.clear();
|
||
spec_mquat_.clear();
|
||
spec_ctrl_.clear();
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCKey::Compile(const mjModel* m) {
|
||
CopyFromSpec();
|
||
|
||
// qpos: allocate or check size
|
||
if (qpos_.empty()) {
|
||
qpos_.resize(m->nq);
|
||
for (int i=0; i < m->nq; i++) {
|
||
qpos_[i] = (double)m->qpos0[i];
|
||
}
|
||
} else if (qpos_.size() != m->nq) {
|
||
throw mjCError(this, "keyframe '%s': invalid qpos size, expected %d, got %d",
|
||
name.c_str(), m->nq, qpos_.size());
|
||
}
|
||
|
||
// qvel: allocate or check size
|
||
if (qvel_.empty()) {
|
||
qvel_.resize(m->nv);
|
||
for (int i=0; i < m->nv; i++) {
|
||
qvel_[i] = 0;
|
||
}
|
||
} else if (qvel_.size() != m->nv) {
|
||
throw mjCError(this, "keyframe '%s': invalid qvel size, expected %d, got %d",
|
||
name.c_str(), m->nv, qvel_.size());
|
||
}
|
||
|
||
// act: allocate or check size
|
||
if (act_.empty()) {
|
||
act_.resize(m->na);
|
||
for (int i=0; i < m->na; i++) {
|
||
act_[i] = 0;
|
||
}
|
||
} else if (act_.size() != m->na) {
|
||
throw mjCError(this, "keyframe '%s': invalid act size, expected %d, got %d",
|
||
name.c_str(), m->na, act_.size());
|
||
}
|
||
|
||
// mpos: allocate or check size
|
||
if (mpos_.empty()) {
|
||
mpos_.resize(3*m->nmocap);
|
||
if (m->nmocap) {
|
||
for (int i=0; i < m->nbody; i++) {
|
||
if (m->body_mocapid[i] >= 0) {
|
||
int mocapid = m->body_mocapid[i];
|
||
mpos_[3*mocapid] = m->body_pos[3*i];
|
||
mpos_[3*mocapid+1] = m->body_pos[3*i+1];
|
||
mpos_[3*mocapid+2] = m->body_pos[3*i+2];
|
||
}
|
||
}
|
||
}
|
||
} else if (mpos_.size() != 3*m->nmocap) {
|
||
throw mjCError(this, "keyframe %d: invalid mpos size, expected length %d", nullptr, id, 3*m->nmocap);
|
||
}
|
||
|
||
// mquat: allocate or check size
|
||
if (mquat_.empty()) {
|
||
mquat_.resize(4*m->nmocap);
|
||
if (m->nmocap) {
|
||
for (int i=0; i < m->nbody; i++) {
|
||
if (m->body_mocapid[i] >= 0) {
|
||
int mocapid = m->body_mocapid[i];
|
||
mquat_[4*mocapid] = m->body_quat[4*i];
|
||
mquat_[4*mocapid+1] = m->body_quat[4*i+1];
|
||
mquat_[4*mocapid+2] = m->body_quat[4*i+2];
|
||
mquat_[4*mocapid+3] = m->body_quat[4*i+3];
|
||
}
|
||
}
|
||
}
|
||
} else if (mquat_.size() != 4*m->nmocap) {
|
||
throw mjCError(this, "keyframe %d: invalid mquat size, expected length %d", nullptr, id, 4*m->nmocap);
|
||
}
|
||
|
||
// ctrl: allocate or check size
|
||
if (ctrl_.empty()) {
|
||
ctrl_.resize(m->nu);
|
||
for (int i=0; i < m->nu; i++) {
|
||
ctrl_[i] = 0;
|
||
}
|
||
} else if (ctrl_.size() != m->nu) {
|
||
throw mjCError(this, "keyframe %d: invalid ctrl size, expected length %d", nullptr, id, m->nu);
|
||
}
|
||
}
|
||
|
||
|
||
//------------------ class mjCPlugin implementation ------------------------------------------------
|
||
|
||
// initialize defaults
|
||
mjCPlugin::mjCPlugin(mjCModel* _model) {
|
||
name = "";
|
||
nstate = -1;
|
||
plugin_slot = -1;
|
||
parent = this;
|
||
model = _model;
|
||
if (_model) compiler = &_model->spec.compiler;
|
||
name.clear();
|
||
plugin_name.clear();
|
||
|
||
// public interface
|
||
mjs_defaultPlugin(&spec);
|
||
elemtype = mjOBJ_PLUGIN;
|
||
spec.plugin_name = &plugin_name;
|
||
spec.info = &info;
|
||
|
||
PointToLocal();
|
||
}
|
||
|
||
|
||
|
||
mjCPlugin::mjCPlugin(const mjCPlugin& other) {
|
||
*this = other;
|
||
id = -1;
|
||
}
|
||
|
||
|
||
|
||
mjCPlugin& mjCPlugin::operator=(const mjCPlugin& other) {
|
||
if (this != &other) {
|
||
this->spec = other.spec;
|
||
*static_cast<mjCPlugin_*>(this) = static_cast<const mjCPlugin_&>(other);
|
||
parent = this;
|
||
plugin_slot = other.plugin_slot;
|
||
}
|
||
PointToLocal();
|
||
return *this;
|
||
}
|
||
|
||
|
||
|
||
void mjCPlugin::PointToLocal() {
|
||
spec.element = static_cast<mjsElement*>(this);
|
||
spec.name = &name;
|
||
spec.info = &info;
|
||
}
|
||
|
||
|
||
|
||
// compiler
|
||
void mjCPlugin::Compile(void) {
|
||
mjCPlugin* plugin_instance = this;
|
||
model->ResolvePlugin(this, plugin_name, name, &plugin_instance);
|
||
const mjpPlugin* plugin = mjp_getPluginAtSlot(plugin_slot);
|
||
|
||
// clear precompiled
|
||
flattened_attributes.clear();
|
||
std::map<std::string, std::string, std::less<> > config_attribs_copy = config_attribs;
|
||
|
||
// concatenate all of the plugin's attribute values (as null-terminated strings) into
|
||
// flattened_attributes, in the order declared in the mjpPlugin
|
||
// each valid attribute found is appended to flattened_attributes and removed from xml_attributes
|
||
for (int i = 0; i < plugin->nattribute; ++i) {
|
||
std::string_view attr(plugin->attributes[i]);
|
||
auto it = config_attribs_copy.find(attr);
|
||
if (it == config_attribs_copy.end()) {
|
||
flattened_attributes.push_back('\0');
|
||
} else {
|
||
auto original_size = flattened_attributes.size();
|
||
flattened_attributes.resize(original_size + it->second.size() + 1);
|
||
std::memcpy(&flattened_attributes[original_size], it->second.c_str(),
|
||
it->second.size() + 1);
|
||
config_attribs_copy.erase(it);
|
||
}
|
||
}
|
||
|
||
// if there are no attributes, add a null terminator
|
||
if (plugin->nattribute == 0) {
|
||
flattened_attributes.push_back('\0');
|
||
}
|
||
|
||
// anything left in xml_attributes at this stage is not a valid attribute
|
||
if (!config_attribs_copy.empty()) {
|
||
std::string error =
|
||
"unrecognized attribute 'plugin:" + config_attribs_copy.begin()->first +
|
||
"' for plugin " + std::string(plugin->name) + "'";
|
||
throw mjCError(parent, "%s", error.c_str());
|
||
}
|
||
}
|