chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix type errors in compiler and tests.

Browse Source 
In preparation for a float32 build of MuJoCo.

PiperOrigin-RevId: 644407674
Change-Id: I60069f0f865ef89fbf613f5a2ca0c336cc63461e
This commit is contained in:
Yuval Tassa
2024-06-18 09:11:39 -07:00
committed by Copybara-Service
parent c8cc2d51b0
commit 8e2f830fd2
21 changed files with 715 additions and 506 deletions
Download Patch File Download Diff File Expand all files Collapse all files
plugin/sdf/bolt.cc
+1 -1
View File
@@ -148,7 +148,7 @@ void Bolt::RegisterPlugin() {
delete reinterpret_cast<Bolt*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<Bolt*>(plugin_data);
sdf->Reset();
plugin/sdf/bowl.cc
+1 -1
View File
@@ -146,7 +146,7 @@ void Bowl::RegisterPlugin() {
delete reinterpret_cast<Bowl*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<Bowl*>(plugin_data);
sdf->Reset();
plugin/sdf/gear.cc
+1 -1
View File
@@ -232,7 +232,7 @@ void Gear::RegisterPlugin() {
delete reinterpret_cast<Gear*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<Gear*>(plugin_data);
sdf->Reset();
plugin/sdf/nut.cc
+1 -1
View File
@@ -148,7 +148,7 @@ void Nut::RegisterPlugin() {
delete reinterpret_cast<Nut*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<Nut*>(plugin_data);
sdf->Reset();
plugin/sdf/sdflib.cc
+1 -1
View File
@@ -180,7 +180,7 @@ void SdfLib::RegisterPlugin() {
delete reinterpret_cast<SdfLib*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
auto sdf = reinterpret_cast<SdfLib*>(plugin_data);
sdf->Reset();
plugin/sdf/torus.cc
+1 -1
View File
@@ -95,7 +95,7 @@ void Torus::RegisterPlugin() {
delete reinterpret_cast<Torus*>(d->plugin_data[instance]);
d->plugin_data[instance] = 0;
};
plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data,
plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data,
int instance) {
// do nothing
};
src/user/user_composite.cc
+16 -16
View File
@@ -31,7 +31,6 @@
#include <mujoco/mujoco.h>
#include "cc/array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_model.h"
@@ -428,12 +427,12 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
}
// compute volume
std::vector<mjtNum> volume(uservert.size()/3);
mjtNum t = 1;
std::vector<double> volume(uservert.size()/3);
double thickness = 1;
if (dim == 2 && plugin.active) {
try {
mjCPlugin* pplugin = static_cast<mjCPlugin*>(plugin.instance);
t = std::stod(pplugin->config_attribs["thickness"], nullptr);
thickness = std::stod(pplugin->config_attribs["thickness"], nullptr);
} catch (const std::invalid_argument& e) {
return comperr(error, "Invalid thickness attribute", error_sz);
}
@@ -441,9 +440,9 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
if (!userface.empty()) {
face = mjXUtil::String2Vector<int>(userface);
for (int j=0; j<face.size()/3; j++) {
mjtNum area[3];
mjtNum edge1[3];
mjtNum edge2[3];
double area[3];
double edge1[3];
double edge2[3];
for (int i=0; i<3; i++) {
edge1[i] = uservert[3*face[3*j+1]+i] - uservert[3*face[3*j]+i];
@@ -452,12 +451,12 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int
mjuu_crossvec(area, edge1, edge2);
for (int i=0; i<3; i++) {
volume[face[3*j+i]] += sqrt(mjuu_dot3(area, area)) / 2 * t;
volume[face[3*j+i]] += sqrt(mjuu_dot3(area, area)) / 2 * thickness;
}
}
} else {
for (int i=0; i<uservert.size()/3; i++) {
volume[i] = 6 * spacing * spacing / 2 * t;
volume[i] = 6 * spacing * spacing / 2 * thickness;
}
}
@@ -754,7 +753,7 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er
}
// create frame
mjtNum normal[3], prev_quat[4];
double normal[3], prev_quat[4];
mjuu_setvec(normal, 0, 1, 0);
mjuu_setvec(prev_quat, 1, 0, 0, 0);
@@ -780,10 +779,11 @@ bool mjCComposite::MakeCable(mjCModel* model, mjsBody* body, char* error, int er
mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]) {
mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix,
double normal[3], double prev_quat[4]) {
char txt_geom[100], txt_site[100], txt_slide[100];
char this_body[100], next_body[100], this_joint[100];
mjtNum dquat[4], this_quat[4];
double dquat[4], this_quat[4];
// set flags
int lastidx = count[0]-2;
@@ -792,7 +792,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtN
bool secondlast = ix==lastidx-1;
// compute edge and tangent vectors
mjtNum edge[3], tprev[3], tnext[3], length_prev = 0;
double edge[3], tprev[3], tnext[3], length_prev = 0;
mjuu_setvec(edge, uservert[3*(ix+1)+0]-uservert[3*ix+0],
uservert[3*(ix+1)+1]-uservert[3*ix+1],
uservert[3*(ix+1)+2]-uservert[3*ix+2]);
@@ -810,7 +810,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtN
}
// update moving frame
mjtNum length = mju_updateFrame(this_quat, normal, edge, tprev, tnext, first);
double length = mjuu_updateFrame(this_quat, normal, edge, tprev, tnext, first);
// create body, joint, and geom names
if (first) {
@@ -845,7 +845,7 @@ mjsBody* mjCComposite::AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtN
mjuu_copyvec(body->quat, this_quat, 4);
} else {
mjuu_setvec(body->pos, length_prev, 0, 0);
mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]};
double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]};
mjuu_mulquat(dquat, negquat, this_quat);
mjuu_copyvec(body->quat, dquat, 4);
}
@@ -1091,7 +1091,7 @@ void mjCComposite::BoxProject(double* pos) {
// cylinder
else if (type==mjCOMPTYPE_CYLINDER) {
double L0 = mju_max(mju_abs(pos[0]), mju_abs(pos[1]));
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
mjuu_normvec(pos, 2);
pos[0] *= size[0]*L0;
pos[1] *= size[1]*L0;
src/user/user_composite.h
+2 -2
View File
@@ -107,7 +107,7 @@ class mjCComposite {
std::string initial; // root boundary type
std::vector<float> uservert; // user-specified vertex positions
std::string userface; // connectivity
mjtNum size[3]; // rope size (meaning depends on the shape)
double size[3]; // rope size (meaning depends on the shape)
mjtCompShape curve[3]; // geometric shape
// body names used in the skin
@@ -137,7 +137,7 @@ class mjCComposite {
private:
mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1);
mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]);
mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]);
// temporary skin vectors
void CopyIntoSkin(mjsSkin* skin);
src/user/user_flexcomp.cc
+12 -17
View File
@@ -14,6 +14,7 @@
#include <algorithm>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstring>
@@ -29,10 +30,7 @@
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
#include "user/user_flexcomp.h"
#include <mujoco/mjspec.h>
#include "user/user_model.h"
@@ -250,12 +248,9 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
}
// apply pose transform to points
mjtNum posn[3], quatn[4];
mju_d2n(posn, pos, 3);
mju_d2n(quatn, quat, 4);
for (int i=0; i < npnt; i++) {
mjtNum newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]};
mju_trnVecPose(newp, posn, quatn, oldp);
double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]};
mjuu_trnVecPose(newp, pos, quat, oldp);
point[3*i] = newp[0];
point[3*i+1] = newp[1];
point[3*i+2] = newp[2];
@@ -570,7 +565,7 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};
// add point
mjtNum pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)),
double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)),
spacing[1]*(iy - 0.5*(count[1]-1))};
point.push_back(pos[0]);
point.push_back(pos[1]);
@@ -578,8 +573,8 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
// add texture coordinates, if not specified explicitly
if (!hastex) {
texcoord.push_back(ix/(mjtNum)mjMAX(count[0]-1, 1));
texcoord.push_back(iy/(mjtNum)mjMAX(count[1]-1, 1));
texcoord.push_back(ix/(double)std::max(count[0]-1, 1));
texcoord.push_back(iy/(double)std::max(count[1]-1, 1));
}
// flip triangles if radial projection is requested
@@ -713,7 +708,7 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) {
// cylinder
else if (type==mjFCOMPTYPE_CYLINDER) {
double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1]));
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
mjuu_normvec(pos, 2);
pos[0] *= size[0]*L0;
pos[1] *= size[1]*L0;
@@ -749,8 +744,8 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
};
for (int i=0; i < point.size()/3; i++) {
mjtNum* pos = point.data() + i*3;
double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1]));
double* pos = point.data() + i*3;
double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
mjuu_normvec(pos, 2);
pos[0] *= size[0]*L0;
pos[1] *= size[1]*L0;
@@ -952,10 +947,10 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) {
// copy faces
element = mesh.face_;
// copy vertices, convert from float to mjtNum
point = vector<mjtNum> (mesh.nvert()*3);
// copy vertices, convert from float to double
point = vector<double> (mesh.nvert()*3);
for (int i=0; i < mesh.nvert()*3; i++) {
point[i] = (mjtNum) mesh.vert_[i];
point[i] = (double) mesh.vert_[i];
}
return true;
src/user/user_flexcomp.h
+1 -1
View File
@@ -89,7 +89,7 @@ class mjCFlexcomp {
// set by user or computed internally
bool rigid; // all vertices are in parent body (all pinned)
bool centered; // all vertex coordinates are (0,0,0) (nothing pinned)
std::vector<mjtNum> point; // flex bodies/vertices
std::vector<double> point; // flex bodies/vertices
std::vector<bool> pinned; // is point pinned (true: no new body)
std::vector<bool> used; // is point used by any element (false: skip)
std::vector<int> element; // flex elements
src/user/user_mesh.cc
+62 -66
View File
@@ -57,11 +57,7 @@
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include "user/user_cache.h"
#include "user/user_model.h"
#include "user/user_objects.h"
@@ -77,7 +73,7 @@ using std::string;
using std::vector;
// compute triangle area, surface normal, center
static mjtNum _triangle(mjtNum* normal, mjtNum* center,
static double _triangle(double* normal, double* center,
const float* v1, const float* v2, const float* v3) {
// center
if (center) {
@@ -89,10 +85,10 @@ static mjtNum _triangle(mjtNum* normal, mjtNum* center,
// normal = (v2-v1) cross (v3-v1)
double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] };
double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] };
mju_cross(normal, b, c);
mjuu_crossvec(normal, b, c);
// get length
double len = mju_norm3(normal);
double len = sqrt(mjuu_dot3(normal, normal));
// ignore small faces
if (len<mjMINVAL) {
@@ -296,7 +292,7 @@ void mjCMesh::LoadSDF() {
pplugin->sdf_aabb(aabb, attributes.data());
mjtNum total = aabb[3] + aabb[4] + aabb[5];
const mjtNum n = 300;
const double n = 300;
int nx, ny, nz;
nx = floor(n / total * aabb[3]) + 1;
ny = floor(n / total * aabb[4]) + 1;
@@ -456,7 +452,7 @@ void mjCMesh::Compile(const mjVFS* vfs) {
int v0 = face_[3*i+0];
int v1 = face_[3*i+1];
int v2 = face_[3*i+2];
mjtNum normal[3];
double normal[3];
float* vtx = vert_.data();
if (_triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) {
halfedge_.push_back(std::pair(v0, v1));
@@ -552,7 +548,7 @@ void mjCMesh::SetBoundingVolume(int faceid) {
node->contype = 1;
node->pos = center_ + 3*faceid;
node->quat = NULL;
mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
double face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) {
int vertid = face_[3*faceid+j];
face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]);
@@ -1263,15 +1259,15 @@ void mjCMesh::ApplyTransformations() {
// rotate
if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) {
// prepare rotation
mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
mjtNum mat[9];
mju_normalize4(quat);
mju_quat2Mat(mat, quat);
double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]};
double mat[9];
mjuu_normvec(quat, 4);
mjuu_quat2mat(mat, quat);
// process vertices
for (int i=0; i < nvert(); i++) {
mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
mju_mulMatTVec3(p1, mat, p0);
double p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]};
mjuu_mulvecmatT(p1, p0, mat);
vert_[3*i] = (float) p1[0];
vert_[3*i+1] = (float) p1[1];
vert_[3*i+2] = (float) p1[2];
@@ -1279,8 +1275,8 @@ void mjCMesh::ApplyTransformations() {
// process normals
for (int i=0; i < nnormal(); i++) {
mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
mju_mulMatTVec3(n1, mat, n0);
double n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]};
mjuu_mulvecmatT(n1, n0, mat);
normal_[3*i] = (float) n1[0];
normal_[3*i+1] = (float) n1[1];
normal_[3*i+2] = (float) n1[2];
@@ -1449,13 +1445,13 @@ void mjCMesh::Process() {
inert[5] = -P[5];
// get quaternion and diagonal inertia
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
double eigval[3], eigvec[9], quattmp[4];
double full[9] = {
inert[0], inert[3], inert[4],
inert[3], inert[1], inert[5],
inert[4], inert[5], inert[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
mjuu_eig3(eigval, eigvec, quattmp, full);
// check eigval - SHOULD NOT OCCUR
if (eigval[2]<=0) {
@@ -1479,12 +1475,12 @@ void mjCMesh::Process() {
// if volume was valid, copy volume quat to shell and stop,
// otherwise use shell quat for coordinate transformations
if (type==mjINERTIA_SHELL && validvolume_>0) {
mju_copy4(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME));
mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4);
continue;
}
// rotate vertices and normals into axis-aligned frame
mju_copy4(GetQuatPtr(type), quattmp);
mjuu_copyvec(GetQuatPtr(type), quattmp, 4);
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
double mat[9];
mjuu_quat2mat(mat, neg);
@@ -1798,16 +1794,16 @@ void mjCMesh::MakeNormal(void) {
}
// get triangle edges
mjtNum vec01[3], vec02[3];
double vec01[3], vec02[3];
for (int j=0; j<3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
}
// compute face normal
mjtNum nrm[3];
mju_cross(nrm, vec01, vec02);
mjtNum area = mju_normalize3(nrm);
double nrm[3];
mjuu_crossvec(nrm, vec01, vec02);
double area = mjuu_normvec(nrm, 3);
// add normal to each vertex with weight = area
for (int j=0; j<3; j++) {
@@ -1833,25 +1829,25 @@ void mjCMesh::MakeNormal(void) {
}
// get triangle edges
mjtNum vec01[3], vec02[3];
double vec01[3], vec02[3];
for (int j=0; j<3; j++) {
vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j];
vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j];
}
// compute face normal
mjtNum nrm[3];
mju_cross(nrm, vec01, vec02);
mjtNum area = mju_normalize3(nrm);
double nrm[3];
mjuu_crossvec(nrm, vec01, vec02);
double area = mjuu_normvec(nrm, 3);
// compare to vertex normal, subtract contribution if dot product too small
for (int j=0; j<3; j++) {
// normalized vertex normal
mjtNum vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]};
mju_normalize3(vnrm);
double vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]};
mjuu_normvec(vnrm, 3);
// dot too small: remove
if (mju_dot3(nrm, vnrm)<0.8) {
if (mjuu_dot3(nrm, vnrm)<0.8) {
for (int k=0; k<3; k++) {
nremove[3*vertid[j]+k] += nrm[k]*area;
}
@@ -1901,28 +1897,28 @@ void mjCMesh::MakeCenter(void) {
int* vertid = face_.data() + 3*i;
// get triangle edges
mjtNum a[3], b[3];
double a[3], b[3];
for (int j=0; j<3; j++) {
a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j];
b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j];
}
// compute face normal
mjtNum nrm[3];
mju_cross(nrm, a, b);
double nrm[3];
mjuu_crossvec(nrm, a, b);
// compute circumradius
mjtNum norm_a_2 = mju_dot3(a, a);
mjtNum norm_b_2 = mju_dot3(b, b);
mjtNum area = mju_norm3(nrm);
double norm_a_2 = mjuu_dot3(a, a);
double norm_b_2 = mjuu_dot3(b, b);
double area = mjuu_normvec(nrm, 3);
// compute circumcenter
mjtNum res[3], vec[3] = {
double res[3], vec[3] = {
norm_a_2 * b[0] - norm_b_2 * a[0],
norm_a_2 * b[1] - norm_b_2 * a[1],
norm_a_2 * b[2] - norm_b_2 * a[2]
};
mju_cross(res, vec, nrm);
mjuu_crossvec(res, vec, nrm);
center_[3*i+0] = res[0]/(2*area*area) + vert_[3*vertid[2]+0];
center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1];
center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2];
@@ -2200,13 +2196,13 @@ void mjCSkin::Compile(const mjVFS* vfs) {
// normalize bindquat
for (int i=0; i < nbone; i++) {
mjtNum quat[4] = {
(mjtNum)bindquat_[4*i],
(mjtNum)bindquat_[4*i+1],
(mjtNum)bindquat_[4*i+2],
(mjtNum)bindquat_[4*i+3]
double quat[4] = {
(double)bindquat_[4*i],
(double)bindquat_[4*i+1],
(double)bindquat_[4*i+2],
(double)bindquat_[4*i+3]
};
mju_normalize4(quat);
mjuu_normvec(quat, 4);
bindquat_[4*i] = (float) quat[0];
bindquat_[4*i+1] = (float) quat[1];
@@ -2562,17 +2558,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// compute global vertex positions
vertxpos = vector<mjtNum> (3*nvert);
vertxpos = vector<double> (3*nvert);
for (int i=0; i < nvert; i++) {
// get body id, set vertxpos = body.xpos0
int b = rigid ? vertbodyid[0] : vertbodyid[i];
mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0);
mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add vertex offset within body if not centered
if (!centered) {
mjtNum offset[3];
mju_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset);
double offset[3];
mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
}
}
@@ -2581,18 +2577,18 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (dim==3) {
for (int e=0; e<nelem; e++) {
const int* edata = elem_.data() + e*(dim+1);
mjtNum* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2];
mjtNum* v3 = vertxpos.data() + 3*edata[3];
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
double* v0 = vertxpos.data() + 3*edata[0];
double* v1 = vertxpos.data() + 3*edata[1];
double* v2 = vertxpos.data() + 3*edata[2];
double* v3 = vertxpos.data() + 3*edata[3];
double v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
double v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
double v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
// detect wrong orientation
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) {
double nrm[3];
mjuu_crossvec(nrm, v01, v02);
if (mjuu_dot3(nrm, v03)>0) {
// flip orientation
int tmp = elem_[e*(dim+1)+1];
elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2];
@@ -2672,9 +2668,9 @@ void mjCFlex::CreateBVH(void) {
}
// compute min and max along each global axis
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vertxpos.data() + 3*edata[0]);
mju_copy3(xmax, vertxpos.data() + 3*edata[0]);
double xmin[3], xmax[3];
mjuu_copyvec(xmin, vertxpos.data() + 3*edata[0], 3);
mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3);
for (int i=1; i <= dim; i++) {
for (int j=0; j<3; j++) {
xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]);
src/user/user_model.cc
+190 -199
View File
@@ -15,6 +15,7 @@
#include "user/user_model.h"
#include <algorithm>
#include <cmath>
#include <csetjmp>
#include <cstddef>
#include <cstdint>
@@ -38,7 +39,6 @@
#include "engine/engine_plugin.h"
#include "engine/engine_setconst.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_api.h"
@@ -69,15 +69,6 @@ using std::vector;
#endif
// copy real-valued vector
template <class T1, class T2>
static void copyvec(T1* dest, T2* src, int n) {
for (int i=0; i<n; i++) {
dest[i] = (T1)src[i];
}
}
//---------------------------------- CONSTRUCTOR AND DESTRUCTOR ------------------------------------
@@ -1434,11 +1425,11 @@ void mjCModel::AutoSpringDamper(mjModel* m) {
for (int i=0; i<ndim; i++) {
inertia += m->dof_invweight0[adr+i];
}
inertia = ((mjtNum)ndim) / mju_max(mjMINVAL, inertia);
inertia = ((mjtNum)ndim) / std::max(mjMINVAL, inertia);
// compute stiffness and damping (same as solref computation)
mjtNum stiffness = inertia / mju_max(mjMINVAL, timeconst*timeconst*dampratio*dampratio);
mjtNum damping = 2 * inertia / mju_max(mjMINVAL, timeconst);
mjtNum stiffness = inertia / std::max(mjMINVAL, timeconst*timeconst*dampratio*dampratio);
mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst);
// assign
m->jnt_stiffness[n] = stiffness;
@@ -1771,14 +1762,14 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_dofadr[i] = (pb->dofnum ? dofadr : -1);
m->body_geomnum[i] = (int)pb->geoms.size();
m->body_geomadr[i] = (!pb->geoms.empty() ? pb->geoms[0]->id : -1);
copyvec(m->body_pos+3*i, pb->pos, 3);
copyvec(m->body_quat+4*i, pb->quat, 4);
copyvec(m->body_ipos+3*i, pb->ipos, 3);
copyvec(m->body_iquat+4*i, pb->iquat, 4);
mjuu_copyvec(m->body_pos+3*i, pb->pos, 3);
mjuu_copyvec(m->body_quat+4*i, pb->quat, 4);
mjuu_copyvec(m->body_ipos+3*i, pb->ipos, 3);
mjuu_copyvec(m->body_iquat+4*i, pb->iquat, 4);
m->body_mass[i] = (mjtNum)pb->mass;
copyvec(m->body_inertia+3*i, pb->inertia, 3);
mjuu_copyvec(m->body_inertia+3*i, pb->inertia, 3);
m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body);
mjuu_copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body);
m->body_contype[i] = pb->contype;
m->body_conaffinity[i] = pb->conaffinity;
@@ -1854,23 +1845,23 @@ void mjCModel::CopyTree(mjModel* m) {
m->jnt_qposadr[jid] = qposadr;
m->jnt_dofadr[jid] = dofadr;
m->jnt_bodyid[jid] = pj->body->id;
copyvec(m->jnt_pos+3*jid, pj->pos, 3);
copyvec(m->jnt_axis+3*jid, pj->axis, 3);
mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3);
mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3);
m->jnt_stiffness[jid] = (mjtNum)pj->stiffness;
copyvec(m->jnt_range+2*jid, pj->range, 2);
copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP);
mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2);
mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2);
mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF);
mjuu_copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP);
m->jnt_margin[jid] = (mjtNum)pj->margin;
copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt);
mjuu_copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt);
// not simple if: rotation already found, or pos not zero, or mis-aligned axis
if (rotfound ||
!IsNullPose(m->jnt_pos+3*jid, NULL) ||
((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) &&
((mju_abs(pj->axis[0])>mjEPS) +
(mju_abs(pj->axis[1])>mjEPS) +
(mju_abs(pj->axis[2])>mjEPS)) > 1)) {
((std::abs(pj->axis[0])>mjEPS) +
(std::abs(pj->axis[1])>mjEPS) +
(std::abs(pj->axis[2])>mjEPS)) > 1)) {
m->body_simple[i] = 0;
}
@@ -1882,9 +1873,9 @@ void mjCModel::CopyTree(mjModel* m) {
// set qpos0 and qpos_spring, check type
switch (pj->type) {
case mjJNT_FREE:
copyvec(m->qpos0+qposadr, pb->pos, 3);
copyvec(m->qpos0+qposadr+3, pb->quat, 4);
mju_copy(m->qpos_spring+qposadr, m->qpos0+qposadr, 7);
mjuu_copyvec(m->qpos0+qposadr, pb->pos, 3);
mjuu_copyvec(m->qpos0+qposadr+3, pb->quat, 4);
mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 7);
break;
case mjJNT_BALL:
@@ -1892,7 +1883,7 @@ void mjCModel::CopyTree(mjModel* m) {
m->qpos0[qposadr+1] = 0;
m->qpos0[qposadr+2] = 0;
m->qpos0[qposadr+3] = 0;
mju_copy4(m->qpos_spring+qposadr, m->qpos0+qposadr);
mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 4);
break;
case mjJNT_SLIDE:
@@ -1910,8 +1901,8 @@ void mjCModel::CopyTree(mjModel* m) {
// set attributes
m->dof_bodyid[dofadr] = pb->id;
m->dof_jntid[dofadr] = jid;
copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF);
copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP);
mjuu_copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF);
mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP);
m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss;
m->dof_armature[dofadr] = (mjtNum)pj->armature;
m->dof_damping[dofadr] = (mjtNum)pj->damping;
@@ -1962,19 +1953,19 @@ void mjCModel::CopyTree(mjModel* m) {
m->geom_matid[gid] = pg->matid;
m->geom_group[gid] = pg->group;
m->geom_priority[gid] = pg->priority;
copyvec(m->geom_size+3*gid, pg->size, 3);
copyvec(m->geom_aabb+6*gid, pg->aabb, 6);
copyvec(m->geom_pos+3*gid, pg->pos, 3);
copyvec(m->geom_quat+4*gid, pg->quat, 4);
copyvec(m->geom_friction+3*gid, pg->friction, 3);
mjuu_copyvec(m->geom_size+3*gid, pg->size, 3);
mjuu_copyvec(m->geom_aabb+6*gid, pg->aabb, 6);
mjuu_copyvec(m->geom_pos+3*gid, pg->pos, 3);
mjuu_copyvec(m->geom_quat+4*gid, pg->quat, 4);
mjuu_copyvec(m->geom_friction+3*gid, pg->friction, 3);
m->geom_solmix[gid] = (mjtNum)pg->solmix;
copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF);
copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP);
mjuu_copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF);
mjuu_copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP);
m->geom_margin[gid] = (mjtNum)pg->margin;
m->geom_gap[gid] = (mjtNum)pg->gap;
copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID);
copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom);
copyvec(m->geom_rgba+4*gid, pg->rgba, 4);
mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID);
mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom);
mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4);
// determine sameframe
if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) {
@@ -2006,11 +1997,11 @@ void mjCModel::CopyTree(mjModel* m) {
m->site_bodyid[sid] = ps->body->id;
m->site_matid[sid] = ps->matid;
m->site_group[sid] = ps->group;
copyvec(m->site_size+3*sid, ps->size, 3);
copyvec(m->site_pos+3*sid, ps->pos, 3);
copyvec(m->site_quat+4*sid, ps->quat, 4);
copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site);
copyvec(m->site_rgba+4*sid, ps->rgba, 4);
mjuu_copyvec(m->site_size+3*sid, ps->size, 3);
mjuu_copyvec(m->site_pos+3*sid, ps->pos, 3);
mjuu_copyvec(m->site_quat+4*sid, ps->quat, 4);
mjuu_copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site);
mjuu_copyvec(m->site_rgba+4*sid, ps->rgba, 4);
// determine sameframe
if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) {
@@ -2038,15 +2029,15 @@ void mjCModel::CopyTree(mjModel* m) {
m->cam_bodyid[cid] = pc->body->id;
m->cam_mode[cid] = pc->mode;
m->cam_targetbodyid[cid] = pc->targetbodyid;
copyvec(m->cam_pos+3*cid, pc->pos, 3);
copyvec(m->cam_quat+4*cid, pc->quat, 4);
mjuu_copyvec(m->cam_pos+3*cid, pc->pos, 3);
mjuu_copyvec(m->cam_quat+4*cid, pc->quat, 4);
m->cam_orthographic[cid] = pc->orthographic;
m->cam_fovy[cid] = (mjtNum)pc->fovy;
m->cam_ipd[cid] = (mjtNum)pc->ipd;
copyvec(m->cam_resolution+2*cid, pc->resolution, 2);
copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2);
copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4);
copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam);
mjuu_copyvec(m->cam_resolution+2*cid, pc->resolution, 2);
mjuu_copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2);
mjuu_copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4);
mjuu_copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam);
}
// loop over lights for this body
@@ -2062,15 +2053,15 @@ void mjCModel::CopyTree(mjModel* m) {
m->light_directional[lid] = (mjtByte)pl->directional;
m->light_castshadow[lid] = (mjtByte)pl->castshadow;
m->light_active[lid] = (mjtByte)pl->active;
copyvec(m->light_pos+3*lid, pl->pos, 3);
copyvec(m->light_dir+3*lid, pl->dir, 3);
mjuu_copyvec(m->light_pos+3*lid, pl->pos, 3);
mjuu_copyvec(m->light_dir+3*lid, pl->dir, 3);
m->light_bulbradius[lid] = pl->bulbradius;
copyvec(m->light_attenuation+3*lid, pl->attenuation, 3);
mjuu_copyvec(m->light_attenuation+3*lid, pl->attenuation, 3);
m->light_cutoff[lid] = pl->cutoff;
m->light_exponent[lid] = pl->exponent;
copyvec(m->light_ambient+3*lid, pl->ambient, 3);
copyvec(m->light_diffuse+3*lid, pl->diffuse, 3);
copyvec(m->light_specular+3*lid, pl->specular, 3);
mjuu_copyvec(m->light_ambient+3*lid, pl->ambient, 3);
mjuu_copyvec(m->light_diffuse+3*lid, pl->diffuse, 3);
mjuu_copyvec(m->light_specular+3*lid, pl->specular, 3);
}
}
@@ -2210,9 +2201,9 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
m->mesh_bvhnum[i] = pme->tree().nbvh;
m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1;
copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3);
copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
mjuu_copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3);
mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
// copy vertices, normals, faces, texcoords, aux data
pme->CopyVert(m->mesh_vert + 3*vert_adr);
@@ -2268,13 +2259,13 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_group[i] = pfl->group;
m->flex_priority[i] = pfl->priority;
m->flex_solmix[i] = (mjtNum)pfl->solmix;
copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF);
copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP);
mjuu_copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF);
mjuu_copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP);
m->flex_radius[i] = (mjtNum)pfl->radius;
copyvec(m->flex_friction + 3 * i, pfl->friction, 3);
mjuu_copyvec(m->flex_friction + 3 * i, pfl->friction, 3);
m->flex_margin[i] = (mjtNum)pfl->margin;
m->flex_gap[i] = (mjtNum)pfl->gap;
copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4);
mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4);
// set fields: mesh-like
m->flex_dim[i] = pfl->dim;
@@ -2338,7 +2329,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// copy or set vert
if (pfl->centered) {
mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
memcpy(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert*sizeof(mjtNum));
@@ -2393,7 +2384,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->skin_matid[i] = psk->matid;
m->skin_group[i] = psk->group;
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
mjuu_copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_inflate[i] = psk->inflate;
m->skin_vertadr[i] = vert_adr;
m->skin_vertnum[i] = psk->get_vert().size()/3;
@@ -2448,7 +2439,7 @@ void mjCModel::CopyObjects(mjModel* m) {
mjCHField* phf = hfields_[i];
// set fields
copyvec(m->hfield_size+4*i, phf->size, 4);
mjuu_copyvec(m->hfield_size+4*i, phf->size, 4);
m->hfield_nrow[i] = phf->nrow;
m->hfield_ncol[i] = phf->ncol;
m->hfield_adr[i] = data_adr;
@@ -2487,14 +2478,14 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->mat_texid[i] = pmat->texid;
m->mat_texuniform[i] = pmat->texuniform;
copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2);
mjuu_copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2);
m->mat_emission[i] = pmat->emission;
m->mat_specular[i] = pmat->specular;
m->mat_shininess[i] = pmat->shininess;
m->mat_reflectance[i] = pmat->reflectance;
m->mat_metallic[i] = pmat->metallic;
m->mat_roughness[i] = pmat->roughness;
copyvec(m->mat_rgba+4*i, pmat->rgba, 4);
mjuu_copyvec(m->mat_rgba+4*i, pmat->rgba, 4);
}
// geom pairs to include
@@ -2503,12 +2494,12 @@ void mjCModel::CopyObjects(mjModel* m) {
m->pair_geom1[i] = pairs_[i]->geom1->id;
m->pair_geom2[i] = pairs_[i]->geom2->id;
m->pair_signature[i] = pairs_[i]->signature;
copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF);
copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF);
copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP);
mjuu_copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF);
mjuu_copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF);
mjuu_copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP);
m->pair_margin[i] = (mjtNum)pairs_[i]->margin;
m->pair_gap[i] = (mjtNum)pairs_[i]->gap;
copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5);
mjuu_copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5);
}
// body pairs to exclude
@@ -2526,9 +2517,9 @@ void mjCModel::CopyObjects(mjModel* m) {
m->eq_obj1id[i] = peq->obj1id;
m->eq_obj2id[i] = peq->obj2id;
m->eq_active0[i] = peq->active;
copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF);
copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP);
copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA);
mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF);
mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP);
mjuu_copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA);
}
// tendons and wraps
@@ -2544,10 +2535,10 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_group[i] = pte->group;
m->tendon_limited[i] = (mjtByte)pte->is_limited();
m->tendon_width[i] = (mjtNum)pte->width;
copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF);
copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF);
mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP);
mjuu_copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF);
mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP);
m->tendon_range[2*i] = (mjtNum)pte->range[0];
m->tendon_range[2*i+1] = (mjtNum)pte->range[1];
m->tendon_margin[i] = (mjtNum)pte->margin;
@@ -2556,8 +2547,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss;
m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0];
m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1];
copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon);
copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
mjuu_copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon);
mjuu_copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
// set wraps
for (int j=0; j<(int)pte->path.size(); j++) {
@@ -2597,15 +2588,15 @@ void mjCModel::CopyObjects(mjModel* m) {
m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited();
m->actuator_actearly[i] = pac->actearly;
m->actuator_cranklength[i] = (mjtNum)pac->cranklength;
copyvec(m->actuator_gear + 6*i, pac->gear, 6);
copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN);
copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN);
copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS);
copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2);
copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2);
copyvec(m->actuator_actrange + 2*i, pac->actrange, 2);
copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2);
copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator);
mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6);
mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN);
mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN);
mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS);
mjuu_copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2);
mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2);
mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2);
mjuu_copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2);
mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator);
}
// sensors
@@ -2625,7 +2616,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->sensor_dim[i] = psen->dim;
m->sensor_cutoff[i] = (mjtNum)psen->cutoff;
m->sensor_noise[i] = (mjtNum)psen->noise;
copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor);
mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor);
// calculate address and advance
m->sensor_adr[i] = adr;
@@ -2690,34 +2681,34 @@ void mjCModel::CopyObjects(mjModel* m) {
for (int i=0; i<nkey; i++) {
// copy data
m->key_time[i] = (mjtNum)keys_[i]->time;
copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq);
copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv);
mjuu_copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq);
mjuu_copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv);
if (na) {
copyvec(m->key_act+i*na, keys_[i]->act_.data(), na);
mjuu_copyvec(m->key_act+i*na, keys_[i]->act_.data(), na);
}
if (nmocap) {
copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap);
copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap);
mjuu_copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap);
mjuu_copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap);
}
// normalize quaternions in m->key_qpos
for (int j=0; j<m->njnt; j++) {
if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) {
mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE));
mjuu_normvec(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE), 4);
}
}
// normalize quaternions in m->key_mquat
for (int j=0; j<nmocap; j++) {
mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
mjuu_normvec(m->key_mquat+i*4*nmocap+4*j, 4);
}
copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu);
mjuu_copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu);
}
// save qpos0 in user model (to recognize changed key_qpos in write)
qpos0.resize(nq);
mju_copy(qpos0.data(), m->qpos0, nq);
mjuu_copyvec(qpos0.data(), m->qpos0, nq);
}
@@ -2727,17 +2718,17 @@ void mjCModel::SaveState(const mjData* d) {
for (auto joint : joints_) {
switch (joint->type) {
case mjJNT_FREE:
mju_copy(joint->qpos, d->qpos + joint->qposadr_, 7);
mju_copy(joint->qvel, d->qvel + joint->dofadr_, 6);
mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 7);
mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 6);
break;
case mjJNT_BALL:
mju_copy(joint->qpos, d->qpos + joint->qposadr_, 4);
mju_copy(joint->qvel, d->qvel + joint->dofadr_, 3);
mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 4);
mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 3);
break;
case mjJNT_HINGE:
case mjJNT_SLIDE:
mju_copy(joint->qpos, d->qpos + joint->qposadr_, 1);
mju_copy(joint->qvel, d->qvel + joint->dofadr_, 1);
mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 1);
mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 1);
break;
}
}
@@ -2745,7 +2736,7 @@ void mjCModel::SaveState(const mjData* d) {
for (auto actuator : actuators_) {
if (actuator->actadr_ != -1) {
actuator->act.assign(actuator->actnum_, 0);
mju_copy(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_);
mjuu_copyvec(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_);
}
}
}
@@ -2767,24 +2758,24 @@ void mjCModel::RestoreState(const mjModel* m, mjData** dest) {
}
switch (joint->type) {
case mjJNT_FREE:
mju_copy(d->qpos + joint->qposadr_, joint->qpos, 7);
mju_copy(d->qvel + joint->dofadr_, joint->qvel, 6);
mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 7);
mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 6);
break;
case mjJNT_BALL:
mju_copy(d->qpos + joint->qposadr_, joint->qpos, 4);
mju_copy(d->qvel + joint->dofadr_, joint->qvel, 3);
mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 4);
mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 3);
break;
case mjJNT_HINGE:
case mjJNT_SLIDE:
mju_copy(d->qpos + joint->qposadr_, joint->qpos, 1);
mju_copy(d->qvel + joint->dofadr_, joint->qvel, 1);
mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 1);
mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 1);
break;
}
}
for (auto actuator : actuators_) {
if (mjuu_defined(actuator->act[0])) {
mju_copy(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_);
mjuu_copyvec(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_);
}
}
}
@@ -3566,14 +3557,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
meaninertia_auto = m->stat.meaninertia;
meanmass_auto = m->stat.meanmass;
meansize_auto = m->stat.meansize;
copyvec(center_auto, m->stat.center, 3);
mjuu_copyvec(center_auto, m->stat.center, 3);
// override model statistics if defined by user
if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent;
if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia;
if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass;
if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize;
if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3);
if (mjuu_defined(stat.center[0])) mjuu_copyvec(m->stat.center, stat.center, 3);
// assert that model has valid references
const char* validationerr = mj_validateReferences(m);
@@ -3652,15 +3643,15 @@ bool mjCModel::CopyBack(const mjModel* m) {
if (m->stat.center[0] != center_auto[0] ||
m->stat.center[1] != center_auto[1] ||
m->stat.center[2] != center_auto[2]) {
mju_copy3(stat.center, m->stat.center);
mjuu_copyvec(stat.center, m->stat.center, 3);
}
// qpos0, qpos_spring
for (int i=0; i<njnt; i++) {
switch (joints_[i]->type) {
case mjJNT_FREE:
copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3);
copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4);
mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3);
mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4);
break;
case mjJNT_SLIDE:
@@ -3674,22 +3665,22 @@ bool mjCModel::CopyBack(const mjModel* m) {
break;
}
}
mju_copy(qpos0.data(), m->qpos0, m->nq);
mjuu_copyvec(qpos0.data(), m->qpos0, m->nq);
// body
mjCBody* pb;
for (int i=0; i<nbody; i++) {
pb = bodies_[i];
copyvec(pb->pos, m->body_pos+3*i, 3);
copyvec(pb->quat, m->body_quat+4*i, 4);
copyvec(pb->ipos, m->body_ipos+3*i, 3);
copyvec(pb->iquat, m->body_iquat+4*i, 4);
mjuu_copyvec(pb->pos, m->body_pos+3*i, 3);
mjuu_copyvec(pb->quat, m->body_quat+4*i, 4);
mjuu_copyvec(pb->ipos, m->body_ipos+3*i, 3);
mjuu_copyvec(pb->iquat, m->body_iquat+4*i, 4);
pb->mass = (double)m->body_mass[i];
copyvec(pb->inertia, m->body_inertia+3*i, 3);
mjuu_copyvec(pb->inertia, m->body_inertia+3*i, 3);
if (nuser_body) {
copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body);
mjuu_copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body);
}
}
@@ -3699,22 +3690,22 @@ bool mjCModel::CopyBack(const mjModel* m) {
pj = joints_[i];
// joint data
copyvec(pj->pos, m->jnt_pos+3*i, 3);
copyvec(pj->axis, m->jnt_axis+3*i, 3);
mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3);
mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3);
pj->stiffness = (double)m->jnt_stiffness[i];
copyvec(pj->range, m->jnt_range+2*i, 2);
copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF);
copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP);
mjuu_copyvec(pj->range, m->jnt_range+2*i, 2);
mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP);
pj->margin = (double)m->jnt_margin[i];
if (nuser_jnt) {
copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt);
mjuu_copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt);
}
// dof data
int j = m->jnt_dofadr[i];
copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
pj->armature = (double)m->dof_armature[j];
pj->damping = (double)m->dof_damping[j];
pj->frictionloss = (double)m->dof_frictionloss[j];
@@ -3725,19 +3716,19 @@ bool mjCModel::CopyBack(const mjModel* m) {
for (int i=0; i<ngeom; i++) {
pg = geoms_[i];
copyvec(pg->size, m->geom_size+3*i, 3);
copyvec(pg->pos, m->geom_pos+3*i, 3);
copyvec(pg->quat, m->geom_quat+4*i, 4);
copyvec(pg->friction, m->geom_friction+3*i, 3);
copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF);
copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP);
copyvec(pg->rgba, m->geom_rgba+4*i, 4);
mjuu_copyvec(pg->size, m->geom_size+3*i, 3);
mjuu_copyvec(pg->pos, m->geom_pos+3*i, 3);
mjuu_copyvec(pg->quat, m->geom_quat+4*i, 4);
mjuu_copyvec(pg->friction, m->geom_friction+3*i, 3);
mjuu_copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP);
mjuu_copyvec(pg->rgba, m->geom_rgba+4*i, 4);
pg->solmix = (double)m->geom_solmix[i];
pg->margin = (double)m->geom_margin[i];
pg->gap = (double)m->geom_gap[i];
if (nuser_geom) {
copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom);
mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom);
}
}
@@ -3745,8 +3736,8 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCMesh* pm;
for (int i=0; i<nmesh; i++) {
pm = meshes_[i];
copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
mjuu_copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3);
mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4);
}
// heightfield
@@ -3762,84 +3753,84 @@ bool mjCModel::CopyBack(const mjModel* m) {
// copy back in reverse row order
for (int j=0; j<nrow; j++) {
int flip = nrow-1-j;
copyvec(userdata + flip*ncol, modeldata+j*ncol, ncol);
mjuu_copyvec(userdata + flip*ncol, modeldata+j*ncol, ncol);
}
}
}
// sites
for (int i=0; i<nsite; i++) {
copyvec(sites_[i]->size, m->site_size + 3 * i, 3);
copyvec(sites_[i]->pos, m->site_pos+3*i, 3);
copyvec(sites_[i]->quat, m->site_quat+4*i, 4);
copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4);
mjuu_copyvec(sites_[i]->size, m->site_size + 3 * i, 3);
mjuu_copyvec(sites_[i]->pos, m->site_pos+3*i, 3);
mjuu_copyvec(sites_[i]->quat, m->site_quat+4*i, 4);
mjuu_copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4);
if (nuser_site) {
copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site);
mjuu_copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site);
}
}
// cameras
for (int i=0; i<ncam; i++) {
copyvec(cameras_[i]->pos, m->cam_pos+3*i, 3);
copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4);
mjuu_copyvec(cameras_[i]->pos, m->cam_pos+3*i, 3);
mjuu_copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4);
cameras_[i]->fovy = (double)m->cam_fovy[i];
cameras_[i]->ipd = (double)m->cam_ipd[i];
copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2);
copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4);
mjuu_copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2);
mjuu_copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4);
if (nuser_cam) {
copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam);
mjuu_copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam);
}
}
// lights
for (int i=0; i<nlight; i++) {
copyvec(lights_[i]->pos, m->light_pos+3*i, 3);
copyvec(lights_[i]->dir, m->light_dir+3*i, 3);
copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3);
mjuu_copyvec(lights_[i]->pos, m->light_pos+3*i, 3);
mjuu_copyvec(lights_[i]->dir, m->light_dir+3*i, 3);
mjuu_copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3);
lights_[i]->cutoff = m->light_cutoff[i];
lights_[i]->exponent = m->light_exponent[i];
copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3);
copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3);
copyvec(lights_[i]->specular, m->light_specular+3*i, 3);
mjuu_copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3);
mjuu_copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3);
mjuu_copyvec(lights_[i]->specular, m->light_specular+3*i, 3);
}
// materials
for (int i=0; i<nmat; i++) {
copyvec(materials_[i]->texrepeat, m->mat_texrepeat+2*i, 2);
mjuu_copyvec(materials_[i]->texrepeat, m->mat_texrepeat+2*i, 2);
materials_[i]->emission = m->mat_emission[i];
materials_[i]->specular = m->mat_specular[i];
materials_[i]->shininess = m->mat_shininess[i];
materials_[i]->reflectance = m->mat_reflectance[i];
copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4);
mjuu_copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4);
}
// pairs
for (int i=0; i<npair; i++) {
copyvec(pairs_[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF);
copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
mjuu_copyvec(pairs_[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
mjuu_copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF);
mjuu_copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
pairs_[i]->margin = (double)m->pair_margin[i];
pairs_[i]->gap = (double)m->pair_gap[i];
copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5);
mjuu_copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5);
}
// equality constraints
for (int i=0; i<neq; i++) {
copyvec(equalities_[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA);
copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF);
copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP);
mjuu_copyvec(equalities_[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA);
mjuu_copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF);
mjuu_copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP);
}
// tendons
for (int i=0; i<ntendon; i++) {
copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);
copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP);
copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4);
mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2);
mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF);
mjuu_copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP);
mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4);
tendons_[i]->width = (double)m->tendon_width[i];
tendons_[i]->margin = (double)m->tendon_margin[i];
tendons_[i]->stiffness = (double)m->tendon_stiffness[i];
@@ -3847,7 +3838,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i];
if (nuser_tendon) {
copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon);
mjuu_copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon);
}
}
@@ -3856,18 +3847,18 @@ bool mjCModel::CopyBack(const mjModel* m) {
for (int i=0; i<nu; i++) {
pa = actuators_[i];
copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN);
copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN);
copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS);
copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2);
copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2);
copyvec(pa->actrange, m->actuator_actrange+2*i, 2);
copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2);
copyvec(pa->gear, m->actuator_gear+6*i, 6);
mjuu_copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN);
mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN);
mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS);
mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2);
mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2);
mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2);
mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2);
mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6);
pa->cranklength = (double)m->actuator_cranklength[i];
if (nuser_actuator) {
copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator);
mjuu_copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator);
}
}
@@ -3877,7 +3868,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
sensors_[i]->noise = (double)m->sensor_noise[i];
if (nuser_sensor) {
copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor);
mjuu_copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor);
}
}
@@ -3900,17 +3891,17 @@ bool mjCModel::CopyBack(const mjModel* m) {
mjCKey* pk = keys_[i];
pk->time = (double)m->key_time[i];
copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq);
copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv);
mjuu_copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq);
mjuu_copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv);
if (na) {
copyvec(pk->act_.data(), m->key_act + i*na, na);
mjuu_copyvec(pk->act_.data(), m->key_act + i*na, na);
}
if (nmocap) {
copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
mjuu_copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap);
mjuu_copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap);
}
if (nu) {
copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu);
mjuu_copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu);
}
}
src/user/user_objects.cc
+74 -78
View File
@@ -37,14 +37,7 @@
#include <mujoco/mjtnum.h>
#include "cc/array_safety.h"
#include "engine/engine_resource.h"
#include "engine/engine_io.h"
#include "engine/engine_passive.h"
#include "engine/engine_plugin.h"
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_solve.h"
#include "engine/engine_util_spatial.h"
#include <mujoco/mjspec.h>
#include "user/user_api.h"
#include "user/user_cache.h"
@@ -104,14 +97,14 @@ const char* FullInertia(double quat[4], double inertia[3], const double fulliner
return nullptr;
}
mjtNum eigval[3], eigvec[9], quattmp[4];
mjtNum full[9] = {
double eigval[3], eigvec[9], quattmp[4];
double full[9] = {
fullinertia[0], fullinertia[3], fullinertia[4],
fullinertia[3], fullinertia[1], fullinertia[5],
fullinertia[4], fullinertia[5], fullinertia[2]
};
mju_eig3(eigval, eigvec, quattmp, full);
mjuu_eig3(eigval, eigvec, quattmp, full);
// check mimimal eigenvalue
if (eigval[2]<mjEPS) {
@@ -270,10 +263,10 @@ mjCError::mjCError(const mjCBase* obj, const char* msg, const char* str, int pos
// used for geom, site, body and camera frames
const char* ResolveOrientation(double* quat, bool degree, const char* sequence,
const mjsOrientation& orient) {
mjtNum axisangle[4];
mjtNum xyaxes[6];
mjtNum zaxis[3];
mjtNum euler[3];
double axisangle[4];
double xyaxes[6];
double zaxis[3];
double euler[3];
mjuu_copyvec(axisangle, orient.axisangle, 4);
mjuu_copyvec(xyaxes, orient.xyaxes, 6);
@@ -391,7 +384,7 @@ mjCBoundingVolumeHierarchy::mjCBoundingVolumeHierarchy() {
// assign position and orientation
void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_element[4]) {
void mjCBoundingVolumeHierarchy::Set(double ipos_element[3], double iquat_element[4]) {
mjuu_copyvec(ipos_, ipos_element, 3);
mjuu_copyvec(iquat_, iquat_element, 4);
}
@@ -425,15 +418,15 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
// visual-only elements.
std::vector<BVElement> elements;
elements.reserve(bvleaf_.size());
mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
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];
mjtNum vert[3] = {element.e->pos[0] - ipos_[0],
double vert[3] = {element.e->pos[0] - ipos_[0],
element.e->pos[1] - ipos_[1],
element.e->pos[2] - ipos_[2]};
mju_rotVecQuat(element.lpos, vert, qinv);
mjuu_rotVecQuat(element.lpos, vert, qinv);
elements.push_back(std::move(element));
}
}
@@ -448,15 +441,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
if (nelements == 0) {
return -1;
}
mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
// inverse transformation
mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
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
mjtNum aamm[6] = {element->e->aabb[0] - element->e->aabb[3],
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],
@@ -465,32 +458,32 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
// update node AAMM
for (int v=0; v<8; v++) {
mjtNum vert[3], box[3];
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) {
mju_rotVecQuat(box, vert, 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];
mju_rotVecQuat(vert, box, qinv);
mjuu_rotVecQuat(vert, box, qinv);
}
AAMM[0] = mjMIN(AAMM[0], vert[0]);
AAMM[1] = mjMIN(AAMM[1], vert[1]);
AAMM[2] = mjMIN(AAMM[2], vert[2]);
AAMM[3] = mjMAX(AAMM[3], vert[0]);
AAMM[4] = mjMAX(AAMM[4], vert[1]);
AAMM[5] = mjMAX(AAMM[5], vert[2]);
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 (mju_abs(AAMM[i]-AAMM[i+3])<mjEPS) {
if (std::abs(AAMM[i]-AAMM[i+3])<mjEPS) {
AAMM[i+0] -= mjEPS;
AAMM[i+3] += mjEPS;
}
@@ -522,7 +515,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(
// find longest axis, by a margin of at least mjEPS, default to 0
int axis = 0;
mjtNum edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] };
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;
@@ -1340,7 +1333,7 @@ void mjCBody::GeomFrame(void) {
}
// check for small mass
if (mass<mjMINVAL) {
if (mass<mjEPS) {
throw mjCError(this, "body mass is too small, cannot compute center of mass");
}
@@ -1476,10 +1469,10 @@ void mjCBody::Compile(void) {
// check and correct mass and inertia
if (id>0) {
// fix minimum
mass = mju_max(mass, model->boundmass);
inertia[0] = mju_max(inertia[0], model->boundinertia);
inertia[1] = mju_max(inertia[1], model->boundinertia);
inertia[2] = mju_max(inertia[2], model->boundinertia);
mass = std::max(mass, model->boundmass);
inertia[0] = std::max(inertia[0], model->boundinertia);
inertia[1] = std::max(inertia[1], model->boundinertia);
inertia[2] = std::max(inertia[2], model->boundinertia);
// check for negative values
if (mass<0 || inertia[0]<0 || inertia[1]<0 ||inertia[2]<0) {
@@ -1509,7 +1502,7 @@ void mjCBody::Compile(void) {
for (int i=0; i<geoms.size(); i++) {
contype |= geoms[i]->contype;
conaffinity |= geoms[i]->conaffinity;
margin = mju_max(margin, geoms[i]->margin);
margin = std::max(margin, geoms[i]->margin);
}
// compute bounding volume hierarchy
@@ -1546,9 +1539,9 @@ void mjCBody::Compile(void) {
// compute body global pose (no joint transformations in qpos0)
if (id>0) {
mjCBody* par = model->Bodies()[parentid];
mju_rotVecQuat(xpos0, pos, par->xquat0);
mju_addTo3(xpos0, par->xpos0);
mju_mulQuat(xquat0, par->xquat0, quat);
mjuu_rotVecQuat(xpos0, pos, par->xquat0);
mjuu_addtovec(xpos0, par->xpos0, 3);
mjuu_mulquat(xquat0, par->xquat0, quat);
}
// compile all sites
@@ -1680,8 +1673,8 @@ void mjCFrame::PointToLocal() {
void mjCFrame::CopyFromSpec() {
*static_cast<mjsFrame*>(this) = spec;
mju_copy3(pos, spec.pos);
mju_copy4(quat, spec.quat);
mjuu_copyvec(pos, spec.pos, 3);
mjuu_copyvec(quat, spec.quat, 4);
}
@@ -2147,7 +2140,7 @@ double mjCGeom::GetRBound(void) {
case mjGEOM_HFIELD:
hsize = hfield->size;
return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] +
mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3]));
std::max(hsize[2]*hsize[2], hsize[3]*hsize[3]));
case mjGEOM_SPHERE:
return size[0];
@@ -2159,7 +2152,7 @@ double mjCGeom::GetRBound(void) {
return sqrt(size[0]*size[0]+size[1]*size[1]);
case mjGEOM_ELLIPSOID:
return mju_max(mju_max(size[0], size[1]), size[2]);
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]);
@@ -2167,9 +2160,9 @@ double mjCGeom::GetRBound(void) {
case mjGEOM_MESH:
case mjGEOM_SDF:
aamm = mesh->aamm();
haabb[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
haabb[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
haabb[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
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:
@@ -2270,18 +2263,21 @@ void mjCGeom::SetFluidCoefs(void) {
// 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::fabs(kz - ky) / std::max(
mjMINVAL, std::fabs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz)));
const double Iyfac = pow2(dz*dz - dx*dx) * std::fabs(kx - kz) / std::max(
mjMINVAL, std::fabs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx)));
const double Izfac = pow2(dx*dx - dy*dy) * std::fabs(ky - kx) / std::max(
mjMINVAL, std::fabs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky)));
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)));
const mjtNum virtual_mass[3] = {
volume * kx / std::max(mjMINVAL, 2-kx),
volume * ky / std::max(mjMINVAL, 2-ky),
volume * kz / std::max(mjMINVAL, 2-kz)};
const mjtNum virtual_inertia[3] = {volume*Ixfac/5, volume*Iyfac/5, volume*Izfac/5};
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],
@@ -2477,9 +2473,9 @@ void mjCGeom::Compile(void) {
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] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
size[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
size[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
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) {
@@ -2497,7 +2493,7 @@ void mjCGeom::Compile(void) {
if (mass==0) {
mass_ = 0;
density = 0;
} else if (GetVolume()>mjMINVAL) {
} else if (GetVolume()>mjEPS) {
mass_ = mass;
density = mass / GetVolume();
SetInertia();
@@ -2847,7 +2843,7 @@ void mjCCamera::Compile(void) {
intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1];
// fovy with principal point at (0, 0)
fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI;
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;
@@ -2942,7 +2938,7 @@ void mjCLight::Compile(void) {
}
// normalize direction, make sure it is not zero
if (mjuu_normvec(dir, 3)<mjMINVAL) {
if (mjuu_normvec(dir, 3)<mjEPS) {
throw mjCError(this, "zero direction in light");
}
@@ -3168,15 +3164,15 @@ void mjCHField::Compile(const mjVFS* vfs) {
// set elevation data to [0-1] range
float emin = 1E+10, emax = -1E+10;
for (int i = 0; i<nrow*ncol; i++) {
emin = mjMIN(emin, data[i]);
emax = mjMAX(emax, data[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>mjMINVAL) {
if (emax-emin>mjEPS) {
data[i] /= (emax - emin);
}
}
@@ -4122,12 +4118,12 @@ void mjCPair::Compile(void) {
// set undefined margin: max
if (!mjuu_defined(margin)) {
margin = mjMAX(geom1->margin, geom2->margin);
margin = std::max(geom1->margin, geom2->margin);
}
// set undefined gap: max
if (!mjuu_defined(gap)) {
gap = mjMAX(geom1->gap, geom2->gap);
gap = std::max(geom1->gap, geom2->gap);
}
// set undefined condim, friction, solref, solimp: different priority
@@ -4165,23 +4161,23 @@ void mjCPair::Compile(void) {
else {
// condim: max
if (condim<0) {
condim = mjMAX(geom1->condim, geom2->condim);
condim = std::max(geom1->condim, geom2->condim);
}
// friction: max
if (!mjuu_defined(friction[0])) {
friction[0] = friction[1] = mju_max(geom1->friction[0], geom2->friction[0]);
friction[2] = mju_max(geom1->friction[1], geom2->friction[1]);
friction[3] = friction[4] = mju_max(geom1->friction[2], geom2->friction[2]);
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>=mjMINVAL && geom2->solmix>=mjMINVAL) {
if (geom1->solmix>=mjEPS && geom2->solmix>=mjEPS) {
mix = geom1->solmix / (geom1->solmix + geom2->solmix);
} else if (geom1->solmix<mjMINVAL && geom2->solmix<mjMINVAL) {
} else if (geom1->solmix<mjEPS && geom2->solmix<mjEPS) {
mix = 0.5;
} else if (geom1->solmix<mjMINVAL) {
} else if (geom1->solmix<mjEPS) {
mix = 0.0;
} else {
mix = 1.0;
@@ -4199,7 +4195,7 @@ void mjCPair::Compile(void) {
// direct: min
else {
for (int i=0; i<mjNREF; i++) {
solref[i] = mju_min(geom1->solref[i], geom2->solref[i]);
solref[i] = std::min(geom1->solref[i], geom2->solref[i]);
}
}
}
src/user/user_objects.h
+11 -11
View File
@@ -98,9 +98,9 @@ class mjCBoundingVolume {
int contype; // contact type
int conaffinity; // contact affinity
const mjtNum* aabb; // axis-aligned bounding box (center, size)
const mjtNum* pos; // position (set by user or Compile1)
const mjtNum* quat; // orientation (set by user or Compile1)
const double* aabb; // axis-aligned bounding box (center, size)
const double* pos; // position (set by user or Compile1)
const double* quat; // orientation (set by user or Compile1)
const int* GetId() const { if (id_) return id_; else return &idval_; }
void SetId(const int* id) { id_ = id; }
@@ -134,7 +134,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
// make bounding volume hierarchy
void CreateBVH(void);
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
void Set(double ipos_element[3], double iquat_element[4]);
void AllocateBoundingVolumes(int nleaf);
void RemoveInactiveVolumes(int nmax);
mjCBoundingVolume* GetBoundingVolume(int id);
@@ -144,7 +144,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
struct BVElement {
const mjCBoundingVolume* e;
// position of the element in the BVH axes
mjtNum lpos[3];
double lpos[3];
};
struct BVElementCompare {
@@ -231,8 +231,8 @@ class mjCBody_ : public mjCBase {
int contype; // OR over geom contypes
int conaffinity; // OR over geom conaffinities
double margin; // MAX over geom margins
mjtNum xpos0[3]; // global position in qpos0
mjtNum xquat0[4]; // global orientation in qpos0
double xpos0[3]; // global position in qpos0
double xquat0[4]; // global orientation in qpos0
// used internally by compiler
int lastdof; // id of last dof
@@ -664,20 +664,20 @@ class mjCFlex_ : public mjCBase {
std::vector<int> shell; // shell fragment vertex ids (dim per fragment)
std::vector<int> elemlayer; // element layer (distance from border)
std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions
std::vector<double> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
std::vector<mjtNum> elemaabb_; // element bounding volume
std::vector<double> elemaabb_; // element bounding volume
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
std::vector<mjtNum> vert_; // vertex positions
std::vector<double> vert_; // vertex positions
std::vector<int> elem_; // element vertex ids
std::vector<float> texcoord_; // vertex texture coordinates
std::string material_; // name of material used for rendering
std::string spec_material_;
std::vector<std::string> spec_vertbody_;
std::vector<mjtNum> spec_vert_;
std::vector<double> spec_vert_;
std::vector<int> spec_elem_;
std::vector<float> spec_texcoord_;
};
src/user/user_util.cc
+277 -68
View File
@@ -25,8 +25,6 @@
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "engine/engine_crossplatform.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
using std::isnan;
using std::string;
@@ -90,22 +88,13 @@ void mjuu_setvec(double* dest, const double x, const double y) {
}
// copy double array
void mjuu_copyvec(double* dest, const double* src, const int n) {
// add to double array
void mjuu_addtovec(double* dest, const double* src, const int n) {
for (int i=0; i<n; i++) {
dest[i] = src[i];
dest[i] += src[i];
}
}
// copy float array
void mjuu_copyvec(float* dest, const float* src, const int n) {
for (int i=0; i<n; i++) {
dest[i] = src[i];
}
}
// zero double array
void mjuu_zerovec(double* dest, const int n) {
for (int i=0; i<n; i++) {
@@ -113,6 +102,12 @@ void mjuu_zerovec(double* dest, const int n) {
}
}
// zero float array
void mjuu_zerovec(float* dest, const int n) {
for (int i=0; i<n; i++) {
dest[i] = 0;
}
}
// dot-product in 3D
double mjuu_dot3(const double* a, const double* b) {
@@ -130,7 +125,7 @@ double mjuu_dist3(const double* a, const double* b) {
double mjuu_L1(const double* a, const double* b, int n) {
double res = 0;
for (int i=0; i<n; i++) {
res += fabs(a[i]-b[i]);
res += std::abs(a[i]-b[i]);
}
return res;
@@ -149,8 +144,36 @@ double mjuu_normvec(double* vec, const int n) {
}
nrm = sqrt(nrm);
// don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i<n; i++) {
vec[i] /= nrm;
}
}
return nrm;
}
// normalize float vector to unit length, return previous length
float mjuu_normvec(float* vec, const int n) {
float nrm = 0;
for (int i=0; i<n; i++) {
vec[i] /= nrm;
nrm += vec[i]*vec[i];
}
if (nrm < mjEPS) {
return 0;
}
nrm = sqrt(nrm);
// don't normalize if nrm is within mjEPS of 1
if (std::abs(nrm - 1) > mjEPS) {
for (int i=0; i<n; i++) {
vec[i] /= nrm;
}
}
return nrm;
@@ -159,33 +182,59 @@ double mjuu_normvec(double* vec, const int n) {
// convert quaternion to rotation matrix
void mjuu_quat2mat(double* res, const double* quat) {
// identity quat: identity mat
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
res[0] = 1;
res[1] = 0;
res[2] = 0;
res[3] = 0;
res[4] = 1;
res[5] = 0;
res[6] = 0;
res[7] = 0;
res[8] = 1;
return;
}
// regular processing
double q00 = quat[0]*quat[0];
double q01 = quat[0]*quat[1];
double q02 = quat[0]*quat[2];
double q03 = quat[0]*quat[3];
double q11 = quat[1]*quat[1];
double q12 = quat[1]*quat[2];
double q13 = quat[1]*quat[3];
double q22 = quat[2]*quat[2];
double q23 = quat[2]*quat[3];
double q33 = quat[3]*quat[3];
res[0] = q00 + q11 - q22 - q33;
res[4] = q00 - q11 + q22 - q33;
res[8] = q00 - q11 - q22 + q33;
res[1] = 2*(quat[1]*quat[2] - quat[0]*quat[3]);
res[2] = 2*(quat[1]*quat[3] + quat[0]*quat[2]);
res[3] = 2*(quat[1]*quat[2] + quat[0]*quat[3]);
res[5] = 2*(quat[2]*quat[3] - quat[0]*quat[1]);
res[6] = 2*(quat[1]*quat[3] - quat[0]*quat[2]);
res[7] = 2*(quat[2]*quat[3] + quat[0]*quat[1]);
res[1] = 2*(q12 - q03);
res[2] = 2*(q13 + q02);
res[3] = 2*(q12 + q03);
res[5] = 2*(q23 - q01);
res[6] = 2*(q13 - q02);
res[7] = 2*(q23 + q01);
}
// multiply two unit quaternions
void mjuu_mulquat(double* res, const double* qa, const double* qb) {
res[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3];
res[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2];
res[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1];
res[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0];
mjuu_normvec(res, 4);
double tmp[4];
tmp[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3];
tmp[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2];
tmp[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1];
tmp[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0];
mjuu_normvec(tmp, 4);
mjuu_copyvec(res, tmp, 4);
}
// multiply vector by 3-by-3 matrix
// multiply matrix by vector, 3-by-3
void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
double tmp[3] = {
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
@@ -198,6 +247,18 @@ void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
}
// multiply transposed matrix by vector, 3-by-3
void mjuu_mulvecmatT(double* res, const double* vec, const double* mat) {
double tmp[3] = {
mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2],
mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2]
};
res[0] = tmp[0];
res[1] = tmp[1];
res[2] = tmp[2];
}
// compute res = R * M * R'
void mjuu_mulRMRT(double* res, const double* R, const double* M) {
double tmp[9];
@@ -228,33 +289,28 @@ void mjuu_mulRMRT(double* res, const double* R, const double* M) {
// multiply two matrices, all 3-by-3
void mjuu_mulmat(double* res, const double* A, const double* B) {
res[0] = A[0]*B[0] + A[1]*B[3] + A[2]*B[6];
res[1] = A[0]*B[1] + A[1]*B[4] + A[2]*B[7];
res[2] = A[0]*B[2] + A[1]*B[5] + A[2]*B[8];
double tmp[9];
tmp[0] = A[0]*B[0] + A[1]*B[3] + A[2]*B[6];
tmp[1] = A[0]*B[1] + A[1]*B[4] + A[2]*B[7];
tmp[2] = A[0]*B[2] + A[1]*B[5] + A[2]*B[8];
res[3] = A[3]*B[0] + A[4]*B[3] + A[5]*B[6];
res[4] = A[3]*B[1] + A[4]*B[4] + A[5]*B[7];
res[5] = A[3]*B[2] + A[4]*B[5] + A[5]*B[8];
tmp[3] = A[3]*B[0] + A[4]*B[3] + A[5]*B[6];
tmp[4] = A[3]*B[1] + A[4]*B[4] + A[5]*B[7];
tmp[5] = A[3]*B[2] + A[4]*B[5] + A[5]*B[8];
res[6] = A[6]*B[0] + A[7]*B[3] + A[8]*B[6];
res[7] = A[6]*B[1] + A[7]*B[4] + A[8]*B[7];
res[8] = A[6]*B[2] + A[7]*B[5] + A[8]*B[8];
tmp[6] = A[6]*B[0] + A[7]*B[3] + A[8]*B[6];
tmp[7] = A[6]*B[1] + A[7]*B[4] + A[8]*B[7];
tmp[8] = A[6]*B[2] + A[7]*B[5] + A[8]*B[8];
mjuu_copyvec(res, tmp, 9);
}
// transpose 3-by-3 matrix
void mjuu_transposemat(double* res, const double* mat) {
res[0] = mat[0];
res[3] = mat[1];
res[6] = mat[2];
res[1] = mat[3];
res[4] = mat[4];
res[7] = mat[5];
res[2] = mat[6];
res[5] = mat[7];
res[8] = mat[8];
double tmp[9] = {mat[0], mat[3], mat[6],
mat[1], mat[4], mat[7],
mat[2], mat[5], mat[8]};
mjuu_copyvec(res, tmp, 9);
}
@@ -454,28 +510,73 @@ void mjuu_offcenter(double* res, const double mass, const double* vec) {
// compute viscosity coefficients from mass and inertia
void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl) {
// compute equivalent box
double equivbox[3];
equivbox[0] = sqrt(mju_max(mjMINVAL, (inertia[1] + inertia[2] - inertia[0])) / mass * 6.0);
equivbox[1] = sqrt(mju_max(mjMINVAL, (inertia[0] + inertia[2] - inertia[1])) / mass * 6.0);
equivbox[2] = sqrt(mju_max(mjMINVAL, (inertia[0] + inertia[1] - inertia[2])) / mass * 6.0);
double ebox[3];
ebox[0] = sqrt(mjMAX(mjEPS, (inertia[1] + inertia[2] - inertia[0])) / mass * 6.0);
ebox[1] = sqrt(mjMAX(mjEPS, (inertia[0] + inertia[2] - inertia[1])) / mass * 6.0);
ebox[2] = sqrt(mjMAX(mjEPS, (inertia[0] + inertia[1] - inertia[2])) / mass * 6.0);
// apply formula for box (or rather cross) viscosity
// torque components
visccoef[0] = scl * 4.0 / 3.0 * equivbox[0] *
(equivbox[1]*equivbox[1]*equivbox[1] + equivbox[2]*equivbox[2]*equivbox[2]);
visccoef[1] = scl * 4.0 / 3.0 * equivbox[1] *
(equivbox[0]*equivbox[0]*equivbox[0] + equivbox[2]*equivbox[2]*equivbox[2]);
visccoef[2] = scl * 4.0 / 3.0 * equivbox[2] *
(equivbox[0]*equivbox[0]*equivbox[0] + equivbox[1]*equivbox[1]*equivbox[1]);
visccoef[0] = scl * 4.0 / 3.0 * ebox[0] * (ebox[1]*ebox[1]*ebox[1] + ebox[2]*ebox[2]*ebox[2]);
visccoef[1] = scl * 4.0 / 3.0 * ebox[1] * (ebox[0]*ebox[0]*ebox[0] + ebox[2]*ebox[2]*ebox[2]);
visccoef[2] = scl * 4.0 / 3.0 * ebox[2] * (ebox[0]*ebox[0]*ebox[0] + ebox[1]*ebox[1]*ebox[1]);
// force components
visccoef[3] = scl * 4*equivbox[1]*equivbox[2];
visccoef[4] = scl * 4*equivbox[0]*equivbox[2];
visccoef[5] = scl * 4*equivbox[0]*equivbox[1];
visccoef[3] = scl * 4*ebox[1]*ebox[2];
visccoef[4] = scl * 4*ebox[0]*ebox[2];
visccoef[5] = scl * 4*ebox[0]*ebox[1];
}
// convert axisAngle to quaternion
static void mjuu_axisAngle2Quat(double res[4], const double axis[3], double angle) {
// zero angle: identity quat
if (angle == 0) {
res[0] = 1;
res[1] = 0;
res[2] = 0;
res[3] = 0;
}
// regular processing
else {
double s = sin(angle*0.5);
res[0] = cos(angle*0.5);
res[1] = axis[0]*s;
res[2] = axis[1]*s;
res[3] = axis[2]*s;
}
}
// rotate vector by quaternion
void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]) {
// zero vec: zero res
if (vec[0] == 0 && vec[1] == 0 && vec[2] == 0) {
res[0] = res[1] = res[2] = 0;
}
// null quat: copy vec
else if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
mjuu_copyvec(res, vec, 3);
}
// regular processing
else {
// tmp = q_w * v + cross(q_xyz, v)
double tmp[3] = {
quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1],
quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[2],
quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0]
};
// res = v + 2 * cross(q_xyz, t)
res[0] = vec[0] + 2 * (quat[2]*tmp[2] - quat[3]*tmp[1]);
res[1] = vec[1] + 2 * (quat[3]*tmp[0] - quat[1]*tmp[2]);
res[2] = vec[2] + 2 * (quat[1]*tmp[1] - quat[2]*tmp[0]);
}
}
// update moving frame along a curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
@@ -486,9 +587,9 @@ void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double
// outputs:
// quat - frame orientation
// normal - unit normal vector
mjtNum mju_updateFrame(mjtNum quat[4], mjtNum normal[3], const mjtNum edge[3],
const mjtNum tprv[3], const mjtNum tnxt[3], int first) {
mjtNum tangent[3], binormal[3];
double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3],
const double tprv[3], const double tnxt[3], int first) {
double tangent[3], binormal[3];
// normalize tangent
mjuu_copyvec(tangent, edge, 3);
@@ -504,13 +605,13 @@ mjtNum mju_updateFrame(mjtNum quat[4], mjtNum normal[3], const mjtNum edge[3],
mjuu_crossvec(normal, binormal, tangent);
mjuu_normvec(normal, 3);
} else {
mjtNum darboux[4];
double darboux[4];
// rotate edge normal about the vertex binormal
mjuu_crossvec(binormal, tprv, tangent);
mjtNum angle = atan2(mjuu_normvec(binormal, 3), mjuu_dot3(tprv, tangent));
mju_axisAngle2Quat(darboux, binormal, angle);
mju_rotVecQuat(normal, normal, darboux);
double angle = atan2(mjuu_normvec(binormal, 3), mjuu_dot3(tprv, tangent));
mjuu_axisAngle2Quat(darboux, binormal, angle);
mjuu_rotVecQuat(normal, normal, darboux);
mjuu_normvec(normal, 3);
// compute edge binormal given tangent and normal
@@ -525,6 +626,114 @@ mjtNum mju_updateFrame(mjtNum quat[4], mjtNum normal[3], const mjtNum edge[3],
}
// eigenvalue decomposition of symmetric 3x3 matrix
static const double kEigEPS = 1E-12;
int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]) {
double D[9], tmp[9], tmp2[9];
double tau, t, c;
int iter, rk, ck, rotk;
// initialize with unit quaternion
quat[0] = 1;
quat[1] = quat[2] = quat[3] = 0;
// Jacobi iteration
for (iter=0; iter < 500; iter++) {
// make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec
mjuu_quat2mat(eigvec, quat);
mjuu_transposemat(tmp2, eigvec);
mjuu_mulmat(tmp, tmp2, mat);
mjuu_mulmat(D, tmp, eigvec);
// assign eigenvalues
eigval[0] = D[0];
eigval[1] = D[4];
eigval[2] = D[8];
// find max off-diagonal element, set indices
if (std::abs(D[1]) > std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) {
rk = 0; // row
ck = 1; // column
rotk = 2; // rotation axis
} else if (std::abs(D[2]) > std::abs(D[5])) {
rk = 0;
ck = 2;
rotk = 1;
} else {
rk = 1;
ck = 2;
rotk = 0;
}
// terminate if max off-diagonal element too small
if (std::abs(D[3*rk+ck]) < kEigEPS) {
break;
}
// 2x2 symmetric Schur decomposition
tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]);
if (tau >= 0) {
t = 1.0/(tau + sqrt(1 + tau*tau));
} else {
t = -1.0/(-tau + sqrt(1 + tau*tau));
}
c = 1.0/sqrt(1 + t*t);
// terminate if cosine too close to 1
if (c > 1.0-kEigEPS) {
break;
}
// express rotation as quaternion
tmp[1] = tmp[2] = tmp[3] = 0;
tmp[rotk+1] = (tau >= 0 ? -sqrt(0.5-0.5*c) : sqrt(0.5-0.5*c));
if (rotk == 1) {
tmp[rotk+1] = -tmp[rotk+1];
}
tmp[0] = sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]);
mjuu_normvec(tmp, 4);
// accumulate quaternion rotation
mjuu_mulquat(quat, quat, tmp);
mjuu_normvec(quat, 4);
}
// sort eigenvalues in decreasing order (bubblesort: 0, 1, 0)
for (int j=0; j < 3; j++) {
int j1 = j%2; // lead index
// only swap if the eigenvalues are different
if (eigval[j1]+kEigEPS < eigval[j1+1]) {
// swap eigenvalues
t = eigval[j1];
eigval[j1] = eigval[j1+1];
eigval[j1+1] = t;
// rotate quaternion
tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4)
tmp[1] = tmp[2] = tmp[3] = 0;
tmp[(j1+2)%3+1] = tmp[0];
mjuu_mulquat(quat, quat, tmp);
mjuu_normvec(quat, 4);
}
}
// recompute eigvec
mjuu_quat2mat(eigvec, quat);
return iter;
}
// transform vector by pose
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4],
const double vec[3]) {
// res = quat*vec + pos
mjuu_rotVecQuat(res, vec, quat);
res[0] += pos[0];
res[1] += pos[1];
res[2] += pos[2];
}
// strip directory from filename
string mjuu_strippath(string filename) {
// find last pathsymbol
src/user/user_util.h
+27 -15
View File
@@ -41,15 +41,23 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z);
// set 2D vector
void mjuu_setvec(double* dest, const double x, const double y);
// copy double array
void mjuu_copyvec(double* dest, const double* src, const int n);
// copy real-valued vector
template <class T1, class T2>
void mjuu_copyvec(T1* dest, const T2* src, int n) {
for (int i=0; i<n; i++) {
dest[i] = (T1)src[i];
}
}
// copy float array
void mjuu_copyvec(float* dest, const float* src, const int n);
// add to double array
void mjuu_addtovec(double* dest, const double* src, const int n);
// zero array
void mjuu_zerovec(double* dest, const int n);
// zero float array
void mjuu_zerovec(float* dest, const int n);
// dot-product in 3D
double mjuu_dot3(const double* a, const double* b);
@@ -62,6 +70,7 @@ double mjuu_L1(const double* a, const double* b, int n);
// normalize vector to unit length, return previous length
// if norm(vec)<mjEPS, return 0 and do not change vector
double mjuu_normvec(double* vec, const int n);
float mjuu_normvec(float* vec, const int n);
// convert quaternion to rotation matrix
void mjuu_quat2mat(double* res, const double* quat);
@@ -69,9 +78,12 @@ void mjuu_quat2mat(double* res, const double* quat);
// multiply two unit quaternions
void mjuu_mulquat(double* res, const double* qa, const double* qb);
// multiply vector by matrix, 3-by-3
// multiply matrix by vector, 3-by-3
void mjuu_mulvecmat(double* res, const double* vec, const double* mat);
// multiply transposed matrix by vector, 3-by-3
void mjuu_mulvecmatT(double* res, const double* vec, const double* mat);
// compute res = R * M * R'
void mjuu_mulRMRT(double* res, const double* R, const double* M);
@@ -127,19 +139,19 @@ void mjuu_offcenter(double* res, const double mass, const double* vec);
// compute viscosity coefficients from mass and inertia
void mjuu_visccoef(double* visccoef, double mass, const double* inertia, double scl=1);
// rotate vector by quaternion
void mjuu_rotVecQuat(double res[3], const double vec[3], const double quat[4]);
// update moving frame along a discrete curve or initialize it, returns edge length
// inputs:
// normal - normal vector computed by a previous call to the function
// edge - edge vector (non-unit tangent vector)
// tprv - unit tangent vector of previous body
// tnxt - unit tangent vector of next body
// first - 1 if the frame requires initialization
// outputs:
// quat - frame orientation
// normal - unit normal vector
double mju_updateFrame(double quat[4], double normal[3], const double edge[3],
double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3],
const double tprv[3], const double tnxt[3], int first);
// eigenvalue decomposition of symmetric 3x3 matrix
int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]);
// transform vector by pose
void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]);
// strip path from filename
std::string mjuu_strippath(std::string filename);
src/xml/xml_native_reader.cc
+1 -1
View File
@@ -2560,7 +2560,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* pbody) {
fcomp.rigid = (n==1);
}
if (ReadAttrTxt(elem, "point", text)){
fcomp.point = String2Vector<mjtNum>(text);
fcomp.point = String2Vector<double>(text);
}
if (ReadAttrTxt(elem, "element", text)){
fcomp.element = String2Vector<int>(text);
test/fixture.cc
+15 -12
View File
@@ -214,18 +214,21 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2,
#undef X
if (maxdif > 0) return maxdif;
// compare arrays
#define X(type, name, nr, nc) \
for (int r = 0; r < m1->nr; r++) { \
for (int c = 0; c < nc; c++) { \
dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \
if (dif > maxdif) { \
maxdif = dif; \
field = #name; \
field += " row: " + std::to_string(r); \
field += " col: " + std::to_string(c); \
} \
} \
// compare arrays, apart from bvh-related ones, as those are sensitive to
// numerical differences when meshes are perfectly symmetric.
#define X(type, name, nr, nc) \
if (strncmp(#name, "bvh_", 4)) { \
for (int r = 0; r < m1->nr; r++) { \
for (int c = 0; c < nc; c++) { \
dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \
if (dif > maxdif) { \
maxdif = dif; \
field = #name; \
field += " row: " + std::to_string(r); \
field += " col: " + std::to_string(c); \
} \
} \
} \
} // NOLINT
MJMODEL_POINTERS
#undef X
test/user/user_mesh_test.cc
+13 -10
View File
@@ -16,6 +16,7 @@
#include <array>
#include <cstddef>
#include <memory>
#include <string>
#include <vector>
@@ -127,7 +128,7 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
TEST_F(MjCMeshTest, LoadOBJWithVFS) {
@@ -152,7 +153,7 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
TEST_F(MjCMeshTest, LoadSTLWithVFS) {
@@ -177,7 +178,7 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) {
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
// ------------- test content_type attributes ----------------------------------
@@ -204,7 +205,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) {
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
TEST_F(MjCMeshTest, LoadOBJWithContentType) {
@@ -229,7 +230,7 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) {
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
TEST_F(MjCMeshTest, LoadSTLWithContentType) {
@@ -254,7 +255,7 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) {
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) {
@@ -304,7 +305,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) {
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem"));
EXPECT_THAT(error, HasSubstr("resource not found via provider or OS"));
}
// ------------- test vertex de-duplication (STL) ------------------------------
@@ -420,7 +421,8 @@ TEST_F(MjCMeshTest, MaxHullVert) {
}
TEST_F(MjCMeshTest, MaxHullVertDefault) {
const std::string xml_path = GetTestDataFilePath(kTorusDefaultMaxhullVertPath);
const std::string xml_path =
GetTestDataFilePath(kTorusDefaultMaxhullVertPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
@@ -465,8 +467,9 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) {
</worldbody>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
mj_deleteModel(model);
}
test/user/user_model_test.cc
+1 -1
View File
@@ -283,7 +283,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
mj_step(m_fuse, d_fuse);
mj_step(m_no_fuse, d_no_fuse);
EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 1e-17))
EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 2e-17))
<< "Velocity should be the same after 1 step";
EXPECT_NE(d_fuse->qvel[0], 0);
test/xml/xml_native_writer_test.cc
+7 -3
View File
@@ -1302,9 +1302,13 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
ASSERT_THAT(error.data(), HasSubstr("file"))
<< error.data() << " from " << xml.c_str();
} else {
// for a particularly difficult example, relax the tolerance
mjtNum tol =
absl::StrContains(p.path().string(), "belt.xml") ? 1e-13 : 0;
mjtNum tol = 0;
// for particularly sensitive models, relax the tolerance
if (absl::StrContains(p.path().string(), "belt.xml") ||
absl::StrContains(p.path().string(), "cable.xml")) {
tol = 1e-13;
}
// compare and delete
std::string field = "";