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

Add quadratic stiffness to flex.

Browse Source 
PiperOrigin-RevId: 827409919
Change-Id: I3dff8ea49fb1726fec4acf5b91138d6d52c4bfba
This commit is contained in:
Alessio Quaglino
2025-11-03 03:00:18 -08:00
committed by Copybara-Service
parent d4f13b4abc
commit 3a7aa84e53
10 changed files with 161 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+5 -5
View File
@@ -863,8 +863,8 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
else {
// get bodies and weights
int nb = 0;
int bid[64];
mjtNum bweight[64];
int bid[729]; // 729 = 27*27
mjtNum bweight[729];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
@@ -1121,8 +1121,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
tran = rot = 0;
for (int side=0; side < 2; side++) {
// get bodies and weights
int nb = 0, bid[32], vid[4], nw = 0;
mjtNum bweight[32], bw[4];
int nb = 0, bid[729], vid[4], nw = 0;
mjtNum bweight[729], bw[4];
// geom
if (con->geom[side] >= 0) {
@@ -1808,7 +1808,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[64];
int nb = 0, bid[729];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
src/engine/engine_passive.c
+3 -3
View File
@@ -206,8 +206,8 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
}
if (m->flex_interp[f]) {
mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
mjtNum xpos[3*mjMAXFLEXNODES], displ[3*mjMAXFLEXNODES], vel[3*mjMAXFLEXNODES];
mjtNum frc[3*mjMAXFLEXNODES], dmp[3*mjMAXFLEXNODES];
mjtNum com[3] = {0};
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
@@ -242,7 +242,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
// compute the Jacobian at the center of mass
mjtNum mat[9] = {0};
mjtNum p[3] = {.5, .5, .5};
mju_defGradient(mat, p, xpos, 1);
mju_defGradient(mat, p, xpos, m->flex_interp[f]);
// find rotation
mjtNum quat[4] = {1, 0, 0, 0};
src/engine/engine_vis_visualize.c
+15 -9
View File
@@ -1390,6 +1390,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mjtNum xpos[mjMAXFLEXNODES];
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int nnode = m->flex_interp[f]+1;
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
@@ -1400,34 +1401,39 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
}
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (i == 0) {
for (int i=0; i < nnode; i++) {
for (int j=0; j < nnode; j++) {
for (int k=0; k < nnode; k++) {
int nn = nnode*nnode;
int offset = 3*(nn*(i+0) + nnode*(j+0) + k);
int offset1 = 3*(nn*(i+1) + nnode*(j+0) + k);
int offset2 = 3*(nn*(i+0) + nnode*(j+1) + k);
int offset3 = 3*(nn*(i+0) + nnode*(j+0) + (k+1));
if (i < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1);
releaseGeom(&thisgeom, scn);
}
if (j == 0) {
if (j < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2);
releaseGeom(&thisgeom, scn);
}
if (k == 0) {
if (k < nnode-1) {
mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN);
if (!thisgeom) {
return;
}
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset3);
releaseGeom(&thisgeom, scn);
}
}