Declare loop variables inside for loop in C++ files.
PiperOrigin-RevId: 518852339 Change-Id: I0174d873d89e48f11b816d337728f213c2798248
This commit is contained in:
committed by
Copybara-Service
parent
145fe7f354
commit
0e7e299bf6
+37
-49
@@ -139,8 +139,6 @@ const char help_title[] =
|
|||||||
|
|
||||||
// init profiler figures
|
// init profiler figures
|
||||||
void profilerinit(mj::Simulate* sim) {
|
void profilerinit(mj::Simulate* sim) {
|
||||||
int i, n;
|
|
||||||
|
|
||||||
// set figures to default
|
// set figures to default
|
||||||
mjv_defaultFigure(&sim->figconstraint);
|
mjv_defaultFigure(&sim->figconstraint);
|
||||||
mjv_defaultFigure(&sim->figcost);
|
mjv_defaultFigure(&sim->figcost);
|
||||||
@@ -225,8 +223,8 @@ void profilerinit(mj::Simulate* sim) {
|
|||||||
sim->figtimer.range[1][1] = 0.4f;
|
sim->figtimer.range[1][1] = 0.4f;
|
||||||
|
|
||||||
// init x axis on history figures (do not show yet)
|
// init x axis on history figures (do not show yet)
|
||||||
for (n=0; n<6; n++)
|
for (int n=0; n<6; n++)
|
||||||
for (i=0; i<mjMAXLINEPNT; i++) {
|
for (int i=0; i<mjMAXLINEPNT; i++) {
|
||||||
sim->figtimer.linedata[n][2*i] = -i;
|
sim->figtimer.linedata[n][2*i] = -i;
|
||||||
sim->figsize.linedata[n][2*i] = -i;
|
sim->figsize.linedata[n][2*i] = -i;
|
||||||
}
|
}
|
||||||
@@ -234,11 +232,9 @@ void profilerinit(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// update profiler figures
|
// update profiler figures
|
||||||
void profilerupdate(mj::Simulate* sim) {
|
void profilerupdate(mj::Simulate* sim) {
|
||||||
int i, n;
|
|
||||||
|
|
||||||
// update constraint figure
|
// update constraint figure
|
||||||
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
|
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
|
||||||
for (i=1; i<5; i++) {
|
for (int i=1; i<5; i++) {
|
||||||
sim->figconstraint.linepnt[i] = sim->figconstraint.linepnt[0];
|
sim->figconstraint.linepnt[i] = sim->figconstraint.linepnt[0];
|
||||||
}
|
}
|
||||||
if (sim->m->opt.solver==mjSOL_PGS) {
|
if (sim->m->opt.solver==mjSOL_PGS) {
|
||||||
@@ -248,7 +244,7 @@ void profilerupdate(mj::Simulate* sim) {
|
|||||||
if (sim->m->opt.solver==mjSOL_CG) {
|
if (sim->m->opt.solver==mjSOL_CG) {
|
||||||
sim->figconstraint.linepnt[4] = 0;
|
sim->figconstraint.linepnt[4] = 0;
|
||||||
}
|
}
|
||||||
for (i=0; i<sim->figconstraint.linepnt[0]; i++) {
|
for (int i=0; i<sim->figconstraint.linepnt[0]; i++) {
|
||||||
// x
|
// x
|
||||||
sim->figconstraint.linedata[0][2*i] = i;
|
sim->figconstraint.linedata[0][2*i] = i;
|
||||||
sim->figconstraint.linedata[1][2*i] = i;
|
sim->figconstraint.linedata[1][2*i] = i;
|
||||||
@@ -266,7 +262,7 @@ void profilerupdate(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// update cost figure
|
// update cost figure
|
||||||
sim->figcost.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
|
sim->figcost.linepnt[0] = mjMIN(mjMIN(sim->d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
|
||||||
for (i=1; i<3; i++) {
|
for (int i=1; i<3; i++) {
|
||||||
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
|
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
|
||||||
}
|
}
|
||||||
if (sim->m->opt.solver==mjSOL_PGS) {
|
if (sim->m->opt.solver==mjSOL_PGS) {
|
||||||
@@ -274,7 +270,7 @@ void profilerupdate(mj::Simulate* sim) {
|
|||||||
sim->figcost.linepnt[2] = 0;
|
sim->figcost.linepnt[2] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i=0; i<sim->figcost.linepnt[0]; i++) {
|
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
|
||||||
// x
|
// x
|
||||||
sim->figcost.linedata[0][2*i] = i;
|
sim->figcost.linedata[0][2*i] = i;
|
||||||
sim->figcost.linedata[1][2*i] = i;
|
sim->figcost.linedata[1][2*i] = i;
|
||||||
@@ -306,9 +302,9 @@ void profilerupdate(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// update figtimer
|
// update figtimer
|
||||||
int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1);
|
int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1);
|
||||||
for (n=0; n<5; n++) {
|
for (int n=0; n<5; n++) {
|
||||||
// shift data
|
// shift data
|
||||||
for (i=pnt-1; i>0; i--) {
|
for (int i=pnt-1; i>0; i--) {
|
||||||
sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1];
|
sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1];
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -329,9 +325,9 @@ void profilerupdate(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// update figsize
|
// update figsize
|
||||||
pnt = mjMIN(201, sim->figsize.linepnt[0]+1);
|
pnt = mjMIN(201, sim->figsize.linepnt[0]+1);
|
||||||
for (n=0; n<6; n++) {
|
for (int n=0; n<6; n++) {
|
||||||
// shift data
|
// shift data
|
||||||
for (i=pnt-1; i>0; i--) {
|
for (int i=pnt-1; i>0; i--) {
|
||||||
sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1];
|
sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1];
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -542,7 +538,6 @@ void watch(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// make physics section of UI
|
// make physics section of UI
|
||||||
void makephysics(mj::Simulate* sim, int oldstate) {
|
void makephysics(mj::Simulate* sim, int oldstate) {
|
||||||
int i;
|
|
||||||
mjOption& opt = sim->m->opt;
|
mjOption& opt = sim->m->opt;
|
||||||
|
|
||||||
mjuiDef defPhysics[] = {
|
mjuiDef defPhysics[] = {
|
||||||
@@ -591,13 +586,13 @@ void makephysics(mj::Simulate* sim, int oldstate) {
|
|||||||
{mjITEM_CHECKINT, "", 2, nullptr, ""},
|
{mjITEM_CHECKINT, "", 2, nullptr, ""},
|
||||||
{mjITEM_END}
|
{mjITEM_END}
|
||||||
};
|
};
|
||||||
for (i=0; i<mjNDISABLE; i++) {
|
for (int i=0; i<mjNDISABLE; i++) {
|
||||||
mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
|
mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
|
||||||
defFlag[0].pdata = sim->disable + i;
|
defFlag[0].pdata = sim->disable + i;
|
||||||
mjui_add(&sim->ui0, defFlag);
|
mjui_add(&sim->ui0, defFlag);
|
||||||
}
|
}
|
||||||
mjui_add(&sim->ui0, defEnableFlags);
|
mjui_add(&sim->ui0, defEnableFlags);
|
||||||
for (i=0; i<mjNENABLE; i++) {
|
for (int i=0; i<mjNENABLE; i++) {
|
||||||
mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
|
mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
|
||||||
defFlag[0].pdata = sim->enable + i;
|
defFlag[0].pdata = sim->enable + i;
|
||||||
mjui_add(&sim->ui0, defFlag);
|
mjui_add(&sim->ui0, defFlag);
|
||||||
@@ -611,8 +606,6 @@ void makephysics(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// make rendering section of UI
|
// make rendering section of UI
|
||||||
void makerendering(mj::Simulate* sim, int oldstate) {
|
void makerendering(mj::Simulate* sim, int oldstate) {
|
||||||
int i, j;
|
|
||||||
|
|
||||||
mjuiDef defRendering[] = {
|
mjuiDef defRendering[] = {
|
||||||
{
|
{
|
||||||
mjITEM_SECTION,
|
mjITEM_SECTION,
|
||||||
@@ -665,7 +658,7 @@ void makerendering(mj::Simulate* sim, int oldstate) {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// add model cameras, up to UI limit
|
// add model cameras, up to UI limit
|
||||||
for (i=0; i<mjMIN(sim->m->ncam, mjMAXUIMULTI-2); i++) {
|
for (int i=0; i<mjMIN(sim->m->ncam, mjMAXUIMULTI-2); i++) {
|
||||||
// prepare name
|
// prepare name
|
||||||
char camname[mjMAXUITEXT] = "\n";
|
char camname[mjMAXUITEXT] = "\n";
|
||||||
if (sim->m->names[sim->m->name_camadr[i]]) {
|
if (sim->m->names[sim->m->name_camadr[i]]) {
|
||||||
@@ -691,10 +684,10 @@ void makerendering(mj::Simulate* sim, int oldstate) {
|
|||||||
{mjITEM_CHECKBYTE, "", 2, nullptr, ""},
|
{mjITEM_CHECKBYTE, "", 2, nullptr, ""},
|
||||||
{mjITEM_END}
|
{mjITEM_END}
|
||||||
};
|
};
|
||||||
for (i=0; i<mjNVISFLAG; i++) {
|
for (int i=0; i<mjNVISFLAG; i++) {
|
||||||
// set name, remove "&"
|
// set name, remove "&"
|
||||||
mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
|
mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
|
||||||
for (j=0; j<strlen(mjVISSTRING[i][0]); j++)
|
for (int j=0; j<strlen(mjVISSTRING[i][0]); j++)
|
||||||
if (mjVISSTRING[i][0][j]=='&') {
|
if (mjVISSTRING[i][0][j]=='&') {
|
||||||
mju_strncpy(
|
mju_strncpy(
|
||||||
defFlag[0].name+j, mjVISSTRING[i][0]+j+1, mju::sizeof_arr(defFlag[0].name)-j);
|
defFlag[0].name+j, mjVISSTRING[i][0]+j+1, mju::sizeof_arr(defFlag[0].name)-j);
|
||||||
@@ -718,7 +711,7 @@ void makerendering(mj::Simulate* sim, int oldstate) {
|
|||||||
mjui_add(&sim->ui0, defTree);
|
mjui_add(&sim->ui0, defTree);
|
||||||
|
|
||||||
mjui_add(&sim->ui0, defOpenGL);
|
mjui_add(&sim->ui0, defOpenGL);
|
||||||
for (i=0; i<mjNRNDFLAG; i++) {
|
for (int i=0; i<mjNRNDFLAG; i++) {
|
||||||
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
|
mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
|
||||||
if (mjRNDSTRING[i][2][0]) {
|
if (mjRNDSTRING[i][2][0]) {
|
||||||
mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
|
mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
|
||||||
@@ -788,7 +781,6 @@ void makegroup(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// make joint section of UI
|
// make joint section of UI
|
||||||
void makejoint(mj::Simulate* sim, int oldstate) {
|
void makejoint(mj::Simulate* sim, int oldstate) {
|
||||||
int i;
|
|
||||||
|
|
||||||
mjuiDef defJoint[] = {
|
mjuiDef defJoint[] = {
|
||||||
{mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"},
|
{mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"},
|
||||||
@@ -805,7 +797,7 @@ void makejoint(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// add scalar joints, exit if UI limit reached
|
// add scalar joints, exit if UI limit reached
|
||||||
int itemcnt = 0;
|
int itemcnt = 0;
|
||||||
for (i=0; i<sim->m->njnt && itemcnt<mjMAXUIITEM; i++)
|
for (int i=0; i<sim->m->njnt && itemcnt<mjMAXUIITEM; i++)
|
||||||
if ((sim->m->jnt_type[i]==mjJNT_HINGE || sim->m->jnt_type[i]==mjJNT_SLIDE)) {
|
if ((sim->m->jnt_type[i]==mjJNT_HINGE || sim->m->jnt_type[i]==mjJNT_SLIDE)) {
|
||||||
// skip if joint group is disabled
|
// skip if joint group is disabled
|
||||||
if (!sim->vopt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->jnt_group[i]))]) {
|
if (!sim->vopt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->jnt_group[i]))]) {
|
||||||
@@ -838,8 +830,6 @@ void makejoint(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// make control section of UI
|
// make control section of UI
|
||||||
void makecontrol(mj::Simulate* sim, int oldstate) {
|
void makecontrol(mj::Simulate* sim, int oldstate) {
|
||||||
int i;
|
|
||||||
|
|
||||||
mjuiDef defControl[] = {
|
mjuiDef defControl[] = {
|
||||||
{mjITEM_SECTION, "Control", oldstate, nullptr, "AC"},
|
{mjITEM_SECTION, "Control", oldstate, nullptr, "AC"},
|
||||||
{mjITEM_BUTTON, "Clear all", 2},
|
{mjITEM_BUTTON, "Clear all", 2},
|
||||||
@@ -856,7 +846,7 @@ void makecontrol(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// add controls, exit if UI limit reached (Clear button already added)
|
// add controls, exit if UI limit reached (Clear button already added)
|
||||||
int itemcnt = 1;
|
int itemcnt = 1;
|
||||||
for (i=0; i<sim->m->nu && itemcnt<mjMAXUIITEM; i++) {
|
for (int i=0; i<sim->m->nu && itemcnt<mjMAXUIITEM; i++) {
|
||||||
// skip if actuator group is disabled
|
// skip if actuator group is disabled
|
||||||
if (!sim->vopt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->actuator_group[i]))]) {
|
if (!sim->vopt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->m->actuator_group[i]))]) {
|
||||||
continue;
|
continue;
|
||||||
@@ -886,11 +876,9 @@ void makecontrol(mj::Simulate* sim, int oldstate) {
|
|||||||
|
|
||||||
// make model-dependent UI sections
|
// make model-dependent UI sections
|
||||||
void makesections(mj::Simulate* sim) {
|
void makesections(mj::Simulate* sim) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// get section open-close state, UI 0
|
// get section open-close state, UI 0
|
||||||
int oldstate0[NSECT0];
|
int oldstate0[NSECT0];
|
||||||
for (i=0; i<NSECT0; i++) {
|
for (int i=0; i<NSECT0; i++) {
|
||||||
oldstate0[i] = 0;
|
oldstate0[i] = 0;
|
||||||
if (sim->ui0.nsect>i) {
|
if (sim->ui0.nsect>i) {
|
||||||
oldstate0[i] = sim->ui0.sect[i].state;
|
oldstate0[i] = sim->ui0.sect[i].state;
|
||||||
@@ -899,7 +887,7 @@ void makesections(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// get section open-close state, UI 1
|
// get section open-close state, UI 1
|
||||||
int oldstate1[NSECT1];
|
int oldstate1[NSECT1];
|
||||||
for (i=0; i<NSECT1; i++) {
|
for (int i=0; i<NSECT1; i++) {
|
||||||
oldstate1[i] = 0;
|
oldstate1[i] = 0;
|
||||||
if (sim->ui1.nsect>i) {
|
if (sim->ui1.nsect>i) {
|
||||||
oldstate1[i] = sim->ui1.sect[i].state;
|
oldstate1[i] = sim->ui1.sect[i].state;
|
||||||
@@ -985,13 +973,11 @@ void copycamera(mj::Simulate* sim) {
|
|||||||
|
|
||||||
// update UI 0 when MuJoCo structures change (except for joint sliders)
|
// update UI 0 when MuJoCo structures change (except for joint sliders)
|
||||||
void updatesettings(mj::Simulate* sim) {
|
void updatesettings(mj::Simulate* sim) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// physics flags
|
// physics flags
|
||||||
for (i=0; i<mjNDISABLE; i++) {
|
for (int i=0; i<mjNDISABLE; i++) {
|
||||||
sim->disable[i] = ((sim->m->opt.disableflags & (1<<i)) !=0);
|
sim->disable[i] = ((sim->m->opt.disableflags & (1<<i)) !=0);
|
||||||
}
|
}
|
||||||
for (i=0; i<mjNENABLE; i++) {
|
for (int i=0; i<mjNENABLE; i++) {
|
||||||
sim->enable[i] = ((sim->m->opt.enableflags & (1<<i)) !=0);
|
sim->enable[i] = ((sim->m->opt.enableflags & (1<<i)) !=0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1094,7 +1080,6 @@ void uiEvent(mjuiState* state) {
|
|||||||
mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
|
mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
|
||||||
mjModel* m = sim->m;
|
mjModel* m = sim->m;
|
||||||
mjData* d = sim->d;
|
mjData* d = sim->d;
|
||||||
int i;
|
|
||||||
char err[200];
|
char err[200];
|
||||||
|
|
||||||
// call UI 0 if event is directed to it
|
// call UI 0 if event is directed to it
|
||||||
@@ -1202,22 +1187,25 @@ void uiEvent(mjuiState* state) {
|
|||||||
|
|
||||||
case 5: // Adjust key
|
case 5: // Adjust key
|
||||||
case 6: // Load key
|
case 6: // Load key
|
||||||
i = sim->key;
|
{
|
||||||
|
int i = sim->key;
|
||||||
d->time = m->key_time[i];
|
d->time = m->key_time[i];
|
||||||
mju_copy(d->qpos, m->key_qpos+i*m->nq, m->nq);
|
mju_copy(d->qpos, m->key_qpos + i * m->nq, m->nq);
|
||||||
mju_copy(d->qvel, m->key_qvel+i*m->nv, m->nv);
|
mju_copy(d->qvel, m->key_qvel + i * m->nv, m->nv);
|
||||||
mju_copy(d->act, m->key_act+i*m->na, m->na);
|
mju_copy(d->act, m->key_act + i * m->na, m->na);
|
||||||
mju_copy(d->mocap_pos, m->key_mpos+i*3*m->nmocap, 3*m->nmocap);
|
mju_copy(d->mocap_pos, m->key_mpos + i * 3 * m->nmocap, 3 * m->nmocap);
|
||||||
mju_copy(d->mocap_quat, m->key_mquat+i*4*m->nmocap, 4*m->nmocap);
|
mju_copy(d->mocap_quat, m->key_mquat + i * 4 * m->nmocap,
|
||||||
mju_copy(d->ctrl, m->key_ctrl+i*m->nu, m->nu);
|
4 * m->nmocap);
|
||||||
|
mju_copy(d->ctrl, m->key_ctrl + i * m->nu, m->nu);
|
||||||
mj_forward(m, d);
|
mj_forward(m, d);
|
||||||
profilerupdate(sim);
|
profilerupdate(sim);
|
||||||
sensorupdate(sim);
|
sensorupdate(sim);
|
||||||
updatesettings(sim);
|
updatesettings(sim);
|
||||||
break;
|
} break;
|
||||||
|
|
||||||
case 7: // Save key
|
case 7: // Save key
|
||||||
i = sim->key;
|
{
|
||||||
|
int i = sim->key;
|
||||||
m->key_time[i] = d->time;
|
m->key_time[i] = d->time;
|
||||||
mju_copy(m->key_qpos+i*m->nq, d->qpos, m->nq);
|
mju_copy(m->key_qpos+i*m->nq, d->qpos, m->nq);
|
||||||
mju_copy(m->key_qvel+i*m->nv, d->qvel, m->nv);
|
mju_copy(m->key_qvel+i*m->nv, d->qvel, m->nv);
|
||||||
@@ -1225,7 +1213,7 @@ void uiEvent(mjuiState* state) {
|
|||||||
mju_copy(m->key_mpos+i*3*m->nmocap, d->mocap_pos, 3*m->nmocap);
|
mju_copy(m->key_mpos+i*3*m->nmocap, d->mocap_pos, 3*m->nmocap);
|
||||||
mju_copy(m->key_mquat+i*4*m->nmocap, d->mocap_quat, 4*m->nmocap);
|
mju_copy(m->key_mquat+i*4*m->nmocap, d->mocap_quat, 4*m->nmocap);
|
||||||
mju_copy(m->key_ctrl+i*m->nu, d->ctrl, m->nu);
|
mju_copy(m->key_ctrl+i*m->nu, d->ctrl, m->nu);
|
||||||
break;
|
} break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1233,14 +1221,14 @@ void uiEvent(mjuiState* state) {
|
|||||||
else if (it && it->sectionid==SECT_PHYSICS) {
|
else if (it && it->sectionid==SECT_PHYSICS) {
|
||||||
// update disable flags in mjOption
|
// update disable flags in mjOption
|
||||||
m->opt.disableflags = 0;
|
m->opt.disableflags = 0;
|
||||||
for (i=0; i<mjNDISABLE; i++)
|
for (int i=0; i<mjNDISABLE; i++)
|
||||||
if (sim->disable[i]) {
|
if (sim->disable[i]) {
|
||||||
m->opt.disableflags |= (1<<i);
|
m->opt.disableflags |= (1<<i);
|
||||||
}
|
}
|
||||||
|
|
||||||
// update enable flags in mjOption
|
// update enable flags in mjOption
|
||||||
m->opt.enableflags = 0;
|
m->opt.enableflags = 0;
|
||||||
for (i=0; i<mjNENABLE; i++)
|
for (int i=0; i<mjNENABLE; i++)
|
||||||
if (sim->enable[i]) {
|
if (sim->enable[i]) {
|
||||||
m->opt.enableflags |= (1<<i);
|
m->opt.enableflags |= (1<<i);
|
||||||
}
|
}
|
||||||
|
|||||||
+12
-18
@@ -122,11 +122,9 @@ bool mjCComposite::AddDefaultJoint(char* error, int error_sz) {
|
|||||||
|
|
||||||
// set defaults, after reading top-level info and skin
|
// set defaults, after reading top-level info and skin
|
||||||
void mjCComposite::SetDefault(void) {
|
void mjCComposite::SetDefault(void) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// determine dimensionality
|
// determine dimensionality
|
||||||
int tmpdim = 0;
|
int tmpdim = 0;
|
||||||
for (i=0; i<3; i++) {
|
for (int i=0; i<3; i++) {
|
||||||
if (count[i]>1) {
|
if (count[i]>1) {
|
||||||
tmpdim++;
|
tmpdim++;
|
||||||
}
|
}
|
||||||
@@ -149,7 +147,7 @@ void mjCComposite::SetDefault(void) {
|
|||||||
type==mjCOMPTYPE_LOOP ||
|
type==mjCOMPTYPE_LOOP ||
|
||||||
type==mjCOMPTYPE_CABLE ||
|
type==mjCOMPTYPE_CABLE ||
|
||||||
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
|
(type==mjCOMPTYPE_GRID && tmpdim==1)) {
|
||||||
for (i=0; i<mjNCOMPKINDS; i++) {
|
for (int i=0; i<mjNCOMPKINDS; i++) {
|
||||||
def[i].geom.group = 0;
|
def[i].geom.group = 0;
|
||||||
def[i].tendon.group = 0;
|
def[i].tendon.group = 0;
|
||||||
}
|
}
|
||||||
@@ -196,7 +194,7 @@ void mjCComposite::SetDefault(void) {
|
|||||||
AdjustSoft(solrefsmooth, solimpsmooth, 1);
|
AdjustSoft(solrefsmooth, solimpsmooth, 1);
|
||||||
|
|
||||||
// soft fix everywhere
|
// soft fix everywhere
|
||||||
for (i=0; i<mjNCOMPKINDS; i++) {
|
for (int i=0; i<mjNCOMPKINDS; i++) {
|
||||||
AdjustSoft(def[i].equality.solref, def[i].equality.solimp, 1);
|
AdjustSoft(def[i].equality.solref, def[i].equality.solimp, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1173,7 +1171,6 @@ void mjCComposite::BoxProject(double* pos) {
|
|||||||
|
|
||||||
// make 3d box, ellipsoid or cylinder
|
// make 3d box, ellipsoid or cylinder
|
||||||
bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz) {
|
bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int error_sz) {
|
||||||
int ix, iy, iz;
|
|
||||||
char txt[100];
|
char txt[100];
|
||||||
|
|
||||||
// check dim
|
// check dim
|
||||||
@@ -1199,9 +1196,9 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro
|
|||||||
ten->name = txt;
|
ten->name = txt;
|
||||||
|
|
||||||
// create bodies, geoms and joints: outside shell only
|
// create bodies, geoms and joints: outside shell only
|
||||||
for (ix=0; ix<count[0]; ix++) {
|
for (int ix=0; ix<count[0]; ix++) {
|
||||||
for (iy=0; iy<count[1]; iy++) {
|
for (int iy=0; iy<count[1]; iy++) {
|
||||||
for (iz=0; iz<count[2]; iz++) {
|
for (int iz=0; iz<count[2]; iz++) {
|
||||||
if (ix==0 || ix==count[0]-1 ||
|
if (ix==0 || ix==count[0]-1 ||
|
||||||
iy==0 || iy==count[1]-1 ||
|
iy==0 || iy==count[1]-1 ||
|
||||||
iz==0 || iz==count[2]-1) {
|
iz==0 || iz==count[2]-1) {
|
||||||
@@ -1297,11 +1294,10 @@ bool mjCComposite::MakeBox(mjCModel* model, mjCBody* body, char* error, int erro
|
|||||||
|
|
||||||
// add shear tendons to 2D
|
// add shear tendons to 2D
|
||||||
void mjCComposite::MakeShear(mjCModel* model) {
|
void mjCComposite::MakeShear(mjCModel* model) {
|
||||||
int ix, iy;
|
|
||||||
char txt[100], txt1[100], txt2[100];
|
char txt[100], txt1[100], txt2[100];
|
||||||
|
|
||||||
for (ix=0; ix<count[0]-1; ix++) {
|
for (int ix=0; ix<count[0]-1; ix++) {
|
||||||
for (iy=0; iy<count[1]-1; iy++) {
|
for (int iy=0; iy<count[1]-1; iy++) {
|
||||||
// recover site names
|
// recover site names
|
||||||
mju::sprintf_arr(txt1, "%sS%d_%d", prefix.c_str(), ix, iy);
|
mju::sprintf_arr(txt1, "%sS%d_%d", prefix.c_str(), ix, iy);
|
||||||
mju::sprintf_arr(txt2, "%sS%d_%d", prefix.c_str(), ix+1, iy+1);
|
mju::sprintf_arr(txt2, "%sS%d_%d", prefix.c_str(), ix+1, iy+1);
|
||||||
@@ -2188,12 +2184,11 @@ void mjCComposite::MakeSkin3(mjCModel* model) {
|
|||||||
// make one face of 3D skin, box
|
// make one face of 3D skin, box
|
||||||
void mjCComposite::MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side,
|
void mjCComposite::MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side,
|
||||||
int& vcnt, const char* format) {
|
int& vcnt, const char* format) {
|
||||||
int i0, i1;
|
|
||||||
char txt[100];
|
char txt[100];
|
||||||
|
|
||||||
// loop over bodies/vertices of specified face
|
// loop over bodies/vertices of specified face
|
||||||
for (i0=0; i0<c0; i0++) {
|
for (int i0=0; i0<c0; i0++) {
|
||||||
for (i1=0; i1<c1; i1++) {
|
for (int i1=0; i1<c1; i1++) {
|
||||||
// vertex
|
// vertex
|
||||||
skin->vert.push_back(0);
|
skin->vert.push_back(0);
|
||||||
skin->vert.push_back(0);
|
skin->vert.push_back(0);
|
||||||
@@ -2248,12 +2243,11 @@ void mjCComposite::MakeSkin3Box(mjCSkin* skin, int c0, int c1, int side,
|
|||||||
// make one face of 3D skin, smooth
|
// make one face of 3D skin, smooth
|
||||||
void mjCComposite::MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
|
void mjCComposite::MakeSkin3Smooth(mjCSkin* skin, int c0, int c1, int side,
|
||||||
const std::map<string, int>& vmap, const char* format) {
|
const std::map<string, int>& vmap, const char* format) {
|
||||||
int i0, i1;
|
|
||||||
char txt00[100], txt01[100], txt10[100], txt11[100];
|
char txt00[100], txt01[100], txt10[100], txt11[100];
|
||||||
|
|
||||||
// loop over bodies/vertices of specified face
|
// loop over bodies/vertices of specified face
|
||||||
for (i0=0; i0<c0; i0++) {
|
for (int i0=0; i0<c0; i0++) {
|
||||||
for (i1=0; i1<c1; i1++) {
|
for (int i1=0; i1<c1; i1++) {
|
||||||
// body names
|
// body names
|
||||||
mju::sprintf_arr(txt00, format, prefix.c_str(), i0, i1);
|
mju::sprintf_arr(txt00, format, prefix.c_str(), i0, i1);
|
||||||
mju::sprintf_arr(txt01, format, prefix.c_str(), i0, i1+1);
|
mju::sprintf_arr(txt01, format, prefix.c_str(), i0, i1+1);
|
||||||
|
|||||||
+61
-69
@@ -387,8 +387,6 @@ double* mjCMesh::GetQuatPtr(mjtMeshType type) {
|
|||||||
|
|
||||||
// set geom size to match mesh
|
// set geom size to match mesh
|
||||||
void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// copy mesh pos into meshpos
|
// copy mesh pos into meshpos
|
||||||
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
|
mjuu_copyvec(meshpos, GetPosPtr(geom->typeinertia), 3);
|
||||||
|
|
||||||
@@ -438,7 +436,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
|||||||
case mjGEOM_SPHERE:
|
case mjGEOM_SPHERE:
|
||||||
// find maximum distance
|
// find maximum distance
|
||||||
geom->size[0] = 0;
|
geom->size[0] = 0;
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
||||||
double dst = mjuu_dist3(v, cen);
|
double dst = mjuu_dist3(v, cen);
|
||||||
geom->size[0] = mjMAX(geom->size[0], dst);
|
geom->size[0] = mjMAX(geom->size[0], dst);
|
||||||
@@ -450,7 +448,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
|||||||
// find maximum distance in XY, separately in Z
|
// find maximum distance in XY, separately in Z
|
||||||
geom->size[0] = 0;
|
geom->size[0] = 0;
|
||||||
geom->size[1] = 0;
|
geom->size[1] = 0;
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
||||||
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
||||||
(v[1]-cen[1])*(v[1]-cen[1]));
|
(v[1]-cen[1])*(v[1]-cen[1]));
|
||||||
@@ -464,7 +462,7 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
|
|||||||
// special handling of capsule: consider curved cap
|
// special handling of capsule: consider curved cap
|
||||||
if (geom->type==mjGEOM_CAPSULE) {
|
if (geom->type==mjGEOM_CAPSULE) {
|
||||||
geom->size[1] = 0;
|
geom->size[1] = 0;
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
// get distance in XY and Z
|
// get distance in XY and Z
|
||||||
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
double v[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
||||||
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
double dst = sqrt((v[0]-cen[0])*(v[0]-cen[0]) +
|
||||||
@@ -515,13 +513,12 @@ quicksortfunc(vertcompare, context, el1, el2) {
|
|||||||
|
|
||||||
// remove repeated vertices
|
// remove repeated vertices
|
||||||
void mjCMesh::RemoveRepeated() {
|
void mjCMesh::RemoveRepeated() {
|
||||||
int i, j;
|
|
||||||
int repeated = 0;
|
int repeated = 0;
|
||||||
|
|
||||||
// allocate sort and redirection indices, set to identity
|
// allocate sort and redirection indices, set to identity
|
||||||
auto index = std::unique_ptr<int[]>(new int[nvert]);
|
auto index = std::unique_ptr<int[]>(new int[nvert]);
|
||||||
auto redirect = std::unique_ptr<int[]>(new int[nvert]);
|
auto redirect = std::unique_ptr<int[]>(new int[nvert]);
|
||||||
for (i=0; i < nvert; i++) {
|
for (int i=0; i < nvert; i++) {
|
||||||
index[i] = redirect[i] = i;
|
index[i] = redirect[i] = i;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -529,7 +526,7 @@ void mjCMesh::RemoveRepeated() {
|
|||||||
mjQUICKSORT(index.get(), nvert, sizeof(int), vertcompare, vert);
|
mjQUICKSORT(index.get(), nvert, sizeof(int), vertcompare, vert);
|
||||||
|
|
||||||
// find repeated vertices, set redirect
|
// find repeated vertices, set redirect
|
||||||
for (i=1; i < nvert; i++) {
|
for (int i=1; i < nvert; i++) {
|
||||||
if (vert[3*index[i]] == vert[3*index[i-1]] &&
|
if (vert[3*index[i]] == vert[3*index[i-1]] &&
|
||||||
vert[3*index[i]+1] == vert[3*index[i-1]+1] &&
|
vert[3*index[i]+1] == vert[3*index[i-1]+1] &&
|
||||||
vert[3*index[i]+2] == vert[3*index[i-1]+2]) {
|
vert[3*index[i]+2] == vert[3*index[i-1]+2]) {
|
||||||
@@ -541,8 +538,8 @@ void mjCMesh::RemoveRepeated() {
|
|||||||
// compress vertices, change face data
|
// compress vertices, change face data
|
||||||
if (repeated) {
|
if (repeated) {
|
||||||
// track redirections until non-redirected vertex, set
|
// track redirections until non-redirected vertex, set
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
j = i;
|
int j = i;
|
||||||
while (redirect[j]!=j) {
|
while (redirect[j]!=j) {
|
||||||
j = redirect[j];
|
j = redirect[j];
|
||||||
}
|
}
|
||||||
@@ -550,8 +547,8 @@ void mjCMesh::RemoveRepeated() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// find good vertices, compress, reuse index to save compressed position
|
// find good vertices, compress, reuse index to save compressed position
|
||||||
j = 0;
|
int j = 0;
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
if (redirect[i]==i) {
|
if (redirect[i]==i) {
|
||||||
index[i] = j;
|
index[i] = j;
|
||||||
memcpy(vert+3*j, vert+3*i, 3*sizeof(float));
|
memcpy(vert+3*j, vert+3*i, 3*sizeof(float));
|
||||||
@@ -562,7 +559,7 @@ void mjCMesh::RemoveRepeated() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// recompute face data to reflect compressed vertices
|
// recompute face data to reflect compressed vertices
|
||||||
for (i=0; i<3*nface; i++) {
|
for (int i=0; i<3*nface; i++) {
|
||||||
face[i] = index[redirect[face[i]]];
|
face[i] = index[redirect[face[i]]];
|
||||||
|
|
||||||
// sanity check, SHOULD NOT OCCUR
|
// sanity check, SHOULD NOT OCCUR
|
||||||
@@ -908,9 +905,8 @@ void mjCMesh::LoadMSH(const mjVFS* vfs) {
|
|||||||
|
|
||||||
// rearange face data if left-handed scaling
|
// rearange face data if left-handed scaling
|
||||||
if (nface && !righthand) {
|
if (nface && !righthand) {
|
||||||
int i, tmp;
|
for (int i=0; i<nface; i++) {
|
||||||
for (i=0; i<nface; i++) {
|
int tmp = face[3*i+1];
|
||||||
tmp = face[3*i+1];
|
|
||||||
face[3*i+1] = face[3*i+2];
|
face[3*i+1] = face[3*i+2];
|
||||||
face[3*i+2] = tmp;
|
face[3*i+2] = tmp;
|
||||||
}
|
}
|
||||||
@@ -932,7 +928,6 @@ void mjCMesh::Process() {
|
|||||||
double area = 0;
|
double area = 0;
|
||||||
double inert[6] = {0, 0, 0, 0, 0, 0};
|
double inert[6] = {0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
int i, j;
|
|
||||||
double nrm[3];
|
double nrm[3];
|
||||||
double cen[3];
|
double cen[3];
|
||||||
|
|
||||||
@@ -943,7 +938,7 @@ void mjCMesh::Process() {
|
|||||||
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
|
float rp[3] = {(float)refpos[0], (float)refpos[1], (float)refpos[2]};
|
||||||
|
|
||||||
// process vertices
|
// process vertices
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
vert[3*i] -= rp[0];
|
vert[3*i] -= rp[0];
|
||||||
vert[3*i+1] -= rp[1];
|
vert[3*i+1] -= rp[1];
|
||||||
vert[3*i+2] -= rp[2];
|
vert[3*i+2] -= rp[2];
|
||||||
@@ -959,7 +954,7 @@ void mjCMesh::Process() {
|
|||||||
mju_quat2Mat(mat, quat);
|
mju_quat2Mat(mat, quat);
|
||||||
|
|
||||||
// process vertices
|
// process vertices
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
mjtNum p1[3], p0[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
||||||
mju_rotVecMatT(p1, p0, mat);
|
mju_rotVecMatT(p1, p0, mat);
|
||||||
vert[3*i] = (float) p1[0];
|
vert[3*i] = (float) p1[0];
|
||||||
@@ -968,7 +963,7 @@ void mjCMesh::Process() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// process normals
|
// process normals
|
||||||
for (i=0; i<nnormal; i++) {
|
for (int i=0; i<nnormal; i++) {
|
||||||
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
|
mjtNum n1[3], n0[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
|
||||||
mju_rotVecMatT(n1, n0, mat);
|
mju_rotVecMatT(n1, n0, mat);
|
||||||
normal[3*i] = (float) n1[0];
|
normal[3*i] = (float) n1[0];
|
||||||
@@ -979,13 +974,13 @@ void mjCMesh::Process() {
|
|||||||
|
|
||||||
// scale
|
// scale
|
||||||
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
|
if (scale[0]!=1 || scale[1]!=1 || scale[2]!=1) {
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
vert[3*i] *= scale[0];
|
vert[3*i] *= scale[0];
|
||||||
vert[3*i+1] *= scale[1];
|
vert[3*i+1] *= scale[1];
|
||||||
vert[3*i+2] *= scale[2];
|
vert[3*i+2] *= scale[2];
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i=0; i<nnormal; i++) {
|
for (int i=0; i<nnormal; i++) {
|
||||||
normal[3*i] *= scale[0];
|
normal[3*i] *= scale[0];
|
||||||
normal[3*i+1] *= scale[1];
|
normal[3*i+1] *= scale[1];
|
||||||
normal[3*i+2] *= scale[2];
|
normal[3*i+2] *= scale[2];
|
||||||
@@ -993,7 +988,7 @@ void mjCMesh::Process() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// normalize normals
|
// normalize normals
|
||||||
for (i=0; i<nnormal; i++) {
|
for (int i=0; i<nnormal; i++) {
|
||||||
// compute length
|
// compute length
|
||||||
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
|
float len = normal[3*i]*normal[3*i] + normal[3*i+1]*normal[3*i+1] + normal[3*i+2]*normal[3*i+2];
|
||||||
|
|
||||||
@@ -1011,9 +1006,9 @@ void mjCMesh::Process() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// find centroid of faces
|
// find centroid of faces
|
||||||
for (i=0; i<nface; i++) {
|
for (int i=0; i<nface; i++) {
|
||||||
// check vertex indices
|
// check vertex indices
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
|
if (face[3*i+j]<0 || face[3*i+j]>=nvert) {
|
||||||
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
|
throw mjCError(this, "vertex index out of range in %s (index = %d)", name.c_str(), i);
|
||||||
}
|
}
|
||||||
@@ -1023,7 +1018,7 @@ void mjCMesh::Process() {
|
|||||||
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
|
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
|
||||||
|
|
||||||
// accumulate
|
// accumulate
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
facecen[j] += a*cen[j];
|
facecen[j] += a*cen[j];
|
||||||
}
|
}
|
||||||
area += a;
|
area += a;
|
||||||
@@ -1036,14 +1031,14 @@ void mjCMesh::Process() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// finalize centroid of faces
|
// finalize centroid of faces
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
facecen[j] /= area;
|
facecen[j] /= area;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// compute CoM and volume from pyramid volumes
|
// compute CoM and volume from pyramid volumes
|
||||||
GetVolumeRef(type) = 0;
|
GetVolumeRef(type) = 0;
|
||||||
for (i=0; i<nface; i++) {
|
for (int i=0; i<nface; i++) {
|
||||||
// get area, normal and center
|
// get area, normal and center
|
||||||
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
|
double a = _triangle(nrm, cen, vert+3*face[3*i], vert+3*face[3*i+1], vert+3*face[3*i+2]);
|
||||||
|
|
||||||
@@ -1058,7 +1053,7 @@ void mjCMesh::Process() {
|
|||||||
|
|
||||||
// add pyramid com
|
// add pyramid com
|
||||||
GetVolumeRef(type) += vol;
|
GetVolumeRef(type) += vol;
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
|
CoM[j] += vol*(cen[j]*3.0/4.0 + facecen[j]/4.0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1070,15 +1065,15 @@ void mjCMesh::Process() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// finalize CoM, save as mesh center
|
// finalize CoM, save as mesh center
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
CoM[j] /= GetVolumeRef(type);
|
CoM[j] /= GetVolumeRef(type);
|
||||||
}
|
}
|
||||||
mjuu_copyvec(GetPosPtr(type), CoM, 3);
|
mjuu_copyvec(GetPosPtr(type), CoM, 3);
|
||||||
|
|
||||||
// re-center mesh at CoM
|
// re-center mesh at CoM
|
||||||
if (type==mjVOLUME_MESH) {
|
if (type==mjVOLUME_MESH) {
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vert[3*i+j] -= CoM[j];
|
vert[3*i+j] -= CoM[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1088,7 +1083,7 @@ void mjCMesh::Process() {
|
|||||||
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
|
const int k[6][2] = {{0, 0}, {1, 1}, {2, 2}, {0, 1}, {0, 2}, {1, 2}};
|
||||||
double P[6] = {0, 0, 0, 0, 0, 0};
|
double P[6] = {0, 0, 0, 0, 0, 0};
|
||||||
GetVolumeRef(type) = 0;
|
GetVolumeRef(type) = 0;
|
||||||
for (i=0; i<nface; i++) {
|
for (int i=0; i<nface; i++) {
|
||||||
float* D = vert+3*face[3*i];
|
float* D = vert+3*face[3*i];
|
||||||
float* E = vert+3*face[3*i+1];
|
float* E = vert+3*face[3*i+1];
|
||||||
float* F = vert+3*face[3*i+2];
|
float* F = vert+3*face[3*i+2];
|
||||||
@@ -1104,7 +1099,7 @@ void mjCMesh::Process() {
|
|||||||
|
|
||||||
// apply formula, accumulate
|
// apply formula, accumulate
|
||||||
GetVolumeRef(type) += vol;
|
GetVolumeRef(type) += vol;
|
||||||
for (j=0; j<6; j++) {
|
for (int j=0; j<6; j++) {
|
||||||
P[j] += def->geom.density*vol /
|
P[j] += def->geom.density*vol /
|
||||||
(type==mjSHELL_MESH ? 12 : 20) * (
|
(type==mjSHELL_MESH ? 12 : 20) * (
|
||||||
2*(D[k[j][0]] * D[k[j][1]] +
|
2*(D[k[j][0]] * D[k[j][1]] +
|
||||||
@@ -1153,7 +1148,7 @@ void mjCMesh::Process() {
|
|||||||
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
|
boxsz[2] = sqrt(6*(eigval[0]+eigval[1]-eigval[2])/mass)/2;
|
||||||
|
|
||||||
// copy quat
|
// copy quat
|
||||||
for (j=0; j<4; j++) {
|
for (int j=0; j<4; j++) {
|
||||||
GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j];
|
GetQuatPtr(type)[j] = type == mjVOLUME_MESH ? quattmp[j] : GetQuatPtr(mjVOLUME_MESH)[j];
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1162,12 +1157,12 @@ void mjCMesh::Process() {
|
|||||||
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
|
double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]};
|
||||||
double mat[9];
|
double mat[9];
|
||||||
mjuu_quat2mat(mat, neg);
|
mjuu_quat2mat(mat, neg);
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
// vertices
|
// vertices
|
||||||
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
const double vec[3] = {vert[3*i], vert[3*i+1], vert[3*i+2]};
|
||||||
double res[3];
|
double res[3];
|
||||||
mjuu_mulvecmat(res, vec, mat);
|
mjuu_mulvecmat(res, vec, mat);
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vert[3*i+j] = (float) res[j];
|
vert[3*i+j] = (float) res[j];
|
||||||
|
|
||||||
// axis-aligned bounding box
|
// axis-aligned bounding box
|
||||||
@@ -1175,12 +1170,12 @@ void mjCMesh::Process() {
|
|||||||
aabb[j+3] = mjMAX(aabb[j+3], res[j]);
|
aabb[j+3] = mjMAX(aabb[j+3], res[j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (i=0; i<nnormal; i++) {
|
for (int i=0; i<nnormal; i++) {
|
||||||
// normals
|
// normals
|
||||||
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
|
const double nrm[3] = {normal[3*i], normal[3*i+1], normal[3*i+2]};
|
||||||
double res[3];
|
double res[3];
|
||||||
mjuu_mulvecmat(res, nrm, mat);
|
mjuu_mulvecmat(res, nrm, mat);
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
normal[3*i+j] = (float) res[j];
|
normal[3*i+j] = (float) res[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1225,7 +1220,7 @@ double& mjCMesh::GetVolumeRef(mjtMeshType type) {
|
|||||||
|
|
||||||
// make graph describing convex hull
|
// make graph describing convex hull
|
||||||
void mjCMesh::MakeGraph(void) {
|
void mjCMesh::MakeGraph(void) {
|
||||||
int i, adr, ok, curlong, totlong, exitcode;
|
int adr, ok, curlong, totlong, exitcode;
|
||||||
double* data;
|
double* data;
|
||||||
facetT* facet, **facetp;
|
facetT* facet, **facetp;
|
||||||
vertexT* vertex, *vertex1, **vertex1p;
|
vertexT* vertex, *vertex1, **vertex1p;
|
||||||
@@ -1241,7 +1236,7 @@ void mjCMesh::MakeGraph(void) {
|
|||||||
if (!data) {
|
if (!data) {
|
||||||
throw mjCError(this, "could not allocate data for qhull");
|
throw mjCError(this, "could not allocate data for qhull");
|
||||||
}
|
}
|
||||||
for (i=0; i<3*nvert; i++) {
|
for (int i=0; i<3*nvert; i++) {
|
||||||
data[i] = (double)vert[i];
|
data[i] = (double)vert[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1282,7 +1277,7 @@ void mjCMesh::MakeGraph(void) {
|
|||||||
int* face_globalid = graph + 2 + 3*numvert + 3*numface;
|
int* face_globalid = graph + 2 + 3*numvert + 3*numface;
|
||||||
|
|
||||||
// fill in graph data
|
// fill in graph data
|
||||||
i = adr = 0;
|
int i = adr = 0;
|
||||||
ok = 1;
|
ok = 1;
|
||||||
FORALLvertices {
|
FORALLvertices {
|
||||||
// point id of this vertex, check
|
// point id of this vertex, check
|
||||||
@@ -1380,9 +1375,10 @@ void mjCMesh::MakeGraph(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// replace global ids with local ids in edge data
|
// replace global ids with local ids in edge data
|
||||||
for (i=0; i<numvert+3*numface; i++) {
|
for (int i=0; i<numvert+3*numface; i++) {
|
||||||
if (edge_localid[i]>=0) {
|
if (edge_localid[i]>=0) {
|
||||||
// search vert_globalid for match
|
// search vert_globalid for match
|
||||||
|
int adr;
|
||||||
for (adr=0; adr<numvert; adr++) {
|
for (adr=0; adr<numvert; adr++) {
|
||||||
if (vert_globalid[adr]==edge_localid[i]) {
|
if (vert_globalid[adr]==edge_localid[i]) {
|
||||||
edge_localid[i] = adr;
|
edge_localid[i] = adr;
|
||||||
@@ -1428,10 +1424,9 @@ void mjCMesh::CopyGraph(void) {
|
|||||||
face = (int*) mju_malloc(3*nface*sizeof(int));
|
face = (int*) mju_malloc(3*nface*sizeof(int));
|
||||||
|
|
||||||
// copy faces
|
// copy faces
|
||||||
int i, j;
|
for (int i=0; i<nface; i++) {
|
||||||
for (i=0; i<nface; i++) {
|
|
||||||
// address in graph
|
// address in graph
|
||||||
j = 2 + 3*numvert + 3*nface + 3*i;
|
int j = 2 + 3*numvert + 3*nface + 3*i;
|
||||||
|
|
||||||
// copy
|
// copy
|
||||||
face[3*i] = graph[j];
|
face[3*i] = graph[j];
|
||||||
@@ -1444,8 +1439,6 @@ void mjCMesh::CopyGraph(void) {
|
|||||||
|
|
||||||
// compute vertex normals
|
// compute vertex normals
|
||||||
void mjCMesh::MakeNormal(void) {
|
void mjCMesh::MakeNormal(void) {
|
||||||
int i, j, k;
|
|
||||||
|
|
||||||
// only if normal data is missing
|
// only if normal data is missing
|
||||||
if (normal) {
|
if (normal) {
|
||||||
return;
|
return;
|
||||||
@@ -1462,16 +1455,16 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// loop over faces, accumulate vertex normals
|
// loop over faces, accumulate vertex normals
|
||||||
for (i=0; i<nface; i++) {
|
for (int i=0; i<nface; i++) {
|
||||||
// get vertex ids
|
// get vertex ids
|
||||||
int vertid[3];
|
int vertid[3];
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vertid[j] = face[3*i+j];
|
vertid[j] = face[3*i+j];
|
||||||
}
|
}
|
||||||
|
|
||||||
// get triangle edges
|
// get triangle edges
|
||||||
mjtNum vec01[3], vec02[3];
|
mjtNum vec01[3], vec02[3];
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vec01[j] = vert[3*vertid[1]+j] - vert[3*vertid[0]+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];
|
vec02[j] = vert[3*vertid[2]+j] - vert[3*vertid[0]+j];
|
||||||
}
|
}
|
||||||
@@ -1482,8 +1475,8 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
mjtNum area = mju_normalize3(nrm);
|
mjtNum area = mju_normalize3(nrm);
|
||||||
|
|
||||||
// add normal to each vertex with weight = area
|
// add normal to each vertex with weight = area
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
for (k=0; k<3; k++) {
|
for (int k=0; k<3; k++) {
|
||||||
normal[3*vertid[j]+k] += nrm[k]*area;
|
normal[3*vertid[j]+k] += nrm[k]*area;
|
||||||
}
|
}
|
||||||
facenormal[3*i+j] = vertid[j];
|
facenormal[3*i+j] = vertid[j];
|
||||||
@@ -1497,16 +1490,16 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
memset(nremove, 0, 3*nnormal*sizeof(float));
|
memset(nremove, 0, 3*nnormal*sizeof(float));
|
||||||
|
|
||||||
// remove contributions from faces at large angles with vertex normal
|
// remove contributions from faces at large angles with vertex normal
|
||||||
for (i=0; i<nface; i++) {
|
for (int i=0; i<nface; i++) {
|
||||||
// get vertex ids
|
// get vertex ids
|
||||||
int vertid[3];
|
int vertid[3];
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vertid[j] = face[3*i+j];
|
vertid[j] = face[3*i+j];
|
||||||
}
|
}
|
||||||
|
|
||||||
// get triangle edges
|
// get triangle edges
|
||||||
mjtNum vec01[3], vec02[3];
|
mjtNum vec01[3], vec02[3];
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
vec01[j] = vert[3*vertid[1]+j] - vert[3*vertid[0]+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];
|
vec02[j] = vert[3*vertid[2]+j] - vert[3*vertid[0]+j];
|
||||||
}
|
}
|
||||||
@@ -1517,14 +1510,14 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
mjtNum area = mju_normalize3(nrm);
|
mjtNum area = mju_normalize3(nrm);
|
||||||
|
|
||||||
// compare to vertex normal, subtract contribution if dot product too small
|
// compare to vertex normal, subtract contribution if dot product too small
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
// normalized vertex normal
|
// normalized vertex normal
|
||||||
mjtNum vnrm[3] = {normal[3*vertid[j]], normal[3*vertid[j]+1], normal[3*vertid[j]+2]};
|
mjtNum vnrm[3] = {normal[3*vertid[j]], normal[3*vertid[j]+1], normal[3*vertid[j]+2]};
|
||||||
mju_normalize3(vnrm);
|
mju_normalize3(vnrm);
|
||||||
|
|
||||||
// dot too small: remove
|
// dot too small: remove
|
||||||
if (mju_dot3(nrm, vnrm)<0.8) {
|
if (mju_dot3(nrm, vnrm)<0.8) {
|
||||||
for (k=0; k<3; k++) {
|
for (int k=0; k<3; k++) {
|
||||||
nremove[3*vertid[j]+k] += nrm[k]*area;
|
nremove[3*vertid[j]+k] += nrm[k]*area;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1532,14 +1525,14 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// apply removal, free nremove
|
// apply removal, free nremove
|
||||||
for (i=0; i<3*nnormal; i++) {
|
for (int i=0; i<3*nnormal; i++) {
|
||||||
normal[i] -= nremove[i];
|
normal[i] -= nremove[i];
|
||||||
}
|
}
|
||||||
mju_free(nremove);
|
mju_free(nremove);
|
||||||
}
|
}
|
||||||
|
|
||||||
// normalize normals
|
// normalize normals
|
||||||
for (i=0; i<nnormal; i++) {
|
for (int i=0; i<nnormal; i++) {
|
||||||
// compute length
|
// compute length
|
||||||
float len = sqrtf(normal[3*i]*normal[3*i] +
|
float len = sqrtf(normal[3*i]*normal[3*i] +
|
||||||
normal[3*i+1]*normal[3*i+1] +
|
normal[3*i+1]*normal[3*i+1] +
|
||||||
@@ -1547,7 +1540,7 @@ void mjCMesh::MakeNormal(void) {
|
|||||||
|
|
||||||
// divide by length
|
// divide by length
|
||||||
if (len>mjMINVAL)
|
if (len>mjMINVAL)
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
normal[3*i+j] /= len;
|
normal[3*i+j] /= len;
|
||||||
} else {
|
} else {
|
||||||
normal[3*i] = normal[3*i+1] = 0;
|
normal[3*i] = normal[3*i+1] = 0;
|
||||||
@@ -1608,7 +1601,6 @@ mjCSkin::~mjCSkin() {
|
|||||||
|
|
||||||
// compiler
|
// compiler
|
||||||
void mjCSkin::Compile(const mjVFS* vfs) {
|
void mjCSkin::Compile(const mjVFS* vfs) {
|
||||||
size_t i, j;
|
|
||||||
|
|
||||||
// load file
|
// load file
|
||||||
if (!file.empty()) {
|
if (!file.empty()) {
|
||||||
@@ -1678,7 +1670,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
|
|||||||
|
|
||||||
// resolve body names
|
// resolve body names
|
||||||
bodyid.resize(nbone);
|
bodyid.resize(nbone);
|
||||||
for (i=0; i<nbone; i++) {
|
for (int i=0; i<nbone; i++) {
|
||||||
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname[i]);
|
mjCBase* pbody = model->FindObject(mjOBJ_BODY, bodyname[i]);
|
||||||
if (!pbody) {
|
if (!pbody) {
|
||||||
throw mjCError(this, "unknown body '%s' in skin", bodyname[i].c_str());
|
throw mjCError(this, "unknown body '%s' in skin", bodyname[i].c_str());
|
||||||
@@ -1701,7 +1693,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
|
|||||||
fill(vw.begin(), vw.end(), 0.0f);
|
fill(vw.begin(), vw.end(), 0.0f);
|
||||||
|
|
||||||
// accumulate vertex weights from all bones
|
// accumulate vertex weights from all bones
|
||||||
for (i=0; i<nbone; i++) {
|
for (int i=0; i<nbone; i++) {
|
||||||
// make sure bone has vertices and sizes match
|
// make sure bone has vertices and sizes match
|
||||||
size_t nbv = vertid[i].size();
|
size_t nbv = vertid[i].size();
|
||||||
if (vertweight[i].size()!=nbv || nbv==0) {
|
if (vertweight[i].size()!=nbv || nbv==0) {
|
||||||
@@ -1709,7 +1701,7 @@ void mjCSkin::Compile(const mjVFS* vfs) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// accumulate weights in global array
|
// accumulate weights in global array
|
||||||
for (j=0; j<nbv; j++) {
|
for (int j=0; j<nbv; j++) {
|
||||||
// get index and check range
|
// get index and check range
|
||||||
int jj = vertid[i][j];
|
int jj = vertid[i][j];
|
||||||
if (jj<0 || jj>=nvert) {
|
if (jj<0 || jj>=nvert) {
|
||||||
@@ -1722,21 +1714,21 @@ void mjCSkin::Compile(const mjVFS* vfs) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// check coverage
|
// check coverage
|
||||||
for (i=0; i<nvert; i++) {
|
for (int i=0; i<nvert; i++) {
|
||||||
if (vw[i]<=mjMINVAL) {
|
if (vw[i]<=mjMINVAL) {
|
||||||
throw mjCError(this, "vertex %d must have positive total weight in skin", NULL, i);
|
throw mjCError(this, "vertex %d must have positive total weight in skin", NULL, i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// normalize vertex weights
|
// normalize vertex weights
|
||||||
for (i=0; i<nbone; i++) {
|
for (int i=0; i<nbone; i++) {
|
||||||
for (j=0; j<vertid[i].size(); j++) {
|
for (int j=0; j<vertid[i].size(); j++) {
|
||||||
vertweight[i][j] /= vw[vertid[i][j]];
|
vertweight[i][j] /= vw[vertid[i][j]];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// normalize bindquat
|
// normalize bindquat
|
||||||
for (i=0; i<nbone; i++) {
|
for (int i=0; i<nbone; i++) {
|
||||||
mjtNum quat[4] = {
|
mjtNum quat[4] = {
|
||||||
(mjtNum)bindquat[4*i],
|
(mjtNum)bindquat[4*i],
|
||||||
(mjtNum)bindquat[4*i+1],
|
(mjtNum)bindquat[4*i+1],
|
||||||
|
|||||||
+150
-168
@@ -181,29 +181,27 @@ mjCModel::mjCModel() {
|
|||||||
|
|
||||||
// destructor
|
// destructor
|
||||||
mjCModel::~mjCModel() {
|
mjCModel::~mjCModel() {
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// delete kinematic tree and all objects allocated in it
|
// delete kinematic tree and all objects allocated in it
|
||||||
delete bodies[0];
|
delete bodies[0];
|
||||||
|
|
||||||
// delete objects allocated in mjCModel
|
// delete objects allocated in mjCModel
|
||||||
for (i=0; i<meshes.size(); i++) delete meshes[i];
|
for (int i=0; i<meshes.size(); i++) delete meshes[i];
|
||||||
for (i=0; i<skins.size(); i++) delete skins[i];
|
for (int i=0; i<skins.size(); i++) delete skins[i];
|
||||||
for (i=0; i<hfields.size(); i++) delete hfields[i];
|
for (int i=0; i<hfields.size(); i++) delete hfields[i];
|
||||||
for (i=0; i<textures.size(); i++) delete textures[i];
|
for (int i=0; i<textures.size(); i++) delete textures[i];
|
||||||
for (i=0; i<materials.size(); i++) delete materials[i];
|
for (int i=0; i<materials.size(); i++) delete materials[i];
|
||||||
for (i=0; i<pairs.size(); i++) delete pairs[i];
|
for (int i=0; i<pairs.size(); i++) delete pairs[i];
|
||||||
for (i=0; i<excludes.size(); i++) delete excludes[i];
|
for (int i=0; i<excludes.size(); i++) delete excludes[i];
|
||||||
for (i=0; i<equalities.size(); i++) delete equalities[i];
|
for (int i=0; i<equalities.size(); i++) delete equalities[i];
|
||||||
for (i=0; i<tendons.size(); i++) delete tendons[i]; // also deletes wraps
|
for (int i=0; i<tendons.size(); i++) delete tendons[i]; // also deletes wraps
|
||||||
for (i=0; i<actuators.size(); i++) delete actuators[i];
|
for (int i=0; i<actuators.size(); i++) delete actuators[i];
|
||||||
for (i=0; i<sensors.size(); i++) delete sensors[i];
|
for (int i=0; i<sensors.size(); i++) delete sensors[i];
|
||||||
for (i=0; i<numerics.size(); i++) delete numerics[i];
|
for (int i=0; i<numerics.size(); i++) delete numerics[i];
|
||||||
for (i=0; i<texts.size(); i++) delete texts[i];
|
for (int i=0; i<texts.size(); i++) delete texts[i];
|
||||||
for (i=0; i<tuples.size(); i++) delete tuples[i];
|
for (int i=0; i<tuples.size(); i++) delete tuples[i];
|
||||||
for (i=0; i<keys.size(); i++) delete keys[i];
|
for (int i=0; i<keys.size(); i++) delete keys[i];
|
||||||
for (i=0; i<plugins.size(); i++) delete plugins[i];
|
for (int i=0; i<plugins.size(); i++) delete plugins[i];
|
||||||
for (i=0; i<defaults.size(); i++) delete defaults[i];
|
for (int i=0; i<defaults.size(); i++) delete defaults[i];
|
||||||
|
|
||||||
// clear pointer lists created in model construction
|
// clear pointer lists created in model construction
|
||||||
meshes.clear();
|
meshes.clear();
|
||||||
@@ -705,32 +703,28 @@ bool mjCModel::IsNullPose(const mjtNum* pos, const mjtNum* quat) {
|
|||||||
|
|
||||||
// make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights
|
// make lists of objects in tree: bodies, geoms, joints, sites, cameras, lights
|
||||||
void mjCModel::MakeLists(mjCBody* body) {
|
void mjCModel::MakeLists(mjCBody* body) {
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// add this body if not world
|
// add this body if not world
|
||||||
if (body!=bodies[0]) {
|
if (body!=bodies[0]) {
|
||||||
bodies.push_back(body);
|
bodies.push_back(body);
|
||||||
}
|
}
|
||||||
|
|
||||||
// add body's geoms, joints, sites, cameras, lights
|
// add body's geoms, joints, sites, cameras, lights
|
||||||
for (i=0; i<body->geoms.size(); i++) geoms.push_back(body->geoms[i]);
|
for (int i=0; i<body->geoms.size(); i++) geoms.push_back(body->geoms[i]);
|
||||||
for (i=0; i<body->joints.size(); i++) joints.push_back(body->joints[i]);
|
for (int i=0; i<body->joints.size(); i++) joints.push_back(body->joints[i]);
|
||||||
for (i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
|
for (int i=0; i<body->sites.size(); i++) sites.push_back(body->sites[i]);
|
||||||
for (i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
|
for (int i=0; i<body->cameras.size(); i++) cameras.push_back(body->cameras[i]);
|
||||||
for (i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
|
for (int i=0; i<body->lights.size(); i++) lights.push_back(body->lights[i]);
|
||||||
|
|
||||||
// recursive call to all child bodies
|
// recursive call to all child bodies
|
||||||
for (i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
|
for (int i=0; i<body->bodies.size(); i++) MakeLists(body->bodies[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// index assets
|
// index assets
|
||||||
void mjCModel::IndexAssets(void) {
|
void mjCModel::IndexAssets(void) {
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// assets referenced in geoms
|
// assets referenced in geoms
|
||||||
for (i=0; i<geoms.size(); i++) {
|
for (int i=0; i<geoms.size(); i++) {
|
||||||
mjCGeom* pgeom = geoms[i];
|
mjCGeom* pgeom = geoms[i];
|
||||||
|
|
||||||
// find material by name
|
// find material by name
|
||||||
@@ -765,7 +759,7 @@ void mjCModel::IndexAssets(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// assets referenced in skins
|
// assets referenced in skins
|
||||||
for (i=0; i<skins.size(); i++) {
|
for (int i=0; i<skins.size(); i++) {
|
||||||
mjCSkin* pskin = skins[i];
|
mjCSkin* pskin = skins[i];
|
||||||
|
|
||||||
// find material by name
|
// find material by name
|
||||||
@@ -780,7 +774,7 @@ void mjCModel::IndexAssets(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// materials referenced in sites
|
// materials referenced in sites
|
||||||
for (i=0; i<sites.size(); i++) {
|
for (int i=0; i<sites.size(); i++) {
|
||||||
mjCSite* psite = sites[i];
|
mjCSite* psite = sites[i];
|
||||||
|
|
||||||
// find material by name
|
// find material by name
|
||||||
@@ -795,7 +789,7 @@ void mjCModel::IndexAssets(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// materials referenced in tendons
|
// materials referenced in tendons
|
||||||
for (i=0; i<tendons.size(); i++) {
|
for (int i=0; i<tendons.size(); i++) {
|
||||||
mjCTendon* pten = tendons[i];
|
mjCTendon* pten = tendons[i];
|
||||||
|
|
||||||
// find material by name
|
// find material by name
|
||||||
@@ -810,7 +804,7 @@ void mjCModel::IndexAssets(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// textures referenced in materials
|
// textures referenced in materials
|
||||||
for (i=0; i<materials.size(); i++) {
|
for (int i=0; i<materials.size(); i++) {
|
||||||
mjCMaterial* pmat = materials[i];
|
mjCMaterial* pmat = materials[i];
|
||||||
|
|
||||||
// find texture by name
|
// find texture by name
|
||||||
@@ -830,10 +824,9 @@ void mjCModel::IndexAssets(void) {
|
|||||||
// if asset name is missing, set to filename
|
// if asset name is missing, set to filename
|
||||||
void mjCModel::SetDefaultNames(void) {
|
void mjCModel::SetDefaultNames(void) {
|
||||||
string stripped;
|
string stripped;
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// meshes
|
// meshes
|
||||||
for (i=0; i<meshes.size(); i++) {
|
for (int i=0; i<meshes.size(); i++) {
|
||||||
if (meshes[i]->name.empty()) {
|
if (meshes[i]->name.empty()) {
|
||||||
stripped = mjuu_strippath(meshes[i]->file);
|
stripped = mjuu_strippath(meshes[i]->file);
|
||||||
meshes[i]->name = mjuu_stripext(stripped);
|
meshes[i]->name = mjuu_stripext(stripped);
|
||||||
@@ -846,7 +839,7 @@ void mjCModel::SetDefaultNames(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// skins
|
// skins
|
||||||
for (i=0; i<skins.size(); i++) {
|
for (int i=0; i<skins.size(); i++) {
|
||||||
if (skins[i]->name.empty()) {
|
if (skins[i]->name.empty()) {
|
||||||
stripped = mjuu_strippath(skins[i]->file);
|
stripped = mjuu_strippath(skins[i]->file);
|
||||||
skins[i]->name = mjuu_stripext(stripped);
|
skins[i]->name = mjuu_stripext(stripped);
|
||||||
@@ -854,7 +847,7 @@ void mjCModel::SetDefaultNames(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// hfields
|
// hfields
|
||||||
for (i=0; i<hfields.size(); i++) {
|
for (int i=0; i<hfields.size(); i++) {
|
||||||
if (hfields[i]->name.empty()) {
|
if (hfields[i]->name.empty()) {
|
||||||
stripped = mjuu_strippath(hfields[i]->file);
|
stripped = mjuu_strippath(hfields[i]->file);
|
||||||
hfields[i]->name = mjuu_stripext(stripped);
|
hfields[i]->name = mjuu_stripext(stripped);
|
||||||
@@ -867,7 +860,7 @@ void mjCModel::SetDefaultNames(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// textures
|
// textures
|
||||||
for (i=0; i<textures.size(); i++) {
|
for (int i=0; i<textures.size(); i++) {
|
||||||
if (textures[i]->name.empty()) {
|
if (textures[i]->name.empty()) {
|
||||||
stripped = mjuu_strippath(textures[i]->file);
|
stripped = mjuu_strippath(textures[i]->file);
|
||||||
textures[i]->name = mjuu_stripext(stripped);
|
textures[i]->name = mjuu_stripext(stripped);
|
||||||
@@ -880,7 +873,7 @@ void mjCModel::SetDefaultNames(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// materials: name check only
|
// materials: name check only
|
||||||
for (i=0; i<materials.size(); i++) {
|
for (int i=0; i<materials.size(); i++) {
|
||||||
if (materials[i]->name.empty()) {
|
if (materials[i]->name.empty()) {
|
||||||
throw mjCError(materials[i], "empty name in material");
|
throw mjCError(materials[i], "empty name in material");
|
||||||
}
|
}
|
||||||
@@ -895,8 +888,6 @@ const int nVEL[4] = {6, 3, 1, 1};
|
|||||||
|
|
||||||
// set array sizes
|
// set array sizes
|
||||||
void mjCModel::SetSizes(void) {
|
void mjCModel::SetSizes(void) {
|
||||||
int i, j;
|
|
||||||
|
|
||||||
// set from object list sizes
|
// set from object list sizes
|
||||||
nbody = (int)bodies.size();
|
nbody = (int)bodies.size();
|
||||||
njnt = (int)joints.size();
|
njnt = (int)joints.size();
|
||||||
@@ -921,13 +912,13 @@ void mjCModel::SetSizes(void) {
|
|||||||
nplugin = (int)plugins.size();
|
nplugin = (int)plugins.size();
|
||||||
|
|
||||||
// nq, nv
|
// nq, nv
|
||||||
for (i=0; i<njnt; i++) {
|
for (int i=0; i<njnt; i++) {
|
||||||
nq += nPOS[joints[i]->type];
|
nq += nPOS[joints[i]->type];
|
||||||
nv += nVEL[joints[i]->type];
|
nv += nVEL[joints[i]->type];
|
||||||
}
|
}
|
||||||
|
|
||||||
// nu, na
|
// nu, na
|
||||||
for (i=0; i<(int)actuators.size(); i++) {
|
for (int i=0; i<(int)actuators.size(); i++) {
|
||||||
if (actuators[i]->dyntype == mjDYN_NONE) {
|
if (actuators[i]->dyntype == mjDYN_NONE) {
|
||||||
nu++;
|
nu++;
|
||||||
} else {
|
} else {
|
||||||
@@ -937,12 +928,12 @@ void mjCModel::SetSizes(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// nbvh
|
// nbvh
|
||||||
for (i=0; i<nbody; i++) {
|
for (int i=0; i<nbody; i++) {
|
||||||
nbvh += bodies[i]->nbvh;
|
nbvh += bodies[i]->nbvh;
|
||||||
}
|
}
|
||||||
|
|
||||||
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
|
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
|
||||||
for (i=0; i<nmesh; i++) {
|
for (int i=0; i<nmesh; i++) {
|
||||||
nmeshvert += meshes[i]->nvert;
|
nmeshvert += meshes[i]->nvert;
|
||||||
nmeshnormal += meshes[i]->nnormal;
|
nmeshnormal += meshes[i]->nnormal;
|
||||||
nmeshface += meshes[i]->nface;
|
nmeshface += meshes[i]->nface;
|
||||||
@@ -951,67 +942,67 @@ void mjCModel::SetSizes(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
|
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
|
||||||
for (i=0; i<nskin; i++) {
|
for (int i=0; i<nskin; i++) {
|
||||||
nskinvert += skins[i]->vert.size()/3;
|
nskinvert += skins[i]->vert.size()/3;
|
||||||
nskintexvert += skins[i]->texcoord.size()/2;
|
nskintexvert += skins[i]->texcoord.size()/2;
|
||||||
nskinface += skins[i]->face.size()/3;
|
nskinface += skins[i]->face.size()/3;
|
||||||
nskinbone += skins[i]->bodyid.size();
|
nskinbone += skins[i]->bodyid.size();
|
||||||
for (j=0; j<skins[i]->bodyid.size(); j++) {
|
for (int j=0; j<skins[i]->bodyid.size(); j++) {
|
||||||
nskinbonevert += skins[i]->vertid[j].size();
|
nskinbonevert += skins[i]->vertid[j].size();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// nhfielddata
|
// nhfielddata
|
||||||
for (i=0; i<nhfield; i++) nhfielddata += hfields[i]->nrow * hfields[i]->ncol;
|
for (int i=0; i<nhfield; i++) nhfielddata += hfields[i]->nrow * hfields[i]->ncol;
|
||||||
|
|
||||||
// ntexdata
|
// ntexdata
|
||||||
for (i=0; i<ntex; i++) ntexdata += 3 * textures[i]->width * textures[i]->height;
|
for (int i=0; i<ntex; i++) ntexdata += 3 * textures[i]->width * textures[i]->height;
|
||||||
|
|
||||||
// nwrap
|
// nwrap
|
||||||
for (i=0; i<ntendon; i++) nwrap += (int)tendons[i]->path.size();
|
for (int i=0; i<ntendon; i++) nwrap += (int)tendons[i]->path.size();
|
||||||
|
|
||||||
// nsensordata
|
// nsensordata
|
||||||
for (i=0; i<nsensor; i++) nsensordata += sensors[i]->dim;
|
for (int i=0; i<nsensor; i++) nsensordata += sensors[i]->dim;
|
||||||
|
|
||||||
// nnumericdata
|
// nnumericdata
|
||||||
for (i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
|
for (int i=0; i<nnumeric; i++) nnumericdata += numerics[i]->size;
|
||||||
|
|
||||||
// ntextdata
|
// ntextdata
|
||||||
for (i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1;
|
for (int i=0; i<ntext; i++) ntextdata += (int)texts[i]->data.size() + 1;
|
||||||
|
|
||||||
// ntupledata
|
// ntupledata
|
||||||
for (i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
|
for (int i=0; i<ntuple; i++) ntupledata += (int)tuples[i]->objtype.size();
|
||||||
|
|
||||||
// npluginattr
|
// npluginattr
|
||||||
for (i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
|
for (int i=0; i<nplugin; i++) npluginattr += (int)plugins[i]->flattened_attributes.size();
|
||||||
|
|
||||||
// nnames
|
// nnames
|
||||||
nnames = (int)modelname.size() + 1;
|
nnames = (int)modelname.size() + 1;
|
||||||
for (i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
|
for (int i=0; i<nbody; i++) nnames += (int)bodies[i]->name.length() + 1;
|
||||||
for (i=0; i<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
|
for (int i=0; i<njnt; i++) nnames += (int)joints[i]->name.length() + 1;
|
||||||
for (i=0; i<ngeom; i++) nnames += (int)geoms[i]->name.length() + 1;
|
for (int i=0; i<ngeom; i++) nnames += (int)geoms[i]->name.length() + 1;
|
||||||
for (i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
|
for (int i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
|
||||||
for (i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
|
for (int i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
|
||||||
for (i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
|
for (int i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
|
||||||
for (i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
|
for (int i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
|
||||||
for (i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
|
for (int i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
|
||||||
for (i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
|
for (int i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
|
||||||
for (i=0; i<ntex; i++) nnames += (int)textures[i]->name.length() + 1;
|
for (int i=0; i<ntex; i++) nnames += (int)textures[i]->name.length() + 1;
|
||||||
for (i=0; i<nmat; i++) nnames += (int)materials[i]->name.length() + 1;
|
for (int i=0; i<nmat; i++) nnames += (int)materials[i]->name.length() + 1;
|
||||||
for (i=0; i<npair; i++) nnames += (int)pairs[i]->name.length() + 1;
|
for (int i=0; i<npair; i++) nnames += (int)pairs[i]->name.length() + 1;
|
||||||
for (i=0; i<nexclude; i++) nnames += (int)excludes[i]->name.length() + 1;
|
for (int i=0; i<nexclude; i++) nnames += (int)excludes[i]->name.length() + 1;
|
||||||
for (i=0; i<neq; i++) nnames += (int)equalities[i]->name.length() + 1;
|
for (int i=0; i<neq; i++) nnames += (int)equalities[i]->name.length() + 1;
|
||||||
for (i=0; i<ntendon; i++) nnames += (int)tendons[i]->name.length() + 1;
|
for (int i=0; i<ntendon; i++) nnames += (int)tendons[i]->name.length() + 1;
|
||||||
for (i=0; i<nu; i++) nnames += (int)actuators[i]->name.length() + 1;
|
for (int i=0; i<nu; i++) nnames += (int)actuators[i]->name.length() + 1;
|
||||||
for (i=0; i<nsensor; i++) nnames += (int)sensors[i]->name.length() + 1;
|
for (int i=0; i<nsensor; i++) nnames += (int)sensors[i]->name.length() + 1;
|
||||||
for (i=0; i<nnumeric; i++) nnames += (int)numerics[i]->name.length() + 1;
|
for (int i=0; i<nnumeric; i++) nnames += (int)numerics[i]->name.length() + 1;
|
||||||
for (i=0; i<ntext; i++) nnames += (int)texts[i]->name.length() + 1;
|
for (int i=0; i<ntext; i++) nnames += (int)texts[i]->name.length() + 1;
|
||||||
for (i=0; i<ntuple; i++) nnames += (int)tuples[i]->name.length() + 1;
|
for (int i=0; i<ntuple; i++) nnames += (int)tuples[i]->name.length() + 1;
|
||||||
for (i=0; i<nkey; i++) nnames += (int)keys[i]->name.length() + 1;
|
for (int i=0; i<nkey; i++) nnames += (int)keys[i]->name.length() + 1;
|
||||||
for (i=0; i<nplugin; i++) nnames += (int)plugins[i]->name.length() + 1;
|
for (int i=0; i<nplugin; i++) nnames += (int)plugins[i]->name.length() + 1;
|
||||||
|
|
||||||
// nemax
|
// nemax
|
||||||
for (i=0; i<neq; i++)
|
for (int i=0; i<neq; i++)
|
||||||
if (equalities[i]->type==mjEQ_CONNECT) {
|
if (equalities[i]->type==mjEQ_CONNECT) {
|
||||||
nemax += 3;
|
nemax += 3;
|
||||||
} else if (equalities[i]->type==mjEQ_WELD) {
|
} else if (equalities[i]->type==mjEQ_WELD) {
|
||||||
@@ -1092,8 +1083,6 @@ void* LRfunc(void* arg) {
|
|||||||
|
|
||||||
// compute actuator lengthrange
|
// compute actuator lengthrange
|
||||||
void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// save options and modify
|
// save options and modify
|
||||||
mjOption saveopt = m->opt;
|
mjOption saveopt = m->opt;
|
||||||
m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_PASSIVE |
|
m->opt.disableflags = mjDSBL_FRICTIONLOSS | mjDSBL_CONTACT | mjDSBL_PASSIVE |
|
||||||
@@ -1107,7 +1096,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
|
|
||||||
// count actuators that need computation
|
// count actuators that need computation
|
||||||
int cnt = 0;
|
int cnt = 0;
|
||||||
for (i=0; i<m->nu; i++) {
|
for (int i=0; i<m->nu; i++) {
|
||||||
// skip depending on mode and type
|
// skip depending on mode and type
|
||||||
int ismuscle = (m->actuator_gaintype[i]==mjGAIN_MUSCLE ||
|
int ismuscle = (m->actuator_gaintype[i]==mjGAIN_MUSCLE ||
|
||||||
m->actuator_biastype[i]==mjBIAS_MUSCLE);
|
m->actuator_biastype[i]==mjBIAS_MUSCLE);
|
||||||
@@ -1132,7 +1121,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
// single thread
|
// single thread
|
||||||
if (!usethread || cnt<2 || nthread<2) {
|
if (!usethread || cnt<2 || nthread<2) {
|
||||||
char err[200];
|
char err[200];
|
||||||
for (i=0; i<m->nu; i++) {
|
for (int i=0; i<m->nu; i++) {
|
||||||
if (!mj_setLengthRange(m, data, i, &LRopt, err, 200)) {
|
if (!mj_setLengthRange(m, data, i, &LRopt, err, 200)) {
|
||||||
throw mjCError(0, err);
|
throw mjCError(0, err);
|
||||||
}
|
}
|
||||||
@@ -1144,7 +1133,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
// allocate mjData for each thread
|
// allocate mjData for each thread
|
||||||
char err[16][200];
|
char err[16][200];
|
||||||
mjData* pdata[16] = {data};
|
mjData* pdata[16] = {data};
|
||||||
for (i=1; i<nthread; i++) {
|
for (int i=1; i<nthread; i++) {
|
||||||
pdata[i] = mj_makeData(m);
|
pdata[i] = mj_makeData(m);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1156,7 +1145,7 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
|
|
||||||
// prepare thread function arguments, clear errors
|
// prepare thread function arguments, clear errors
|
||||||
LRThreadArg arg[16];
|
LRThreadArg arg[16];
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
LRThreadArg temp = {m, pdata[i], i*num, num, &LRopt, err[i], 200};
|
LRThreadArg temp = {m, pdata[i], i*num, num, &LRopt, err[i], 200};
|
||||||
arg[i] = temp;
|
arg[i] = temp;
|
||||||
err[i][0] = 0;
|
err[i][0] = 0;
|
||||||
@@ -1166,12 +1155,12 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
#if defined(_WIN32) || defined(__APPLE__)
|
#if defined(_WIN32) || defined(__APPLE__)
|
||||||
// launch threads
|
// launch threads
|
||||||
thread th[16];
|
thread th[16];
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
th[i] = thread(LRfunc, arg+i);
|
th[i] = thread(LRfunc, arg+i);
|
||||||
}
|
}
|
||||||
|
|
||||||
// wait for threads to finish
|
// wait for threads to finish
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
th[i].join();
|
th[i].join();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1179,23 +1168,23 @@ void mjCModel::LengthRange(mjModel* m, mjData* data) {
|
|||||||
#else
|
#else
|
||||||
// launch threads
|
// launch threads
|
||||||
pthread_t th[16];
|
pthread_t th[16];
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
pthread_create(th+i, NULL, LRfunc, arg+i);
|
pthread_create(th+i, NULL, LRfunc, arg+i);
|
||||||
}
|
}
|
||||||
|
|
||||||
// wait for threads to finish
|
// wait for threads to finish
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
pthread_join(th[i], NULL);
|
pthread_join(th[i], NULL);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// free mjData allocated here
|
// free mjData allocated here
|
||||||
for (i=1; i<nthread; i++) {
|
for (int i=1; i<nthread; i++) {
|
||||||
mj_deleteData(pdata[i]);
|
mj_deleteData(pdata[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// report first error
|
// report first error
|
||||||
for (i=0; i<nthread; i++) {
|
for (int i=0; i<nthread; i++) {
|
||||||
if (err[i][0]) {
|
if (err[i][0]) {
|
||||||
throw mjCError(0, err[i]);
|
throw mjCError(0, err[i]);
|
||||||
}
|
}
|
||||||
@@ -1326,14 +1315,13 @@ void mjCModel::CopyNames(mjModel* m) {
|
|||||||
|
|
||||||
// copy objects inside kinematic tree
|
// copy objects inside kinematic tree
|
||||||
void mjCModel::CopyTree(mjModel* m) {
|
void mjCModel::CopyTree(mjModel* m) {
|
||||||
int i, j, j1;
|
|
||||||
int jntadr = 0; // addresses in global arrays
|
int jntadr = 0; // addresses in global arrays
|
||||||
int dofadr = 0;
|
int dofadr = 0;
|
||||||
int qposadr = 0;
|
int qposadr = 0;
|
||||||
int bvh_adr = 0;
|
int bvh_adr = 0;
|
||||||
|
|
||||||
// main loop over bodies
|
// main loop over bodies
|
||||||
for (i=0; i<nbody; i++) {
|
for (int i=0; i<nbody; i++) {
|
||||||
// get body and parent pointers
|
// get body and parent pointers
|
||||||
mjCBody* pb = bodies[i];
|
mjCBody* pb = bodies[i];
|
||||||
mjCBody* par = bodies[pb->parentid];
|
mjCBody* par = bodies[pb->parentid];
|
||||||
@@ -1370,7 +1358,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// count free joints
|
// count free joints
|
||||||
int cntfree = 0;
|
int cntfree = 0;
|
||||||
for (j=0; j<(int)pb->joints.size(); j++) {
|
for (int j=0; j<(int)pb->joints.size(); j++) {
|
||||||
cntfree += (pb->joints[j]->type == mjJNT_FREE);
|
cntfree += (pb->joints[j]->type == mjJNT_FREE);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1396,7 +1384,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
m->body_sameframe[i] = IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i);
|
m->body_sameframe[i] = IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i);
|
||||||
|
|
||||||
// init simple: sameframe, and (self-root, or parent is fixed child of world)
|
// init simple: sameframe, and (self-root, or parent is fixed child of world)
|
||||||
j = m->body_parentid[i];
|
int j = m->body_parentid[i];
|
||||||
m->body_simple[i] = (m->body_sameframe[i] &&
|
m->body_simple[i] = (m->body_sameframe[i] &&
|
||||||
(m->body_rootid[i]==i ||
|
(m->body_rootid[i]==i ||
|
||||||
(m->body_parentid[j]==0 &&
|
(m->body_parentid[j]==0 &&
|
||||||
@@ -1409,7 +1397,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// loop over joints for this body
|
// loop over joints for this body
|
||||||
int rotfound = 0;
|
int rotfound = 0;
|
||||||
for (j=0; j<(int)pb->joints.size(); j++) {
|
for (int j=0; j<(int)pb->joints.size(); j++) {
|
||||||
// get pointer and id
|
// get pointer and id
|
||||||
mjCJoint* pj = pb->joints[j];
|
mjCJoint* pj = pb->joints[j];
|
||||||
int jid = pj->id;
|
int jid = pj->id;
|
||||||
@@ -1470,7 +1458,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// set dof fields for this joint
|
// set dof fields for this joint
|
||||||
for (j1=0; j1<nVEL[pj->type]; j1++) {
|
for (int j1=0; j1<nVEL[pj->type]; j1++) {
|
||||||
// set attributes
|
// set attributes
|
||||||
m->dof_bodyid[dofadr] = pb->id;
|
m->dof_bodyid[dofadr] = pb->id;
|
||||||
m->dof_jntid[dofadr] = jid;
|
m->dof_jntid[dofadr] = jid;
|
||||||
@@ -1494,7 +1482,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// loop over geoms for this body
|
// loop over geoms for this body
|
||||||
for (j=0; j<(int)pb->geoms.size(); j++) {
|
for (int j=0; j<(int)pb->geoms.size(); j++) {
|
||||||
// get pointer and id
|
// get pointer and id
|
||||||
mjCGeom* pg = pb->geoms[j];
|
mjCGeom* pg = pb->geoms[j];
|
||||||
int gid = pg->id;
|
int gid = pg->id;
|
||||||
@@ -1549,7 +1537,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// loop over sites for this body
|
// loop over sites for this body
|
||||||
for (j=0; j<(int)pb->sites.size(); j++) {
|
for (int j=0; j<(int)pb->sites.size(); j++) {
|
||||||
// get pointer and id
|
// get pointer and id
|
||||||
mjCSite* ps = pb->sites[j];
|
mjCSite* ps = pb->sites[j];
|
||||||
int sid = ps->id;
|
int sid = ps->id;
|
||||||
@@ -1582,7 +1570,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// loop over cameras for this body
|
// loop over cameras for this body
|
||||||
for (j=0; j<(int)pb->cameras.size(); j++) {
|
for (int j=0; j<(int)pb->cameras.size(); j++) {
|
||||||
// get pointer and id
|
// get pointer and id
|
||||||
mjCCamera* pc = pb->cameras[j];
|
mjCCamera* pc = pb->cameras[j];
|
||||||
int cid = pc->id;
|
int cid = pc->id;
|
||||||
@@ -1599,7 +1587,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// loop over lights for this body
|
// loop over lights for this body
|
||||||
for (j=0; j<(int)pb->lights.size(); j++) {
|
for (int j=0; j<(int)pb->lights.size(); j++) {
|
||||||
// get pointer and id
|
// get pointer and id
|
||||||
mjCLight* pl = pb->lights[j];
|
mjCLight* pl = pb->lights[j];
|
||||||
int lid = pl->id;
|
int lid = pl->id;
|
||||||
@@ -1629,12 +1617,12 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// compute nM and dof_Madr
|
// compute nM and dof_Madr
|
||||||
nM = 0;
|
nM = 0;
|
||||||
for (i=0; i<nv; i++) {
|
for (int i=0; i<nv; i++) {
|
||||||
// set address of this dof
|
// set address of this dof
|
||||||
m->dof_Madr[i] = nM;
|
m->dof_Madr[i] = nM;
|
||||||
|
|
||||||
// count ancestor dofs including self
|
// count ancestor dofs including self
|
||||||
j = i;
|
int j = i;
|
||||||
while (j>=0) {
|
while (j>=0) {
|
||||||
nM++;
|
nM++;
|
||||||
j = m->dof_parentid[j];
|
j = m->dof_parentid[j];
|
||||||
@@ -1647,7 +1635,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
m->nD = nD;
|
m->nD = nD;
|
||||||
|
|
||||||
// compute subtreedofs in backward pass over bodies
|
// compute subtreedofs in backward pass over bodies
|
||||||
for (i = nbody - 1; i > 0; i--) {
|
for (int i = nbody - 1; i > 0; i--) {
|
||||||
// add body dofs to self count
|
// add body dofs to self count
|
||||||
bodies[i]->subtreedofs += bodies[i]->dofnum;
|
bodies[i]->subtreedofs += bodies[i]->dofnum;
|
||||||
|
|
||||||
@@ -1662,12 +1650,12 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// compute nB
|
// compute nB
|
||||||
nB = 0;
|
nB = 0;
|
||||||
for (i = 0; i < nbody; i++) {
|
for (int i = 0; i < nbody; i++) {
|
||||||
// add subtree dofs (including self)
|
// add subtree dofs (including self)
|
||||||
nB += bodies[i]->subtreedofs;
|
nB += bodies[i]->subtreedofs;
|
||||||
|
|
||||||
// add dofs in ancestor bodies
|
// add dofs in ancestor bodies
|
||||||
j = bodies[i]->parentid;
|
int j = bodies[i]->parentid;
|
||||||
while (j > 0) {
|
while (j > 0) {
|
||||||
nB += bodies[j]->dofnum;
|
nB += bodies[j]->dofnum;
|
||||||
j = bodies[j]->parentid;
|
j = bodies[j]->parentid;
|
||||||
@@ -1677,7 +1665,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// set dof_simplenum
|
// set dof_simplenum
|
||||||
int scnt = 0;
|
int scnt = 0;
|
||||||
for (i=nv-1; i>=0; i--) {
|
for (int i=nv-1; i>=0; i--) {
|
||||||
// dof in simple body
|
// dof in simple body
|
||||||
if (m->body_simple[m->dof_bodyid[i]]) {
|
if (m->body_simple[m->dof_bodyid[i]]) {
|
||||||
scnt++;
|
scnt++;
|
||||||
@@ -1696,7 +1684,7 @@ void mjCModel::CopyTree(mjModel* m) {
|
|||||||
|
|
||||||
// copy objects outside kinematic tree
|
// copy objects outside kinematic tree
|
||||||
void mjCModel::CopyObjects(mjModel* m) {
|
void mjCModel::CopyObjects(mjModel* m) {
|
||||||
int i, j, adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
|
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
|
||||||
int bonevert_adr, graph_adr, data_adr;
|
int bonevert_adr, graph_adr, data_adr;
|
||||||
|
|
||||||
// sizes outside call to mj_makeModel
|
// sizes outside call to mj_makeModel
|
||||||
@@ -1712,7 +1700,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
texcoord_adr = 0;
|
texcoord_adr = 0;
|
||||||
face_adr = 0;
|
face_adr = 0;
|
||||||
graph_adr = 0;
|
graph_adr = 0;
|
||||||
for (i=0; i<nmesh; i++) {
|
for (int i=0; i<nmesh; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCMesh* pme = meshes[i];
|
mjCMesh* pme = meshes[i];
|
||||||
|
|
||||||
@@ -1756,7 +1744,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
texcoord_adr = 0;
|
texcoord_adr = 0;
|
||||||
bone_adr = 0;
|
bone_adr = 0;
|
||||||
bonevert_adr = 0;
|
bonevert_adr = 0;
|
||||||
for (i=0; i<nskin; i++) {
|
for (int i=0; i<nskin; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCSkin* psk = skins[i];
|
mjCSkin* psk = skins[i];
|
||||||
|
|
||||||
@@ -1789,7 +1777,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
psk->bodyid.size()*sizeof(int));
|
psk->bodyid.size()*sizeof(int));
|
||||||
|
|
||||||
// copy per-bone vertex data, advance vertex counter
|
// copy per-bone vertex data, advance vertex counter
|
||||||
for (j=0; j<m->skin_bonenum[i]; j++) {
|
for (int j=0; j<m->skin_bonenum[i]; j++) {
|
||||||
// set fields
|
// set fields
|
||||||
m->skin_bonevertadr[bone_adr+j] = bonevert_adr;
|
m->skin_bonevertadr[bone_adr+j] = bonevert_adr;
|
||||||
m->skin_bonevertnum[bone_adr+j] = (int)psk->vertid[j].size();
|
m->skin_bonevertnum[bone_adr+j] = (int)psk->vertid[j].size();
|
||||||
@@ -1813,7 +1801,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// hfields
|
// hfields
|
||||||
data_adr = 0;
|
data_adr = 0;
|
||||||
for (i=0; i<nhfield; i++) {
|
for (int i=0; i<nhfield; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCHField* phf = hfields[i];
|
mjCHField* phf = hfields[i];
|
||||||
|
|
||||||
@@ -1832,7 +1820,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// textures
|
// textures
|
||||||
data_adr = 0;
|
data_adr = 0;
|
||||||
for (i=0; i<ntex; i++) {
|
for (int i=0; i<ntex; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCTexture* ptex = textures[i];
|
mjCTexture* ptex = textures[i];
|
||||||
|
|
||||||
@@ -1850,7 +1838,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// materials
|
// materials
|
||||||
for (i=0; i<nmat; i++) {
|
for (int i=0; i<nmat; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCMaterial* pmat = materials[i];
|
mjCMaterial* pmat = materials[i];
|
||||||
|
|
||||||
@@ -1866,7 +1854,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// geom pairs to include
|
// geom pairs to include
|
||||||
for (i=0; i<npair; i++) {
|
for (int i=0; i<npair; i++) {
|
||||||
m->pair_dim[i] = pairs[i]->condim;
|
m->pair_dim[i] = pairs[i]->condim;
|
||||||
m->pair_geom1[i] = pairs[i]->geom1;
|
m->pair_geom1[i] = pairs[i]->geom1;
|
||||||
m->pair_geom2[i] = pairs[i]->geom2;
|
m->pair_geom2[i] = pairs[i]->geom2;
|
||||||
@@ -1879,12 +1867,12 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// body pairs to exclude
|
// body pairs to exclude
|
||||||
for (i=0; i<nexclude; i++) {
|
for (int i=0; i<nexclude; i++) {
|
||||||
m->exclude_signature[i] = excludes[i]->signature;
|
m->exclude_signature[i] = excludes[i]->signature;
|
||||||
}
|
}
|
||||||
|
|
||||||
// equality constraints
|
// equality constraints
|
||||||
for (i=0; i<neq; i++) {
|
for (int i=0; i<neq; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCEquality* peq = equalities[i];
|
mjCEquality* peq = equalities[i];
|
||||||
|
|
||||||
@@ -1900,7 +1888,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// tendons and wraps
|
// tendons and wraps
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<ntendon; i++) {
|
for (int i=0; i<ntendon; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCTendon* pte = tendons[i];
|
mjCTendon* pte = tendons[i];
|
||||||
|
|
||||||
@@ -1927,7 +1915,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
|
copyvec(m->tendon_rgba+4*i, pte->rgba, 4);
|
||||||
|
|
||||||
// set wraps
|
// set wraps
|
||||||
for (j=0; j<(int)pte->path.size(); j++) {
|
for (int j=0; j<(int)pte->path.size(); j++) {
|
||||||
m->wrap_type[adr+j] = pte->path[j]->type;
|
m->wrap_type[adr+j] = pte->path[j]->type;
|
||||||
m->wrap_objid[adr+j] = pte->path[j]->objid;
|
m->wrap_objid[adr+j] = pte->path[j]->objid;
|
||||||
m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->prm;
|
m->wrap_prm[adr+j] = (mjtNum)pte->path[j]->prm;
|
||||||
@@ -1942,7 +1930,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// actuators
|
// actuators
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<nu; i++) {
|
for (int i=0; i<nu; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCActuator* pac = actuators[i];
|
mjCActuator* pac = actuators[i];
|
||||||
|
|
||||||
@@ -1974,7 +1962,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// sensors
|
// sensors
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<nsensor; i++) {
|
for (int i=0; i<nsensor; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCSensor* psen = sensors[i];
|
mjCSensor* psen = sensors[i];
|
||||||
|
|
||||||
@@ -1998,17 +1986,17 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// numeric fields
|
// numeric fields
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<nnumeric; i++) {
|
for (int i=0; i<nnumeric; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCNumeric* pcu = numerics[i];
|
mjCNumeric* pcu = numerics[i];
|
||||||
|
|
||||||
// set fields
|
// set fields
|
||||||
m->numeric_adr[i] = adr;
|
m->numeric_adr[i] = adr;
|
||||||
m->numeric_size[i] = pcu->size;
|
m->numeric_size[i] = pcu->size;
|
||||||
for (j=0; j<(int)pcu->data.size(); j++) {
|
for (int j=0; j<(int)pcu->data.size(); j++) {
|
||||||
m->numeric_data[adr+j] = (mjtNum)pcu->data[j];
|
m->numeric_data[adr+j] = (mjtNum)pcu->data[j];
|
||||||
}
|
}
|
||||||
for (j=(int)pcu->data.size(); j<(int)pcu->size; j++) {
|
for (int j=(int)pcu->data.size(); j<(int)pcu->size; j++) {
|
||||||
m->numeric_data[adr+j] = 0;
|
m->numeric_data[adr+j] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2018,7 +2006,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// text fields
|
// text fields
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<ntext; i++) {
|
for (int i=0; i<ntext; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCText* pte = texts[i];
|
mjCText* pte = texts[i];
|
||||||
|
|
||||||
@@ -2033,14 +2021,14 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
|
|
||||||
// tuple fields
|
// tuple fields
|
||||||
adr = 0;
|
adr = 0;
|
||||||
for (i=0; i<ntuple; i++) {
|
for (int i=0; i<ntuple; i++) {
|
||||||
// get pointer
|
// get pointer
|
||||||
mjCTuple* ptu = tuples[i];
|
mjCTuple* ptu = tuples[i];
|
||||||
|
|
||||||
// set fields
|
// set fields
|
||||||
m->tuple_adr[i] = adr;
|
m->tuple_adr[i] = adr;
|
||||||
m->tuple_size[i] = (int)ptu->objtype.size();
|
m->tuple_size[i] = (int)ptu->objtype.size();
|
||||||
for (j=0; j<m->tuple_size[i]; j++) {
|
for (int j=0; j<m->tuple_size[i]; j++) {
|
||||||
m->tuple_objtype[adr+j] = (int)ptu->objtype[j];
|
m->tuple_objtype[adr+j] = (int)ptu->objtype[j];
|
||||||
m->tuple_objid[adr+j] = ptu->objid[j];
|
m->tuple_objid[adr+j] = ptu->objid[j];
|
||||||
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j];
|
m->tuple_objprm[adr+j] = (mjtNum)ptu->objprm[j];
|
||||||
@@ -2051,7 +2039,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// copy keyframe data
|
// copy keyframe data
|
||||||
for (i=0; i<nkey; i++) {
|
for (int i=0; i<nkey; i++) {
|
||||||
// copy data
|
// copy data
|
||||||
m->key_time[i] = (mjtNum)keys[i]->time;
|
m->key_time[i] = (mjtNum)keys[i]->time;
|
||||||
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq);
|
copyvec(m->key_qpos+i*nq, keys[i]->qpos.data(), nq);
|
||||||
@@ -2065,14 +2053,14 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// normalize quaternions in m->key_qpos
|
// normalize quaternions in m->key_qpos
|
||||||
for (j=0; j<m->njnt; j++) {
|
for (int j=0; j<m->njnt; j++) {
|
||||||
if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) {
|
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));
|
mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// normalize quaternions in m->key_mquat
|
// normalize quaternions in m->key_mquat
|
||||||
for (j=0; j<nmocap; j++) {
|
for (int j=0; j<nmocap; j++) {
|
||||||
mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
|
mju_normalize4(m->key_mquat+i*4*nmocap+4*j);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2103,35 +2091,33 @@ static void changeframe(double childpos[3], double childquat[4],
|
|||||||
|
|
||||||
// reindex elements during fuse
|
// reindex elements during fuse
|
||||||
void mjCModel::FuseReindex(mjCBody* body) {
|
void mjCModel::FuseReindex(mjCBody* body) {
|
||||||
size_t i;
|
|
||||||
|
|
||||||
// set parentid and weldid of children
|
// set parentid and weldid of children
|
||||||
for (i=0; i<body->bodies.size(); i++) {
|
for (int i=0; i<body->bodies.size(); i++) {
|
||||||
body->bodies[i]->parentid = body->id;
|
body->bodies[i]->parentid = body->id;
|
||||||
body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ?
|
body->bodies[i]->weldid = (!body->bodies[i]->joints.empty() ?
|
||||||
body->bodies[i]->id : body->weldid);
|
body->bodies[i]->id : body->weldid);
|
||||||
}
|
}
|
||||||
|
|
||||||
// joints
|
// joints
|
||||||
for (i=0; i<body->joints.size(); i++) {
|
for (int i=0; i<body->joints.size(); i++) {
|
||||||
body->joints[i]->id = (int)joints.size();
|
body->joints[i]->id = (int)joints.size();
|
||||||
joints.push_back(body->joints[i]);
|
joints.push_back(body->joints[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// geoms
|
// geoms
|
||||||
for (i=0; i<body->geoms.size(); i++) {
|
for (int i=0; i<body->geoms.size(); i++) {
|
||||||
body->geoms[i]->id = (int)geoms.size();
|
body->geoms[i]->id = (int)geoms.size();
|
||||||
geoms.push_back(body->geoms[i]);
|
geoms.push_back(body->geoms[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// sites
|
// sites
|
||||||
for (i=0; i<body->sites.size(); i++) {
|
for (int i=0; i<body->sites.size(); i++) {
|
||||||
body->sites[i]->id = (int)sites.size();
|
body->sites[i]->id = (int)sites.size();
|
||||||
sites.push_back(body->sites[i]);
|
sites.push_back(body->sites[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
// process children recursively
|
// process children recursively
|
||||||
for (i=0; i<body->bodies.size(); i++) {
|
for (int i=0; i<body->bodies.size(); i++) {
|
||||||
FuseReindex(body->bodies[i]);
|
FuseReindex(body->bodies[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2140,8 +2126,6 @@ void mjCModel::FuseReindex(mjCBody* body) {
|
|||||||
|
|
||||||
// fuse static bodies with their parent
|
// fuse static bodies with their parent
|
||||||
void mjCModel::FuseStatic(void) {
|
void mjCModel::FuseStatic(void) {
|
||||||
int i, j, k;
|
|
||||||
|
|
||||||
// skip if model has potential to reference elements with changed ids
|
// skip if model has potential to reference elements with changed ids
|
||||||
if (!skins.empty() ||
|
if (!skins.empty() ||
|
||||||
!pairs.empty() ||
|
!pairs.empty() ||
|
||||||
@@ -2157,7 +2141,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// process fusable bodies
|
// process fusable bodies
|
||||||
for (i=1; i<bodies.size(); i++) {
|
for (int i=1; i<bodies.size(); i++) {
|
||||||
// get body and parent
|
// get body and parent
|
||||||
mjCBody* body = bodies[i];
|
mjCBody* body = bodies[i];
|
||||||
mjCBody* par = bodies[body->parentid];
|
mjCBody* par = bodies[body->parentid];
|
||||||
@@ -2195,7 +2179,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
// compute total mass
|
// compute total mass
|
||||||
par->mass = 0;
|
par->mass = 0;
|
||||||
mjuu_setvec(par->locipos, 0, 0, 0);
|
mjuu_setvec(par->locipos, 0, 0, 0);
|
||||||
for (j=0; j<2; j++) {
|
for (int j=0; j<2; j++) {
|
||||||
par->mass += mass[j];
|
par->mass += mass[j];
|
||||||
par->locipos[0] += mass[j]*ipos[j][0];
|
par->locipos[0] += mass[j]*ipos[j][0];
|
||||||
par->locipos[1] += mass[j]*ipos[j][1];
|
par->locipos[1] += mass[j]*ipos[j][1];
|
||||||
@@ -2219,7 +2203,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
|
|
||||||
// add inertias
|
// add inertias
|
||||||
double toti[6] = {0, 0, 0, 0, 0, 0};
|
double toti[6] = {0, 0, 0, 0, 0, 0};
|
||||||
for (j=0; j<2; j++) {
|
for (int j=0; j<2; j++) {
|
||||||
double inertA[6], inertB[6];
|
double inertA[6], inertB[6];
|
||||||
double dpos[3] = {
|
double dpos[3] = {
|
||||||
ipos[j][0] - par->locipos[0],
|
ipos[j][0] - par->locipos[0],
|
||||||
@@ -2229,7 +2213,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
|
|
||||||
mjuu_globalinertia(inertA, inertia[j], iquat[j]);
|
mjuu_globalinertia(inertA, inertia[j], iquat[j]);
|
||||||
mjuu_offcenter(inertB, mass[j], dpos);
|
mjuu_offcenter(inertB, mass[j], dpos);
|
||||||
for (k=0; k<6; k++) {
|
for (int k=0; k<6; k++) {
|
||||||
toti[k] += inertA[k] + inertB[k];
|
toti[k] += inertA[k] + inertB[k];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2247,7 +2231,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
//------------- replace body with its children in parent body list
|
//------------- replace body with its children in parent body list
|
||||||
|
|
||||||
// change frames of child bodies
|
// change frames of child bodies
|
||||||
for (j=0; j<body->bodies.size(); j++)
|
for (int j=0; j<body->bodies.size(); j++)
|
||||||
changeframe(body->bodies[j]->locpos, body->bodies[j]->locquat,
|
changeframe(body->bodies[j]->locpos, body->bodies[j]->locquat,
|
||||||
body->locpos, body->locquat);
|
body->locpos, body->locquat);
|
||||||
|
|
||||||
@@ -2280,7 +2264,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
//------------- assign geoms and sites to parent, change frames
|
//------------- assign geoms and sites to parent, change frames
|
||||||
|
|
||||||
// geoms
|
// geoms
|
||||||
for (j=0; j<body->geoms.size(); j++) {
|
for (int j=0; j<body->geoms.size(); j++) {
|
||||||
// assign
|
// assign
|
||||||
body->geoms[j]->body = par;
|
body->geoms[j]->body = par;
|
||||||
par->geoms.push_back(body->geoms[j]);
|
par->geoms.push_back(body->geoms[j]);
|
||||||
@@ -2290,7 +2274,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// sites
|
// sites
|
||||||
for (j=0; j<body->sites.size(); j++) {
|
for (int j=0; j<body->sites.size(); j++) {
|
||||||
// assign
|
// assign
|
||||||
body->sites[j]->body = par;
|
body->sites[j]->body = par;
|
||||||
par->sites.push_back(body->sites[j]);
|
par->sites.push_back(body->sites[j]);
|
||||||
@@ -2321,7 +2305,7 @@ void mjCModel::FuseStatic(void) {
|
|||||||
//------------- re-index bodies, joints, geoms, sites
|
//------------- re-index bodies, joints, geoms, sites
|
||||||
|
|
||||||
// body ids
|
// body ids
|
||||||
for (j=0; j<bodies.size(); j++) {
|
for (int j=0; j<bodies.size(); j++) {
|
||||||
bodies[j]->id = j;
|
bodies[j]->id = j;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2872,8 +2856,6 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
|
|||||||
|
|
||||||
// get numeric data back from mjModel
|
// get numeric data back from mjModel
|
||||||
bool mjCModel::CopyBack(const mjModel* m) {
|
bool mjCModel::CopyBack(const mjModel* m) {
|
||||||
int i, j;
|
|
||||||
|
|
||||||
// check for null pointer
|
// check for null pointer
|
||||||
if (!m) {
|
if (!m) {
|
||||||
errInfo = mjCError(0, "mjModel pointer is null in CopyBack");
|
errInfo = mjCError(0, "mjModel pointer is null in CopyBack");
|
||||||
@@ -2905,7 +2887,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
visual = m->vis;
|
visual = m->vis;
|
||||||
|
|
||||||
// qpos0, qpos_spring
|
// qpos0, qpos_spring
|
||||||
for (i=0; i<njnt; i++) {
|
for (int i=0; i<njnt; i++) {
|
||||||
switch (joints[i]->type) {
|
switch (joints[i]->type) {
|
||||||
case mjJNT_FREE:
|
case mjJNT_FREE:
|
||||||
copyvec(bodies[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3);
|
copyvec(bodies[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3);
|
||||||
@@ -2927,7 +2909,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
|
|
||||||
// body
|
// body
|
||||||
mjCBody* pb;
|
mjCBody* pb;
|
||||||
for (i=0; i<nbody; i++) {
|
for (int i=0; i<nbody; i++) {
|
||||||
pb = bodies[i];
|
pb = bodies[i];
|
||||||
|
|
||||||
copyvec(pb->locpos, m->body_pos+3*i, 3);
|
copyvec(pb->locpos, m->body_pos+3*i, 3);
|
||||||
@@ -2944,7 +2926,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
|
|
||||||
// joint and dof
|
// joint and dof
|
||||||
mjCJoint* pj;
|
mjCJoint* pj;
|
||||||
for (i=0; i<njnt; i++) {
|
for (int i=0; i<njnt; i++) {
|
||||||
pj = joints[i];
|
pj = joints[i];
|
||||||
|
|
||||||
// joint data
|
// joint data
|
||||||
@@ -2961,7 +2943,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// dof data
|
// dof data
|
||||||
j = m->jnt_dofadr[i];
|
int j = m->jnt_dofadr[i];
|
||||||
copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
|
copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF);
|
||||||
copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
|
copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP);
|
||||||
pj->armature = (double)m->dof_armature[j];
|
pj->armature = (double)m->dof_armature[j];
|
||||||
@@ -2971,7 +2953,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
|
|
||||||
// geom
|
// geom
|
||||||
mjCGeom* pg;
|
mjCGeom* pg;
|
||||||
for (i=0; i<ngeom; i++) {
|
for (int i=0; i<ngeom; i++) {
|
||||||
pg = geoms[i];
|
pg = geoms[i];
|
||||||
|
|
||||||
copyvec(pg->size, m->geom_size+3*i, 3);
|
copyvec(pg->size, m->geom_size+3*i, 3);
|
||||||
@@ -2991,7 +2973,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// sites
|
// sites
|
||||||
for (i=0; i<nsite; i++) {
|
for (int i=0; i<nsite; i++) {
|
||||||
copyvec(sites[i]->size, m->site_size + 3 * i, 3);
|
copyvec(sites[i]->size, m->site_size + 3 * i, 3);
|
||||||
copyvec(sites[i]->locpos, m->site_pos+3*i, 3);
|
copyvec(sites[i]->locpos, m->site_pos+3*i, 3);
|
||||||
copyvec(sites[i]->locquat, m->site_quat+4*i, 4);
|
copyvec(sites[i]->locquat, m->site_quat+4*i, 4);
|
||||||
@@ -3003,7 +2985,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// cameras
|
// cameras
|
||||||
for (i=0; i<ncam; i++) {
|
for (int i=0; i<ncam; i++) {
|
||||||
copyvec(cameras[i]->pos, m->cam_pos+3*i, 3);
|
copyvec(cameras[i]->pos, m->cam_pos+3*i, 3);
|
||||||
copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
|
copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
|
||||||
cameras[i]->fovy = (double)m->cam_fovy[i];
|
cameras[i]->fovy = (double)m->cam_fovy[i];
|
||||||
@@ -3015,7 +2997,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// lights
|
// lights
|
||||||
for (i=0; i<nlight; i++) {
|
for (int i=0; i<nlight; i++) {
|
||||||
copyvec(lights[i]->pos, m->light_pos+3*i, 3);
|
copyvec(lights[i]->pos, m->light_pos+3*i, 3);
|
||||||
copyvec(lights[i]->dir, m->light_dir+3*i, 3);
|
copyvec(lights[i]->dir, m->light_dir+3*i, 3);
|
||||||
copyvec(lights[i]->attenuation, m->light_attenuation+3*i, 3);
|
copyvec(lights[i]->attenuation, m->light_attenuation+3*i, 3);
|
||||||
@@ -3027,7 +3009,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// materials
|
// materials
|
||||||
for (i=0; i<nmat; i++) {
|
for (int i=0; i<nmat; i++) {
|
||||||
copyvec(materials[i]->texrepeat, m->mat_texrepeat+2*i, 2);
|
copyvec(materials[i]->texrepeat, m->mat_texrepeat+2*i, 2);
|
||||||
materials[i]->emission = m->mat_emission[i];
|
materials[i]->emission = m->mat_emission[i];
|
||||||
materials[i]->specular = m->mat_specular[i];
|
materials[i]->specular = m->mat_specular[i];
|
||||||
@@ -3037,7 +3019,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// pairs
|
// pairs
|
||||||
for (i=0; i<npair; i++) {
|
for (int i=0; i<npair; i++) {
|
||||||
copyvec(pairs[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
|
copyvec(pairs[i]->solref, m->pair_solref+mjNREF*i, mjNREF);
|
||||||
copyvec(pairs[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
|
copyvec(pairs[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP);
|
||||||
pairs[i]->margin = (double)m->pair_margin[i];
|
pairs[i]->margin = (double)m->pair_margin[i];
|
||||||
@@ -3046,14 +3028,14 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// equality constraints
|
// equality constraints
|
||||||
for (i=0; i<neq; i++) {
|
for (int i=0; i<neq; i++) {
|
||||||
copyvec(equalities[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA);
|
copyvec(equalities[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA);
|
||||||
copyvec(equalities[i]->solref, m->eq_solref+mjNREF*i, mjNREF);
|
copyvec(equalities[i]->solref, m->eq_solref+mjNREF*i, mjNREF);
|
||||||
copyvec(equalities[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP);
|
copyvec(equalities[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP);
|
||||||
}
|
}
|
||||||
|
|
||||||
// tendons
|
// tendons
|
||||||
for (i=0; i<ntendon; i++) {
|
for (int i=0; i<ntendon; i++) {
|
||||||
copyvec(tendons[i]->range, m->tendon_range+2*i, 2);
|
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]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF);
|
||||||
copyvec(tendons[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
|
copyvec(tendons[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP);
|
||||||
@@ -3073,7 +3055,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
|
|
||||||
// actuators
|
// actuators
|
||||||
mjCActuator* pa;
|
mjCActuator* pa;
|
||||||
for (i=0; i<nu; i++) {
|
for (int i=0; i<nu; i++) {
|
||||||
pa = actuators[i];
|
pa = actuators[i];
|
||||||
|
|
||||||
copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN);
|
copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN);
|
||||||
@@ -3092,7 +3074,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// sensors
|
// sensors
|
||||||
for (i=0; i<nsensor; i++) {
|
for (int i=0; i<nsensor; i++) {
|
||||||
sensors[i]->cutoff = (double)m->sensor_cutoff[i];
|
sensors[i]->cutoff = (double)m->sensor_cutoff[i];
|
||||||
sensors[i]->noise = (double)m->sensor_noise[i];
|
sensors[i]->noise = (double)m->sensor_noise[i];
|
||||||
|
|
||||||
@@ -3102,21 +3084,21 @@ bool mjCModel::CopyBack(const mjModel* m) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// numeric data
|
// numeric data
|
||||||
for (i=0; i<nnumeric; i++) {
|
for (int i=0; i<nnumeric; i++) {
|
||||||
for (j=0; j<m->numeric_size[i]; j++) {
|
for (int j=0; j<m->numeric_size[i]; j++) {
|
||||||
numerics[i]->data[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
|
numerics[i]->data[j] = (double)m->numeric_data[m->numeric_adr[i]+j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// tuple data
|
// tuple data
|
||||||
for (i=0; i<ntuple; i++) {
|
for (int i=0; i<ntuple; i++) {
|
||||||
for (j=0; j<m->tuple_size[i]; j++) {
|
for (int j=0; j<m->tuple_size[i]; j++) {
|
||||||
tuples[i]->objprm[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
|
tuples[i]->objprm[j] = (double)m->tuple_objprm[m->tuple_adr[i]+j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// keyframes
|
// keyframes
|
||||||
for (i=0; i<m->nkey; i++) {
|
for (int i=0; i<m->nkey; i++) {
|
||||||
mjCKey* pk = keys[i];
|
mjCKey* pk = keys[i];
|
||||||
|
|
||||||
pk->time = (double)m->key_time[i];
|
pk->time = (double)m->key_time[i];
|
||||||
|
|||||||
+67
-82
@@ -356,15 +356,13 @@ mjCBody::mjCBody(mjCModel* _model) {
|
|||||||
|
|
||||||
// destructor
|
// destructor
|
||||||
mjCBody::~mjCBody() {
|
mjCBody::~mjCBody() {
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// delete objects allocated here
|
// delete objects allocated here
|
||||||
for (i=0; i<bodies.size(); i++) delete bodies[i];
|
for (int i=0; i<bodies.size(); i++) delete bodies[i];
|
||||||
for (i=0; i<geoms.size(); i++) delete geoms[i];
|
for (int i=0; i<geoms.size(); i++) delete geoms[i];
|
||||||
for (i=0; i<joints.size(); i++) delete joints[i];
|
for (int i=0; i<joints.size(); i++) delete joints[i];
|
||||||
for (i=0; i<sites.size(); i++) delete sites[i];
|
for (int i=0; i<sites.size(); i++) delete sites[i];
|
||||||
for (i=0; i<cameras.size(); i++) delete cameras[i];
|
for (int i=0; i<cameras.size(); i++) delete cameras[i];
|
||||||
for (i=0; i<lights.size(); i++) delete lights[i];
|
for (int i=0; i<lights.size(); i++) delete lights[i];
|
||||||
|
|
||||||
bodies.clear();
|
bodies.clear();
|
||||||
geoms.clear();
|
geoms.clear();
|
||||||
@@ -576,14 +574,14 @@ mjCBase* mjCBody::FindObject(mjtObj type, string _name, bool recursive) {
|
|||||||
|
|
||||||
// compute geom inertial frame: ipos, iquat, mass, inertia
|
// compute geom inertial frame: ipos, iquat, mass, inertia
|
||||||
void mjCBody::GeomFrame(void) {
|
void mjCBody::GeomFrame(void) {
|
||||||
int i, sz;
|
int sz;
|
||||||
double com[3] = {0, 0, 0};
|
double com[3] = {0, 0, 0};
|
||||||
double toti[6] = {0, 0, 0, 0, 0, 0};
|
double toti[6] = {0, 0, 0, 0, 0, 0};
|
||||||
vector<mjCGeom*> sel;
|
vector<mjCGeom*> sel;
|
||||||
|
|
||||||
// select geoms based on group
|
// select geoms based on group
|
||||||
sel.clear();
|
sel.clear();
|
||||||
for (i=0; i<geoms.size(); i++) {
|
for (int i=0; i<geoms.size(); i++) {
|
||||||
if (geoms[i]->group>=model->inertiagrouprange[0] &&
|
if (geoms[i]->group>=model->inertiagrouprange[0] &&
|
||||||
geoms[i]->group<=model->inertiagrouprange[1]) {
|
geoms[i]->group<=model->inertiagrouprange[1]) {
|
||||||
sel.push_back(geoms[i]);
|
sel.push_back(geoms[i]);
|
||||||
@@ -603,7 +601,7 @@ void mjCBody::GeomFrame(void) {
|
|||||||
else if (sz>1) {
|
else if (sz>1) {
|
||||||
// compute total mass and center of mass
|
// compute total mass and center of mass
|
||||||
mass = 0;
|
mass = 0;
|
||||||
for (i=0; i<sz; i++) {
|
for (int i=0; i<sz; i++) {
|
||||||
mass += sel[i]->mass;
|
mass += sel[i]->mass;
|
||||||
com[0] += sel[i]->mass * sel[i]->pos[0];
|
com[0] += sel[i]->mass * sel[i]->pos[0];
|
||||||
com[1] += sel[i]->mass * sel[i]->pos[1];
|
com[1] += sel[i]->mass * sel[i]->pos[1];
|
||||||
@@ -621,7 +619,7 @@ void mjCBody::GeomFrame(void) {
|
|||||||
ipos[2] = com[2]/mass;
|
ipos[2] = com[2]/mass;
|
||||||
|
|
||||||
// add geom inertias
|
// add geom inertias
|
||||||
for (i=0; i<sz; i++) {
|
for (int i=0; i<sz; i++) {
|
||||||
double inert0[6], inert1[6];
|
double inert0[6], inert1[6];
|
||||||
double dpos[3] = {
|
double dpos[3] = {
|
||||||
sel[i]->pos[0] - ipos[0],
|
sel[i]->pos[0] - ipos[0],
|
||||||
@@ -820,8 +818,6 @@ int mjCBody::MakeBVH(std::vector<mjCGeom *>& elements, int lev) {
|
|||||||
|
|
||||||
// compiler
|
// compiler
|
||||||
void mjCBody::Compile(void) {
|
void mjCBody::Compile(void) {
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
// resize userdata
|
// resize userdata
|
||||||
if (userdata.size() > model->nuser_body) {
|
if (userdata.size() > model->nuser_body) {
|
||||||
throw mjCError(this, "user has more values than nuser_body in body '%s' (id = %d)",
|
throw mjCError(this, "user has more values than nuser_body in body '%s' (id = %d)",
|
||||||
@@ -839,7 +835,7 @@ void mjCBody::Compile(void) {
|
|||||||
mjuu_normvec(iquat, 4);
|
mjuu_normvec(iquat, 4);
|
||||||
|
|
||||||
// set parentid and weldid of children
|
// set parentid and weldid of children
|
||||||
for (i=0; i<bodies.size(); i++) {
|
for (int i=0; i<bodies.size(); i++) {
|
||||||
bodies[i]->parentid = id;
|
bodies[i]->parentid = id;
|
||||||
bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid);
|
bodies[i]->weldid = (!bodies[i]->joints.empty() ? bodies[i]->id : weldid);
|
||||||
}
|
}
|
||||||
@@ -857,7 +853,7 @@ void mjCBody::Compile(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// compile all geoms, phase 1
|
// compile all geoms, phase 1
|
||||||
for (i=0; i<geoms.size(); i++) {
|
for (int i=0; i<geoms.size(); i++) {
|
||||||
geoms[i]->inferinertia = id>0 &&
|
geoms[i]->inferinertia = id>0 &&
|
||||||
(!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
|
(!explicitinertial || model->inertiafromgeom == mjINERTIAFROMGEOM_TRUE) &&
|
||||||
geoms[i]->group >= model->inertiagrouprange[0] &&
|
geoms[i]->group >= model->inertiagrouprange[0] &&
|
||||||
@@ -924,7 +920,7 @@ void mjCBody::Compile(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// make local frames of geoms
|
// make local frames of geoms
|
||||||
for (i=0; i<geoms.size(); i++) {
|
for (int i=0; i<geoms.size(); i++) {
|
||||||
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
|
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -935,7 +931,7 @@ void mjCBody::Compile(void) {
|
|||||||
|
|
||||||
// compile all joints, count dofs
|
// compile all joints, count dofs
|
||||||
dofnum = 0;
|
dofnum = 0;
|
||||||
for (i=0; i<joints.size(); i++) {
|
for (int i=0; i<joints.size(); i++) {
|
||||||
dofnum += joints[i]->Compile();
|
dofnum += joints[i]->Compile();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -946,7 +942,7 @@ void mjCBody::Compile(void) {
|
|||||||
|
|
||||||
// check for rotation dof after ball joint
|
// check for rotation dof after ball joint
|
||||||
bool hasball = false;
|
bool hasball = false;
|
||||||
for (i=0; i<joints.size(); i++) {
|
for (int i=0; i<joints.size(); i++) {
|
||||||
if ((joints[i]->type==mjJNT_BALL || joints[i]->type==mjJNT_HINGE) && hasball) {
|
if ((joints[i]->type==mjJNT_BALL || joints[i]->type==mjJNT_HINGE) && hasball) {
|
||||||
throw mjCError(this, "ball followed by rotation in body '%s'", name.c_str());
|
throw mjCError(this, "ball followed by rotation in body '%s'", name.c_str());
|
||||||
}
|
}
|
||||||
@@ -962,13 +958,13 @@ void mjCBody::Compile(void) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// compile all sites
|
// compile all sites
|
||||||
for (i=0; i<sites.size(); i++) sites[i]->Compile();
|
for (int i=0; i<sites.size(); i++) sites[i]->Compile();
|
||||||
|
|
||||||
// compile all cameras
|
// compile all cameras
|
||||||
for (i=0; i<cameras.size(); i++) cameras[i]->Compile();
|
for (int i=0; i<cameras.size(); i++) cameras[i]->Compile();
|
||||||
|
|
||||||
// compile all lights
|
// compile all lights
|
||||||
for (i=0; i<lights.size(); i++) lights[i]->Compile();
|
for (int i=0; i<lights.size(); i++) lights[i]->Compile();
|
||||||
|
|
||||||
// plugin
|
// plugin
|
||||||
if (is_plugin) {
|
if (is_plugin) {
|
||||||
@@ -2217,11 +2213,10 @@ mjCTexture::~mjCTexture() {
|
|||||||
// insert random dots
|
// insert random dots
|
||||||
static void randomdot(unsigned char* rgb, const double* markrgb,
|
static void randomdot(unsigned char* rgb, const double* markrgb,
|
||||||
int width, int height, double probability) {
|
int width, int height, double probability) {
|
||||||
int r, c, j;
|
for (int r=0; r<height; r++) {
|
||||||
for (r=0; r<height; r++) {
|
for (int c=0; c<width; c++) {
|
||||||
for (c=0; c<width; c++) {
|
|
||||||
if (rand()<probability*RAND_MAX) {
|
if (rand()<probability*RAND_MAX) {
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
rgb[3*(r*width+c)+j] = (mjtByte)(255*markrgb[j]);
|
rgb[3*(r*width+c)+j] = (mjtByte)(255*markrgb[j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2251,25 +2246,23 @@ static void interp(unsigned char* rgb, const double* rgb1, const double* rgb2, d
|
|||||||
// make checker pattern for one side
|
// make checker pattern for one side
|
||||||
static void checker(unsigned char* rgb, const unsigned char* RGB1, const unsigned char* RGB2,
|
static void checker(unsigned char* rgb, const unsigned char* RGB1, const unsigned char* RGB2,
|
||||||
int width, int height) {
|
int width, int height) {
|
||||||
int r, c;
|
for (int r=0; r<height/2; r++) {
|
||||||
|
for (int c=0; c<width/2; c++) {
|
||||||
for (r=0; r<height/2; r++) {
|
|
||||||
for (c=0; c<width/2; c++) {
|
|
||||||
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (r=height/2; r<height; r++) {
|
for (int r=height/2; r<height; r++) {
|
||||||
for (c=width/2; c<width; c++) {
|
for (int c=width/2; c<width; c++) {
|
||||||
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (r=0; r<height/2; r++) {
|
for (int r=0; r<height/2; r++) {
|
||||||
for (c=width/2; c<width; c++) {
|
for (int c=width/2; c<width; c++) {
|
||||||
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
for (r=height/2; r<height; r++) {
|
for (int r=height/2; r<height; r++) {
|
||||||
for (c=0; c<width/2; c++) {
|
for (int c=0; c<width/2; c++) {
|
||||||
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
memcpy(rgb+3*(r*width+c), RGB2, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2280,10 +2273,8 @@ static void checker(unsigned char* rgb, const unsigned char* RGB1, const unsigne
|
|||||||
// make builtin: 2D
|
// make builtin: 2D
|
||||||
void mjCTexture::Builtin2D(void) {
|
void mjCTexture::Builtin2D(void) {
|
||||||
unsigned char RGB1[3], RGB2[3], RGBm[3];
|
unsigned char RGB1[3], RGB2[3], RGBm[3];
|
||||||
int r, c, j;
|
|
||||||
|
|
||||||
// convert fixed colors
|
// convert fixed colors
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
RGB1[j] = (mjtByte)(255*rgb1[j]);
|
RGB1[j] = (mjtByte)(255*rgb1[j]);
|
||||||
RGB2[j] = (mjtByte)(255*rgb2[j]);
|
RGB2[j] = (mjtByte)(255*rgb2[j]);
|
||||||
RGBm[j] = (mjtByte)(255*markrgb[j]);
|
RGBm[j] = (mjtByte)(255*markrgb[j]);
|
||||||
@@ -2293,8 +2284,8 @@ void mjCTexture::Builtin2D(void) {
|
|||||||
|
|
||||||
// gradient
|
// gradient
|
||||||
if (builtin==mjBUILTIN_GRADIENT) {
|
if (builtin==mjBUILTIN_GRADIENT) {
|
||||||
for (r=0; r<height; r++) {
|
for (int r=0; r<height; r++) {
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
// compute normalized coordinates and radius
|
// compute normalized coordinates and radius
|
||||||
double x = 2*c/((double)(width-1)) - 1;
|
double x = 2*c/((double)(width-1)) - 1;
|
||||||
double y = 1 - 2*r/((double)(height-1));
|
double y = 1 - 2*r/((double)(height-1));
|
||||||
@@ -2313,8 +2304,8 @@ void mjCTexture::Builtin2D(void) {
|
|||||||
|
|
||||||
// flat
|
// flat
|
||||||
else if (builtin==mjBUILTIN_FLAT) {
|
else if (builtin==mjBUILTIN_FLAT) {
|
||||||
for (r=0; r<height; r++) {
|
for (int r=0; r<height; r++) {
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
memcpy(rgb+3*(r*width+c), RGB1, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2324,11 +2315,11 @@ void mjCTexture::Builtin2D(void) {
|
|||||||
|
|
||||||
// edge
|
// edge
|
||||||
if (mark==mjMARK_EDGE) {
|
if (mark==mjMARK_EDGE) {
|
||||||
for (r=0; r<height; r++) {
|
for (int r=0; r<height; r++) {
|
||||||
memcpy(rgb+3*(r*width+0), RGBm, 3);
|
memcpy(rgb+3*(r*width+0), RGBm, 3);
|
||||||
memcpy(rgb+3*(r*width+width-1), RGBm, 3);
|
memcpy(rgb+3*(r*width+width-1), RGBm, 3);
|
||||||
}
|
}
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
memcpy(rgb+3*(0*width+c), RGBm, 3);
|
memcpy(rgb+3*(0*width+c), RGBm, 3);
|
||||||
memcpy(rgb+3*((height-1)*width+c), RGBm, 3);
|
memcpy(rgb+3*((height-1)*width+c), RGBm, 3);
|
||||||
}
|
}
|
||||||
@@ -2336,10 +2327,10 @@ void mjCTexture::Builtin2D(void) {
|
|||||||
|
|
||||||
// cross
|
// cross
|
||||||
else if (mark==mjMARK_CROSS) {
|
else if (mark==mjMARK_CROSS) {
|
||||||
for (r=0; r<height; r++) {
|
for (int r=0; r<height; r++) {
|
||||||
memcpy(rgb+3*(r*width+width/2), RGBm, 3);
|
memcpy(rgb+3*(r*width+width/2), RGBm, 3);
|
||||||
}
|
}
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
memcpy(rgb+3*(height/2*width+c), RGBm, 3);
|
memcpy(rgb+3*(height/2*width+c), RGBm, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2355,10 +2346,9 @@ void mjCTexture::Builtin2D(void) {
|
|||||||
// make builtin: Cube
|
// make builtin: Cube
|
||||||
void mjCTexture::BuiltinCube(void) {
|
void mjCTexture::BuiltinCube(void) {
|
||||||
unsigned char RGB1[3], RGB2[3], RGBm[3], RGBi[3];
|
unsigned char RGB1[3], RGB2[3], RGBm[3], RGBi[3];
|
||||||
int r, c, j;
|
|
||||||
|
|
||||||
// convert fixed colors
|
// convert fixed colors
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
RGB1[j] = (mjtByte)(255*rgb1[j]);
|
RGB1[j] = (mjtByte)(255*rgb1[j]);
|
||||||
RGB2[j] = (mjtByte)(255*rgb2[j]);
|
RGB2[j] = (mjtByte)(255*rgb2[j]);
|
||||||
RGBm[j] = (mjtByte)(255*markrgb[j]);
|
RGBm[j] = (mjtByte)(255*markrgb[j]);
|
||||||
@@ -2368,8 +2358,8 @@ void mjCTexture::BuiltinCube(void) {
|
|||||||
|
|
||||||
// gradient
|
// gradient
|
||||||
if (builtin==mjBUILTIN_GRADIENT) {
|
if (builtin==mjBUILTIN_GRADIENT) {
|
||||||
for (r=0; r<width; r++) {
|
for (int r=0; r<width; r++) {
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
// compute normalized pixel coordinates
|
// compute normalized pixel coordinates
|
||||||
double x = 2*c/((double)(width-1)) - 1;
|
double x = 2*c/((double)(width-1)) - 1;
|
||||||
double y = 1 - 2*r/((double)(width-1));
|
double y = 1 - 2*r/((double)(width-1));
|
||||||
@@ -2404,8 +2394,8 @@ void mjCTexture::BuiltinCube(void) {
|
|||||||
|
|
||||||
// flat
|
// flat
|
||||||
else if (builtin==mjBUILTIN_FLAT) {
|
else if (builtin==mjBUILTIN_FLAT) {
|
||||||
for (r=0; r<width; r++) {
|
for (int r=0; r<width; r++) {
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
// set sides and up
|
// set sides and up
|
||||||
memcpy(rgb+0*3*width*width+3*(r*width+c), RGB1, 3);
|
memcpy(rgb+0*3*width*width+3*(r*width+c), RGB1, 3);
|
||||||
memcpy(rgb+1*3*width*width+3*(r*width+c), RGB1, 3);
|
memcpy(rgb+1*3*width*width+3*(r*width+c), RGB1, 3);
|
||||||
@@ -2423,12 +2413,12 @@ void mjCTexture::BuiltinCube(void) {
|
|||||||
|
|
||||||
// edge
|
// edge
|
||||||
if (mark==mjMARK_EDGE) {
|
if (mark==mjMARK_EDGE) {
|
||||||
for (j=0; j<6; j++) {
|
for (int j=0; j<6; j++) {
|
||||||
for (r=0; r<width; r++) {
|
for (int r=0; r<width; r++) {
|
||||||
memcpy(rgb+j*3*width*width+3*(r*width+0), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*(r*width+0), RGBm, 3);
|
||||||
memcpy(rgb+j*3*width*width+3*(r*width+width-1), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*(r*width+width-1), RGBm, 3);
|
||||||
}
|
}
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
memcpy(rgb+j*3*width*width+3*(0*width+c), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*(0*width+c), RGBm, 3);
|
||||||
memcpy(rgb+j*3*width*width+3*((width-1)*width+c), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*((width-1)*width+c), RGBm, 3);
|
||||||
}
|
}
|
||||||
@@ -2437,11 +2427,11 @@ void mjCTexture::BuiltinCube(void) {
|
|||||||
|
|
||||||
// cross
|
// cross
|
||||||
else if (mark==mjMARK_CROSS) {
|
else if (mark==mjMARK_CROSS) {
|
||||||
for (j=0; j<6; j++) {
|
for (int j=0; j<6; j++) {
|
||||||
for (r=0; r<width; r++) {
|
for (int r=0; r<width; r++) {
|
||||||
memcpy(rgb+j*3*width*width+3*(r*width+width/2), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*(r*width+width/2), RGBm, 3);
|
||||||
}
|
}
|
||||||
for (c=0; c<width; c++) {
|
for (int c=0; c<width; c++) {
|
||||||
memcpy(rgb+j*3*width*width+3*(width/2*width+c), RGBm, 3);
|
memcpy(rgb+j*3*width*width+3*(width/2*width+c), RGBm, 3);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2637,8 +2627,6 @@ void mjCTexture::Load2D(string filename, const mjVFS* vfs) {
|
|||||||
|
|
||||||
// load cube or skybox from single file (repeated or grid)
|
// load cube or skybox from single file (repeated or grid)
|
||||||
void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
||||||
int i, j, k, s;
|
|
||||||
|
|
||||||
// check gridsize
|
// check gridsize
|
||||||
if (gridsize[0]<1 || gridsize[1]<1 || gridsize[0]*gridsize[1]>12) {
|
if (gridsize[0]<1 || gridsize[1]<1 || gridsize[0]*gridsize[1]>12) {
|
||||||
throw mjCError(this,
|
throw mjCError(this,
|
||||||
@@ -2685,9 +2673,9 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
|||||||
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
// process grid
|
// process grid
|
||||||
for (k=0; k<gridsize[0]*gridsize[1]; k++) {
|
for (int k=0; k<gridsize[0]*gridsize[1]; k++) {
|
||||||
// decode face symbol
|
// decode face symbol
|
||||||
i = -1;
|
int i = -1;
|
||||||
if (gridlayout[k]=='R') {
|
if (gridlayout[k]=='R') {
|
||||||
i = 0;
|
i = 0;
|
||||||
} else if (gridlayout[k]=='L') {
|
} else if (gridlayout[k]=='L') {
|
||||||
@@ -2709,7 +2697,7 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
|||||||
// extract sub-image
|
// extract sub-image
|
||||||
int rstart = width*(k/gridsize[1]);
|
int rstart = width*(k/gridsize[1]);
|
||||||
int cstart = width*(k%gridsize[1]);
|
int cstart = width*(k%gridsize[1]);
|
||||||
for (j=0; j<width; j++) {
|
for (int j=0; j<width; j++) {
|
||||||
memcpy(rgb+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
|
memcpy(rgb+i*3*width*width+j*3*width, image.data()+(j+rstart)*3*w+3*cstart, 3*width);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2719,11 +2707,11 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// set undefined faces to rgb1
|
// set undefined faces to rgb1
|
||||||
for (i=0; i<6; i++) {
|
for (int i=0; i<6; i++) {
|
||||||
if (!loaded[i]) {
|
if (!loaded[i]) {
|
||||||
for (k=0; k<width; k++) {
|
for (int k=0; k<width; k++) {
|
||||||
for (s=0; s<width; s++) {
|
for (int s=0; s<width; s++) {
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
|
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -2739,13 +2727,11 @@ void mjCTexture::LoadCubeSingle(string filename, const mjVFS* vfs) {
|
|||||||
|
|
||||||
// load cube or skybox from separate file
|
// load cube or skybox from separate file
|
||||||
void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
|
void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
|
||||||
int i, j, k, s;
|
|
||||||
|
|
||||||
// keep track of which faces were defined
|
// keep track of which faces were defined
|
||||||
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
int loaded[6] = {0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
// process nonempty files
|
// process nonempty files
|
||||||
for (i=0; i<6; i++) {
|
for (int i=0; i<6; i++) {
|
||||||
if (!cubefiles[i].empty()) {
|
if (!cubefiles[i].empty()) {
|
||||||
// remove path from file if necessary
|
// remove path from file if necessary
|
||||||
if (model->strippath) {
|
if (model->strippath) {
|
||||||
@@ -2796,11 +2782,11 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// set undefined faces to rgb1
|
// set undefined faces to rgb1
|
||||||
for (i=0; i<6; i++) {
|
for (int i=0; i<6; i++) {
|
||||||
if (!loaded[i]) {
|
if (!loaded[i]) {
|
||||||
for (k=0; k<width; k++) {
|
for (int k=0; k<width; k++) {
|
||||||
for (s=0; s<width; s++) {
|
for (int s=0; s<width; s++) {
|
||||||
for (j=0; j<3; j++) {
|
for (int j=0; j<3; j++) {
|
||||||
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
|
rgb[i*3*width*width + 3*(k*width+s) + j] = (mjtByte)(255*rgb1[j]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -4419,12 +4405,11 @@ mjCKey::~mjCKey() {
|
|||||||
|
|
||||||
// compiler
|
// compiler
|
||||||
void mjCKey::Compile(const mjModel* m) {
|
void mjCKey::Compile(const mjModel* m) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// qpos: allocate or check size
|
// qpos: allocate or check size
|
||||||
if (qpos.empty()) {
|
if (qpos.empty()) {
|
||||||
qpos.resize(m->nq);
|
qpos.resize(m->nq);
|
||||||
for (i=0; i<m->nq; i++) {
|
for (int i=0; i<m->nq; i++) {
|
||||||
qpos[i] = (double)m->qpos0[i];
|
qpos[i] = (double)m->qpos0[i];
|
||||||
}
|
}
|
||||||
} else if (qpos.size()!=m->nq) {
|
} else if (qpos.size()!=m->nq) {
|
||||||
@@ -4434,7 +4419,7 @@ void mjCKey::Compile(const mjModel* m) {
|
|||||||
// qvel: allocate or check size
|
// qvel: allocate or check size
|
||||||
if (qvel.empty()) {
|
if (qvel.empty()) {
|
||||||
qvel.resize(m->nv);
|
qvel.resize(m->nv);
|
||||||
for (i=0; i<m->nv; i++) {
|
for (int i=0; i<m->nv; i++) {
|
||||||
qvel[i] = 0;
|
qvel[i] = 0;
|
||||||
}
|
}
|
||||||
} else if (qvel.size()!=m->nv) {
|
} else if (qvel.size()!=m->nv) {
|
||||||
@@ -4444,7 +4429,7 @@ void mjCKey::Compile(const mjModel* m) {
|
|||||||
// act: allocate or check size
|
// act: allocate or check size
|
||||||
if (act.empty()) {
|
if (act.empty()) {
|
||||||
act.resize(m->na);
|
act.resize(m->na);
|
||||||
for (i=0; i<m->na; i++) {
|
for (int i=0; i<m->na; i++) {
|
||||||
act[i] = 0;
|
act[i] = 0;
|
||||||
}
|
}
|
||||||
} else if (act.size()!=m->na) {
|
} else if (act.size()!=m->na) {
|
||||||
@@ -4455,7 +4440,7 @@ void mjCKey::Compile(const mjModel* m) {
|
|||||||
if (mpos.empty()) {
|
if (mpos.empty()) {
|
||||||
mpos.resize(3*m->nmocap);
|
mpos.resize(3*m->nmocap);
|
||||||
if (m->nmocap) {
|
if (m->nmocap) {
|
||||||
for (i=0; i<m->nbody; i++) {
|
for (int i=0; i<m->nbody; i++) {
|
||||||
if (m->body_mocapid[i]>=0) {
|
if (m->body_mocapid[i]>=0) {
|
||||||
int mocapid = m->body_mocapid[i];
|
int mocapid = m->body_mocapid[i];
|
||||||
mpos[3*mocapid] = m->body_pos[3*i];
|
mpos[3*mocapid] = m->body_pos[3*i];
|
||||||
@@ -4472,7 +4457,7 @@ void mjCKey::Compile(const mjModel* m) {
|
|||||||
if (mquat.empty()) {
|
if (mquat.empty()) {
|
||||||
mquat.resize(4*m->nmocap);
|
mquat.resize(4*m->nmocap);
|
||||||
if (m->nmocap) {
|
if (m->nmocap) {
|
||||||
for (i=0; i<m->nbody; i++) {
|
for (int i=0; i<m->nbody; i++) {
|
||||||
if (m->body_mocapid[i]>=0) {
|
if (m->body_mocapid[i]>=0) {
|
||||||
int mocapid = m->body_mocapid[i];
|
int mocapid = m->body_mocapid[i];
|
||||||
mquat[4*mocapid] = m->body_quat[4*i];
|
mquat[4*mocapid] = m->body_quat[4*i];
|
||||||
@@ -4489,7 +4474,7 @@ void mjCKey::Compile(const mjModel* m) {
|
|||||||
// ctrl: allocate or check size
|
// ctrl: allocate or check size
|
||||||
if (ctrl.empty()) {
|
if (ctrl.empty()) {
|
||||||
ctrl.resize(m->nu);
|
ctrl.resize(m->nu);
|
||||||
for (i=0; i<m->nu; i++) {
|
for (int i=0; i<m->nu; i++) {
|
||||||
ctrl[i] = 0;
|
ctrl[i] = 0;
|
||||||
}
|
}
|
||||||
} else if (ctrl.size()!=m->nu) {
|
} else if (ctrl.size()!=m->nu) {
|
||||||
|
|||||||
@@ -141,9 +141,8 @@ double mjuu_L1(const double* a, const double* b, int n) {
|
|||||||
// normalize vector to unit length, return previous length
|
// normalize vector to unit length, return previous length
|
||||||
double mjuu_normvec(double* vec, const int n) {
|
double mjuu_normvec(double* vec, const int n) {
|
||||||
double nrm = 0;
|
double nrm = 0;
|
||||||
int i;
|
|
||||||
|
|
||||||
for (i=0; i<n; i++) {
|
for (int i=0; i<n; i++) {
|
||||||
nrm += vec[i]*vec[i];
|
nrm += vec[i]*vec[i];
|
||||||
}
|
}
|
||||||
if (nrm < mjEPS) {
|
if (nrm < mjEPS) {
|
||||||
@@ -151,7 +150,7 @@ double mjuu_normvec(double* vec, const int n) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
nrm = sqrt(nrm);
|
nrm = sqrt(nrm);
|
||||||
for (i=0; i<n; i++) {
|
for (int i=0; i<n; i++) {
|
||||||
vec[i] /= nrm;
|
vec[i] /= nrm;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -730,8 +730,6 @@ void mjXReader::PrintSchema(std::stringstream& str, bool html, bool pad) {
|
|||||||
// main entry point for XML parser
|
// main entry point for XML parser
|
||||||
// mjCModel is allocated here; caller is responsible for deallocation
|
// mjCModel is allocated here; caller is responsible for deallocation
|
||||||
void mjXReader::Parse(XMLElement* root) {
|
void mjXReader::Parse(XMLElement* root) {
|
||||||
XMLElement *section;
|
|
||||||
|
|
||||||
// check schema
|
// check schema
|
||||||
if (!schema.GetError().empty()) {
|
if (!schema.GetError().empty()) {
|
||||||
throw mjXError(0, "XML Schema Construction Error: %s\n",
|
throw mjXError(0, "XML Schema Construction Error: %s\n",
|
||||||
@@ -757,86 +755,86 @@ void mjXReader::Parse(XMLElement* root) {
|
|||||||
|
|
||||||
//------------------- parse MuJoCo sections embedded in all XML formats
|
//------------------- parse MuJoCo sections embedded in all XML formats
|
||||||
|
|
||||||
for (section = root->FirstChildElement("compiler"); section;
|
for (XMLElement* section = root->FirstChildElement("compiler"); section;
|
||||||
section = section->NextSiblingElement("compiler")) {
|
section = section->NextSiblingElement("compiler")) {
|
||||||
Compiler(section, model);
|
Compiler(section, model);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("option"); section;
|
for (XMLElement* section = root->FirstChildElement("option"); section;
|
||||||
section = section->NextSiblingElement("option")) {
|
section = section->NextSiblingElement("option")) {
|
||||||
Option(section, &model->option);
|
Option(section, &model->option);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("size"); section;
|
for (XMLElement* section = root->FirstChildElement("size"); section;
|
||||||
section = section->NextSiblingElement("size")) {
|
section = section->NextSiblingElement("size")) {
|
||||||
Size(section, model);
|
Size(section, model);
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------ parse MJCF-specific sections
|
//------------------ parse MJCF-specific sections
|
||||||
|
|
||||||
for (section = root->FirstChildElement("visual"); section;
|
for (XMLElement* section = root->FirstChildElement("visual"); section;
|
||||||
section = section->NextSiblingElement("visual")) {
|
section = section->NextSiblingElement("visual")) {
|
||||||
Visual(section);
|
Visual(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("statistic"); section;
|
for (XMLElement* section = root->FirstChildElement("statistic"); section;
|
||||||
section = section->NextSiblingElement("statistic")) {
|
section = section->NextSiblingElement("statistic")) {
|
||||||
Statistic(section);
|
Statistic(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
readingdefaults = true;
|
readingdefaults = true;
|
||||||
for (section = root->FirstChildElement("default"); section;
|
for (XMLElement* section = root->FirstChildElement("default"); section;
|
||||||
section = section->NextSiblingElement("default")) {
|
section = section->NextSiblingElement("default")) {
|
||||||
Default(section, -1);
|
Default(section, -1);
|
||||||
}
|
}
|
||||||
readingdefaults = false;
|
readingdefaults = false;
|
||||||
|
|
||||||
for (section = root->FirstChildElement("extension"); section;
|
for (XMLElement* section = root->FirstChildElement("extension"); section;
|
||||||
section = section->NextSiblingElement("extension")) {
|
section = section->NextSiblingElement("extension")) {
|
||||||
Extension(section);
|
Extension(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("custom"); section;
|
for (XMLElement* section = root->FirstChildElement("custom"); section;
|
||||||
section = section->NextSiblingElement("custom")) {
|
section = section->NextSiblingElement("custom")) {
|
||||||
Custom(section);
|
Custom(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("asset"); section;
|
for (XMLElement* section = root->FirstChildElement("asset"); section;
|
||||||
section = section->NextSiblingElement("asset")) {
|
section = section->NextSiblingElement("asset")) {
|
||||||
Asset(section);
|
Asset(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("worldbody"); section;
|
for (XMLElement* section = root->FirstChildElement("worldbody"); section;
|
||||||
section = section->NextSiblingElement("worldbody")) {
|
section = section->NextSiblingElement("worldbody")) {
|
||||||
Body(section, model->GetWorld());
|
Body(section, model->GetWorld());
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("contact"); section;
|
for (XMLElement* section = root->FirstChildElement("contact"); section;
|
||||||
section = section->NextSiblingElement("contact")) {
|
section = section->NextSiblingElement("contact")) {
|
||||||
Contact(section);
|
Contact(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("equality"); section;
|
for (XMLElement* section = root->FirstChildElement("equality"); section;
|
||||||
section = section->NextSiblingElement("equality")) {
|
section = section->NextSiblingElement("equality")) {
|
||||||
Equality(section);
|
Equality(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("tendon"); section;
|
for (XMLElement* section = root->FirstChildElement("tendon"); section;
|
||||||
section = section->NextSiblingElement("tendon")) {
|
section = section->NextSiblingElement("tendon")) {
|
||||||
Tendon(section);
|
Tendon(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("actuator"); section;
|
for (XMLElement* section = root->FirstChildElement("actuator"); section;
|
||||||
section = section->NextSiblingElement("actuator")) {
|
section = section->NextSiblingElement("actuator")) {
|
||||||
Actuator(section);
|
Actuator(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("sensor"); section;
|
for (XMLElement* section = root->FirstChildElement("sensor"); section;
|
||||||
section = section->NextSiblingElement("sensor")) {
|
section = section->NextSiblingElement("sensor")) {
|
||||||
Sensor(section);
|
Sensor(section);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (section = root->FirstChildElement("keyframe"); section;
|
for (XMLElement* section = root->FirstChildElement("keyframe"); section;
|
||||||
section = section->NextSiblingElement("keyframe")) {
|
section = section->NextSiblingElement("keyframe")) {
|
||||||
Keyframe(section);
|
Keyframe(section);
|
||||||
}
|
}
|
||||||
@@ -1605,7 +1603,6 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
|
|||||||
|
|
||||||
// actuator element parser
|
// actuator element parser
|
||||||
void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
|
void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
|
||||||
int n;
|
|
||||||
string text, type;
|
string text, type;
|
||||||
double diameter;
|
double diameter;
|
||||||
|
|
||||||
@@ -1672,6 +1669,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
|
|||||||
// explicit attributes
|
// explicit attributes
|
||||||
if (type=="general") {
|
if (type=="general") {
|
||||||
// explicit attributes
|
// explicit attributes
|
||||||
|
int n;
|
||||||
if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) {
|
if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) {
|
||||||
pact->dyntype = (mjtDyn)n;
|
pact->dyntype = (mjtDyn)n;
|
||||||
}
|
}
|
||||||
@@ -1811,7 +1809,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
|
|||||||
ReadAttr(elem, "fvmax", 1, pact->gainprm+8, text);
|
ReadAttr(elem, "fvmax", 1, pact->gainprm+8, text);
|
||||||
|
|
||||||
// biasprm = gainprm
|
// biasprm = gainprm
|
||||||
for (n=0; n<9; n++) {
|
for (int n=0; n<9; n++) {
|
||||||
pact->biasprm[n] = pact->gainprm[n];
|
pact->biasprm[n] = pact->gainprm[n];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1135,7 +1135,6 @@ void mjXWriter::Extension(XMLElement* root) {
|
|||||||
// custom section
|
// custom section
|
||||||
void mjXWriter::Custom(XMLElement* root) {
|
void mjXWriter::Custom(XMLElement* root) {
|
||||||
XMLElement* elem;
|
XMLElement* elem;
|
||||||
int i, j;
|
|
||||||
|
|
||||||
// get sizes, skip section if empty
|
// get sizes, skip section if empty
|
||||||
int nnum = model->NumObjects(mjOBJ_NUMERIC);
|
int nnum = model->NumObjects(mjOBJ_NUMERIC);
|
||||||
@@ -1151,7 +1150,7 @@ void mjXWriter::Custom(XMLElement* root) {
|
|||||||
XMLElement* section = InsertEnd(root, "custom");
|
XMLElement* section = InsertEnd(root, "custom");
|
||||||
|
|
||||||
// write all numerics
|
// write all numerics
|
||||||
for (i=0; i<nnum; i++) {
|
for (int i=0; i<nnum; i++) {
|
||||||
mjCNumeric* ptr = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i);
|
mjCNumeric* ptr = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i);
|
||||||
elem = InsertEnd(section, "numeric");
|
elem = InsertEnd(section, "numeric");
|
||||||
WriteAttrTxt(elem, "name", ptr->name);
|
WriteAttrTxt(elem, "name", ptr->name);
|
||||||
@@ -1160,7 +1159,7 @@ void mjXWriter::Custom(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write all texts
|
// write all texts
|
||||||
for (i=0; i<ntxt; i++) {
|
for (int i=0; i<ntxt; i++) {
|
||||||
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
|
mjCText* ptr = (mjCText*)model->GetObject(mjOBJ_TEXT, i);
|
||||||
elem = InsertEnd(section, "text");
|
elem = InsertEnd(section, "text");
|
||||||
WriteAttrTxt(elem, "name", ptr->name);
|
WriteAttrTxt(elem, "name", ptr->name);
|
||||||
@@ -1168,13 +1167,13 @@ void mjXWriter::Custom(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write all tuples
|
// write all tuples
|
||||||
for (i=0; i<ntup; i++) {
|
for (int i=0; i<ntup; i++) {
|
||||||
mjCTuple* ptr = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i);
|
mjCTuple* ptr = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i);
|
||||||
elem = InsertEnd(section, "tuple");
|
elem = InsertEnd(section, "tuple");
|
||||||
WriteAttrTxt(elem, "name", ptr->name);
|
WriteAttrTxt(elem, "name", ptr->name);
|
||||||
|
|
||||||
// write objects in tuple
|
// write objects in tuple
|
||||||
for (j=0; j<(int)ptr->objtype.size(); j++) {
|
for (int j=0; j<(int)ptr->objtype.size(); j++) {
|
||||||
XMLElement* obj = InsertEnd(elem, "element");
|
XMLElement* obj = InsertEnd(elem, "element");
|
||||||
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j]));
|
WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype[j]));
|
||||||
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str());
|
WriteAttrTxt(obj, "objname", ptr->objname[j].c_str());
|
||||||
@@ -1191,7 +1190,6 @@ void mjXWriter::Custom(XMLElement* root) {
|
|||||||
// asset section
|
// asset section
|
||||||
void mjXWriter::Asset(XMLElement* root) {
|
void mjXWriter::Asset(XMLElement* root) {
|
||||||
XMLElement* elem;
|
XMLElement* elem;
|
||||||
int i;
|
|
||||||
|
|
||||||
// get sizes
|
// get sizes
|
||||||
int ntex = model->NumObjects(mjOBJ_TEXTURE);
|
int ntex = model->NumObjects(mjOBJ_TEXTURE);
|
||||||
@@ -1210,7 +1208,7 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
|
|
||||||
// write textures
|
// write textures
|
||||||
mjCTexture deftex(0);
|
mjCTexture deftex(0);
|
||||||
for (i=0; i<ntex; i++) {
|
for (int i=0; i<ntex; i++) {
|
||||||
// create element
|
// create element
|
||||||
mjCTexture* ptex = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i);
|
mjCTexture* ptex = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i);
|
||||||
elem = InsertEnd(section, "texture");
|
elem = InsertEnd(section, "texture");
|
||||||
@@ -1260,7 +1258,7 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write materials
|
// write materials
|
||||||
for (i=0; i<nmat; i++) {
|
for (int i=0; i<nmat; i++) {
|
||||||
// create element and write
|
// create element and write
|
||||||
mjCMaterial* pmat = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i);
|
mjCMaterial* pmat = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i);
|
||||||
elem = InsertEnd(section, "material");
|
elem = InsertEnd(section, "material");
|
||||||
@@ -1268,7 +1266,7 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write meshes
|
// write meshes
|
||||||
for (i=0; i<nmesh; i++) {
|
for (int i=0; i<nmesh; i++) {
|
||||||
// create element and write
|
// create element and write
|
||||||
mjCMesh* pmesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
|
mjCMesh* pmesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i);
|
||||||
elem = InsertEnd(section, "mesh");
|
elem = InsertEnd(section, "mesh");
|
||||||
@@ -1276,7 +1274,7 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write skins
|
// write skins
|
||||||
for (i=0; i<nskin; i++) {
|
for (int i=0; i<nskin; i++) {
|
||||||
// create element and write
|
// create element and write
|
||||||
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
|
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
|
||||||
elem = InsertEnd(section, "skin");
|
elem = InsertEnd(section, "skin");
|
||||||
@@ -1284,7 +1282,7 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write hfields
|
// write hfields
|
||||||
for (i=0; i<nhfield; i++) {
|
for (int i=0; i<nhfield; i++) {
|
||||||
// create element
|
// create element
|
||||||
mjCHField* phf = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i);
|
mjCHField* phf = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i);
|
||||||
elem = InsertEnd(section, "hfield");
|
elem = InsertEnd(section, "hfield");
|
||||||
@@ -1306,7 +1304,6 @@ void mjXWriter::Asset(XMLElement* root) {
|
|||||||
// recursive body writer
|
// recursive body writer
|
||||||
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
||||||
double unitq[4] = {1, 0, 0, 0};
|
double unitq[4] = {1, 0, 0, 0};
|
||||||
unsigned int i;
|
|
||||||
|
|
||||||
if (!body) {
|
if (!body) {
|
||||||
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
|
throw mjXError(0, "missing body in XML write"); // SHOULD NOT OCCUR
|
||||||
@@ -1340,27 +1337,27 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write joints
|
// write joints
|
||||||
for (i=0; i<body->joints.size(); i++) {
|
for (int i=0; i<body->joints.size(); i++) {
|
||||||
OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def);
|
OneJoint(InsertEnd(elem, "joint"), body->joints[i], body->joints[i]->def);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write geoms
|
// write geoms
|
||||||
for (i=0; i<body->geoms.size(); i++) {
|
for (int i=0; i<body->geoms.size(); i++) {
|
||||||
OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def);
|
OneGeom(InsertEnd(elem, "geom"), body->geoms[i], body->geoms[i]->def);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write sites
|
// write sites
|
||||||
for (i=0; i<body->sites.size(); i++) {
|
for (int i=0; i<body->sites.size(); i++) {
|
||||||
OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def);
|
OneSite(InsertEnd(elem, "site"), body->sites[i], body->sites[i]->def);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write cameras
|
// write cameras
|
||||||
for (i=0; i<body->cameras.size(); i++) {
|
for (int i=0; i<body->cameras.size(); i++) {
|
||||||
OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def);
|
OneCamera(InsertEnd(elem, "camera"), body->cameras[i], body->cameras[i]->def);
|
||||||
}
|
}
|
||||||
|
|
||||||
// write lights
|
// write lights
|
||||||
for (i=0; i<body->lights.size(); i++) {
|
for (int i=0; i<body->lights.size(); i++) {
|
||||||
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
|
OneLight(InsertEnd(elem, "light"), body->lights[i], body->lights[i]->def);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1387,7 +1384,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write child bodies recursively
|
// write child bodies recursively
|
||||||
for (i=0; i<body->bodies.size(); i++) {
|
for (int i=0; i<body->bodies.size(); i++) {
|
||||||
Body(InsertEnd(elem, "body"), body->bodies[i]);
|
Body(InsertEnd(elem, "body"), body->bodies[i]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -1397,7 +1394,6 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) {
|
|||||||
// collision section
|
// collision section
|
||||||
void mjXWriter::Contact(XMLElement* root) {
|
void mjXWriter::Contact(XMLElement* root) {
|
||||||
XMLElement* elem;
|
XMLElement* elem;
|
||||||
int i;
|
|
||||||
|
|
||||||
// get number of pairs of each type
|
// get number of pairs of each type
|
||||||
int npair = model->NumObjects(mjOBJ_PAIR);
|
int npair = model->NumObjects(mjOBJ_PAIR);
|
||||||
@@ -1412,7 +1408,7 @@ void mjXWriter::Contact(XMLElement* root) {
|
|||||||
XMLElement* section = InsertEnd(root, "contact");
|
XMLElement* section = InsertEnd(root, "contact");
|
||||||
|
|
||||||
// write all geom pairs
|
// write all geom pairs
|
||||||
for (i=0; i<npair; i++) {
|
for (int i=0; i<npair; i++) {
|
||||||
// create element and write
|
// create element and write
|
||||||
mjCPair* ppair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i);
|
mjCPair* ppair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i);
|
||||||
elem = InsertEnd(section, "pair");
|
elem = InsertEnd(section, "pair");
|
||||||
@@ -1420,7 +1416,7 @@ void mjXWriter::Contact(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// write all exclude pairs
|
// write all exclude pairs
|
||||||
for (i=0; i<nexclude; i++) {
|
for (int i=0; i<nexclude; i++) {
|
||||||
// create element
|
// create element
|
||||||
mjCBodyPair* pexclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i);
|
mjCBodyPair* pexclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i);
|
||||||
elem = InsertEnd(section, "exclude");
|
elem = InsertEnd(section, "exclude");
|
||||||
|
|||||||
+2
-2
@@ -74,7 +74,7 @@ void mjXURDF::Clear(void) {
|
|||||||
void mjXURDF::Parse(XMLElement* root) {
|
void mjXURDF::Parse(XMLElement* root) {
|
||||||
std::string name, text;
|
std::string name, text;
|
||||||
XMLElement *elem, *temp;
|
XMLElement *elem, *temp;
|
||||||
int id_parent, id_child, i;
|
int id_parent, id_child;
|
||||||
|
|
||||||
// set compiler defaults suitable for URDF
|
// set compiler defaults suitable for URDF
|
||||||
model->strippath = true;
|
model->strippath = true;
|
||||||
@@ -158,7 +158,7 @@ void mjXURDF::Parse(XMLElement* root) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// find all top-level bodies, call recursive tree constructor
|
// find all top-level bodies, call recursive tree constructor
|
||||||
for (i=0; i<(int)urName.size(); i++) {
|
for (int i=0; i<(int)urName.size(); i++) {
|
||||||
if (urParent[i] < 0) {
|
if (urParent[i] < 0) {
|
||||||
AddToTree(i);
|
AddToTree(i);
|
||||||
}
|
}
|
||||||
|
|||||||
+8
-13
@@ -282,8 +282,6 @@ static int _max(int a, int b) {
|
|||||||
|
|
||||||
// print schema as text
|
// print schema as text
|
||||||
void mjXSchema::Print(std::stringstream& str, int level) {
|
void mjXSchema::Print(std::stringstream& str, int level) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// replace body with (world)body
|
// replace body with (world)body
|
||||||
string name1 = (name=="body" ? "(world)body" : name);
|
string name1 = (name=="body" ? "(world)body" : name);
|
||||||
|
|
||||||
@@ -297,7 +295,7 @@ void mjXSchema::Print(std::stringstream& str, int level) {
|
|||||||
|
|
||||||
// attributes
|
// attributes
|
||||||
int cnt = _max(baselen, 30);
|
int cnt = _max(baselen, 30);
|
||||||
for (i=0; i<(int)attr.size(); i++) {
|
for (int i=0; i<(int)attr.size(); i++) {
|
||||||
if (cnt>60) {
|
if (cnt>60) {
|
||||||
str << "\n";
|
str << "\n";
|
||||||
printspace(str, (cnt = _max(30, baselen)), " ");
|
printspace(str, (cnt = _max(30, baselen)), " ");
|
||||||
@@ -310,7 +308,7 @@ void mjXSchema::Print(std::stringstream& str, int level) {
|
|||||||
str << "\n";
|
str << "\n";
|
||||||
|
|
||||||
// children
|
// children
|
||||||
for (i=0; i<(int)child.size(); i++) {
|
for (int i=0; i<(int)child.size(); i++) {
|
||||||
child[i]->Print(str, level+1);
|
child[i]->Print(str, level+1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -319,8 +317,6 @@ void mjXSchema::Print(std::stringstream& str, int level) {
|
|||||||
|
|
||||||
// print schema as HTML table
|
// print schema as HTML table
|
||||||
void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
|
void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
|
||||||
int i;
|
|
||||||
|
|
||||||
// replace body with (world)body
|
// replace body with (world)body
|
||||||
string name1 = (name=="body" ? "(world)body" : name);
|
string name1 = (name=="body" ? "(world)body" : name);
|
||||||
|
|
||||||
@@ -350,7 +346,7 @@ void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
|
|||||||
// attributes
|
// attributes
|
||||||
str << "\t<td class=\"at\">";
|
str << "\t<td class=\"at\">";
|
||||||
if (!attr.empty()) {
|
if (!attr.empty()) {
|
||||||
for (i=0; i<(int)attr.size(); i++) {
|
for (int i=0; i<(int)attr.size(); i++) {
|
||||||
str << attr[i] << " ";
|
str << attr[i] << " ";
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
@@ -359,7 +355,7 @@ void mjXSchema::PrintHTML(std::stringstream& str, int level, bool pad) {
|
|||||||
str << "</td>\n</tr>\n";
|
str << "</td>\n</tr>\n";
|
||||||
|
|
||||||
// children
|
// children
|
||||||
for (i=0; i<(int)child.size(); i++) {
|
for (int i=0; i<(int)child.size(); i++) {
|
||||||
child[i]->PrintHTML(str, level+1, pad);
|
child[i]->PrintHTML(str, level+1, pad);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -394,7 +390,6 @@ bool mjXSchema::NameMatch(XMLElement* elem, int level) {
|
|||||||
|
|
||||||
// validator
|
// validator
|
||||||
XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
||||||
int i;
|
|
||||||
bool missing;
|
bool missing;
|
||||||
char msg[100];
|
char msg[100];
|
||||||
XMLElement *bad, *sub;
|
XMLElement *bad, *sub;
|
||||||
@@ -414,7 +409,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
|||||||
const XMLAttribute* attribute = elem->FirstAttribute();
|
const XMLAttribute* attribute = elem->FirstAttribute();
|
||||||
while (attribute) {
|
while (attribute) {
|
||||||
missing = true;
|
missing = true;
|
||||||
for (i=0; i<(int)attr.size(); i++) {
|
for (int i=0; i<(int)attr.size(); i++) {
|
||||||
if (attr[i]==attribute->Name()) {
|
if (attr[i]==attribute->Name()) {
|
||||||
missing = false;
|
missing = false;
|
||||||
break;
|
break;
|
||||||
@@ -445,7 +440,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// clear reference counts
|
// clear reference counts
|
||||||
for (i=0; i<(int)child.size(); i++) {
|
for (int i=0; i<(int)child.size(); i++) {
|
||||||
child[i]->refcnt = 0;
|
child[i]->refcnt = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -454,7 +449,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
|||||||
while (sub) {
|
while (sub) {
|
||||||
// find in child array, update refcnt
|
// find in child array, update refcnt
|
||||||
missing = true;
|
missing = true;
|
||||||
for (i=0; i<(int)child.size(); i++) {
|
for (int i=0; i<(int)child.size(); i++) {
|
||||||
if (child[i]->NameMatch(sub, level+1)) {
|
if (child[i]->NameMatch(sub, level+1)) {
|
||||||
// check sub-tree
|
// check sub-tree
|
||||||
if ((bad = child[i]->Check(sub, level+1))) {
|
if ((bad = child[i]->Check(sub, level+1))) {
|
||||||
@@ -481,7 +476,7 @@ XMLElement* mjXSchema::Check(XMLElement* elem, int level) {
|
|||||||
|
|
||||||
// enforce sub-element types
|
// enforce sub-element types
|
||||||
msg[0] = 0;
|
msg[0] = 0;
|
||||||
for (i=0; i<(int)child.size(); i++) {
|
for (int i=0; i<(int)child.size(); i++) {
|
||||||
switch (child[i]->type) {
|
switch (child[i]->type) {
|
||||||
case '!':
|
case '!':
|
||||||
if (child[i]->refcnt != 1)
|
if (child[i]->refcnt != 1)
|
||||||
|
|||||||
@@ -92,8 +92,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
|
|||||||
|
|
||||||
// compare to nudged solution
|
// compare to nudged solution
|
||||||
mju_copy(res_nudge, res, n);
|
mju_copy(res_nudge, res, n);
|
||||||
int i;
|
for (int i=0; i < n; i++) {
|
||||||
for (i=0; i < n; i++) {
|
|
||||||
// nudge down
|
// nudge down
|
||||||
res_nudge[i] = res[i] - eps;
|
res_nudge[i] = res[i] - eps;
|
||||||
if (lower) {
|
if (lower) {
|
||||||
|
|||||||
Reference in New Issue
Block a user