diff --git a/include/mujoco/mjrender.h b/include/mujoco/mjrender.h
index 8c68f2300a..bd3c884318 100644
--- a/include/mujoco/mjrender.h
+++ b/include/mujoco/mjrender.h
@@ -131,6 +131,7 @@ struct mjrContext_ { // custom OpenGL context
int nskin; // number of skins
unsigned int* skinvertVBO; // skin vertex position VBOs (nskin)
unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin)
+ unsigned int* skincolorVBO; // skin vertex color VBOs (nskin)
unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin)
unsigned int* skinfaceVBO; // skin face index VBOs (nskin)
diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h
index bd56493c5b..69adc61285 100644
--- a/include/mujoco/mjvisualize.h
+++ b/include/mujoco/mjvisualize.h
@@ -281,6 +281,7 @@ struct mjvScene_ { // abstract scene passed to OpenGL renderer
int* skinvertnum; // number of vertices in skin (nskin)
float* skinvert; // skin vertex data (nskin)
float* skinnormal; // skin normal data (nskin)
+ float* skinuserdata; // skin vertex scalar userdata (nskin) eg. for colorcoded visualisaton. normalized [0.0,..,1.0]
// OpenGL lights
int nlight; // number of lights currently in buffer
diff --git a/model/plugin/belt.xml b/model/plugin/belt.xml
index 050785ca39..607d947e06 100644
--- a/model/plugin/belt.xml
+++ b/model/plugin/belt.xml
@@ -57,20 +57,24 @@
1.0 0. 0. ">
-
-
+
+
+
+
+
-
-
-
+
+
+
+
-
+
\ No newline at end of file
diff --git a/model/plugin/cable.xml b/model/plugin/cable.xml
index 5edd11ccf6..724450ba80 100644
--- a/model/plugin/cable.xml
+++ b/model/plugin/cable.xml
@@ -36,7 +36,7 @@
-
+
diff --git a/model/plugin/coil.xml b/model/plugin/coil.xml
index 8686941c1c..70a8ea62b6 100644
--- a/model/plugin/coil.xml
+++ b/model/plugin/coil.xml
@@ -68,20 +68,26 @@
0.1 -0. 0.25132741">
-
-
+
+
+
+
+
-
+
-
-
+
+
+
+
+
-
+
@@ -99,6 +105,6 @@
-
+
\ No newline at end of file
diff --git a/model/plugin/elastoplastic_test.xml b/model/plugin/elastoplastic_test.xml
new file mode 100644
index 0000000000..2719b02986
--- /dev/null
+++ b/model/plugin/elastoplastic_test.xml
@@ -0,0 +1,38 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/plugin/elasticity/CMakeLists.txt b/plugin/elasticity/CMakeLists.txt
index b16cd3e8a7..58388be3d2 100644
--- a/plugin/elasticity/CMakeLists.txt
+++ b/plugin/elasticity/CMakeLists.txt
@@ -12,6 +12,8 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+#include (GenerateExportHeader)
+
set(MUJOCO_ELASTICITY_INCLUDE
${CMAKE_CURRENT_SOURCE_DIR}/../..
${CMAKE_CURRENT_SOURCE_DIR}/../../src
@@ -25,10 +27,26 @@ set(MUJOCO_ELASTICITY_SRCS
solid.h
)
+if (WIN32)
+ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin/plugin/ ) #change output directory of plugin to be found by simulate.exe
+ #set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib/ ) #change output directory of static plugin to be at the right place at debug time
+ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) #export also .lib along the .dll for static linking
+ set(BUILD_SHARED_LIBS TRUE)
+
+ #add_library(elasticity SHARED ${MUJOCO_ELASTICITY_SRCS})
+
+ #target_sources(mujoco PRIVATE ${MUJOCO_ELASTICITY_SRCS})
+
+ #add_link_options("LINKER:-v")
+
+endif()
+
add_library(elasticity SHARED)
target_sources(elasticity PRIVATE ${MUJOCO_ELASTICITY_SRCS})
target_include_directories(elasticity PRIVATE ${MUJOCO_ELASTICITY_INCLUDE})
target_link_libraries(elasticity PRIVATE mujoco)
+
+
target_compile_options(
elasticity
PRIVATE ${AVX_COMPILE_OPTIONS}
@@ -36,9 +54,12 @@ target_compile_options(
${EXTRA_COMPILE_OPTIONS}
${MUJOCO_CXX_FLAGS}
)
+
target_link_options(
elasticity
PRIVATE
${MUJOCO_MACOS_LINK_OPTIONS}
${EXTRA_LINK_OPTIONS}
)
+
+
diff --git a/plugin/elasticity/cable.cc b/plugin/elasticity/cable.cc
index d719272875..45f07b6d57 100644
--- a/plugin/elasticity/cable.cc
+++ b/plugin/elasticity/cable.cc
@@ -26,6 +26,29 @@
namespace mujoco::plugin::elasticity {
namespace {
+
+//create color scale palette
+void scalar2rgba(float rgba[4], mjtNum Scalar, mjtNum min, mjtNum max) {
+ mjtNum tmp;
+ tmp = (Scalar - min) / (max - min);
+ if (tmp < 0.2) {
+ rgba[0] = 0.90 - 0.70 * (tmp / 0.2);
+ rgba[2] = 0.80 * (tmp / 0.2) + 0.05;
+ rgba[1] = 0.0;
+ }
+ else if (tmp > 0.8) {
+ rgba[0] = 0.65 * ((tmp - 0.2) / 0.8) + 0.20;
+ rgba[2] = 0.2 * ((tmp - 0.8) / 0.2) + 0.75;
+ rgba[1] = 0.95;
+ }
+ else {
+ rgba[0] = 0.65 * ((tmp - 0.2) / 0.8) + 0.20;
+ rgba[2] = 0.85;
+ rgba[1] = (tmp-0.2)/0.6 * 0.95;
+ }
+}
+
+
// compute quaternion difference between two frames in joint coordinates
void QuatDiff(mjtNum* quat, const mjtNum body_quat[4],
const mjtNum joint_quat[4], bool pullback) {
@@ -50,21 +73,83 @@ void QuatDiff(mjtNum* quat, const mjtNum body_quat[4],
// scl - scaling of the force
// outputs:
// qfrc - local torque contribution
-void LocalForce(mjtNum qfrc[3], const mjtNum stiffness[4],
- const mjtNum quat[4], const mjtNum omega0[3],
- const mjtNum xquat[4], mjtNum scl) {
- mjtNum omega[3], lfrc[3];
-
- // compute curvature
- mju_quat2Vel(omega, quat, scl);
-
- // subtract omega0 in reference configuration
- mjtNum tmp[] = {
- - stiffness[0]*(omega[0] - omega0[0]) / stiffness[3],
- - stiffness[1]*(omega[1] - omega0[1]) / stiffness[3],
- - stiffness[2]*(omega[2] - omega0[2]) / stiffness[3],
- };
-
+void LocalForce(mjtNum qfrc[3], mujoco::plugin::elasticity::Cable::stiffness_ Sel, mujoco::plugin::elasticity::Cable::stiffness_consts_ Sconst,
+ const mjtNum quat[4], mjtNum omega0[3], mjtNum last_stress[3], mjtNum last_strain[3], int userdatatype, mjtNum* userdata, const mjtNum xquat[4], mjtNum scl) {
+
+ mjtNum omega[3], lfrc[3];
+
+ // compute curvature
+ mju_quat2Vel(omega, quat, scl);
+
+ // change of curvature from reference configuration
+ mjtNum d_omega[] = {
+ omega[0] - omega0[0],
+ omega[1] - omega0[1],
+ omega[2] - omega0[2]
+ };
+
+ //actual strain
+ mjtNum strain[] = {
+ d_omega[0] / (Sel.L_Dyz + omega0[0]),
+ d_omega[1] / (Sel.L_Dy + omega0[1]),
+ d_omega[2] / (Sel.L_Dz + omega0[2]),
+ };
+
+ mjtNum Yield_stress_top[] = {
+ strain[0] * Sconst.Gp + Sconst.sig_tp_G,
+ strain[1] * Sconst.Ep + Sconst.sig_tp_E,
+ strain[2] * Sconst.Ep + Sconst.sig_tp_E
+ };
+
+ mjtNum Yield_stress_bottom[] = {
+ Yield_stress_top[0] - Sconst.sigY * 2,
+ Yield_stress_top[1] - Sconst.sigY * 2,
+ Yield_stress_top[2] - Sconst.sigY * 2,
+ };
+ mjtNum plastic_strain[] = {
+ last_strain[0] - last_stress[0] / Sconst.G,
+ last_strain[1] - last_stress[1] / Sconst.E,
+ last_strain[2] - last_stress[2] / Sconst.E,
+ };
+
+ mjtNum elstress[] = {
+ (strain[0] - plastic_strain[0])* Sconst.G,
+ (strain[1] - plastic_strain[1])* Sconst.E,
+ (strain[2] - plastic_strain[2])* Sconst.E,
+ };
+
+ mjtNum stress[] = {
+ (elstress[0] <= Yield_stress_top[0] && elstress[0] >= Yield_stress_bottom[0]) ? elstress[0] : (strain[0] - last_strain[0]) > 0 ? strain[0] * Sconst.Gp + Sconst.sig_tp_G : strain[0] * Sconst.Gp + Sconst.sig_tp_G - 2 * Sconst.sigY,
+ (elstress[1] <= Yield_stress_top[1] && elstress[1] >= Yield_stress_bottom[1]) ? elstress[1] : (strain[1] - last_strain[1]) > 0 ? strain[1] * Sconst.Ep + Sconst.sig_tp_E : strain[1] * Sconst.Ep + Sconst.sig_tp_E - 2 * Sconst.sigY,
+ (elstress[2] <= Yield_stress_top[2] && elstress[2] >= Yield_stress_bottom[2]) ? elstress[2] : (strain[2] - last_strain[2]) > 0 ? strain[2] * Sconst.Ep + Sconst.sig_tp_E : strain[2] * Sconst.Ep + Sconst.sig_tp_E - 2 * Sconst.sigY,
+ };
+
+ // torque from calculated beam stress
+ mjtNum tmp[] = {
+ -stress[0] * Sel.J_Dyz,
+ -stress[1] * Sel.Iy_Dy,
+ -stress[2] * Sel.Iz_Dz,
+ };
+
+ last_stress[0] = stress[0];
+ last_stress[1] = stress[1];
+ last_stress[2] = stress[2];
+
+ last_strain[0] = strain[0];
+ last_strain[1] = strain[1];
+ last_strain[2] = strain[2];
+
+ //calculate scalar data for visualization and readout
+ if (userdatatype & 1) {
+ *userdata = mju_sqrt((pow(stress[0] - stress[1], 2) + pow(stress[1] - stress[2], 2) + pow(stress[2] - stress[0], 2)) / 2); //von Mieses principal stress
+ }
+ else if (userdatatype & 2) {
+ *userdata = mju_sqrt(pow(omega0[0], 2) + pow(omega0[1], 2) + pow(omega0[2], 2)); //element reference curvature
+ }
+ else if (userdatatype & 4) {
+ *userdata = mju_sqrt(pow(d_omega[0], 2) + pow(d_omega[1], 2) + pow(d_omega[2], 2)); //momentary curvature change
+ }
+
// rotate into global frame
if (xquat) {
mju_rotVecQuat(lfrc, tmp, xquat);
@@ -72,6 +157,7 @@ void LocalForce(mjtNum qfrc[3], const mjtNum stiffness[4],
mju_copy3(lfrc, tmp);
}
+
// add to total qfrc
mju_addToScl3(qfrc, lfrc, scl);
}
@@ -87,7 +173,6 @@ bool CheckAttr(const char* name, const mjModel* m, int instance) {
} // namespace
-
// factory function
std::optional Cable::Create(
const mjModel* m, mjData* d, int instance) {
@@ -105,7 +190,53 @@ Cable::Cable(const mjModel* m, mjData* d, int instance) {
std::string flat = mj_getPluginConfig(m, instance, "flat");
mjtNum G = strtod(mj_getPluginConfig(m, instance, "twist"), nullptr);
mjtNum E = strtod(mj_getPluginConfig(m, instance, "bend"), nullptr);
+ mjtNum YieldStress = strtod(mj_getPluginConfig(m, instance, "yieldstress"), nullptr);
+ mjtNum poisson = E / (2 * G) - 1; //calculate poission's ratio from E and G
+ mjtNum UtStress = strtod(mj_getPluginConfig(m, instance, "ultimatestress"), nullptr);
+ mjtNum UtStrain = strtod(mj_getPluginConfig(m, instance, "ultimatestrain"), nullptr);
+ mjtNum YieldStrain_G = 1, YieldStrain_E = 1, dYield_UtY = 0, k_deltaStrain_G = 0, k_deltaStrain_E = 0;
+
+ if (YieldStress) {
+ YieldStrain_G = YieldStress / G;
+ YieldStrain_E = YieldStress / E;
+
+ if (!UtStress || !UtStrain) {
+ //switch from strain hardening to constant stress model for plastic domain
+ UtStress = YieldStress;
+ UtStrain = 1.0;
+ mju_warning("ultimateStress or ultimateStrain paramters not set: perfect plasticity is assumed (max Stress = Yield Stress)");
+ }
+ else {
+ dYield_UtY = UtStress - YieldStress; //strain hardening stress delta
+ k_deltaStrain_G = 5 / (UtStrain - YieldStrain_G); //exponential constant for torsional strain hardening
+ k_deltaStrain_E = 5 / (UtStrain - YieldStrain_E); //exponential constant for bending strain hardening
+
+ //check if paramters are reasonable. Yieldstrain must not be larger than ultimate strain
+ if (YieldStress / (E > G ? G : E) > UtStrain || UtStress < YieldStress) {
+ mju_error("Error: elasto-plastic material parameters are not reasonable !\nPlease make sure, that Yield Stress < ultimate Stress and Yield Strain (=Yield Stress / Stiffness) < ultimate Strain.");
+ }
+ }
+ }
+ else {
+ mju_warning("YieldStress is notset: perfect elasticity is assumed !");
+ YieldStress = 1e20;
+ UtStress = YieldStress;
+ UtStrain = 1.0;
+ }
+
+ mjtNum Ep = (UtStress - YieldStress) / (UtStrain - YieldStress / E); //plastic hardenening tension stiffness
+ mjtNum Gp = Ep / (2 * (1 + poisson)); //plastic hardening shear stiffness
+ Sconst.E = E; //youngs modulus
+ Sconst.G = G; //shear modulus
+ Sconst.Ep = Ep; //youngs modulus in plastic domain
+ Sconst.Gp = Gp; //shear modulus in plastic domain
+ Sconst.sigY = YieldStress;
+ Sconst.sig_tp_G = YieldStress - Gp * YieldStrain_G; // plastic yield stress curve offset torsion
+ Sconst.sig_tp_E = YieldStress - Ep * YieldStrain_E; // plastic yield stress curve offset tension
+
+ //exponential model interpolation, reference: https://monarch.qucosa.de/api/qucosa%3A19721/attachment/ATT-0/ Page 10-19
+
// count plugin bodies
n = 0;
for (int i = 1; i < m->nbody; i++) {
@@ -117,10 +248,17 @@ Cable::Cable(const mjModel* m, mjData* d, int instance) {
}
// allocate arrays
- prev.assign(n, 0); // index of previous body
- next.assign(n, 0); // index of next body
- omega0.assign(3*n, 0); // reference curvature
- stiffness.assign(4*n, 0); // material parameters
+ prev.assign(n, 0); // index of previous body
+ next.assign(n, 0); // index of next body
+ omega0.assign(3 * n, 0); // reference curvature
+ stress.assign(3 * n, 0); // keep track of previous beam stresses
+ strain.assign(3 * n, 0); // keep track of previous beam strains
+ userdata.assign(3 * n, 0); // stress, reference curvature or curvature change
+ userdatamin = 0; //limits for scalar coloring
+ userdatamax = UtStress;
+
+ Sel.resize(n); // material parameters for each beam
+
// run forward kinematics to populate xquat (mjData not yet initialized)
mju_zero(d->mocap_quat, 4*m->nmocap);
@@ -131,7 +269,7 @@ Cable::Cable(const mjModel* m, mjData* d, int instance) {
for (int b = 0; b < n; b++) {
int i = i0 + b;
if (m->body_plugin[i] != instance) {
- mju_error("This body does not have the requested plugin instance");
+ mju_error("This body does not have the requested plugin instance");
}
bool first = (b == 0), last = (b == n-1);
prev[b] = first ? 0 : -1;
@@ -145,32 +283,60 @@ Cable::Cable(const mjModel* m, mjData* d, int instance) {
mju_zero3(omega0.data()+3*b);
}
- // compute physical parameters
+ // compute physical parameters based on exponential elasto-plastic model
int geom_i = m->body_geomadr[i];
- mjtNum J = 0, Iy = 0, Iz = 0;
- if (m->geom_type[geom_i] == mjGEOM_CYLINDER ||
- m->geom_type[geom_i] == mjGEOM_CAPSULE) {
+ mjtNum J = 0, Iy = 0, Iz = 0, L = 0;
+
+ //Dyz,Dy, Dz are the distances of the outer fabrics to the center axis of the beam
+ mjtNum L_Dyz = 0, L_Dy = 0, L_Dz = 0, J_Dyz = 0, Iy_Dy = 0, Iz_Dz = 0, Dyz = 0, Dy = 0, Dz = 0;
+
+ L = mju_dist3(d->xpos + 3 * i, d->xpos + 3 * (i + prev[b])); // distance from previous body
+ if (m->geom_type[geom_i] == mjGEOM_CYLINDER ||
+ m->geom_type[geom_i] == mjGEOM_CAPSULE) {
// https://en.wikipedia.org/wiki/Torsion_constant#Circle
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
- J = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 2;
- Iy = Iz = mjPI * pow(m->geom_size[3*geom_i+0], 4) / 4.;
+ Dyz = Dy = Dz = m->geom_size[3*geom_i+0]; //radius
+ J = mjPI * pow(Dyz, 4) / 2;
+ Iy = Iz = mjPI * pow(Dyz, 4) / 4.;
+ J_Dyz = J / Dyz;
+ Iy_Dy = Iz_Dz = Iy / Dyz;
+ L_Dyz = L_Dy = L_Dz = L / Dyz;
+
+
} else if (m->geom_type[geom_i] == mjGEOM_BOX) {
// https://en.wikipedia.org/wiki/Torsion_constant#Rectangle
// https://en.wikipedia.org/wiki/List_of_second_moments_of_area
- mjtNum h = m->geom_size[3*geom_i+1];
- mjtNum w = m->geom_size[3*geom_i+2];
+ mjtNum h = m->geom_size[3*geom_i+1]; //half size height !
+ mjtNum w = m->geom_size[3*geom_i+2]; //half size width !
mjtNum a = std::max(h, w);
mjtNum b = std::min(h, w);
J = a*pow(b, 3)*(16./3.-3.36*b/a*(1-pow(b, 4)/pow(a, 4)/12));
- Iy = pow(2 * w, 3) * 2 * h / 12.;
- Iz = pow(2 * h, 3) * 2 * w / 12.;
+ Iy = pow(2 * w, 3) * 2 * h / 12.;
+ Iz = pow(2 * h, 3) * 2 * w / 12.;
+ Dyz = sqrt(pow(h, 2) + pow(w, 2));
+ Dy = w;
+ Dz = h;
+ J_Dyz = J / Dyz; //diagonal distance of edge
+ Iy_Dy = Iy / Dy;
+ Iz_Dz = Iz / Dz;
+ L_Dyz = L / Dyz;
+ L_Dy = L / Dy;
+ L_Dz = L / Dz;
+
}
- stiffness[4*b+0] = J * G;
- stiffness[4*b+1] = Iy * E;
- stiffness[4*b+2] = Iz * E;
- stiffness[4*b+3] =
- prev[b] ? mju_dist3(d->xpos+3*i, d->xpos+3*(i+prev[b])) : 0;
- }
+ // precalculated elementwise constants for faster calculation of exponential model
+ Sel[b].J_Dyz = J_Dyz;
+ Sel[b].Iy_Dy = Iy_Dy;
+ Sel[b].Iz_Dz = Iz_Dz;
+ Sel[b].L_Dyz = L_Dyz;
+ Sel[b].L_Dy = L_Dy;
+ Sel[b].L_Dz = L_Dz;
+ Sel[b].Dyz = Dyz;
+ Sel[b].Dy = Dy;
+ Sel[b].Dz = Dz;
+
+ //reference: https://www.continuummechanics.org/beambending.html
+ }
}
void Cable::Compute(const mjModel* m, mjData* d, int instance) {
@@ -183,22 +349,22 @@ void Cable::Compute(const mjModel* m, mjData* d, int instance) {
}
// if no stiffness, skip body
- if (!stiffness[b*4+0] && !stiffness[b*4+1] && !stiffness[b*4+2]) {
+ if (!Sel[b].J_Dyz && !Sel[b].Iy_Dy && !Sel[b].Iz_Dz && !Sel[b].L_Dyz && !Sel[b].L_Dy && !Sel[b].L_Dz) {
continue;
}
// elastic forces
mjtNum quat[4] = {0};
mjtNum xfrc[3] = {0};
-
+ mjtNum userdata = 0;
// local orientation
if (prev[b]) {
int qadr = m->jnt_qposadr[m->body_jntadr[i]] + m->body_dofnum[i]-3;
- QuatDiff(quat, m->body_quat+4*i, d->qpos+qadr, 0);
-
- // contribution of orientation i-1 to xfrc i
- LocalForce(xfrc, stiffness.data()+4*b, quat, omega0.data()+3*b,
- d->xquat+4*(i+prev[b]), 1);
+ QuatDiff(quat, m->body_quat+4*i, d->qpos+qadr, 0);
+ // contribution of orientation i-1 to xfrc i
+
+ LocalForce(xfrc, Sel[b], Sconst, quat, omega0.data() + 3 * b, stress.data() + 3 * b, strain.data() + 3 * b, 1, &userdata, d->xquat + 4 * (i + prev[b]), 1);
+
}
if (next[b]) {
@@ -208,12 +374,14 @@ void Cable::Compute(const mjModel* m, mjData* d, int instance) {
// local orientation
int qadr = m->jnt_qposadr[m->body_jntadr[in]] + m->body_dofnum[in]-3;
QuatDiff(quat, m->body_quat+4*in, d->qpos+qadr, 1);
-
// contribution of orientation i+1 to xfrc i
- LocalForce(xfrc, stiffness.data()+4*bn, quat, omega0.data()+3*bn,
- d->xquat+4*i, -1);
+ LocalForce(xfrc, Sel[bn], Sconst, quat, omega0.data()+3*bn, stress.data() + 3 * bn, strain.data() + 3 * bn, 1, &userdata, d->xquat+4*i, -1);
+
}
+ // set geometry color based on userdata
+ scalar2rgba(&m->geom_rgba[i * 4], userdata, userdatamin, userdatamax); //fixed legend range 0...ultimate strain + 10%
+
// convert from global coordinates and apply torque to com
mj_applyFT(m, d, 0, xfrc, d->xpos+3*i, i, d->qfrc_passive);
}
@@ -228,7 +396,7 @@ void Cable::RegisterPlugin() {
plugin.name = "mujoco.elasticity.cable";
plugin.type |= mjPLUGIN_PASSIVE;
- const char* attributes[] = {"twist", "bend", "flat"};
+ const char* attributes[] = {"twist", "bend", "flat", "yieldstress", "ultimatestress", "ultimatestrain"};
plugin.nattribute = sizeof(attributes) / sizeof(attributes[0]);
plugin.attributes = attributes;
plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
diff --git a/plugin/elasticity/cable.h b/plugin/elasticity/cable.h
index 186bb1274e..fc78da0da4 100644
--- a/plugin/elasticity/cable.h
+++ b/plugin/elasticity/cable.h
@@ -15,6 +15,8 @@
#ifndef MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
#define MUJOCO_SRC_PLUGIN_ELASTICITY_CABLE_H_
+#include
+
#include
#include
@@ -23,6 +25,7 @@
#include
+
namespace mujoco::plugin::elasticity {
class Cable {
@@ -42,8 +45,39 @@ class Cable {
int n; // number of bodies in the cable
std::vector prev; // indices of previous bodies (n x 1)
std::vector next; // indices of next bodies (n x 1)
- std::vector stiffness; // stiffness parameters (n x 4)
+
+
+ //element depending stiffness constants
+ struct stiffness_ {
+ mjtNum L_Dyz; // =L/Dyz L:= beam Length, Dyz: distance to outermost Edge of Beam on yz plane
+ mjtNum L_Dy; // =L/Dy Dy: distance to outermost Edge of Beam in y direction
+ mjtNum L_Dz; // =L/Dz Dz: distance to outermost Edge of Beam in z direction
+ mjtNum J_Dyz; // =J/Dyz
+ mjtNum Iy_Dy; // =Iy/Dy
+ mjtNum Iz_Dz; // =Iz/Dz
+ mjtNum Dyz; //
+ mjtNum Dy; //
+ mjtNum Dz; //
+ };
+ //material constants
+ struct stiffness_consts_ {
+ mjtNum E; //youngs modulus
+ mjtNum G; //shear modulus
+ mjtNum Ep; //strain hardening tension stiffness
+ mjtNum Gp; //strain hardening shear stiffness
+ mjtNum sigY; // =Yieldstress
+ mjtNum sig_tp_G; // plastic yield stress curve offset
+ mjtNum sig_tp_E; // plastic yield stress curve offset
+ };
+
std::vector omega0; // reference curvature (n x 3)
+ std::vector stress; // last stress in beam (n x 3)
+ std::vector strain; // last strain in beam (n x 3)
+ std::vector userdata; // export of userdata from plugin (n x 3) e.g. stress for colorcoded vis
+ mjtNum userdatamin; //storage of min values of userdata
+ mjtNum userdatamax; //storage of max values of userdata
+ std::vector Sel;
+ stiffness_consts_ Sconst;
private:
Cable(const mjModel* m, mjData* d, int instance);
diff --git a/python/mujoco/render.cc b/python/mujoco/render.cc
index 6c38348d0a..2a7f4d5fe1 100644
--- a/python/mujoco/render.cc
+++ b/python/mujoco/render.cc
@@ -54,6 +54,7 @@ class MjWrapper : public WrapperBase {
X(skinvertVBO);
X(skinnormalVBO);
X(skintexcoordVBO);
+ X(skincolorVBO);
X(skinfaceVBO);
X(charWidth);
X(charWidthBig);
@@ -94,6 +95,7 @@ MjrContextWrapper::MjWrapper()
X(texture),
X_SKIN(skinvertVBO),
X_SKIN(skinnormalVBO),
+ X_SKIN(skincolorVBO),
X_SKIN(skintexcoordVBO),
X_SKIN(skinfaceVBO),
X(charWidth),
@@ -118,6 +120,7 @@ MjrContextWrapper::MjWrapper(const MjModelWrapper& model, int fontscale)
X(texture),
X_SKIN(skinvertVBO),
X_SKIN(skinnormalVBO),
+ X_SKIN(skincolorVBO),
X_SKIN(skintexcoordVBO),
X_SKIN(skinfaceVBO),
X(charWidth),
@@ -235,6 +238,7 @@ PYBIND11_MODULE(_render, pymodule) {
X(texture);
X(skinvertVBO);
X(skinnormalVBO);
+ X(skincolorVBO);
X(skintexcoordVBO);
X(skinfaceVBO);
X(charWidth);
diff --git a/src/engine/engine_vis_init.c b/src/engine/engine_vis_init.c
index 341c9124d2..2883359720 100644
--- a/src/engine/engine_vis_init.c
+++ b/src/engine/engine_vis_init.c
@@ -159,13 +159,15 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
scn->skinvertnum = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvert = (float*) mju_malloc(3*totvert*sizeof(float));
scn->skinnormal = (float*) mju_malloc(3*totvert*sizeof(float));
+ scn->skinuserdata = (float*) mju_malloc(3*totvert*sizeof(float));
// check allocation
if (!scn->skinfacenum ||
!scn->skinvertadr ||
!scn->skinvertnum ||
!scn->skinvert ||
- !scn->skinnormal) {
+ !scn->skinnormal ||
+ !scn->skinuserdata) {
mju_error("Could not allocate skin buffers");
}
@@ -192,6 +194,7 @@ void mjv_freeScene(mjvScene* scn) {
mju_free(scn->skinvertnum);
mju_free(scn->skinvert);
mju_free(scn->skinnormal);
+ mju_free(scn->skinuserdata);
// clear data structure
mjv_defaultScene(scn);
diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c
index 577fd40eee..f081d80c4d 100644
--- a/src/engine/engine_vis_visualize.c
+++ b/src/engine/engine_vis_visualize.c
@@ -1888,6 +1888,8 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
// clear positions and normals
memset(scn->skinvert + 3*vertadr, 0, 3*vertnum*sizeof(float));
memset(scn->skinnormal + 3*vertadr, 0, 3*vertnum*sizeof(float));
+ memset(scn->skinuserdata + 3*vertadr, 0, 3*vertnum * sizeof(float));
+
if (opt->skingroup[m->skin_group[i]]) {
// accumulate positions from all bones
@@ -1914,6 +1916,13 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
mju_mulQuat(quat, d->xquat+4*bodyid, quatneg);
mju_quat2Mat(rotate, quat);
+ //apply dynamic skin colors from geom_rgba
+ mjtNum skin_rgb[3] = {
+ (mjtNum)m->geom_rgba[4 * bodyid],
+ (mjtNum)m->geom_rgba[4 * bodyid + 1],
+ (mjtNum)m->geom_rgba[4 * bodyid + 2]
+ };
+
// compute translation
mjtNum translate[3];
mju_rotVecMat(translate, bindpos, rotate);
@@ -1943,6 +1952,13 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
scn->skinvert[3*(vertadr+vid)] += vweight*(float)pos1[0];
scn->skinvert[3*(vertadr+vid)+1] += vweight*(float)pos1[1];
scn->skinvert[3*(vertadr+vid)+2] += vweight*(float)pos1[2];
+
+ //colorize each vertex based on userdata and location
+ //vcolor = mju_sqrt(skin_rgb[0] * skin_rgb[0] + skin_rgb[1] * skin_rgb[1] + skin_rgb[2] * skin_rgb[2]);
+ scn->skinuserdata[3 * (vertadr + vid)] = skin_rgb[0];
+ scn->skinuserdata[3 * (vertadr + vid) + 1] = skin_rgb[1];
+ scn->skinuserdata[3 * (vertadr + vid) + 2] = skin_rgb[2];
+
}
}
diff --git a/src/render/render_context.c b/src/render/render_context.c
index 849137fd0d..f31fd646cd 100644
--- a/src/render/render_context.c
+++ b/src/render/render_context.c
@@ -1406,12 +1406,14 @@ static void makeSkin(const mjModel* m, mjrContext* con) {
// allocate VBO names
con->skinvertVBO = (unsigned int*) mju_malloc(nskin*sizeof(int));
con->skinnormalVBO = (unsigned int*) mju_malloc(nskin*sizeof(int));
+ con->skincolorVBO = (unsigned int*) mju_malloc(nskin*sizeof(int));
con->skintexcoordVBO = (unsigned int*) mju_malloc(nskin*sizeof(int));
con->skinfaceVBO = (unsigned int*) mju_malloc(nskin*sizeof(int));
// generage VBOs
glGenBuffers(nskin, con->skinvertVBO);
glGenBuffers(nskin, con->skinnormalVBO);
+ glGenBuffers(nskin, con->skincolorVBO);
glGenBuffers(nskin, con->skintexcoordVBO);
glGenBuffers(nskin, con->skinfaceVBO);
@@ -1775,11 +1777,13 @@ void mjr_freeContext(mjrContext* con) {
// delete VBOs
glDeleteBuffers(con->nskin, con->skinvertVBO);
glDeleteBuffers(con->nskin, con->skinnormalVBO);
+ glDeleteBuffers(con->nskin, con->skincolorVBO);
glDeleteBuffers(con->nskin, con->skintexcoordVBO);
glDeleteBuffers(con->nskin, con->skinfaceVBO);
mju_free(con->skinvertVBO);
mju_free(con->skinnormalVBO);
+ mju_free(con->skincolorVBO);
mju_free(con->skintexcoordVBO);
mju_free(con->skinfaceVBO);
}
diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c
index a51dbc062b..5e4bdc9c13 100644
--- a/src/render/render_gl3.c
+++ b/src/render/render_gl3.c
@@ -414,6 +414,11 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
glBindBuffer(GL_ARRAY_BUFFER, con->skinnormalVBO[geom->objid]);
glNormalPointer(GL_FLOAT, 0, NULL);
+ // vertex colors
+ glEnableClientState(GL_COLOR_ARRAY);
+ glBindBuffer(GL_ARRAY_BUFFER, con->skincolorVBO[geom->objid]);
+ glColorPointer(3, GL_FLOAT, 0, NULL);
+
// vertex texture coordinates
if (con->skintexcoordVBO[geom->objid]) {
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
@@ -434,6 +439,7 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
+ glDisableClientState(GL_COLOR_ARRAY);
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
break;
}
@@ -708,6 +714,13 @@ void mjr_render(mjrRect viewport, mjvScene* scn, const mjrContext* con) {
3*scn->skinvertnum[i]*sizeof(float),
scn->skinnormal + 3*scn->skinvertadr[i],
GL_STREAM_DRAW);
+
+ // upload colors to VBO
+ glBindBuffer(GL_ARRAY_BUFFER, con->skincolorVBO[i]);
+ glBufferData(GL_ARRAY_BUFFER,
+ 3*scn->skinvertnum[i] * sizeof(float),
+ scn->skinuserdata + 3*scn->skinvertadr[i],
+ GL_STREAM_DRAW);
}
// determine drawbuffer; may be changed by stereo later