Initial open sourcing of MuJoCo.
PiperOrigin-RevId: 450374687 Change-Id: Ie3225a46ce095fc28ae8e63c326a640261f562bb
This commit is contained in:
Saran Tunyasuvunakool
committed by
Copybara-Service
Copybara-Service
parent
0e5d062302
commit
1913a02b40
@@ -0,0 +1,27 @@
|
||||
# Copyright 2021 DeepMind Technologies Limited
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# https://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
set(MUJOCO_USER_SRCS
|
||||
user_composite.cc
|
||||
user_composite.h
|
||||
user_mesh.cc
|
||||
user_model.cc
|
||||
user_model.h
|
||||
user_objects.cc
|
||||
user_objects.h
|
||||
user_util.cc
|
||||
user_util.h
|
||||
)
|
||||
|
||||
target_sources(mujoco PRIVATE ${MUJOCO_USER_SRCS})
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,106 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_USER_USER_COMPOSITE_H_
|
||||
#define MUJOCO_SRC_USER_USER_COMPOSITE_H_
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "user/user_model.h"
|
||||
#include "user/user_objects.h"
|
||||
|
||||
typedef enum _mjtCompType {
|
||||
mjCOMPTYPE_PARTICLE = 0,
|
||||
mjCOMPTYPE_GRID,
|
||||
mjCOMPTYPE_ROPE,
|
||||
mjCOMPTYPE_LOOP,
|
||||
mjCOMPTYPE_CLOTH,
|
||||
mjCOMPTYPE_BOX,
|
||||
mjCOMPTYPE_CYLINDER,
|
||||
mjCOMPTYPE_ELLIPSOID,
|
||||
|
||||
mjNCOMPTYPES
|
||||
} mjtCompType;
|
||||
|
||||
|
||||
typedef enum _mjtCompKind {
|
||||
mjCOMPKIND_JOINT = 0,
|
||||
mjCOMPKIND_TWIST,
|
||||
mjCOMPKIND_STRETCH,
|
||||
mjCOMPKIND_TENDON,
|
||||
mjCOMPKIND_SHEAR,
|
||||
|
||||
mjNCOMPKINDS
|
||||
} mjtCompKind;
|
||||
|
||||
|
||||
class mjCComposite {
|
||||
public:
|
||||
mjCComposite(void);
|
||||
|
||||
void SetDefault(void);
|
||||
void AdjustSoft(mjtNum* solref, mjtNum* solimp, int level);
|
||||
|
||||
bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
|
||||
bool MakeParticle(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
bool MakeGrid(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
bool MakeRope(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
bool MakeCloth(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
bool MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz);
|
||||
|
||||
void MakeShear(mjCModel* model);
|
||||
void MakeSkin2(mjCModel* model);
|
||||
void MakeSkin2Subgrid(mjCModel* model);
|
||||
|
||||
void MakeSkin3(mjCModel* model);
|
||||
void MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side, int& vcnt, const char* format);
|
||||
void MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
|
||||
const std::map<std::string, int>& vmap, const char* format);
|
||||
mjCBody* AddClothBody(mjCModel* model, mjCBody* body, int ix, int iy, int ix1, int iy1);
|
||||
mjCBody* AddRopeBody(mjCModel* model, mjCBody* body, int ix, int ix1);
|
||||
|
||||
void BoxProject(double* pos);
|
||||
|
||||
// common properties
|
||||
std::string prefix; // name prefix
|
||||
mjtCompType type; // composite type
|
||||
int count[3]; // geom count in each dimension
|
||||
double spacing; // spacing between elements
|
||||
double offset[3]; // position offset for particle and grid
|
||||
std::vector<int> pin; // pin elements of grid (do not create main joint)
|
||||
double flatinertia; // flatten ineria of cloth elements; 0: disable
|
||||
mjtNum solrefsmooth[mjNREF]; // solref for smoothing equality
|
||||
mjtNum solimpsmooth[mjNIMP]; // solimp for smoothing equality
|
||||
|
||||
// skin
|
||||
bool skin; // generate skin
|
||||
bool skintexcoord; // generate texture coordinates
|
||||
std::string skinmaterial; // skin material
|
||||
float skinrgba[4]; // skin rgba
|
||||
float skininflate; // inflate skin
|
||||
int skinsubgrid; // number of skin subgrid points; 0: none (2D only)
|
||||
|
||||
// element options
|
||||
bool add[mjNCOMPKINDS]; // add element
|
||||
mjCDef def[mjNCOMPKINDS]; // defaults
|
||||
|
||||
// computed internally
|
||||
int dim; // dimensionality
|
||||
};
|
||||
|
||||
#endif // MUJOCO_SRC_USER_USER_COMPOSITE_H_
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,254 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_USER_USER_MODEL_H_
|
||||
#define MUJOCO_SRC_USER_USER_MODEL_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <mujoco/mjdata.h>
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "user/user_objects.h"
|
||||
|
||||
typedef enum _mjtInertiaFromGeom {
|
||||
mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required
|
||||
mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element
|
||||
mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing
|
||||
} mjtInertiaFromGeom;
|
||||
|
||||
|
||||
|
||||
//---------------------------------- class mjCModel ------------------------------------------------
|
||||
// mjCModel contains everything needed to generate the low-level model.
|
||||
// It can be constructed manually by calling 'Add' functions and setting
|
||||
// the public fields of the various objects. Alternatively it can constructed
|
||||
// by loading an XML file via mjCXML. Once an mjCModel object is
|
||||
// constructed, 'Compile' can be called to generate the corresponding mjModel object
|
||||
// (which is the low-level model). The mjCModel object can then be deleted.
|
||||
|
||||
class mjCModel {
|
||||
friend class mjCBody;
|
||||
friend class mjCJoint;
|
||||
friend class mjCGeom;
|
||||
friend class mjCMesh;
|
||||
friend class mjCSkin;
|
||||
friend class mjCHField;
|
||||
friend class mjCPair;
|
||||
friend class mjCBodyPair;
|
||||
friend class mjCSite;
|
||||
friend class mjCEquality;
|
||||
friend class mjCTendon;
|
||||
friend class mjCWrap;
|
||||
friend class mjCActuator;
|
||||
friend class mjCSensor;
|
||||
friend class mjCNumeric;
|
||||
friend class mjCTuple;
|
||||
friend class mjCKey;
|
||||
friend class mjXReader;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
mjCModel(); // constructor
|
||||
~mjCModel(); // destructor
|
||||
|
||||
mjModel* Compile(const mjVFS* vfs = 0); // COMPILER: construct mjModel
|
||||
bool CopyBack(const mjModel*); // DECOMPILER: copy numeric back
|
||||
void FuseStatic(void); // fuse static bodies with parent
|
||||
void FuseReindex(mjCBody* body); // reindex elements during fuse
|
||||
|
||||
|
||||
//------------------------ API for adding model elements
|
||||
mjCMesh* AddMesh(mjCDef* def = 0); // mesh
|
||||
mjCSkin* AddSkin(void); // skin
|
||||
mjCHField* AddHField(void); // heightfield
|
||||
mjCTexture* AddTexture(void); // texture
|
||||
mjCMaterial*AddMaterial(mjCDef* def = 0); // material
|
||||
mjCPair* AddPair(mjCDef* def = 0); // geom pair for inclusion
|
||||
mjCBodyPair*AddExclude(void); // body pair for exclusion
|
||||
mjCEquality*AddEquality(mjCDef* def = 0); // equality constraint
|
||||
mjCTendon* AddTendon(mjCDef* def = 0); // tendon
|
||||
mjCActuator*AddActuator(mjCDef* def = 0); // actuator
|
||||
mjCSensor* AddSensor(void); // sensor
|
||||
mjCNumeric* AddNumeric(void); // custom numeric
|
||||
mjCText* AddText(void); // custom text
|
||||
mjCTuple* AddTuple(void); // custom tuple
|
||||
mjCKey* AddKey(void); // keyframe
|
||||
|
||||
//------------------------ API for access to model elements (outside tree)
|
||||
int NumObjects(mjtObj type); // number of objects in specified list
|
||||
mjCBase* GetObject(mjtObj type, int id); // pointer to specified object
|
||||
|
||||
//------------------------ API for access to other variables
|
||||
bool IsCompiled(void); // is model already compiled
|
||||
int GetFixed(void); // number of fixed massless bodies
|
||||
mjCError GetError(void); // copy of error object
|
||||
mjCBody* GetWorld(void); // pointer to world body
|
||||
mjCDef* FindDef(std::string name); // find default class name
|
||||
mjCDef* AddDef(std::string name, int parentid); // add default class to array
|
||||
mjCBase* FindObject(mjtObj type, std::string name); // find object given type and name
|
||||
bool IsNullPose(const mjtNum* pos, const mjtNum* quat); // detect null pose
|
||||
|
||||
|
||||
//------------------------ global data
|
||||
std::string comment; // comment at top of XML
|
||||
std::string modelfiledir; // path to model file
|
||||
std::vector<mjCDef*> defaults; // settings for each defaults class
|
||||
|
||||
//------------------------ compiler settings
|
||||
double boundmass; // enfore minimum body mass
|
||||
double boundinertia; // enfore minimum body diagonal inertia
|
||||
double settotalmass; // rescale masses and inertias; <=0: ignore
|
||||
bool balanceinertia; // automatically impose A + B >= C rule
|
||||
bool strippath; // automatically strip paths from mesh files
|
||||
bool fitaabb; // meshfit to aabb instead of inertia box
|
||||
bool global; // local or global coordinates
|
||||
bool degree; // angles in radians or degrees
|
||||
char euler[3]; // sequence for euler rotations
|
||||
std::string meshdir; // mesh and hfield directory
|
||||
std::string texturedir; // texture directory
|
||||
bool discardvisual; // discard visual geoms in parser
|
||||
bool convexhull; // compute mesh convex hulls
|
||||
bool usethread; // use multiple threads to speed up compiler
|
||||
bool fusestatic; // fuse static bodies with parent
|
||||
int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom)
|
||||
int inertiagrouprange[2]; // range of geom groups used to compute inertia
|
||||
mjLROpt LRopt; // options for lengthrange computation
|
||||
|
||||
//------------------------ statistics override (if defined)
|
||||
double meaninertia; // mean diagonal inertia
|
||||
double meanmass; // mean body mass
|
||||
double meansize; // mean body size
|
||||
double extent; // spatial extent
|
||||
double center[3]; // center of model
|
||||
|
||||
//------------------------ engine data
|
||||
std::string modelname; // model name
|
||||
mjOption option; // options
|
||||
mjVisual visual; // visual options
|
||||
int nemax; // max number of equality constraints
|
||||
int njmax; // max number of constraints (Jacobian rows)
|
||||
int nconmax; // max number of detected contacts (mjContact array size)
|
||||
int nstack; // number of fields in mjData stack
|
||||
int nuserdata; // number extra fields in mjData
|
||||
int nuser_body; // number of mjtNums in body_user
|
||||
int nuser_jnt; // number of mjtNums in jnt_user
|
||||
int nuser_geom; // number of mjtNums in geom_user
|
||||
int nuser_site; // number of mjtNums in site_user
|
||||
int nuser_cam; // number of mjtNums in cam_user
|
||||
int nuser_tendon; // number of mjtNums in tendon_user
|
||||
int nuser_actuator; // number of mjtNums in actuator_user
|
||||
int nuser_sensor; // number of mjtNums in sensor_user
|
||||
|
||||
private:
|
||||
void Clear(void); // clear objects allocated by Compile
|
||||
|
||||
template <class T> // add object of any type
|
||||
T* AddObject(std::vector<T*>& list, std::string type);
|
||||
|
||||
template <class T> // add object of any type, with def parameter
|
||||
T* AddObjectDef(std::vector<T*>& list, std::string type, mjCDef* def);
|
||||
|
||||
//------------------------ compile phases
|
||||
void MakeLists(mjCBody* body); // make lists of bodies, geoms, joints, sites
|
||||
void IndexAssets(void); // convert asset names into indices
|
||||
void SetDefaultNames(void); // if mesh or hfield name is missing, set to filename
|
||||
void SetSizes(void); // compute sizes
|
||||
void AutoSpringDamper(mjModel*);// automatic stiffness and damping computation
|
||||
void LengthRange(mjModel*, mjData*); // compute actuator lengthrange
|
||||
void CopyNames(mjModel*); // copy names, compute name addresses
|
||||
void CopyObjects(mjModel*); // copy objects outside kinematic tree
|
||||
void CopyTree(mjModel*); // copy objects inside kinematic tree
|
||||
|
||||
//------------------------ sizes
|
||||
// sizes set from object list lengths
|
||||
int nbody; // number of bodies
|
||||
int njnt; // number of joints
|
||||
int ngeom; // number of geoms
|
||||
int nsite; // number of sites
|
||||
int ncam; // number of cameras
|
||||
int nlight; // number of lights
|
||||
int nmesh; // number of meshes
|
||||
int nskin; // number of skins
|
||||
int nhfield; // number of height fields
|
||||
int ntex; // number of textures
|
||||
int nmat; // number of materials
|
||||
int npair; // number of geom pairs in pair array
|
||||
int nexclude; // number of excluded body pairs
|
||||
int neq; // number of equality constraints
|
||||
int ntendon; // number of tendons
|
||||
int nsensor; // number of sensors
|
||||
int nnumeric; // number of numeric fields
|
||||
int ntext; // number of text fields
|
||||
int ntuple; // number of tuple fields
|
||||
int nkey; // number of keyframes
|
||||
int nmocap; // number of mocap bodies
|
||||
|
||||
// sizes computed by Compile
|
||||
int nq; // number of generalized coordinates = dim(qpos)
|
||||
int nv; // number of degrees of freedom = dim(qvel)
|
||||
int nu; // number of actuators/controls
|
||||
int na; // number of activation variables
|
||||
int nmeshvert; // number of vertices in all meshes
|
||||
int nmeshtexvert; // number of texture coordinates in all meshes
|
||||
int nmeshface; // number of triangular faces in all meshes
|
||||
int nmeshgraph; // number of shorts in mesh auxiliary data
|
||||
int nskinvert; // number of vertices in all skins
|
||||
int nskintexvert; // number of vertices with texcoord in all skins
|
||||
int nskinface; // number of faces in all skins
|
||||
int nskinbone; // number of bones in all skins
|
||||
int nskinbonevert; // number of vertices in all skins
|
||||
int nhfielddata; // number of data points in all hfields
|
||||
int ntexdata; // number of texture bytes
|
||||
int nwrap; // number of wrap objects in all tendon paths
|
||||
int nsensordata; // number of mjtNums in sensor data vector
|
||||
int nnumericdata; // number of mjtNums in all custom fields
|
||||
int ntextdata; // number of chars in all text fields, including 0
|
||||
int ntupledata; // number of objects in all tuple fields
|
||||
int nnames; // number of chars in all names
|
||||
int nM; // number of non-zeros in sparse inertia matrix
|
||||
|
||||
//------------------------ object lists
|
||||
// objects created here
|
||||
std::vector<mjCMesh*> meshes; // list of meshes
|
||||
std::vector<mjCSkin*> skins; // list of skins
|
||||
std::vector<mjCHField*> hfields; // list of height fields
|
||||
std::vector<mjCTexture*> textures; // list of textures
|
||||
std::vector<mjCMaterial*> materials; // list of materials
|
||||
std::vector<mjCPair*> pairs; // list of geom pairs to include
|
||||
std::vector<mjCBodyPair*> excludes; // list of body pairs to exclude
|
||||
std::vector<mjCEquality*> equalities; // list of equality constraints
|
||||
std::vector<mjCTendon*> tendons; // list of tendons
|
||||
std::vector<mjCActuator*> actuators; // list of actuators
|
||||
std::vector<mjCSensor*> sensors; // list of sensors
|
||||
std::vector<mjCNumeric*> numerics; // list of numeric fields
|
||||
std::vector<mjCText*> texts; // list of text fields
|
||||
std::vector<mjCTuple*> tuples; // list of tuple fields
|
||||
std::vector<mjCKey*> keys; // list of keyframe fields
|
||||
|
||||
// pointers to objects created inside kinematic tree
|
||||
std::vector<mjCBody*> bodies; // list of bodies
|
||||
std::vector<mjCJoint*> joints; // list of joints allowing motion relative to parent
|
||||
std::vector<mjCGeom*> geoms; // list of geoms attached to this body
|
||||
std::vector<mjCSite*> sites; // list of sites attached to this body
|
||||
std::vector<mjCCamera*> cameras; // list of cameras
|
||||
std::vector<mjCLight*> lights; // list of lights
|
||||
|
||||
//------------------------ internal variables
|
||||
bool compiled; // already compiled flag (cannot be compiled again)
|
||||
mjCError errInfo; // last error info
|
||||
int fixCount; // how many bodies have been fixed
|
||||
std::vector<mjtNum> qpos0; // save qpos0, to recognize changed key_qpos in write
|
||||
};
|
||||
#endif // MUJOCO_SRC_USER_USER_MODEL_H_
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,986 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_USER_USER_OBJECTS_H_
|
||||
#define MUJOCO_SRC_USER_USER_OBJECTS_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "lodepng.h"
|
||||
#include <mujoco/mjmodel.h>
|
||||
|
||||
// forward declarations of all mjC/X classes
|
||||
class mjCError;
|
||||
class mjCAlternative;
|
||||
class mjCBase;
|
||||
class mjCBody;
|
||||
class mjCJoint;
|
||||
class mjCGeom;
|
||||
class mjCSite;
|
||||
class mjCCamera;
|
||||
class mjCLight;
|
||||
class mjCMesh;
|
||||
class mjCSkin;
|
||||
class mjCTexture;
|
||||
class mjCMaterial;
|
||||
class mjCPair;
|
||||
class mjCBodyPair;
|
||||
class mjCEquality;
|
||||
class mjCTendon;
|
||||
class mjCWrap;
|
||||
class mjCActuator;
|
||||
class mjCSensor;
|
||||
class mjCNumeric;
|
||||
class mjCText;
|
||||
class mjCTuple;
|
||||
class mjCDef;
|
||||
class mjCMesh; // defined in user_mesh
|
||||
class mjCModel; // defined in user_model
|
||||
class mjXWriter; // defined in xml_native
|
||||
class mjXURDF; // defined in xml_urdf
|
||||
|
||||
|
||||
//------------------------- helper classes and constants -------------------------------------------
|
||||
|
||||
// number of positive size parameters for each geom type
|
||||
const int mjGEOMINFO[mjNGEOMTYPES] = {3, 0, 1, 2, 3, 2, 3, 0};
|
||||
|
||||
|
||||
// builtin type for procedural textures
|
||||
typedef enum _mjtBuiltin {
|
||||
mjBUILTIN_NONE = 0, // no builtin
|
||||
mjBUILTIN_GRADIENT, // circular gradient: rgb1->rgb2->rgb3
|
||||
mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2
|
||||
mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down
|
||||
} mjtBuiltin;
|
||||
|
||||
|
||||
// mark type for procedural textures
|
||||
typedef enum _mjtMark {
|
||||
mjMARK_NONE = 0, // no mark
|
||||
mjMARK_EDGE, // paint edges
|
||||
mjMARK_CROSS, // paint cross
|
||||
mjMARK_RANDOM // paint random dots
|
||||
} mjtMark;
|
||||
|
||||
|
||||
// error information
|
||||
class mjCError {
|
||||
public:
|
||||
mjCError(const mjCBase* obj = 0,
|
||||
const char* msg = 0,
|
||||
const char* str = 0,
|
||||
int pos1 = 0,
|
||||
int pos2 = 0);
|
||||
|
||||
char message[500]; // error message
|
||||
bool warning; // is this a warning instead of error
|
||||
};
|
||||
|
||||
|
||||
// alternative specifications of frame orientation
|
||||
class mjCAlternative {
|
||||
public:
|
||||
mjCAlternative(); // constuctor
|
||||
const char* Set(double* quat, double* inertia, // set frame quat and diag. inertia
|
||||
bool degree, // angle format: degree/radian
|
||||
const char* sequence); // euler sequence format: "xyz"
|
||||
|
||||
double axisangle[4]; // rotation axis and angle
|
||||
double xyaxes[6]; // x and y axes
|
||||
double zaxis[3]; // z axis (use minimal rotation)
|
||||
double euler[3]; // euler rotations
|
||||
double fullinertia[6]; // non-axis-aligned inertia matrix
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCBase ----------------------------------------------------------
|
||||
// Generic functionality for all derived classes
|
||||
|
||||
class mjCBase {
|
||||
friend class mjCDef;
|
||||
|
||||
public:
|
||||
std::string name; // object name
|
||||
std::string classname; // defaults class name
|
||||
int id; // object id
|
||||
int xmlpos[2]; // row and column in xml file
|
||||
mjCDef* def; // defaults class used to init this object
|
||||
|
||||
protected:
|
||||
mjCBase(); // constructor
|
||||
mjCModel* model; // pointer to model that created object
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCBody -----------------------------------------------
|
||||
// Describes a rigid body
|
||||
|
||||
class mjCBody : public mjCBase {
|
||||
friend class mjCJoint;
|
||||
friend class mjCGeom;
|
||||
friend class mjCSite;
|
||||
friend class mjCCamera;
|
||||
friend class mjCLight;
|
||||
friend class mjCEquality;
|
||||
friend class mjCPair;
|
||||
friend class mjCModel;
|
||||
friend class mjXReader;
|
||||
friend class mjXWriter;
|
||||
friend class mjXURDF;
|
||||
|
||||
public:
|
||||
// API for adding objects to body
|
||||
mjCBody* AddBody(mjCDef* = 0);
|
||||
mjCJoint* AddJoint(mjCDef* = 0, bool isfree = false);
|
||||
mjCGeom* AddGeom(mjCDef* = 0);
|
||||
mjCSite* AddSite(mjCDef* = 0);
|
||||
mjCCamera* AddCamera(mjCDef* = 0);
|
||||
mjCLight* AddLight(mjCDef* = 0);
|
||||
|
||||
// API for accessing objects
|
||||
int NumObjects(mjtObj type);
|
||||
mjCBase* GetObject(mjtObj type, int id);
|
||||
mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true);
|
||||
|
||||
// setup child local frame, take into account change
|
||||
void MakeLocal(double* locpos, double* locquat, const double* pos, const double* quat);
|
||||
|
||||
// variables set by user or 'Compile'
|
||||
bool mocap; // is this a mocap body
|
||||
double pos[3]; // frame position
|
||||
double quat[4]; // frame orientation
|
||||
double ipos[3]; // inertial frame position
|
||||
double iquat[4]; // inertial frame orientation
|
||||
double mass; // mass
|
||||
double inertia[3]; // diagonal inertia (in i-frame)
|
||||
std::vector<double> userdata; // user data
|
||||
mjCAlternative alt; // alternative orientation specification
|
||||
mjCAlternative ialt; // alternative for inertial frame
|
||||
|
||||
// variables computed by 'Compile' and 'AddXXX'
|
||||
private:
|
||||
mjCBody(mjCModel*); // constructor
|
||||
~mjCBody(); // destructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
void GeomFrame(void); // get inertial info from geoms
|
||||
|
||||
double locpos[3]; // position relative to parent
|
||||
double locquat[4]; // orientation relative to parent
|
||||
double locipos[3]; // inertial position frame, rel. to local frame
|
||||
double lociquat[4]; // inertial frame orientation
|
||||
int parentid; // parent index in global array
|
||||
int weldid; // top index of body we are welded to
|
||||
int dofnum; // number of motion dofs for body
|
||||
int mocapid; // mocap id, -1: not mocap
|
||||
bool explicit_inertial; // whether inertial clause was explicitly stated
|
||||
|
||||
int lastdof; // id of last dof (used by compiler)
|
||||
|
||||
// objects allocated by Add functions
|
||||
std::vector<mjCBody*> bodies; // child bodies
|
||||
std::vector<mjCGeom*> geoms; // geoms attached to this body
|
||||
std::vector<mjCJoint*> joints; // joints allowing motion relative to parent
|
||||
std::vector<mjCSite*> sites; // sites attached to this body
|
||||
std::vector<mjCCamera*> cameras; // cameras attached to this body
|
||||
std::vector<mjCLight*> lights; // lights attached to this body
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCJoint ---------------------------------------------------------
|
||||
// Describes a motion degree of freedom of a body relative to its parent
|
||||
|
||||
class mjCJoint : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCEquality;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
friend class mjXURDF;
|
||||
|
||||
public:
|
||||
// variables set by user: joint properties
|
||||
mjtJoint type; // type of Joint
|
||||
int group; // used for rendering
|
||||
bool limited; // does joint have limits
|
||||
double pos[3]; // anchor position
|
||||
double axis[3]; // joint axis
|
||||
double stiffness; // stiffness coefficient
|
||||
double springdamper[2]; // timeconst, dampratio
|
||||
double range[2]; // joint limits
|
||||
mjtNum solref_limit[mjNREF]; // solver reference: joint limits
|
||||
mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits
|
||||
mjtNum solref_friction[mjNREF]; // solver reference: dof friction
|
||||
mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction
|
||||
double margin; // margin value for joint limit detection
|
||||
double ref; // value at reference configuration: qpos0
|
||||
double springref; // spring reference value: qpos_spring
|
||||
std::vector<double> userdata; // user data
|
||||
|
||||
// variables set by user: dof properties
|
||||
double armature; // armature inertia (mass for slider)
|
||||
double damping; // damping coefficient
|
||||
double frictionloss; // friction loss
|
||||
|
||||
double urdfeffort; // store effort field from urdf
|
||||
|
||||
private:
|
||||
mjCJoint(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
int Compile(void); // compiler; return dofnum
|
||||
|
||||
mjCBody* body; // joint's body
|
||||
double locpos[3]; // anchor position in child or parent
|
||||
double locaxis[3]; // joint axis in child or parent
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCGeom ----------------------------------------------------------
|
||||
// Describes a geometric shape belonging to a body
|
||||
|
||||
class mjCGeom : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCMesh;
|
||||
friend class mjCPair;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
friend class mjXURDF;
|
||||
|
||||
public:
|
||||
double GetVolume(void); // compute geom volume
|
||||
void SetInertia(void); // compute and set geom inertia
|
||||
|
||||
// Compute all coefs modeling the interaction with the surrounding fluid.
|
||||
void SetFluidCoefs(void);
|
||||
// Compute the kappa coefs of the added inertia due to the surrounding fluid.
|
||||
double GetAddedMassKappa(double dx, double dy, double dz);
|
||||
|
||||
// variables set by user and copied into mjModel
|
||||
mjtGeom type; // geom type
|
||||
int contype; // contact type
|
||||
int conaffinity; // contact affinity
|
||||
int condim; // contact dimensionality
|
||||
int group; // used for rendering
|
||||
int priority; // contact priority
|
||||
double size[3]; // geom-specific size parameters
|
||||
double friction[3]; // one-sided friction coefficients: slide, roll, spin
|
||||
double solmix; // solver mixing for contact pairs
|
||||
mjtNum solref[mjNREF]; // solver reference
|
||||
mjtNum solimp[mjNIMP]; // solver impedance
|
||||
double margin; // margin for contact detection
|
||||
double gap; // include in solver if dist<margin-gap
|
||||
mjtNum fluid_switch; // whether ellipsoid-fluid model is active
|
||||
mjtNum fluid_coefs[5]; // tunable ellipsoid-fluid interaction coefs
|
||||
mjtNum fluid[mjNFLUID]; // compile-time fluid-interaction parameters
|
||||
std::string hfield; // hfield attached to geom
|
||||
std::string mesh; // mesh attached to geom
|
||||
double fitscale; // scale mesh uniformly
|
||||
std::string material; // name of material used for rendering
|
||||
std::vector<double> userdata; // user data
|
||||
float rgba[4]; // rgba when material is omitted
|
||||
|
||||
// variables set by user and used during compilation
|
||||
double _mass; // used to compute density
|
||||
double density; // used to compute mass and inertia (from volume)
|
||||
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
|
||||
mjCAlternative alt; // alternative orientation specifications
|
||||
|
||||
// variables set by user or 'Compile1'
|
||||
double pos[3]; // position
|
||||
double quat[4]; // orientation
|
||||
|
||||
private:
|
||||
mjCGeom(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
void Compile(void); // compiler
|
||||
double GetRBound(void); // compute bounding sphere radius
|
||||
|
||||
int matid; // id of geom's material
|
||||
int meshid; // id of geom's mesh (-1: none)
|
||||
int hfieldid; // id of geom's hfield (-1: none)
|
||||
double mass; // mass
|
||||
double inertia[3]; // local diagonal inertia
|
||||
double locpos[3]; // local position
|
||||
double locquat[4]; // local orientation
|
||||
mjCBody* body; // geom's body
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCSite ----------------------------------------------------------
|
||||
// Describes a site on a body
|
||||
|
||||
class mjCSite : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
friend class mjXURDF;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
mjtGeom type; // geom type for rendering
|
||||
int group; // group id, used for visualization
|
||||
double size[3]; // geom size for rendering
|
||||
double pos[3]; // position
|
||||
double quat[4]; // orientation
|
||||
std::string material; // name of material for rendering
|
||||
std::vector<double> userdata; // user data
|
||||
float rgba[4]; // rgba when material is omitted
|
||||
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
|
||||
mjCAlternative alt; // alternative orientation specification
|
||||
|
||||
// variables computed by 'compile' and 'mjCBody::addSite'
|
||||
private:
|
||||
mjCSite(mjCModel* = 0, mjCDef* = 0); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
mjCBody* body; // site's body
|
||||
double locpos[3]; // local position
|
||||
double locquat[4]; // local orientation
|
||||
int matid; // material id for rendering
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCCamera --------------------------------------------------------
|
||||
// Describes a camera, attached to a body
|
||||
|
||||
class mjCCamera : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
mjtCamLight mode; // tracking mode
|
||||
std::string targetbody; // target body for orientation
|
||||
double fovy; // y-field of view
|
||||
double ipd; // inter-pupilary distance
|
||||
double pos[3]; // position
|
||||
double quat[4]; // orientation
|
||||
std::vector<double> userdata; // user data
|
||||
mjCAlternative alt; // alternative orientation specification
|
||||
|
||||
private:
|
||||
mjCCamera(mjCModel* = 0, mjCDef* = 0); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
mjCBody* body; // camera's body
|
||||
double locpos[3]; // local position
|
||||
double locquat[4]; // local orientation
|
||||
int targetbodyid; // id of target body; -1: none
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCLight ---------------------------------------------------------
|
||||
// Describes a light, attached to a body
|
||||
|
||||
class mjCLight : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
mjtCamLight mode; // tracking mode
|
||||
std::string targetbody; // target body for orientation
|
||||
bool directional; // directional light
|
||||
bool castshadow; // does light cast shadows
|
||||
bool active; // is light active
|
||||
double pos[3]; // position
|
||||
double dir[3]; // direction
|
||||
float attenuation[3]; // OpenGL attenuation (quadratic model)
|
||||
float cutoff; // OpenGL cutoff
|
||||
float exponent; // OpenGL exponent
|
||||
float ambient[3]; // ambient color
|
||||
float diffuse[3]; // diffuse color
|
||||
float specular[3]; // specular color
|
||||
|
||||
private:
|
||||
mjCLight(mjCModel* = 0, mjCDef* = 0); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
mjCBody* body; // light's body
|
||||
double locpos[3]; // local position
|
||||
double locdir[3]; // local direction
|
||||
int targetbodyid; // id of target body; -1: none
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCMesh ----------------------------------------------------------
|
||||
// Describes a mesh
|
||||
|
||||
class mjCMesh: public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCGeom;
|
||||
friend class mjCBody;
|
||||
friend class mjCSkin;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
void GetPos(double* pos); // get position
|
||||
void GetQuat(double* quat); // get orientation
|
||||
void FitGeom(mjCGeom* geom, double* meshpos); // approximate mesh with simple geom
|
||||
|
||||
std::string file; // mesh file
|
||||
double refpos[3]; // reference position (translate)
|
||||
double refquat[4]; // reference orientation (rotate)
|
||||
double scale[3]; // rescale mesh
|
||||
bool smoothnormal; // do not exclude large-angle faces from normals
|
||||
|
||||
std::vector<float> uservert; // user vertex data
|
||||
std::vector<float> usernormal; // user normal data
|
||||
std::vector<float> usertexcoord; // user texcoord data
|
||||
std::vector<int> userface; // user face data
|
||||
|
||||
private:
|
||||
mjCMesh(mjCModel* = 0, mjCDef* = 0); // constructor
|
||||
~mjCMesh(); // destructor
|
||||
void Compile(const mjVFS* vfs); // compiler
|
||||
void LoadOBJ(const mjVFS* vfs); // load mesh in wavefront OBJ format
|
||||
void LoadSTL(const mjVFS* vfs); // load mesh in STL BIN format
|
||||
void LoadMSH(const mjVFS* vfs); // load mesh in MSH BIN format
|
||||
void MakeGraph(void); // make graph of convex hull
|
||||
void CopyGraph(void); // copy graph into face data
|
||||
void MakeNormal(void); // compute vertex normals
|
||||
void Process(void); // apply transformations
|
||||
void RemoveRepeated(void); // remove repeated vertices
|
||||
|
||||
// mesh properties computed by Compile
|
||||
double pos[3]; // CoM position
|
||||
double quat[4]; // inertia orientation
|
||||
double boxsz[3]; // half-sizes of equivalent inertia box
|
||||
double aabb[3]; // half-sizes of axis-aligned bounding box
|
||||
|
||||
// mesh data to be copied into mjModel
|
||||
int nvert; // number of vertices
|
||||
int nface; // number of faces
|
||||
int szgraph; // size of graph data in ints
|
||||
float* vert; // vertex data (3*nvert), relative to (pos, quat)
|
||||
float* normal; // vertex normal data (3*nvert)
|
||||
float* texcoord; // vertex texcoord data (2*nvert, or NULL)
|
||||
int* face; // face vertex indices (3*nface)
|
||||
int* graph; // convex graph data
|
||||
|
||||
bool needhull; // needs convex hull for collisions
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCSkin ----------------------------------------------------------
|
||||
// Describes a skin
|
||||
|
||||
class mjCSkin: public mjCBase {
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
std::string file; // skin file
|
||||
std::string material; // name of material used for rendering
|
||||
float rgba[4]; // rgba when material is omitted
|
||||
float inflate; // inflate in normal direction
|
||||
|
||||
// mesh
|
||||
std::vector<float> vert; // vertex positions
|
||||
std::vector<float> texcoord; // texture coordinates
|
||||
std::vector<int> face; // faces
|
||||
|
||||
// skin
|
||||
std::vector<std::string> bodyname; // body names
|
||||
std::vector<float> bindpos; // bind pos
|
||||
std::vector<float> bindquat; // bind quat
|
||||
std::vector<std::vector<int>> vertid; // vertex ids
|
||||
std::vector<std::vector<float>> vertweight; // vertex weights
|
||||
|
||||
private:
|
||||
mjCSkin(mjCModel* = 0); // constructor
|
||||
~mjCSkin(); // destructor
|
||||
void Compile(const mjVFS* vfs); // compiler
|
||||
void LoadSKN(const mjVFS* vfs); // load skin in SKN BIN format
|
||||
|
||||
int matid; // material id
|
||||
std::vector<int> bodyid; // body ids
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCHField --------------------------------------------------------
|
||||
// Describes a height field
|
||||
|
||||
class mjCHField : public mjCBase {
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
std::string file; // file: (nrow, ncol, [elevation data])
|
||||
double size[4]; // hfield size (ignore referencing geom size)
|
||||
int nrow; // number of rows
|
||||
int ncol; // number of columns
|
||||
float* data; // elevation data, row-major format
|
||||
|
||||
private:
|
||||
mjCHField(mjCModel* model); // constructor
|
||||
~mjCHField(); // destructor
|
||||
void Compile(const mjVFS* vfs); // compiler
|
||||
|
||||
void LoadCustom(std::string filename, const mjVFS* vfs); // load from custom format
|
||||
void LoadPNG(std::string filename, const mjVFS* vfs); // load from PNG format
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCTexture -------------------------------------------------------
|
||||
// Describes a texture
|
||||
|
||||
class mjCTexture : public mjCBase {
|
||||
friend class mjCModel;
|
||||
friend class mjXReader;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
mjtTexture type; // texture type
|
||||
|
||||
// method 1: builtin
|
||||
mjtBuiltin builtin; // builtin type
|
||||
mjtMark mark; // mark type
|
||||
double rgb1[3]; // first color for builtin
|
||||
double rgb2[3]; // second color for builtin
|
||||
double markrgb[3]; // mark color
|
||||
double random; // probability of random dots
|
||||
int height; // height in pixels (square for cube and skybox)
|
||||
int width; // width in pixels
|
||||
|
||||
// method 2: single file
|
||||
std::string file; // png file to load; use for all sides of cube
|
||||
int gridsize[2]; // size of grid for composite file; (1,1)-repeat
|
||||
char gridlayout[13]; // row-major: L,R,F,B,U,D for faces; . for unused
|
||||
|
||||
// method 3: separate files
|
||||
std::string cubefiles[6]; // different file for each side of the cube
|
||||
|
||||
// flip options
|
||||
bool hflip; // horizontal flip
|
||||
bool vflip; // vertical flip
|
||||
|
||||
private:
|
||||
mjCTexture(mjCModel*); // constructor
|
||||
~mjCTexture(); // destructior
|
||||
void Compile(const mjVFS* vfs); // compiler
|
||||
|
||||
void Builtin2D(void); // make builtin 2D
|
||||
void BuiltinCube(void); // make builtin cube
|
||||
void Load2D(std::string filename, const mjVFS* vfs); // load 2D from file
|
||||
void LoadCubeSingle(std::string filename, const mjVFS* vfs); // load cube from single file
|
||||
void LoadCubeSeparate(const mjVFS* vfs); // load cube from separate files
|
||||
|
||||
void LoadFlip(std::string filename, const mjVFS* vfs, // load and flip
|
||||
std::vector<unsigned char>& image,
|
||||
unsigned int& w, unsigned int& h);
|
||||
|
||||
void LoadPNG(std::string filename, const mjVFS* vfs,
|
||||
std::vector<unsigned char>& image,
|
||||
unsigned int& w, unsigned int& h);
|
||||
void LoadCustom(std::string filename, const mjVFS* vfs,
|
||||
std::vector<unsigned char>& image,
|
||||
unsigned int& w, unsigned int& h);
|
||||
|
||||
mjtByte* rgb; // rgb data
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCMaterial ------------------------------------------------------
|
||||
// Describes a material for rendering
|
||||
|
||||
class mjCMaterial : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
std::string texture; // name of texture (empty: none)
|
||||
bool texuniform; // make texture cube uniform
|
||||
float texrepeat[2]; // texture repetition for 2D mapping
|
||||
float emission; // emission
|
||||
float specular; // specular
|
||||
float shininess; // shininess
|
||||
float reflectance; // reflectance
|
||||
float rgba[4]; // rgba
|
||||
|
||||
private:
|
||||
mjCMaterial(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int texid; // id of material
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCPair ----------------------------------------------------------
|
||||
// Predefined geom pair for collision detection
|
||||
|
||||
class mjCPair : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
// parameters set by user
|
||||
std::string geomname1; // name of geom 1
|
||||
std::string geomname2; // name of geom 2
|
||||
|
||||
// optional parameters: computed from geoms if not set by user
|
||||
int condim; // contact dimensionality
|
||||
mjtNum solref[mjNREF]; // solver reference
|
||||
mjtNum solimp[mjNIMP]; // solver impedance
|
||||
double margin; // margin for contact detection
|
||||
double gap; // include in solver if dist<margin-gap
|
||||
double friction[5]; // full contact friction
|
||||
|
||||
int GetSignature(void) {
|
||||
return signature;
|
||||
}
|
||||
|
||||
private:
|
||||
mjCPair(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int geom1; // id of geom1
|
||||
int geom2; // id of geom2
|
||||
int signature; // (body1+1)<<16 + body2+1
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCBodyPair ------------------------------------------------------
|
||||
// Body pair specification, use to exclude pairs
|
||||
|
||||
class mjCBodyPair : public mjCBase {
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
// parameters set by user
|
||||
std::string bodyname1; // name of geom 1
|
||||
std::string bodyname2; // name of geom 2
|
||||
|
||||
int GetSignature(void) {
|
||||
return signature;
|
||||
}
|
||||
|
||||
private:
|
||||
mjCBodyPair(mjCModel*); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int body1; // id of body1
|
||||
int body2; // id of body2
|
||||
int signature; // (body1+1)<<16 + body2+1
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCEquality ------------------------------------------------------
|
||||
// Describes an equality constraint
|
||||
|
||||
class mjCEquality : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCBody;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
mjtEq type; // constraint type
|
||||
std::string name1; // name of object 1
|
||||
std::string name2; // name of object 2
|
||||
bool active; // initial activation state
|
||||
mjtNum solref[mjNREF]; // solver reference
|
||||
mjtNum solimp[mjNIMP]; // solver impedance
|
||||
double data[mjNEQDATA]; // type-dependent data
|
||||
|
||||
private:
|
||||
mjCEquality(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int obj1id; // id of object 1
|
||||
int obj2id; // id of object 2
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCTendon --------------------------------------------------------
|
||||
// Describes a tendon
|
||||
|
||||
class mjCTendon : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// API for adding wrapping objects
|
||||
void WrapSite(std::string name, int row=-1, int col=-1); // site
|
||||
void WrapGeom(std::string name, std::string side, int row=-1, int col=-1); // geom
|
||||
void WrapJoint(std::string name, double coef, int row=-1, int col=-1); // joint
|
||||
void WrapPulley(double divisor, int row=-1, int col=-1); // pulley
|
||||
|
||||
// API for access to wrapping objects
|
||||
int NumWraps(void); // number of wraps
|
||||
mjCWrap* GetWrap(int); // pointer to wrap
|
||||
|
||||
// variables set by user
|
||||
int group; // group for visualization
|
||||
std::string material; // name of material for rendering
|
||||
bool limited; // does tendon have limits
|
||||
double width; // width for rendering
|
||||
mjtNum solref_limit[mjNREF]; // solver reference: tendon limits
|
||||
mjtNum solimp_limit[mjNIMP]; // solver impedance: tendon limits
|
||||
mjtNum solref_friction[mjNREF]; // solver reference: tendon friction
|
||||
mjtNum solimp_friction[mjNIMP]; // solver impedance: tendon friction
|
||||
double range[2]; // length limits
|
||||
double margin; // margin value for tendon limit detection
|
||||
double stiffness; // stiffness coefficient
|
||||
double damping; // damping coefficient
|
||||
double frictionloss; // friction loss
|
||||
double springlength; // spring length; -1: use qpos_spring
|
||||
std::vector<double> userdata; // user data
|
||||
float rgba[4]; // rgba when material is omitted
|
||||
|
||||
private:
|
||||
mjCTendon(mjCModel* = 0, mjCDef* = 0); // constructor
|
||||
~mjCTendon(); // destructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int matid; // material id for rendering
|
||||
std::vector<mjCWrap*> path; // wrapping objects
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCWrap ----------------------------------------------------------
|
||||
// Describes a tendon wrap object
|
||||
|
||||
class mjCWrap : public mjCBase {
|
||||
friend class mjCTendon;
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
mjtWrap type; // wrap object type
|
||||
int objid; // wrap object id (in array corresponding to type)
|
||||
int sideid; // side site id; -1 if not applicable
|
||||
double prm; // parameter: divisor, coefficient
|
||||
std::string sidesite; // name of side site
|
||||
|
||||
private:
|
||||
mjCWrap(mjCModel*, mjCTendon*); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
mjCTendon* tendon; // tendon owning this wrap
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCActuator ------------------------------------------------------
|
||||
// Describes an actuator
|
||||
|
||||
class mjCActuator : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user or API
|
||||
int group; // group for visualization
|
||||
bool ctrllimited; // are control limits defined
|
||||
bool forcelimited; // are force limits defined
|
||||
bool actlimited; // are activation limits defined
|
||||
mjtDyn dyntype; // dynamics type
|
||||
mjtTrn trntype; // transmission type
|
||||
mjtGain gaintype; // gain type
|
||||
mjtBias biastype; // bias type
|
||||
double dynprm[mjNDYN]; // dynamics parameters
|
||||
double gainprm[mjNGAIN]; // gain parameters
|
||||
double biasprm[mjNGAIN]; // bias parameters
|
||||
double ctrlrange[2]; // control range
|
||||
double forcerange[2]; // force range
|
||||
double actrange[2]; // activation range
|
||||
double lengthrange[2]; // length range
|
||||
double gear[6]; // length and transmitted force scaling
|
||||
double cranklength; // crank length, for slider-crank only
|
||||
std::vector<double> userdata; // user data
|
||||
std::string target; // transmission target name
|
||||
std::string slidersite; // site defining cylinder, for slider-crank only
|
||||
|
||||
private:
|
||||
mjCActuator(mjCModel* = 0, mjCDef* = 0);// constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int trnid[2]; // id of transmission target
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCSensor --------------------------------------------------------
|
||||
// Describes a sensor
|
||||
|
||||
class mjCSensor : public mjCBase {
|
||||
friend class mjCDef;
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
// variables set by user or API
|
||||
mjtSensor type; // type of sensor
|
||||
mjtDataType datatype; // data type for sensor measurement
|
||||
mjtStage needstage; // compute stage needed to simulate sensor
|
||||
mjtObj objtype; // type of sensorized object
|
||||
std::string objname; // name of sensorized object
|
||||
mjtObj reftype;
|
||||
std::string refname;
|
||||
int dim; // number of scalar outputs
|
||||
double cutoff; // cutoff for real and positive datatypes
|
||||
double noise; // noise stdev
|
||||
std::vector<double> userdata; // user data
|
||||
|
||||
private:
|
||||
mjCSensor(mjCModel*); // constructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
int objid; // id of sensorized object
|
||||
int refid; // id of reference frame
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCNumeric -------------------------------------------------------
|
||||
// Describes a custom data field
|
||||
|
||||
class mjCNumeric : public mjCBase {
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
std::vector<double> data; // initialization data
|
||||
int size; // array size, can be bigger than data.size()
|
||||
|
||||
private:
|
||||
mjCNumeric(mjCModel*); // constructor
|
||||
~mjCNumeric(); // destructor
|
||||
void Compile(void); // compiler
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCText ----------------------------------------------------------
|
||||
// Describes a custom text field
|
||||
|
||||
class mjCText : public mjCBase {
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
std::string data; // string
|
||||
|
||||
private:
|
||||
mjCText(mjCModel*); // constructor
|
||||
~mjCText(); // destructor
|
||||
void Compile(void); // compiler
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCTuple ---------------------------------------------------------
|
||||
// Describes a custom tuple field
|
||||
|
||||
class mjCTuple : public mjCBase {
|
||||
friend class mjCModel;
|
||||
|
||||
public:
|
||||
// variables set by user
|
||||
std::vector<mjtObj> objtype; // object types
|
||||
std::vector<std::string> objname; // object names
|
||||
std::vector<double> objprm; // object parameters
|
||||
|
||||
private:
|
||||
mjCTuple(mjCModel*); // constructor
|
||||
~mjCTuple(); // destructor
|
||||
void Compile(void); // compiler
|
||||
|
||||
std::vector<int> objid; // object ids
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCKey -----------------------------------------------------------
|
||||
// Describes a keyframe
|
||||
|
||||
class mjCKey : public mjCBase {
|
||||
friend class mjCModel;
|
||||
friend class mjXWriter;
|
||||
|
||||
public:
|
||||
double time; // time
|
||||
std::vector<double> qpos; // qpos
|
||||
std::vector<double> qvel; // qvel
|
||||
std::vector<double> act; // act
|
||||
std::vector<double> mpos; // mocap pos
|
||||
std::vector<double> mquat; // mocap quat
|
||||
|
||||
private:
|
||||
mjCKey(mjCModel*); // constructor
|
||||
~mjCKey(); // destructor
|
||||
void Compile(const mjModel* m); // compiler
|
||||
};
|
||||
|
||||
|
||||
|
||||
//------------------------- class mjCDef -----------------------------------------------------------
|
||||
// Describes one set of defaults
|
||||
|
||||
class mjCDef {
|
||||
public:
|
||||
mjCDef(void); // constructor
|
||||
void Compile(const mjCModel* model); // compiler
|
||||
|
||||
// identifiers
|
||||
std::string name; // class name
|
||||
int parentid; // id of parent class
|
||||
std::vector<int> childid; // ids of child classes
|
||||
|
||||
// default objects
|
||||
mjCJoint joint;
|
||||
mjCGeom geom;
|
||||
mjCSite site;
|
||||
mjCCamera camera;
|
||||
mjCLight light;
|
||||
mjCMesh mesh;
|
||||
mjCMaterial material;
|
||||
mjCPair pair;
|
||||
mjCEquality equality;
|
||||
mjCTendon tendon;
|
||||
mjCActuator actuator;
|
||||
};
|
||||
|
||||
#endif // MUJOCO_SRC_USER_USER_OBJECTS_H_
|
||||
@@ -0,0 +1,555 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "user/user_util.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdio>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
#include <mujoco/mjmodel.h>
|
||||
#include "engine/engine_file.h"
|
||||
#include "engine/engine_macro.h"
|
||||
#include "engine/engine_util_errmem.h"
|
||||
|
||||
using std::isnan;
|
||||
using std::string;
|
||||
using std::numeric_limits;
|
||||
|
||||
|
||||
// set value of NAN here; needs <limits>
|
||||
const double mjNAN = numeric_limits<double>::quiet_NaN();
|
||||
|
||||
|
||||
// check if numeric variable is defined
|
||||
bool mjuu_defined(const double num) {
|
||||
return !isnan(num);
|
||||
}
|
||||
|
||||
|
||||
// compute address of M[g1][g2] where M is triangular n-by-n
|
||||
int mjuu_matadr(int g1, int g2, const int n) {
|
||||
if (g1<0 || g2<0 || g1>=n || g2>=n) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (g1>g2) {
|
||||
int tmp = g1;
|
||||
g1 = g2;
|
||||
g2 = tmp;
|
||||
}
|
||||
|
||||
return g1*n + g2;
|
||||
}
|
||||
|
||||
|
||||
// set 4D vector
|
||||
void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w) {
|
||||
dest[0] = x;
|
||||
dest[1] = y;
|
||||
dest[2] = z;
|
||||
dest[3] = w;
|
||||
}
|
||||
void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w) {
|
||||
dest[0] = (float)x;
|
||||
dest[1] = (float)y;
|
||||
dest[2] = (float)z;
|
||||
dest[3] = (float)w;
|
||||
}
|
||||
|
||||
|
||||
// set 3D vector
|
||||
void mjuu_setvec(double* dest, const double x, const double y, const double z) {
|
||||
dest[0] = x;
|
||||
dest[1] = y;
|
||||
dest[2] = z;
|
||||
}
|
||||
void mjuu_setvec(float* dest, const double x, const double y, const double z) {
|
||||
dest[0] = (float)x;
|
||||
dest[1] = (float)y;
|
||||
dest[2] = (float)z;
|
||||
}
|
||||
|
||||
|
||||
// set 2D vector
|
||||
void mjuu_setvec(double* dest, const double x, const double y) {
|
||||
dest[0] = x;
|
||||
dest[1] = y;
|
||||
}
|
||||
|
||||
|
||||
// copy double array
|
||||
void mjuu_copyvec(double* dest, const double* src, const int n) {
|
||||
for (int i=0; i<n; 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++) {
|
||||
dest[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// dot-product in 3D
|
||||
double mjuu_dot3(const double* a, const double* b) {
|
||||
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
|
||||
}
|
||||
|
||||
|
||||
// distance beween 3D points
|
||||
double mjuu_dist3(const double* a, const double* b) {
|
||||
return sqrt((a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]));
|
||||
}
|
||||
|
||||
|
||||
// L1 norm between vectors
|
||||
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]);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
// normalize vector to unit length, return previous length
|
||||
double mjuu_normvec(double* vec, const int n) {
|
||||
double nrm = 0;
|
||||
int i;
|
||||
|
||||
for (i=0; i<n; i++) {
|
||||
nrm += vec[i]*vec[i];
|
||||
}
|
||||
if (nrm < mjEPS) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
nrm = sqrt(nrm);
|
||||
for (i=0; i<n; i++) {
|
||||
vec[i] /= nrm;
|
||||
}
|
||||
|
||||
return nrm;
|
||||
}
|
||||
|
||||
|
||||
// convert quaternion to rotation matrix
|
||||
void mjuu_quat2mat(double* res, const double* quat) {
|
||||
double q00 = quat[0]*quat[0];
|
||||
double q11 = quat[1]*quat[1];
|
||||
double q22 = quat[2]*quat[2];
|
||||
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]);
|
||||
}
|
||||
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
|
||||
// multiply vector by 3-by-3 matrix
|
||||
void mjuu_mulvecmat(double* res, const double* vec, const double* mat) {
|
||||
res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2];
|
||||
res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2];
|
||||
res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2];
|
||||
}
|
||||
|
||||
|
||||
// compute res = R * M * R'
|
||||
void mjuu_mulRMRT(double* res, const double* R, const double* M) {
|
||||
double tmp[9];
|
||||
|
||||
// tmp = R*M
|
||||
tmp[0] = R[0]*M[0] + R[1]*M[3] + R[2]*M[6];
|
||||
tmp[1] = R[0]*M[1] + R[1]*M[4] + R[2]*M[7];
|
||||
tmp[2] = R[0]*M[2] + R[1]*M[5] + R[2]*M[8];
|
||||
tmp[3] = R[3]*M[0] + R[4]*M[3] + R[5]*M[6];
|
||||
tmp[4] = R[3]*M[1] + R[4]*M[4] + R[5]*M[7];
|
||||
tmp[5] = R[3]*M[2] + R[4]*M[5] + R[5]*M[8];
|
||||
tmp[6] = R[6]*M[0] + R[7]*M[3] + R[8]*M[6];
|
||||
tmp[7] = R[6]*M[1] + R[7]*M[4] + R[8]*M[7];
|
||||
tmp[8] = R[6]*M[2] + R[7]*M[5] + R[8]*M[8];
|
||||
|
||||
// res = tmp*R'
|
||||
res[0] = tmp[0]*R[0] + tmp[1]*R[1] + tmp[2]*R[2];
|
||||
res[1] = tmp[0]*R[3] + tmp[1]*R[4] + tmp[2]*R[5];
|
||||
res[2] = tmp[0]*R[6] + tmp[1]*R[7] + tmp[2]*R[8];
|
||||
res[3] = tmp[3]*R[0] + tmp[4]*R[1] + tmp[5]*R[2];
|
||||
res[4] = tmp[3]*R[3] + tmp[4]*R[4] + tmp[5]*R[5];
|
||||
res[5] = tmp[3]*R[6] + tmp[4]*R[7] + tmp[5]*R[8];
|
||||
res[6] = tmp[6]*R[0] + tmp[7]*R[1] + tmp[8]*R[2];
|
||||
res[7] = tmp[6]*R[3] + tmp[7]*R[4] + tmp[8]*R[5];
|
||||
res[8] = tmp[6]*R[6] + tmp[7]*R[7] + tmp[8]*R[8];
|
||||
}
|
||||
|
||||
|
||||
// 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];
|
||||
|
||||
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];
|
||||
|
||||
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];
|
||||
}
|
||||
|
||||
|
||||
// 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];
|
||||
}
|
||||
|
||||
|
||||
// convert global to local axis relative to given frame
|
||||
void mjuu_localaxis(double* al, const double* ag, const double* quat) {
|
||||
double mat[9];
|
||||
double qneg[4] = {quat[0], -quat[1], -quat[2], -quat[3]};
|
||||
mjuu_quat2mat(mat, qneg);
|
||||
mjuu_mulvecmat(al, ag, mat);
|
||||
}
|
||||
|
||||
|
||||
// convert global to local position relative to given frame
|
||||
void mjuu_localpos(double* pl, const double* pg, const double* pos, const double* quat) {
|
||||
double a[3] = {pg[0]-pos[0], pg[1]-pos[1], pg[2]-pos[2]};
|
||||
mjuu_localaxis(pl, a, quat);
|
||||
}
|
||||
|
||||
|
||||
// compute quaternion rotation from parent to child
|
||||
void mjuu_localquat(double* local, const double* child, const double* parent) {
|
||||
double pneg[4] = {parent[0], -parent[1], -parent[2], -parent[3]};
|
||||
mjuu_mulquat(local, pneg, child);
|
||||
}
|
||||
|
||||
|
||||
// compute vector cross-product a = b x c
|
||||
void mjuu_crossvec(double* a, const double* b, const double* c) {
|
||||
a[0] = b[1]*c[2] - b[2]*c[1];
|
||||
a[1] = b[2]*c[0] - b[0]*c[2];
|
||||
a[2] = b[0]*c[1] - b[1]*c[0];
|
||||
}
|
||||
|
||||
|
||||
// compute normal vector to given triangle, return length
|
||||
double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c) {
|
||||
double v1[3] = {b[0]-a[0], b[1]-a[1], b[2]-a[2]};
|
||||
double v2[3] = {c[0]-a[0], c[1]-a[1], c[2]-a[2]};
|
||||
double res;
|
||||
|
||||
mjuu_crossvec(normal, v1, v2);
|
||||
if ((res=mjuu_normvec(normal, 3)) < mjEPS) {
|
||||
normal[0] = normal[1] = 0;
|
||||
normal[2] = 1;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
// compute quaternion as minimal rotation from [0;0;1] to vec
|
||||
void mjuu_z2quat(double* quat, const double* vec) {
|
||||
double z[3] = {0, 0, 1};
|
||||
mjuu_crossvec(quat+1, z, vec);
|
||||
double s = mjuu_normvec(quat+1, 3);
|
||||
if (s<1E-10) {
|
||||
quat[1] = 1;
|
||||
quat[2] = quat[3] = 0;
|
||||
}
|
||||
double ang = atan2(s, vec[2]);
|
||||
quat[0] = cos(ang/2);
|
||||
quat[1] *= sin(ang/2);
|
||||
quat[2] *= sin(ang/2);
|
||||
quat[3] *= sin(ang/2);
|
||||
}
|
||||
|
||||
|
||||
// compute quaternion given frame (axes are in matrix columns)
|
||||
void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z) {
|
||||
const double* mat[3] = {x, y, z}; // mat[c][r] indexing
|
||||
|
||||
// q0 largest
|
||||
if (mat[0][0]+mat[1][1]+mat[2][2]>0) {
|
||||
quat[0] = 0.5 * sqrt(1 + mat[0][0] + mat[1][1] + mat[2][2]);
|
||||
quat[1] = 0.25 * (mat[1][2] - mat[2][1]) / quat[0];
|
||||
quat[2] = 0.25 * (mat[2][0] - mat[0][2]) / quat[0];
|
||||
quat[3] = 0.25 * (mat[0][1] - mat[1][0]) / quat[0];
|
||||
}
|
||||
|
||||
// q1 largest
|
||||
else if (mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2]) {
|
||||
quat[1] = 0.5 * sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]);
|
||||
quat[0] = 0.25 * (mat[1][2] - mat[2][1]) / quat[1];
|
||||
quat[2] = 0.25 * (mat[1][0] + mat[0][1]) / quat[1];
|
||||
quat[3] = 0.25 * (mat[2][0] + mat[0][2]) / quat[1];
|
||||
}
|
||||
|
||||
// q2 largest
|
||||
else if (mat[1][1]>mat[2][2]) {
|
||||
quat[2] = 0.5 * sqrt(1 - mat[0][0] + mat[1][1] - mat[2][2]);
|
||||
quat[0] = 0.25 * (mat[2][0] - mat[0][2]) / quat[2];
|
||||
quat[1] = 0.25 * (mat[1][0] + mat[0][1]) / quat[2];
|
||||
quat[3] = 0.25 * (mat[2][1] + mat[1][2]) / quat[2];
|
||||
}
|
||||
|
||||
// q3 largest
|
||||
else {
|
||||
quat[3] = 0.5 * sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
|
||||
quat[0] = 0.25 * (mat[0][1] - mat[1][0]) / quat[3];
|
||||
quat[1] = 0.25 * (mat[2][0] + mat[0][2]) / quat[3];
|
||||
quat[2] = 0.25 * (mat[2][1] + mat[1][2]) / quat[3];
|
||||
}
|
||||
|
||||
mjuu_normvec(quat, 4);
|
||||
}
|
||||
|
||||
|
||||
// invert frame transformation
|
||||
void mjuu_frameinvert(double* newpos, double* newquat,
|
||||
const double* oldpos, const double* oldquat) {
|
||||
// position
|
||||
mjuu_localaxis(newpos, oldpos, oldquat);
|
||||
newpos[0] = -newpos[0];
|
||||
newpos[1] = -newpos[1];
|
||||
newpos[2] = -newpos[2];
|
||||
|
||||
// orientation
|
||||
newquat[0] = oldquat[0];
|
||||
newquat[1] = -oldquat[1];
|
||||
newquat[2] = -oldquat[2];
|
||||
newquat[3] = -oldquat[3];
|
||||
}
|
||||
|
||||
|
||||
// accumulate frame transformations (forward kinematics)
|
||||
void mjuu_frameaccum(double* pos, double* quat,
|
||||
const double* addpos, const double* addquat) {
|
||||
double mat[9], vec[3], qtmp[4];
|
||||
mjuu_quat2mat(mat, quat);
|
||||
mjuu_mulvecmat(vec, addpos, mat);
|
||||
pos[0] += vec[0];
|
||||
pos[1] += vec[1];
|
||||
pos[2] += vec[2];
|
||||
mjuu_mulquat(qtmp, quat, addquat);
|
||||
mjuu_copyvec(quat, qtmp, 4);
|
||||
}
|
||||
|
||||
|
||||
// invert frame accumulation
|
||||
void mjuu_frameaccuminv(double* pos, double* quat,
|
||||
const double* addpos, const double* addquat) {
|
||||
double mat[9], vec[3], qtmp[4];
|
||||
double qneg[4] = {addquat[0], -addquat[1], -addquat[2], -addquat[3]};
|
||||
mjuu_mulquat(qtmp, quat, qneg);
|
||||
mjuu_copyvec(quat, qtmp, 4);
|
||||
mjuu_quat2mat(mat, quat);
|
||||
mjuu_mulvecmat(vec, addpos, mat);
|
||||
pos[0] -= vec[0];
|
||||
pos[1] -= vec[1];
|
||||
pos[2] -= vec[2];
|
||||
}
|
||||
|
||||
|
||||
// convert local_inertia[3] to global_inertia[6]
|
||||
void mjuu_globalinertia(double* global, const double* local, const double* quat) {
|
||||
double mat[9];
|
||||
mjuu_quat2mat(mat, quat);
|
||||
|
||||
double tmp[9] = {
|
||||
mat[0]*local[0], mat[3]*local[0], mat[6]*local[0],
|
||||
mat[1]*local[1], mat[4]*local[1], mat[7]*local[1],
|
||||
mat[2]*local[2], mat[5]*local[2], mat[8]*local[2]
|
||||
};
|
||||
|
||||
global[0] = mat[0]*tmp[0] + mat[1]*tmp[3] + mat[2]*tmp[6];
|
||||
global[1] = mat[3]*tmp[1] + mat[4]*tmp[4] + mat[5]*tmp[7];
|
||||
global[2] = mat[6]*tmp[2] + mat[7]*tmp[5] + mat[8]*tmp[8];
|
||||
global[3] = mat[0]*tmp[1] + mat[1]*tmp[4] + mat[2]*tmp[7];
|
||||
global[4] = mat[0]*tmp[2] + mat[1]*tmp[5] + mat[2]*tmp[8];
|
||||
global[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8];
|
||||
}
|
||||
|
||||
|
||||
// compute off-center correction to inertia matrix
|
||||
// mass * [y^2+z^2, -x*y, -x*z; -x*y, x^2+z^2, -y*z; -x*z, -y*z, x^2+y^2]
|
||||
void mjuu_offcenter(double* res, const double mass, const double* vec) {
|
||||
res[0] = mass*(vec[1]*vec[1] + vec[2]*vec[2]);
|
||||
res[1] = mass*(vec[0]*vec[0] + vec[2]*vec[2]);
|
||||
res[2] = mass*(vec[0]*vec[0] + vec[1]*vec[1]);
|
||||
res[3] = -mass*vec[0]*vec[1];
|
||||
res[4] = -mass*vec[0]*vec[2];
|
||||
res[5] = -mass*vec[1]*vec[2];
|
||||
}
|
||||
|
||||
|
||||
// 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(mjMAX(mjMINVAL, (inertia[1] + inertia[2] - inertia[0])) / mass * 6.0);
|
||||
equivbox[1] = sqrt(mjMAX(mjMINVAL, (inertia[0] + inertia[2] - inertia[1])) / mass * 6.0);
|
||||
equivbox[2] = sqrt(mjMAX(mjMINVAL, (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]);
|
||||
|
||||
// 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];
|
||||
}
|
||||
|
||||
|
||||
// strip directory from filename
|
||||
string mjuu_strippath(string filename) {
|
||||
// find last pathsymbol
|
||||
size_t start = filename.find_last_of("/\\");
|
||||
|
||||
// no path found: return original
|
||||
if (start==string::npos) {
|
||||
return filename;
|
||||
}
|
||||
|
||||
// return name without path
|
||||
else {
|
||||
return filename.substr(start+1, filename.size()-start-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// strip extension
|
||||
string mjuu_stripext(string filename) {
|
||||
// find last dot
|
||||
size_t end = filename.find_last_of('.');
|
||||
|
||||
// no path found: return original
|
||||
if (end==string::npos) {
|
||||
return filename;
|
||||
}
|
||||
|
||||
// return name without extension
|
||||
else {
|
||||
return filename.substr(0, end);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// is directory path absolute
|
||||
bool mjuu_isabspath(string path) {
|
||||
// empty: not absolute
|
||||
if (path.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check first char
|
||||
const char* str = path.c_str();
|
||||
if (str[0]=='\\' || str[0]=='/') {
|
||||
return true;
|
||||
}
|
||||
|
||||
// find ":/" or ":\"
|
||||
if (path.find(":/")!=string::npos || path.find(":\\")!=string::npos) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// get directory path of file
|
||||
string mjuu_getfiledir(string filename) {
|
||||
// no filename
|
||||
if (filename.empty()) {
|
||||
return "";
|
||||
}
|
||||
|
||||
// find last pathsymbol
|
||||
size_t last = filename.find_last_of("/\\");
|
||||
|
||||
// no pathsymbol: unknown dir
|
||||
if (last==string::npos) {
|
||||
return "";
|
||||
}
|
||||
|
||||
// extract path from filename
|
||||
return filename.substr(0, last+1);
|
||||
}
|
||||
|
||||
|
||||
// assemble full filename
|
||||
string mjuu_makefullname(string filedir, string meshdir, string filename) {
|
||||
// filename has absolute path: filename
|
||||
if (mjuu_isabspath(filename)) {
|
||||
return filename;
|
||||
}
|
||||
|
||||
// meshdir has absolute path: meshdir + filename
|
||||
if (mjuu_isabspath(meshdir)) {
|
||||
return meshdir + filename;
|
||||
}
|
||||
|
||||
// default
|
||||
return filedir + meshdir + filename;
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
// Copyright 2021 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef MUJOCO_SRC_USER_USER_UTIL_H_
|
||||
#define MUJOCO_SRC_USER_USER_UTIL_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
|
||||
extern const double mjNAN; // used to mark undefined fields
|
||||
const double mjEPS = 1E-14; // minimum value in various calculations
|
||||
const double mjMINMASS = 1E-6; // minimum mass allowed
|
||||
|
||||
|
||||
// check if numeric variable is defined: !_isnan(num)
|
||||
bool mjuu_defined(const double num);
|
||||
|
||||
// compute linear address of M[g1][g2] where M is triangular n-by-n
|
||||
// return -1 if inputs are invalid
|
||||
int mjuu_matadr(int g1, int g2, const int n);
|
||||
|
||||
// set 4D vector
|
||||
void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w);
|
||||
void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w);
|
||||
|
||||
// set 3D vector
|
||||
void mjuu_setvec(double* dest, const double x, const double y, const double z);
|
||||
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 float array
|
||||
void mjuu_copyvec(float* dest, const float* src, const int n);
|
||||
|
||||
// zero array
|
||||
void mjuu_zerovec(double* dest, const int n);
|
||||
|
||||
// dot-product in 3D
|
||||
double mjuu_dot3(const double* a, const double* b);
|
||||
|
||||
// distance beween 3D points
|
||||
double mjuu_dist3(const double* a, const double* b);
|
||||
|
||||
// L1 norm between vectors
|
||||
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);
|
||||
|
||||
// convert quaternion to rotation matrix
|
||||
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
|
||||
void mjuu_mulvecmat(double* res, const double* vec, const double* mat);
|
||||
|
||||
// compute res = R * M * R'
|
||||
void mjuu_mulRMRT(double* res, const double* R, const double* M);
|
||||
|
||||
// compute res = A * B, all 3-by-3
|
||||
void mjuu_mulmat(double* res, const double* A, const double* B);
|
||||
|
||||
// transpose 3-by-3 matrix
|
||||
void mjuu_transposemat(double* res, const double* mat);
|
||||
|
||||
// convert global to local axis relative to given frame
|
||||
void mjuu_localaxis(double* al, const double* ag, const double* quat);
|
||||
|
||||
// convert global to local position relative to given frame
|
||||
void mjuu_localpos(double* pl, const double* pg, const double* pos, const double* quat);
|
||||
|
||||
// compute quaternion rotation from parent to child
|
||||
void mjuu_localquat(double* local, const double* child, const double* parent);
|
||||
|
||||
// compute vector cross-product a = b x c
|
||||
void mjuu_crossvec(double* a, const double* b, const double* c);
|
||||
|
||||
// compute normal vector to given triangle (uses float for OpenGL)
|
||||
double mjuu_makenormal(double* normal, const float* a, const float* b, const float* c);
|
||||
|
||||
// compute quaternion corresponding to minimal rotation from [0;0;1] to vec
|
||||
void mjuu_z2quat(double* quat, const double* vec);
|
||||
|
||||
// compute quaternion corresponding to given rotation matrix (i.e. frame)
|
||||
void mjuu_frame2quat(double* quat, const double* x, const double* y, const double* z);
|
||||
|
||||
// invert frame transformation
|
||||
void mjuu_frameinvert(double* newpos, double* newquat,
|
||||
const double* oldpos, const double* oldquat);
|
||||
|
||||
// accumulate frame transformations
|
||||
void mjuu_frameaccum(double* pos, double* quat,
|
||||
const double* addpos, const double* addquat);
|
||||
|
||||
// invert frame accumulation
|
||||
void mjuu_frameaccuminv(double* pos, double* quat,
|
||||
const double* addpos, const double* addquat);
|
||||
|
||||
// convert local_inertia[3] to global_inertia[6]
|
||||
void mjuu_globalinertia(double* global, const double* local, const double* quat);
|
||||
|
||||
// compute off-center correction to inertia matrix
|
||||
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);
|
||||
|
||||
// strip path from filename
|
||||
std::string mjuu_strippath(std::string filename);
|
||||
|
||||
// strip extension from filename
|
||||
std::string mjuu_stripext(std::string filename);
|
||||
|
||||
// check if path is absolute
|
||||
bool mjuu_isabspath(std::string path);
|
||||
|
||||
// get path from filename
|
||||
std::string mjuu_getfiledir(std::string filename);
|
||||
|
||||
// assemble full filename
|
||||
std::string mjuu_makefullname(std::string filedir, std::string meshdir, std::string filename);
|
||||
|
||||
#endif // MUJOCO_SRC_USER_USER_UTIL_H_
|
||||
Reference in New Issue
Block a user