1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812 |
- // Copyright 2021 DeepMind Technologies Limited
- //
- // Licensed under the Apache License, Version 2.0 (the "License");
- // you may not use this file except in compliance with the License.
- // You may obtain a copy of the License at
- //
- // http://www.apache.org/licenses/LICENSE-2.0
- //
- // Unless required by applicable law or agreed to in writing, software
- // distributed under the License is distributed on an "AS IS" BASIS,
- // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- // See the License for the specific language governing permissions and
- // limitations under the License.
- #include "simulate.h"
- #include <algorithm>
- #include <atomic>
- #include <chrono>
- #include <climits>
- #include <cstdio>
- #include <cstring>
- #include <memory>
- #include <optional>
- #include <ratio>
- #include <string>
- #include <type_traits>
- #include <utility>
- #include "lodepng.h"
- #include <mujoco/mjdata.h>
- #include <mujoco/mjui.h>
- #include <mujoco/mjvisualize.h>
- #include <mujoco/mjxmacro.h>
- #include <mujoco/mujoco.h>
- #include "platform_ui_adapter.h"
- #include "array_safety.h"
- // When launched via an App Bundle on macOS, the working directory is the path to the App Bundle's
- // resource directory. This causes files to be saved into the bundle, which is not the desired
- // behavior. Instead, we open a save dialog box to ask the user where to put the file.
- // Since the dialog box logic needs to be written in Objective-C, we separate it into a different
- // source file.
- #ifdef __APPLE__
- std::string GetSavePath(const char* filename);
- #else
- static std::string GetSavePath(const char* filename) {
- return filename;
- }
- #endif
- namespace {
- namespace mj = ::mujoco;
- namespace mju = ::mujoco::sample_util;
- using Seconds = std::chrono::duration<double>;
- using Milliseconds = std::chrono::duration<double, std::milli>;
- template <typename T>
- inline bool IsDifferent(const T& a, const T& b) {
- if constexpr (std::is_array_v<T>) {
- static_assert(std::rank_v<T> == 1);
- for (int i = 0; i < std::extent_v<T>; ++i) {
- if (a[i] != b[i]) {
- return true;
- }
- }
- return false;
- } else {
- return a != b;
- }
- }
- template <typename T>
- inline void CopyScalar(T& dst, const T& src) {
- dst = src;
- }
- template <typename T, int N>
- inline void CopyArray(T (&dst)[N], const T (&src)[N]) {
- for (int i = 0; i < N; ++i) {
- dst[i] = src[i];
- }
- }
- template <typename T>
- inline void Copy(T& dst, const T& src) {
- if constexpr (std::is_array_v<T>) {
- CopyArray(dst, src);
- } else {
- CopyScalar(dst, src);
- }
- }
- //------------------------------------------- global -----------------------------------------------
- const double zoom_increment = 0.02; // ratio of one click-wheel zoom increment to vertical extent
- // section ids
- enum {
- // left ui
- SECT_FILE = 0,
- SECT_OPTION,
- SECT_SIMULATION,
- SECT_WATCH,
- SECT_PHYSICS,
- SECT_RENDERING,
- SECT_VISUALIZATION,
- SECT_GROUP,
- NSECT0,
- // right ui
- SECT_JOINT = 0,
- SECT_CONTROL,
- NSECT1
- };
- // file section of UI
- const mjuiDef defFile[] = {
- {mjITEM_SECTION, "File", mjPRESERVE, nullptr, "AF"},
- {mjITEM_BUTTON, "Save xml", 2, nullptr, ""},
- {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""},
- {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"},
- {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"},
- {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"},
- {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"},
- {mjITEM_END}
- };
- // help strings
- const char help_content[] =
- "Space\n"
- "+ -\n"
- "Left / Right arrow\n"
- "Tab / Shift-Tab\n"
- "[ ]\n"
- "Esc\n"
- "Double-click\n"
- "Page Up\n"
- "Right double-click\n"
- "Ctrl Right double-click\n"
- "Scroll, middle drag\n"
- "Left drag\n"
- "[Shift] right drag\n"
- "Ctrl [Shift] drag\n"
- "Ctrl [Shift] right drag\n"
- "F1\n"
- "F2\n"
- "F3\n"
- "F4\n"
- "F5\n"
- "UI right-button hold\n"
- "UI title double-click";
- const char help_title[] =
- "Play / Pause\n"
- "Speed Up / Down\n"
- "Step Back / Forward\n"
- "Toggle Left / Right UI\n"
- "Cycle cameras\n"
- "Free camera\n"
- "Select\n"
- "Select parent\n"
- "Center camera\n"
- "Tracking camera\n"
- "Zoom\n"
- "View Orbit\n"
- "View Pan\n"
- "Object Rotate\n"
- "Object Translate\n"
- "Help\n"
- "Info\n"
- "Profiler\n"
- "Sensors\n"
- "Full screen\n"
- "Show UI shortcuts\n"
- "Expand/collapse all";
- //-------------------------------- profiler, sensor, info, watch -----------------------------------
- // number of lines in the Constraint ("Counts") and Cost ("Convergence") figures
- static constexpr int kConstraintNum = 5;
- static constexpr int kCostNum = 3;
- // init profiler figures
- void InitializeProfiler(mj::Simulate* sim) {
- // set figures to default
- mjv_defaultFigure(&sim->figconstraint);
- mjv_defaultFigure(&sim->figcost);
- mjv_defaultFigure(&sim->figtimer);
- mjv_defaultFigure(&sim->figsize);
- // titles
- mju::strcpy_arr(sim->figconstraint.title, "Counts");
- mju::strcpy_arr(sim->figcost.title, "Convergence (log 10)");
- mju::strcpy_arr(sim->figsize.title, "Dimensions");
- mju::strcpy_arr(sim->figtimer.title, "CPU time (msec)");
- // x-labels
- mju::strcpy_arr(sim->figconstraint.xlabel, "Solver iteration");
- mju::strcpy_arr(sim->figcost.xlabel, "Solver iteration");
- mju::strcpy_arr(sim->figsize.xlabel, "Video frame");
- mju::strcpy_arr(sim->figtimer.xlabel, "Video frame");
- // y-tick number formats
- mju::strcpy_arr(sim->figconstraint.yformat, "%.0f");
- mju::strcpy_arr(sim->figcost.yformat, "%.1f");
- mju::strcpy_arr(sim->figsize.yformat, "%.0f");
- mju::strcpy_arr(sim->figtimer.yformat, "%.2f");
- // colors
- sim->figconstraint.figurergba[0] = 0.1f;
- sim->figcost.figurergba[2] = 0.2f;
- sim->figsize.figurergba[0] = 0.1f;
- sim->figtimer.figurergba[2] = 0.2f;
- sim->figconstraint.figurergba[3] = 0.5f;
- sim->figcost.figurergba[3] = 0.5f;
- sim->figsize.figurergba[3] = 0.5f;
- sim->figtimer.figurergba[3] = 0.5f;
- // repeat line colors for constraint and cost figures
- mjvFigure* fig = &sim->figcost;
- for (int i=kCostNum; i<mjMAXLINE; i++) {
- fig->linergb[i][0] = fig->linergb[i - kCostNum][0];
- fig->linergb[i][1] = fig->linergb[i - kCostNum][1];
- fig->linergb[i][2] = fig->linergb[i - kCostNum][2];
- }
- fig = &sim->figconstraint;
- for (int i=kConstraintNum; i<mjMAXLINE; i++) {
- fig->linergb[i][0] = fig->linergb[i - kConstraintNum][0];
- fig->linergb[i][1] = fig->linergb[i - kConstraintNum][1];
- fig->linergb[i][2] = fig->linergb[i - kConstraintNum][2];
- }
- // legends
- mju::strcpy_arr(sim->figconstraint.linename[0], "total");
- mju::strcpy_arr(sim->figconstraint.linename[1], "active");
- mju::strcpy_arr(sim->figconstraint.linename[2], "changed");
- mju::strcpy_arr(sim->figconstraint.linename[3], "evals");
- mju::strcpy_arr(sim->figconstraint.linename[4], "updates");
- mju::strcpy_arr(sim->figcost.linename[0], "improvement");
- mju::strcpy_arr(sim->figcost.linename[1], "gradient");
- mju::strcpy_arr(sim->figcost.linename[2], "lineslope");
- mju::strcpy_arr(sim->figsize.linename[0], "dof");
- mju::strcpy_arr(sim->figsize.linename[1], "body");
- mju::strcpy_arr(sim->figsize.linename[2], "constraint");
- mju::strcpy_arr(sim->figsize.linename[3], "sqrt(nnz)");
- mju::strcpy_arr(sim->figsize.linename[4], "contact");
- mju::strcpy_arr(sim->figsize.linename[5], "iteration");
- mju::strcpy_arr(sim->figtimer.linename[0], "total");
- mju::strcpy_arr(sim->figtimer.linename[1], "collision");
- mju::strcpy_arr(sim->figtimer.linename[2], "prepare");
- mju::strcpy_arr(sim->figtimer.linename[3], "solve");
- mju::strcpy_arr(sim->figtimer.linename[4], "other");
- // grid sizes
- sim->figconstraint.gridsize[0] = 5;
- sim->figconstraint.gridsize[1] = 5;
- sim->figcost.gridsize[0] = 5;
- sim->figcost.gridsize[1] = 5;
- sim->figsize.gridsize[0] = 3;
- sim->figsize.gridsize[1] = 5;
- sim->figtimer.gridsize[0] = 3;
- sim->figtimer.gridsize[1] = 5;
- // minimum ranges
- sim->figconstraint.range[0][0] = 0;
- sim->figconstraint.range[0][1] = 20;
- sim->figconstraint.range[1][0] = 0;
- sim->figconstraint.range[1][1] = 80;
- sim->figcost.range[0][0] = 0;
- sim->figcost.range[0][1] = 20;
- sim->figcost.range[1][0] = -15;
- sim->figcost.range[1][1] = 5;
- sim->figsize.range[0][0] = -200;
- sim->figsize.range[0][1] = 0;
- sim->figsize.range[1][0] = 0;
- sim->figsize.range[1][1] = 100;
- sim->figtimer.range[0][0] = -200;
- sim->figtimer.range[0][1] = 0;
- sim->figtimer.range[1][0] = 0;
- sim->figtimer.range[1][1] = 0.4f;
- // init x axis on history figures (do not show yet)
- for (int n=0; n<6; n++) {
- for (int i=0; i<mjMAXLINEPNT; i++) {
- sim->figtimer.linedata[n][2*i] = -i;
- sim->figsize.linedata[n][2*i] = -i;
- }
- }
- }
- // update profiler figures
- void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
- // reset lines in Constraint and Cost figures
- memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
- memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
- // number of islands that have diagnostics
- int nisland = mjMIN(d->solver_nisland, mjNISLAND);
- // iterate over islands
- for (int k=0; k < nisland; k++) {
- // ==== update Constraint ("Counts") figure
- // number of points to plot, starting line
- int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
- int start = kConstraintNum * k;
- sim->figconstraint.linepnt[start + 0] = npoints;
- for (int i=1; i < kConstraintNum; i++) {
- sim->figconstraint.linepnt[start + i] = npoints;
- }
- if (m->opt.solver == mjSOL_PGS) {
- sim->figconstraint.linepnt[start + 3] = 0;
- sim->figconstraint.linepnt[start + 4] = 0;
- }
- if (m->opt.solver == mjSOL_CG) {
- sim->figconstraint.linepnt[start + 4] = 0;
- }
- for (int i=0; i<npoints; i++) {
- // x
- sim->figconstraint.linedata[start + 0][2*i] = i;
- sim->figconstraint.linedata[start + 1][2*i] = i;
- sim->figconstraint.linedata[start + 2][2*i] = i;
- sim->figconstraint.linedata[start + 3][2*i] = i;
- sim->figconstraint.linedata[start + 4][2*i] = i;
- // y
- int nefc = nisland == 1 ? d->nefc : d->island_efcnum[k];
- sim->figconstraint.linedata[start + 0][2*i+1] = nefc;
- const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
- sim->figconstraint.linedata[start + 1][2*i+1] = stat->nactive;
- sim->figconstraint.linedata[start + 2][2*i+1] = stat->nchange;
- sim->figconstraint.linedata[start + 3][2*i+1] = stat->neval;
- sim->figconstraint.linedata[start + 4][2*i+1] = stat->nupdate;
- }
- // update cost figure
- start = kCostNum * k;
- sim->figcost.linepnt[start + 0] = npoints;
- for (int i=1; i<kCostNum; i++) {
- sim->figcost.linepnt[start + i] = npoints;
- }
- if (m->opt.solver==mjSOL_PGS) {
- sim->figcost.linepnt[start + 1] = 0;
- sim->figcost.linepnt[start + 2] = 0;
- }
- for (int i=0; i<sim->figcost.linepnt[0]; i++) {
- // x
- sim->figcost.linedata[start + 0][2*i] = i;
- sim->figcost.linedata[start + 1][2*i] = i;
- sim->figcost.linedata[start + 2][2*i] = i;
- // y
- const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
- sim->figcost.linedata[start + 0][2*i + 1] =
- mju_log10(mju_max(mjMINVAL, stat->improvement));
- sim->figcost.linedata[start + 1][2*i + 1] =
- mju_log10(mju_max(mjMINVAL, stat->gradient));
- sim->figcost.linedata[start + 2][2*i + 1] =
- mju_log10(mju_max(mjMINVAL, stat->lineslope));
- }
- }
- // get timers: total, collision, prepare, solve, other
- mjtNum total = d->timer[mjTIMER_STEP].duration;
- int number = d->timer[mjTIMER_STEP].number;
- if (!number) {
- total = d->timer[mjTIMER_FORWARD].duration;
- number = d->timer[mjTIMER_FORWARD].number;
- }
- if (number) { // skip update if no measurements
- float tdata[5] = {
- static_cast<float>(total/number),
- static_cast<float>(d->timer[mjTIMER_POS_COLLISION].duration/number),
- static_cast<float>(d->timer[mjTIMER_POS_MAKE].duration/number) +
- static_cast<float>(d->timer[mjTIMER_POS_PROJECT].duration/number),
- static_cast<float>(d->timer[mjTIMER_CONSTRAINT].duration/number),
- 0
- };
- tdata[4] = tdata[0] - tdata[1] - tdata[2] - tdata[3];
- // update figtimer
- int pnt = mjMIN(201, sim->figtimer.linepnt[0]+1);
- for (int n=0; n<5; n++) {
- // shift data
- for (int i=pnt-1; i>0; i--) {
- sim->figtimer.linedata[n][2*i+1] = sim->figtimer.linedata[n][2*i-1];
- }
- // assign new
- sim->figtimer.linepnt[n] = pnt;
- sim->figtimer.linedata[n][1] = tdata[n];
- }
- }
- // get total number of iterations and nonzeros
- mjtNum sqrt_nnz = 0;
- int solver_niter = 0;
- for (int island=0; island < nisland; island++) {
- sqrt_nnz += mju_sqrt(d->solver_nnz[island]);
- solver_niter += d->solver_niter[island];
- }
- // get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
- float sdata[6] = {
- static_cast<float>(m->nv),
- static_cast<float>(m->nbody),
- static_cast<float>(d->nefc),
- static_cast<float>(sqrt_nnz),
- static_cast<float>(d->ncon),
- static_cast<float>(solver_niter)
- };
- // update figsize
- int pnt = mjMIN(201, sim->figsize.linepnt[0]+1);
- for (int n=0; n<6; n++) {
- // shift data
- for (int i=pnt-1; i>0; i--) {
- sim->figsize.linedata[n][2*i+1] = sim->figsize.linedata[n][2*i-1];
- }
- // assign new
- sim->figsize.linepnt[n] = pnt;
- sim->figsize.linedata[n][1] = sdata[n];
- }
- }
- // show profiler figures
- void ShowProfiler(mj::Simulate* sim, mjrRect rect) {
- mjrRect viewport = {
- rect.left + rect.width - rect.width/4,
- rect.bottom,
- rect.width/4,
- rect.height/4
- };
- mjr_figure(viewport, &sim->figtimer, &sim->platform_ui->mjr_context());
- viewport.bottom += rect.height/4;
- mjr_figure(viewport, &sim->figsize, &sim->platform_ui->mjr_context());
- viewport.bottom += rect.height/4;
- mjr_figure(viewport, &sim->figcost, &sim->platform_ui->mjr_context());
- viewport.bottom += rect.height/4;
- mjr_figure(viewport, &sim->figconstraint, &sim->platform_ui->mjr_context());
- }
- // init sensor figure
- void InitializeSensor(mj::Simulate* sim) {
- mjvFigure& figsensor = sim->figsensor;
- // set figure to default
- mjv_defaultFigure(&figsensor);
- figsensor.figurergba[3] = 0.5f;
- // set flags
- figsensor.flg_extend = 1;
- figsensor.flg_barplot = 1;
- figsensor.flg_symmetric = 1;
- // title
- mju::strcpy_arr(figsensor.title, "Sensor data");
- // y-tick nubmer format
- mju::strcpy_arr(figsensor.yformat, "%.1f");
- // grid size
- figsensor.gridsize[0] = 2;
- figsensor.gridsize[1] = 3;
- // minimum range
- figsensor.range[0][0] = 0;
- figsensor.range[0][1] = 0;
- figsensor.range[1][0] = -1;
- figsensor.range[1][1] = 1;
- }
- // update sensor figure
- void UpdateSensor(mj::Simulate* sim, const mjModel* m, const mjData* d) {
- mjvFigure& figsensor = sim->figsensor;
- static const int maxline = 10;
- // clear linepnt
- for (int i=0; i<maxline; i++) {
- figsensor.linepnt[i] = 0;
- }
- // start with line 0
- int lineid = 0;
- // loop over sensors
- for (int n=0; n<m->nsensor; n++) {
- // go to next line if type is different
- if (n>0 && m->sensor_type[n]!=m->sensor_type[n-1]) {
- lineid = mjMIN(lineid+1, maxline-1);
- }
- // get info about this sensor
- mjtNum cutoff = (m->sensor_cutoff[n]>0 ? m->sensor_cutoff[n] : 1);
- int adr = m->sensor_adr[n];
- int dim = m->sensor_dim[n];
- // data pointer in line
- int p = figsensor.linepnt[lineid];
- // fill in data for this sensor
- for (int i=0; i<dim; i++) {
- // check size
- if ((p+2*i)>=mjMAXLINEPNT/2) {
- break;
- }
- // x
- figsensor.linedata[lineid][2*p+4*i] = adr+i;
- figsensor.linedata[lineid][2*p+4*i+2] = adr+i;
- // y
- figsensor.linedata[lineid][2*p+4*i+1] = 0;
- figsensor.linedata[lineid][2*p+4*i+3] = d->sensordata[adr+i]/cutoff;
- }
- // update linepnt
- figsensor.linepnt[lineid] = mjMIN(mjMAXLINEPNT-1, figsensor.linepnt[lineid]+2*dim);
- }
- }
- // show sensor figure
- void ShowSensor(mj::Simulate* sim, mjrRect rect) {
- // constant width with and without profiler
- int width = sim->profiler ? rect.width/3 : rect.width/4;
- // render figure on the right
- mjrRect viewport = {
- rect.left + rect.width - width,
- rect.bottom,
- width,
- rect.height/3
- };
- mjr_figure(viewport, &sim->figsensor, &sim->platform_ui->mjr_context());
- }
- // load state from history buffer
- static void LoadScrubState(mj::Simulate* sim) {
- // get index into circular buffer
- int i = (sim->scrub_index + sim->history_cursor_) % sim->nhistory_;
- i = (i + sim->nhistory_) % sim->nhistory_;
- // load state
- mjtNum* state = &sim->history_[i * sim->state_size_];
- mj_setState(sim->m_, sim->d_, state, mjSTATE_INTEGRATION);
- // call forward dynamics
- mj_forward(sim->m_, sim->d_);
- }
- // update an entire section of ui0
- static void mjui0_update_section(mj::Simulate* sim, int section) {
- mjui_update(section, -1, &sim->ui0, &sim->uistate, &sim->platform_ui->mjr_context());
- }
- // prepare info text
- void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
- char (&title)[mj::Simulate::kMaxFilenameLength],
- char (&content)[mj::Simulate::kMaxFilenameLength]) {
- char tmp[20];
- // number of islands with statistics
- int nisland = mjMIN(d->solver_nisland, mjNISLAND);
- // compute solver error (maximum over islands)
- mjtNum solerr = 0;
- for (int i=0; i < nisland; i++) {
- mjtNum solerr_i = 0;
- if (d->solver_niter[i]) {
- int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
- const mjSolverStat* stat = d->solver + i*mjNSOLVER + ind;
- solerr_i = mju_min(stat->improvement, stat->gradient);
- if (solerr_i==0) {
- solerr_i = mju_max(stat->improvement, stat->gradient);
- }
- }
- solerr = mju_max(solerr, solerr_i);
- }
- solerr = mju_log10(mju_max(mjMINVAL, solerr));
- // format FPS text
- char fps[10];
- if (sim->fps_ < 1) {
- mju::sprintf_arr(fps, "%0.1f ", sim->fps_);
- } else {
- mju::sprintf_arr(fps, "%.0f ", sim->fps_);
- }
- // total iterations of all islands with statistics
- int solver_niter = 0;
- for (int i=0; i < nisland; i++) {
- solver_niter += d->solver_niter[i];
- }
- // prepare info text
- mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory");
- mju::sprintf_arr(content,
- "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%s\n%.1f%% of %s",
- d->time,
- d->nefc, d->ncon,
- sim->run ?
- d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) :
- d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number),
- solerr, solver_niter,
- fps,
- 100*d->maxuse_arena/(double)(d->narena),
- mju_writeNumBytes(d->narena));
- // add Energy if enabled
- {
- if (mjENABLED(mjENBL_ENERGY)) {
- mju::sprintf_arr(tmp, "\n%.3f", d->energy[0]+d->energy[1]);
- mju::strcat_arr(content, tmp);
- mju::strcat_arr(title, "\nEnergy");
- }
- // add FwdInv if enabled
- if (mjENABLED(mjENBL_FWDINV)) {
- mju::sprintf_arr(tmp, "\n%.1f %.1f",
- mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[0])),
- mju_log10(mju_max(mjMINVAL, d->solver_fwdinv[1])));
- mju::strcat_arr(content, tmp);
- mju::strcat_arr(title, "\nFwdInv");
- }
- // add islands if enabled
- if (mjENABLED(mjENBL_ISLAND)) {
- mju::sprintf_arr(tmp, "\n%d", d->nisland);
- mju::strcat_arr(content, tmp);
- mju::strcat_arr(title, "\nIslands");
- }
- }
- }
- // sprintf forwarding, to avoid compiler warning in x-macro
- void PrintField(char (&str)[mjMAXUINAME], void* ptr) {
- mju::sprintf_arr(str, "%g", *static_cast<mjtNum*>(ptr));
- }
- // update watch
- void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) {
- // clear
- sim->ui0.sect[SECT_WATCH].item[2].multi.nelem = 1;
- mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid field");
- // prepare symbols needed by xmacro
- MJDATA_POINTERS_PREAMBLE(m);
- // find specified field in mjData arrays, update value
- #define X(TYPE, NAME, NR, NC) \
- if (!mju::strcmp_arr(#NAME, sim->field) && \
- !mju::strcmp_arr(#TYPE, "mjtNum")) { \
- if (sim->index >= 0 && sim->index < m->NR * NC) { \
- PrintField(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], d->NAME + sim->index); \
- } else { \
- mju::strcpy_arr(sim->ui0.sect[SECT_WATCH].item[2].multi.name[0], "invalid index"); \
- } \
- return; \
- }
- MJDATA_POINTERS
- #undef X
- }
- //---------------------------------- UI construction -----------------------------------------------
- // make physics section of UI
- void MakePhysicsSection(mj::Simulate* sim) {
- mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt;
- mjuiDef defPhysics[] = {
- {mjITEM_SECTION, "Physics", mjPRESERVE, nullptr, "AP"},
- {mjITEM_SELECT, "Integrator", 2, &(opt->integrator), "Euler\nRK4\nimplicit\nimplicitfast"},
- {mjITEM_SELECT, "Cone", 2, &(opt->cone), "Pyramidal\nElliptic"},
- {mjITEM_SELECT, "Jacobian", 2, &(opt->jacobian), "Dense\nSparse\nAuto"},
- {mjITEM_SELECT, "Solver", 2, &(opt->solver), "PGS\nCG\nNewton"},
- {mjITEM_SEPARATOR, "Algorithmic Parameters", mjPRESERVE},
- {mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"},
- {mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"},
- {mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"},
- {mjITEM_EDITINT, "LS Iter", 2, &(opt->ls_iterations), "1 0 100"},
- {mjITEM_EDITNUM, "LS Tol", 2, &(opt->ls_tolerance), "1 0 0.1"},
- {mjITEM_EDITINT, "Noslip Iter", 2, &(opt->noslip_iterations), "1 0 1000"},
- {mjITEM_EDITNUM, "Noslip Tol", 2, &(opt->noslip_tolerance), "1 0 1"},
- {mjITEM_EDITINT, "CCD Iter", 2, &(opt->ccd_iterations), "1 0 1000"},
- {mjITEM_EDITNUM, "CCD Tol", 2, &(opt->ccd_tolerance), "1 0 1"},
- {mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"},
- {mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"},
- {mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"},
- {mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE},
- {mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"},
- {mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"},
- {mjITEM_EDITNUM, "Magnetic", 2, opt->magnetic, "3"},
- {mjITEM_EDITNUM, "Density", 2, &(opt->density), "1"},
- {mjITEM_EDITNUM, "Viscosity", 2, &(opt->viscosity), "1"},
- {mjITEM_EDITNUM, "Imp Ratio", 2, &(opt->impratio), "1"},
- {mjITEM_SEPARATOR, "Disable Flags", mjPRESERVE},
- {mjITEM_END}
- };
- mjuiDef defEnableFlags[] = {
- {mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE},
- {mjITEM_END}
- };
- mjuiDef defOverride[] = {
- {mjITEM_SEPARATOR, "Contact Override", mjPRESERVE},
- {mjITEM_EDITNUM, "Margin", 2, &(opt->o_margin), "1"},
- {mjITEM_EDITNUM, "Sol Imp", 2, &(opt->o_solimp), "5"},
- {mjITEM_EDITNUM, "Sol Ref", 2, &(opt->o_solref), "2"},
- {mjITEM_EDITNUM, "Friction", 2, &(opt->o_friction), "5"},
- {mjITEM_END}
- };
- mjuiDef defDisableActuator[] = {
- {mjITEM_SEPARATOR, "Actuator Group Enable", mjPRESERVE},
- {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator+0, ""},
- {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator+1, ""},
- {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator+2, ""},
- {mjITEM_CHECKBYTE, "Act Group 3", 2, sim->enableactuator+3, ""},
- {mjITEM_CHECKBYTE, "Act Group 4", 2, sim->enableactuator+4, ""},
- {mjITEM_CHECKBYTE, "Act Group 5", 2, sim->enableactuator+5, ""},
- {mjITEM_END}
- };
- // add physics
- mjui_add(&sim->ui0, defPhysics);
- // add flags programmatically
- mjuiDef defFlag[] = {
- {mjITEM_CHECKINT, "", 2, nullptr, ""},
- {mjITEM_END}
- };
- for (int i=0; i<mjNDISABLE; i++) {
- mju::strcpy_arr(defFlag[0].name, mjDISABLESTRING[i]);
- defFlag[0].pdata = sim->disable + i;
- mjui_add(&sim->ui0, defFlag);
- }
- mjui_add(&sim->ui0, defEnableFlags);
- for (int i=0; i<mjNENABLE; i++) {
- mju::strcpy_arr(defFlag[0].name, mjENABLESTRING[i]);
- defFlag[0].pdata = sim->enable + i;
- mjui_add(&sim->ui0, defFlag);
- }
- // add contact override
- mjui_add(&sim->ui0, defOverride);
- // add actuator group enable/disable
- mjui_add(&sim->ui0, defDisableActuator);
- // make some subsections closed by default
- for (int i=0; i < sim->ui0.sect[SECT_PHYSICS].nitem; i++) {
- mjuiItem* it = sim->ui0.sect[SECT_PHYSICS].item + i;
- // close less useful subsections
- if (it->type == mjITEM_SEPARATOR) {
- if (mju::strcmp_arr(it->name, "Actuator Group Enable") &&
- mju::strcmp_arr(it->name, "Contact Override") &&
- mju::strcmp_arr(it->name, "Physical Parameters")) {
- it->state = mjSEPCLOSED+1;
- }
- }
- }
- }
- // make rendering section of UI
- void MakeRenderingSection(mj::Simulate* sim, const mjModel* m) {
- mjuiDef defRendering[] = {
- {mjITEM_SECTION, "Rendering", mjPRESERVE, nullptr, "AR"},
- {mjITEM_SELECT, "Camera", 2, &(sim->camera), "Free\nTracking"},
- {mjITEM_SELECT, "Label", 2, &(sim->opt.label),
- "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
- "Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"
- },
- {mjITEM_SELECT, "Frame", 2, &(sim->opt.frame),
- "None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld"
- },
- {mjITEM_BUTTON, "Copy camera", 2, nullptr, ""},
- {mjITEM_SEPARATOR, "Model Elements", 1},
- {mjITEM_END}
- };
- mjuiDef defOpenGL[] = {
- {mjITEM_SEPARATOR, "OpenGL Effects", 1},
- {mjITEM_END}
- };
- // add model cameras, up to UI limit
- for (int i=0; i<mjMIN(m->ncam, mjMAXUIMULTI-2); i++) {
- // prepare name
- char camname[mjMAXUINAME] = "\n";
- if (m->names[m->name_camadr[i]]) {
- mju::strcat_arr(camname, m->names+m->name_camadr[i]);
- } else {
- mju::sprintf_arr(camname, "\nCamera %d", i);
- }
- // check string length
- if (mju::strlen_arr(camname) + mju::strlen_arr(defRendering[1].other)>=mjMAXUITEXT-1) {
- break;
- }
- // add camera
- mju::strcat_arr(defRendering[1].other, camname);
- }
- // add rendering standard
- mjui_add(&sim->ui0, defRendering);
- // add flags programmatically
- mjuiDef defFlag[] = {
- {mjITEM_CHECKBYTE, "", 2, nullptr, ""},
- {mjITEM_END}
- };
- for (int i=0; i<mjNVISFLAG; i++) {
- // set name
- mju::strcpy_arr(defFlag[0].name, mjVISSTRING[i][0]);
- // set shortcut and data
- if (mjVISSTRING[i][2][0]) {
- mju::sprintf_arr(defFlag[0].other, " %s", mjVISSTRING[i][2]);
- } else {
- mju::sprintf_arr(defFlag[0].other, "");
- }
- defFlag[0].pdata = sim->opt.flags + i;
- mjui_add(&sim->ui0, defFlag);
- }
- // create tree slider
- mjuiDef defTree[] = {
- {mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 20"},
- {mjITEM_SLIDERINT, "Flex layer", 2, &sim->opt.flex_layer, "0 10"},
- {mjITEM_END}
- };
- mjui_add(&sim->ui0, defTree);
- // add rendering flags
- mjui_add(&sim->ui0, defOpenGL);
- for (int i=0; i<mjNRNDFLAG; i++) {
- // set name
- mju::strcpy_arr(defFlag[0].name, mjRNDSTRING[i][0]);
- // set shortcut and data
- if (mjRNDSTRING[i][2][0]) {
- mju::sprintf_arr(defFlag[0].other, " %s", mjRNDSTRING[i][2]);
- } else {
- mju::sprintf_arr(defFlag[0].other, "");
- }
- defFlag[0].pdata = sim->scn.flags + i;
- mjui_add(&sim->ui0, defFlag);
- }
- }
- // make visualization section of UI
- void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) {
- mjStatistic* stat = sim->is_passive_ ? &sim->scnstate_.model.stat : &sim->m_->stat;
- mjVisual* vis = sim->is_passive_ ? &sim->scnstate_.model.vis : &sim->m_->vis;
- mjuiDef defVisualization[] = {
- {mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"},
- {mjITEM_SEPARATOR, "Headlight", 1},
- {mjITEM_RADIO, "Active", 5, &(vis->headlight.active), "Off\nOn"},
- {mjITEM_EDITFLOAT, "Ambient", 2, &(vis->headlight.ambient), "3"},
- {mjITEM_EDITFLOAT, "Diffuse", 2, &(vis->headlight.diffuse), "3"},
- {mjITEM_EDITFLOAT, "Specular", 2, &(vis->headlight.specular), "3"},
- {mjITEM_SEPARATOR, "Free Camera", 1},
- {mjITEM_RADIO, "Orthographic", 2, &(vis->global.orthographic), "No\nYes"},
- {mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"},
- {mjITEM_EDITNUM, "Center", 2, &(stat->center), "3"},
- {mjITEM_EDITFLOAT, "Azimuth", 2, &(vis->global.azimuth), "1"},
- {mjITEM_EDITFLOAT, "Elevation", 2, &(vis->global.elevation), "1"},
- {mjITEM_BUTTON, "Align", 2, nullptr, "CA"},
- {mjITEM_SEPARATOR, "Global", 1},
- {mjITEM_EDITNUM, "Extent", 2, &(stat->extent), "1"},
- {mjITEM_RADIO, "Inertia", 5, &(vis->global.ellipsoidinertia), "Box\nEllipsoid"},
- {mjITEM_RADIO, "BVH active", 5, &(vis->global.bvactive), "False\nTrue"},
- {mjITEM_SEPARATOR, "Map", 1},
- {mjITEM_EDITFLOAT, "Stiffness", 2, &(vis->map.stiffness), "1"},
- {mjITEM_EDITFLOAT, "Rot stiffness", 2, &(vis->map.stiffnessrot), "1"},
- {mjITEM_EDITFLOAT, "Force", 2, &(vis->map.force), "1"},
- {mjITEM_EDITFLOAT, "Torque", 2, &(vis->map.torque), "1"},
- {mjITEM_EDITFLOAT, "Alpha", 2, &(vis->map.alpha), "1"},
- {mjITEM_EDITFLOAT, "Fog start", 2, &(vis->map.fogstart), "1"},
- {mjITEM_EDITFLOAT, "Fog end", 2, &(vis->map.fogend), "1"},
- {mjITEM_EDITFLOAT, "Z near", 2, &(vis->map.znear), "1"},
- {mjITEM_EDITFLOAT, "Z far", 2, &(vis->map.zfar), "1"},
- {mjITEM_EDITFLOAT, "Haze", 2, &(vis->map.haze), "1"},
- {mjITEM_EDITFLOAT, "Shadow clip", 2, &(vis->map.shadowclip), "1"},
- {mjITEM_EDITFLOAT, "Shadow scale", 2, &(vis->map.shadowscale), "1"},
- {mjITEM_SEPARATOR, "Scale", mjPRESERVE},
- {mjITEM_EDITNUM, "All (meansize)", 2, &(stat->meansize), "1"},
- {mjITEM_EDITFLOAT, "Force width", 2, &(vis->scale.forcewidth), "1"},
- {mjITEM_EDITFLOAT, "Contact width", 2, &(vis->scale.contactwidth), "1"},
- {mjITEM_EDITFLOAT, "Contact height", 2, &(vis->scale.contactheight), "1"},
- {mjITEM_EDITFLOAT, "Connect", 2, &(vis->scale.connect), "1"},
- {mjITEM_EDITFLOAT, "Com", 2, &(vis->scale.com), "1"},
- {mjITEM_EDITFLOAT, "Camera", 2, &(vis->scale.camera), "1"},
- {mjITEM_EDITFLOAT, "Light", 2, &(vis->scale.light), "1"},
- {mjITEM_EDITFLOAT, "Select point", 2, &(vis->scale.selectpoint), "1"},
- {mjITEM_EDITFLOAT, "Joint length", 2, &(vis->scale.jointlength), "1"},
- {mjITEM_EDITFLOAT, "Joint width", 2, &(vis->scale.jointwidth), "1"},
- {mjITEM_EDITFLOAT, "Actuator length", 2, &(vis->scale.actuatorlength), "1"},
- {mjITEM_EDITFLOAT, "Actuator width", 2, &(vis->scale.actuatorwidth), "1"},
- {mjITEM_EDITFLOAT, "Frame length", 2, &(vis->scale.framelength), "1"},
- {mjITEM_EDITFLOAT, "Frame width", 2, &(vis->scale.framewidth), "1"},
- {mjITEM_EDITFLOAT, "Constraint", 2, &(vis->scale.constraint), "1"},
- {mjITEM_EDITFLOAT, "Slider-crank", 2, &(vis->scale.slidercrank), "1"},
- {mjITEM_SEPARATOR, "RGBA", mjPRESERVE},
- {mjITEM_EDITFLOAT, "fog", 2, &(vis->rgba.fog), "4"},
- {mjITEM_EDITFLOAT, "haze", 2, &(vis->rgba.haze), "4"},
- {mjITEM_EDITFLOAT, "force", 2, &(vis->rgba.force), "4"},
- {mjITEM_EDITFLOAT, "inertia", 2, &(vis->rgba.inertia), "4"},
- {mjITEM_EDITFLOAT, "joint", 2, &(vis->rgba.joint), "4"},
- {mjITEM_EDITFLOAT, "actuator", 2, &(vis->rgba.actuator), "4"},
- {mjITEM_EDITFLOAT, "actnegative", 2, &(vis->rgba.actuatornegative), "4"},
- {mjITEM_EDITFLOAT, "actpositive", 2, &(vis->rgba.actuatorpositive), "4"},
- {mjITEM_EDITFLOAT, "com", 2, &(vis->rgba.com), "4"},
- {mjITEM_EDITFLOAT, "camera", 2, &(vis->rgba.camera), "4"},
- {mjITEM_EDITFLOAT, "light", 2, &(vis->rgba.light), "4"},
- {mjITEM_EDITFLOAT, "selectpoint", 2, &(vis->rgba.selectpoint), "4"},
- {mjITEM_EDITFLOAT, "connect", 2, &(vis->rgba.connect), "4"},
- {mjITEM_EDITFLOAT, "contactpoint", 2, &(vis->rgba.contactpoint), "4"},
- {mjITEM_EDITFLOAT, "contactforce", 2, &(vis->rgba.contactforce), "4"},
- {mjITEM_EDITFLOAT, "contactfriction", 2, &(vis->rgba.contactfriction), "4"},
- {mjITEM_EDITFLOAT, "contacttorque", 2, &(vis->rgba.contacttorque), "4"},
- {mjITEM_EDITFLOAT, "contactgap", 2, &(vis->rgba.contactgap), "4"},
- {mjITEM_EDITFLOAT, "rangefinder", 2, &(vis->rgba.rangefinder), "4"},
- {mjITEM_EDITFLOAT, "constraint", 2, &(vis->rgba.constraint), "4"},
- {mjITEM_EDITFLOAT, "slidercrank", 2, &(vis->rgba.slidercrank), "4"},
- {mjITEM_EDITFLOAT, "crankbroken", 2, &(vis->rgba.crankbroken), "4"},
- {mjITEM_EDITFLOAT, "frustum", 2, &(vis->rgba.frustum), "4"},
- {mjITEM_EDITFLOAT, "bv", 2, &(vis->rgba.bv), "4"},
- {mjITEM_EDITFLOAT, "bvactive", 2, &(vis->rgba.bvactive), "4"},
- {mjITEM_END}
- };
- // add visualization section
- mjui_add(&sim->ui0, defVisualization);
- }
- // make group section of UI
- void MakeGroupSection(mj::Simulate* sim) {
- mjuiDef defGroup[] = {
- {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"},
- {mjITEM_SEPARATOR, "Geom groups", 1},
- {mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"},
- {mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup+1, " 1"},
- {mjITEM_CHECKBYTE, "Geom 2", 2, sim->opt.geomgroup+2, " 2"},
- {mjITEM_CHECKBYTE, "Geom 3", 2, sim->opt.geomgroup+3, " 3"},
- {mjITEM_CHECKBYTE, "Geom 4", 2, sim->opt.geomgroup+4, " 4"},
- {mjITEM_CHECKBYTE, "Geom 5", 2, sim->opt.geomgroup+5, " 5"},
- {mjITEM_SEPARATOR, "Site groups", 1},
- {mjITEM_CHECKBYTE, "Site 0", 2, sim->opt.sitegroup, "S0"},
- {mjITEM_CHECKBYTE, "Site 1", 2, sim->opt.sitegroup+1, "S1"},
- {mjITEM_CHECKBYTE, "Site 2", 2, sim->opt.sitegroup+2, "S2"},
- {mjITEM_CHECKBYTE, "Site 3", 2, sim->opt.sitegroup+3, "S3"},
- {mjITEM_CHECKBYTE, "Site 4", 2, sim->opt.sitegroup+4, "S4"},
- {mjITEM_CHECKBYTE, "Site 5", 2, sim->opt.sitegroup+5, "S5"},
- {mjITEM_SEPARATOR, "Joint groups", 1},
- {mjITEM_CHECKBYTE, "Joint 0", 2, sim->opt.jointgroup, ""},
- {mjITEM_CHECKBYTE, "Joint 1", 2, sim->opt.jointgroup+1, ""},
- {mjITEM_CHECKBYTE, "Joint 2", 2, sim->opt.jointgroup+2, ""},
- {mjITEM_CHECKBYTE, "Joint 3", 2, sim->opt.jointgroup+3, ""},
- {mjITEM_CHECKBYTE, "Joint 4", 2, sim->opt.jointgroup+4, ""},
- {mjITEM_CHECKBYTE, "Joint 5", 2, sim->opt.jointgroup+5, ""},
- {mjITEM_SEPARATOR, "Tendon groups", 1},
- {mjITEM_CHECKBYTE, "Tendon 0", 2, sim->opt.tendongroup, ""},
- {mjITEM_CHECKBYTE, "Tendon 1", 2, sim->opt.tendongroup+1, ""},
- {mjITEM_CHECKBYTE, "Tendon 2", 2, sim->opt.tendongroup+2, ""},
- {mjITEM_CHECKBYTE, "Tendon 3", 2, sim->opt.tendongroup+3, ""},
- {mjITEM_CHECKBYTE, "Tendon 4", 2, sim->opt.tendongroup+4, ""},
- {mjITEM_CHECKBYTE, "Tendon 5", 2, sim->opt.tendongroup+5, ""},
- {mjITEM_SEPARATOR, "Actuator groups", 1},
- {mjITEM_CHECKBYTE, "Actuator 0", 2, sim->opt.actuatorgroup, ""},
- {mjITEM_CHECKBYTE, "Actuator 1", 2, sim->opt.actuatorgroup+1, ""},
- {mjITEM_CHECKBYTE, "Actuator 2", 2, sim->opt.actuatorgroup+2, ""},
- {mjITEM_CHECKBYTE, "Actuator 3", 2, sim->opt.actuatorgroup+3, ""},
- {mjITEM_CHECKBYTE, "Actuator 4", 2, sim->opt.actuatorgroup+4, ""},
- {mjITEM_CHECKBYTE, "Actuator 5", 2, sim->opt.actuatorgroup+5, ""},
- {mjITEM_SEPARATOR, "Flex groups", 1},
- {mjITEM_CHECKBYTE, "Flex 0", 2, sim->opt.flexgroup, ""},
- {mjITEM_CHECKBYTE, "Flex 1", 2, sim->opt.flexgroup+1, ""},
- {mjITEM_CHECKBYTE, "Flex 2", 2, sim->opt.flexgroup+2, ""},
- {mjITEM_CHECKBYTE, "Flex 3", 2, sim->opt.flexgroup+3, ""},
- {mjITEM_CHECKBYTE, "Flex 4", 2, sim->opt.flexgroup+4, ""},
- {mjITEM_CHECKBYTE, "Flex 5", 2, sim->opt.flexgroup+5, ""},
- {mjITEM_SEPARATOR, "Skin groups", 1},
- {mjITEM_CHECKBYTE, "Skin 0", 2, sim->opt.skingroup, ""},
- {mjITEM_CHECKBYTE, "Skin 1", 2, sim->opt.skingroup+1, ""},
- {mjITEM_CHECKBYTE, "Skin 2", 2, sim->opt.skingroup+2, ""},
- {mjITEM_CHECKBYTE, "Skin 3", 2, sim->opt.skingroup+3, ""},
- {mjITEM_CHECKBYTE, "Skin 4", 2, sim->opt.skingroup+4, ""},
- {mjITEM_CHECKBYTE, "Skin 5", 2, sim->opt.skingroup+5, ""},
- {mjITEM_END}
- };
- // add section
- mjui_add(&sim->ui0, defGroup);
- }
- // make joint section of UI
- void MakeJointSection(mj::Simulate* sim) {
- mjuiDef defJoint[] = {
- {mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"},
- {mjITEM_END}
- };
- mjuiDef defSlider[] = {
- {mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"},
- {mjITEM_END}
- };
- // add section
- mjui_add(&sim->ui1, defJoint);
- defSlider[0].state = 4;
- // add scalar joints, exit if UI limit reached
- int itemcnt = 0;
- for (int i=0; i < sim->jnt_type_.size() && itemcnt<mjMAXUIITEM; i++) {
- if ((sim->jnt_type_[i]==mjJNT_HINGE || sim->jnt_type_[i]==mjJNT_SLIDE)) {
- // skip if joint group is disabled
- if (!sim->opt.jointgroup[mjMAX(0, mjMIN(mjNGROUP-1, sim->jnt_group_[i]))]) {
- continue;
- }
- // set data and name
- if (!sim->is_passive_) {
- defSlider[0].pdata = &sim->d_->qpos[sim->m_->jnt_qposadr[i]];
- } else {
- defSlider[0].pdata = &sim->qpos_[sim->jnt_qposadr_[i]];
- }
- if (!sim->jnt_names_[i].empty()) {
- mju::strcpy_arr(defSlider[0].name, sim->jnt_names_[i].c_str());
- } else {
- mju::sprintf_arr(defSlider[0].name, "joint %d", i);
- }
- // set range
- if (sim->jnt_range_[i].has_value())
- mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
- sim->jnt_range_[i]->first, sim->jnt_range_[i]->second);
- else if (sim->jnt_type_[i]==mjJNT_SLIDE) {
- mju::strcpy_arr(defSlider[0].other, "-1 1");
- } else {
- mju::strcpy_arr(defSlider[0].other, "-3.1416 3.1416");
- }
- // add and count
- mjui_add(&sim->ui1, defSlider);
- itemcnt++;
- }
- }
- }
- // make control section of UI
- void MakeControlSection(mj::Simulate* sim) {
- mjuiDef defControl[] = {
- {mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"},
- {mjITEM_BUTTON, "Clear all", 2},
- {mjITEM_END}
- };
- mjuiDef defSlider[] = {
- {mjITEM_SLIDERNUM, "", 2, nullptr, "0 1"},
- {mjITEM_END}
- };
- // add section
- mjui_add(&sim->ui1, defControl);
- // add controls, exit if UI limit reached (Clear button already added)
- int itemcnt = 1;
- for (int i=0; i < sim->actuator_ctrlrange_.size() && itemcnt<mjMAXUIITEM; i++) {
- // skip if actuator vis group is disabled
- int group = sim->actuator_group_[i];
- if (!sim->opt.actuatorgroup[mjMAX(0, mjMIN(mjNGROUP-1, group))]) {
- continue;
- }
- // grey out if actuator group is disabled
- if (group >= 0 && group <= 30 && sim->m_->opt.disableactuator & (1 << group)) {
- defSlider[0].state = 0;
- } else {
- defSlider[0].state = 2;
- }
- // set data and name
- if (!sim->is_passive_) {
- defSlider[0].pdata = &sim->d_->ctrl[i];
- } else {
- defSlider[0].pdata = &sim->ctrl_[i];
- }
- if (!sim->actuator_names_[i].empty()) {
- mju::strcpy_arr(defSlider[0].name, sim->actuator_names_[i].c_str());
- } else {
- mju::sprintf_arr(defSlider[0].name, "control %d", i);
- }
- // set range
- if (sim->actuator_ctrlrange_[i].has_value())
- mju::sprintf_arr(defSlider[0].other, "%.4g %.4g",
- sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second);
- else {
- mju::strcpy_arr(defSlider[0].other, "-1 1");
- }
- // add and count
- mjui_add(&sim->ui1, defSlider);
- itemcnt++;
- }
- }
- // make model-dependent UI sections
- void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) {
- // clear model-dependent sections of UI
- sim->ui0.nsect = SECT_PHYSICS;
- sim->ui1.nsect = 0;
- // make
- MakePhysicsSection(sim);
- MakeRenderingSection(sim, m);
- MakeVisualizationSection(sim, m);
- MakeGroupSection(sim);
- MakeJointSection(sim);
- MakeControlSection(sim);
- }
- //---------------------------------- utility functions ---------------------------------------------
- // align and scale view
- void AlignAndScaleView(mj::Simulate* sim, const mjModel* m) {
- // use default free camera parameters
- mjv_defaultFreeCamera(m, &sim->cam);
- }
- // copy qpos to clipboard as key
- void CopyPose(mj::Simulate* sim, const mjModel* m, const mjData* d) {
- char clipboard[5000] = "<key qpos='";
- char buf[200];
- // prepare string
- for (int i=0; i<m->nq; i++) {
- mju::sprintf_arr(buf, i==m->nq-1 ? "%g" : "%g ", d->qpos[i]);
- mju::strcat_arr(clipboard, buf);
- }
- mju::strcat_arr(clipboard, "'/>");
- // copy to clipboard
- sim->platform_ui->SetClipboardString(clipboard);
- }
- // millisecond timer, for MuJoCo built-in profiler
- mjtNum Timer() {
- static auto start = mj::Simulate::Clock::now();
- auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start);
- return elapsed.count();
- }
- // clear all times
- void ClearTimers(mjData* d) {
- for (int i=0; i<mjNTIMER; i++) {
- d->timer[i].duration = 0;
- d->timer[i].number = 0;
- }
- }
- // copy current camera to clipboard as MJCF specification
- void CopyCamera(mj::Simulate* sim) {
- mjvGLCamera* camera = sim->scn.camera;
- char clipboard[500];
- mjtNum cam_right[3];
- mjtNum cam_forward[3];
- mjtNum cam_up[3];
- // get camera spec from the GLCamera
- mju_f2n(cam_forward, camera[0].forward, 3);
- mju_f2n(cam_up, camera[0].up, 3);
- mju_cross(cam_right, cam_forward, cam_up);
- // make MJCF camera spec
- mju::sprintf_arr(clipboard,
- "<camera pos=\"%.3f %.3f %.3f\" xyaxes=\"%.3f %.3f %.3f %.3f %.3f %.3f\"/>\n",
- (camera[0].pos[0] + camera[1].pos[0]) / 2,
- (camera[0].pos[1] + camera[1].pos[1]) / 2,
- (camera[0].pos[2] + camera[1].pos[2]) / 2,
- cam_right[0], cam_right[1], cam_right[2],
- camera[0].up[0], camera[0].up[1], camera[0].up[2]);
- // copy spec into clipboard
- sim->platform_ui->SetClipboardString(clipboard);
- }
- // update UI 0 when MuJoCo structures change (except for joint sliders)
- void UpdateSettings(mj::Simulate* sim, const mjModel* m) {
- // physics flags
- for (int i=0; i<mjNDISABLE; i++) {
- int new_value = ((m->opt.disableflags & (1<<i)) != 0);
- if (sim->disable[i] != new_value) {
- sim->disable[i] = new_value;
- sim->pending_.ui_update_physics = true;
- }
- }
- for (int i=0; i<mjNENABLE; i++) {
- int new_value = ((m->opt.enableflags & (1<<i)) != 0);
- if (sim->enable[i] != new_value) {
- sim->enable[i] = new_value;
- sim->pending_.ui_update_physics = true;
- }
- }
- for (int i=0; i<mjNGROUP; i++) {
- int enabled = ((m->opt.disableactuator & (1<<i)) == 0);
- if (sim->enableactuator[i] != enabled) {
- sim->enableactuator[i] = enabled;
- sim->pending_.ui_update_physics = true;
- sim->pending_.ui_remake_ctrl = true;
- }
- }
- // camera
- int old_camera = sim->camera;
- if (sim->cam.type==mjCAMERA_FIXED) {
- sim->camera = 2 + sim->cam.fixedcamid;
- } else if (sim->cam.type==mjCAMERA_TRACKING) {
- sim->camera = 1;
- } else {
- sim->camera = 0;
- }
- if (old_camera != sim->camera) {
- sim->pending_.ui_update_rendering = true;
- }
- }
- // Compute suitable font scale.
- int ComputeFontScale(const mj::PlatformUIAdapter& platform_ui) {
- // compute framebuffer-to-window ratio
- auto [buf_width, buf_height] = platform_ui.GetFramebufferSize();
- auto [win_width, win_height] = platform_ui.GetWindowSize();
- double b2w = static_cast<double>(buf_width) / win_width;
- // compute PPI
- double PPI = b2w * platform_ui.GetDisplayPixelsPerInch();
- // estimate font scaling, guard against unrealistic PPI
- int fs;
- if (buf_width > win_width) {
- fs = mju_round(b2w * 100);
- } else if (PPI>50 && PPI<350) {
- fs = mju_round(PPI);
- } else {
- fs = 150;
- }
- fs = mju_round(fs * 0.02) * 50;
- fs = mjMIN(300, mjMAX(100, fs));
- return fs;
- }
- //---------------------------------- UI handlers ---------------------------------------------------
- // determine enable/disable item state given category
- int UiPredicate(int category, void* userdata) {
- mj::Simulate* sim = static_cast<mj::Simulate*>(userdata);
- switch (category) {
- case 2: // require model
- return sim->m_ || sim->is_passive_;
- case 3: // require model and nkey
- return (sim->m_ || sim->is_passive_) && sim->nkey_;
- case 4: // require model and paused
- return sim->m_ && !sim->run;
- case 5: // require model and fully managed mode
- return !sim->is_passive_ && sim->m_;
- default:
- return 1;
- }
- }
- // set window layout
- void UiLayout(mjuiState* state) {
- mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
- mjrRect* rect = state->rect;
- // set number of rectangles
- state->nrect = 4;
- // rect 1: UI 0
- rect[1].left = 0;
- rect[1].width = sim->ui0_enable ? sim->ui0.width : 0;
- rect[1].bottom = 0;
- rect[1].height = rect[0].height;
- // rect 2: UI 1
- rect[2].width = sim->ui1_enable ? sim->ui1.width : 0;
- rect[2].left = mjMAX(0, rect[0].width - rect[2].width);
- rect[2].bottom = 0;
- rect[2].height = rect[0].height;
- // rect 3: 3D plot (everything else is an overlay)
- rect[3].left = rect[1].width;
- rect[3].width = mjMAX(0, rect[0].width - rect[1].width - rect[2].width);
- rect[3].bottom = 0;
- rect[3].height = rect[0].height;
- }
- // modify UI
- void UiModify(mjUI* ui, mjuiState* state, mjrContext* con) {
- mjui_resize(ui, con);
- // remake aux buffer only if missing or different
- int id = ui->auxid;
- if (con->auxFBO[id] == 0 ||
- con->auxFBO_r[id] == 0 ||
- con->auxColor[id] == 0 ||
- con->auxColor_r[id] == 0 ||
- con->auxWidth[id] != ui->width ||
- con->auxHeight[id] != ui->maxheight ||
- con->auxSamples[id] != ui->spacing.samples) {
- mjr_addAux(id, ui->width, ui->maxheight, ui->spacing.samples, con);
- }
- UiLayout(state);
- mjui_update(-1, -1, ui, state, con);
- }
- // handle UI event
- void UiEvent(mjuiState* state) {
- mj::Simulate* sim = static_cast<mj::Simulate*>(state->userdata);
- // call UI 0 if event is directed to it
- if ((state->dragrect==sim->ui0.rectid) ||
- (state->dragrect==0 && state->mouserect==sim->ui0.rectid) ||
- state->type==mjEVENT_KEY) {
- // process UI event
- mjuiItem* it = mjui_event(&sim->ui0, state, &sim->platform_ui->mjr_context());
- // file section
- if (it && it->sectionid==SECT_FILE) {
- switch (it->itemid) {
- case 0: // Save xml
- sim->pending_.save_xml = GetSavePath("mjmodel.xml");
- break;
- case 1: // Save mjb
- sim->pending_.save_mjb = GetSavePath("mjmodel.mjb");
- break;
- case 2: // Print model
- sim->pending_.print_model = GetSavePath("MJMODEL.TXT");
- break;
- case 3: // Print data
- sim->pending_.print_data = GetSavePath("MJDATA.TXT");
- break;
- case 4: // Quit
- sim->exitrequest.store(1);
- break;
- case 5: // Screenshot
- sim->screenshotrequest.store(true);
- break;
- }
- }
- // option section
- else if (it && it->sectionid==SECT_OPTION) {
- if (it->pdata == &sim->spacing) {
- sim->ui0.spacing = mjui_themeSpacing(sim->spacing);
- sim->ui1.spacing = mjui_themeSpacing(sim->spacing);
- } else if (it->pdata == &sim->color) {
- sim->ui0.color = mjui_themeColor(sim->color);
- sim->ui1.color = mjui_themeColor(sim->color);
- } else if (it->pdata == &sim->font) {
- mjr_changeFont(50*(sim->font+1), &sim->platform_ui->mjr_context());
- } else if (it->pdata == &sim->fullscreen) {
- sim->platform_ui->ToggleFullscreen();
- } else if (it->pdata == &sim->vsync) {
- sim->platform_ui->SetVSync(sim->vsync);
- }
- // modify UI
- UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context());
- UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
- }
- // simulation section
- else if (it && it->sectionid==SECT_SIMULATION) {
- switch (it->itemid) {
- case 1: // Reset
- sim->pending_.reset = true;
- break;
- case 2: // Reload
- sim->uiloadrequest.fetch_add(1);
- break;
- case 3: // Align
- sim->pending_.align = true;
- break;
- case 4: // Copy pose
- sim->pending_.copy_pose = true;
- break;
- case 5: // Adjust key
- case 6: // Load key
- sim->pending_.load_key = true;
- break;
- case 7: // Save key
- sim->pending_.save_key = true;
- break;
- case 11: // History scrubber
- sim->run = 0;
- sim->pending_.load_from_history = true;
- mjui0_update_section(sim, SECT_SIMULATION);
- break;
- }
- }
- // physics section
- else if (it && it->sectionid==SECT_PHYSICS && sim->m_) {
- mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt;
- // update disable flags in mjOption
- opt->disableflags = 0;
- for (int i=0; i<mjNDISABLE; i++) {
- if (sim->disable[i]) {
- opt->disableflags |= (1<<i);
- }
- }
- // update enable flags in mjOption
- opt->enableflags = 0;
- for (int i=0; i<mjNENABLE; i++) {
- if (sim->enable[i]) {
- opt->enableflags |= (1<<i);
- }
- }
- // update disableactuator bitflag in mjOption
- bool group_changed = false;
- for (int i=0; i<mjNGROUP; i++) {
- if ((!sim->enableactuator[i]) != (opt->disableactuator & (1<<i))) {
- group_changed = true;
- if (!sim->enableactuator[i]) {
- // disable actuator group i
- opt->disableactuator |= (1<<i);
- } else {
- // enable actuator group i
- opt->disableactuator &= ~(1<<i);
- }
- }
- }
- // remake control section if actuator disable group changed
- if (group_changed) {
- sim->pending_.ui_remake_ctrl = true;
- }
- }
- // rendering section
- else if (it && it->sectionid==SECT_RENDERING) {
- // set camera in mjvCamera
- if (sim->camera==0) {
- sim->cam.type = mjCAMERA_FREE;
- } else if (sim->camera==1) {
- if (sim->pert.select>0) {
- sim->cam.type = mjCAMERA_TRACKING;
- sim->cam.trackbodyid = sim->pert.select;
- sim->cam.fixedcamid = -1;
- } else {
- sim->cam.type = mjCAMERA_FREE;
- sim->camera = 0;
- mjui0_update_section(sim, SECT_RENDERING);
- }
- } else {
- sim->cam.type = mjCAMERA_FIXED;
- sim->cam.fixedcamid = sim->camera - 2;
- }
- // copy camera spec to clipboard (as MJCF element)
- if (it->itemid == 3) {
- CopyCamera(sim);
- }
- }
- // visualization section
- else if (it && it->sectionid==SECT_VISUALIZATION) {
- if (!mju::strcmp_arr(it->name, "Align")) {
- sim->pending_.align = true;
- }
- }
- // group section
- else if (it && it->sectionid==SECT_GROUP) {
- // remake joint section if joint group changed
- if (it->name[0]=='J' && it->name[1]=='o') {
- sim->ui1.nsect = SECT_JOINT;
- MakeJointSection(sim);
- sim->ui1.nsect = NSECT1;
- UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
- }
- // remake control section if actuator group changed
- if (it->name[0]=='A' && it->name[1]=='c') {
- sim->pending_.ui_remake_ctrl = true;
- }
- }
- // stop if UI processed event
- if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) {
- return;
- }
- }
- // call UI 1 if event is directed to it
- if ((state->dragrect==sim->ui1.rectid) ||
- (state->dragrect==0 && state->mouserect==sim->ui1.rectid) ||
- state->type==mjEVENT_KEY) {
- // process UI event
- mjuiItem* it = mjui_event(&sim->ui1, state, &sim->platform_ui->mjr_context());
- // control section
- if (it && it->sectionid==SECT_CONTROL) {
- // clear controls
- if (it->itemid==0) {
- sim->pending_.zero_ctrl = true;
- }
- }
- // stop if UI processed event
- if (it!=nullptr || (state->type==mjEVENT_KEY && state->key==0)) {
- return;
- }
- }
- // shortcut not handled by UI
- if (state->type==mjEVENT_KEY && state->key!=0) {
- switch (state->key) {
- case ' ': // Mode
- if (!sim->is_passive_ && sim->m_) {
- sim->run = 1 - sim->run;
- sim->pert.active = 0;
- if (sim->run) sim->scrub_index = 0; // reset scrubber
- mjui0_update_section(sim, -1);
- }
- break;
- case mjKEY_RIGHT: // step forward
- if (!sim->is_passive_ && sim->m_ && !sim->run) {
- ClearTimers(sim->d_);
- // currently in scrubber: increment scrub, load state, update slider UI
- if (sim->scrub_index < 0) {
- sim->scrub_index++;
- sim->pending_.load_from_history = true;
- mjui0_update_section(sim, SECT_SIMULATION);
- }
- // not in scrubber: step, add to history buffer
- else {
- mj_step(sim->m_, sim->d_);
- sim->AddToHistory();
- }
- UpdateProfiler(sim, sim->m_, sim->d_);
- UpdateSensor(sim, sim->m_, sim->d_);
- UpdateSettings(sim, sim->m_);
- }
- break;
- case mjKEY_LEFT: // step backward
- if (!sim->is_passive_ && sim->m_) {
- sim->run = 0;
- ClearTimers(sim->d_);
- // decrement scrub, load state
- sim->scrub_index = mjMAX(sim->scrub_index - 1, 1 - sim->nhistory_);
- sim->pending_.load_from_history = true;
- // update slider UI, profiler, sensor
- mjui0_update_section(sim, SECT_SIMULATION);
- UpdateProfiler(sim, sim->m_, sim->d_);
- UpdateSensor(sim, sim->m_, sim->d_);
- }
- break;
- case mjKEY_PAGE_UP: // select parent body
- if ((sim->m_ || sim->is_passive_) && sim->pert.select > 0) {
- sim->pert.select = sim->body_parentid_[sim->pert.select];
- sim->pert.flexselect = -1;
- sim->pert.skinselect = -1;
- // stop perturbation if world reached
- if (sim->pert.select<=0) {
- sim->pert.active = 0;
- }
- }
- break;
- case ']': // cycle up fixed cameras
- if ((sim->m_ || !sim->is_passive_) && sim->ncam_) {
- sim->cam.type = mjCAMERA_FIXED;
- // camera = {0 or 1} are reserved for the free and tracking cameras
- if (sim->camera < 2 || sim->camera == 2 + sim->ncam_ - 1) {
- sim->camera = 2;
- } else {
- sim->camera += 1;
- }
- sim->cam.fixedcamid = sim->camera - 2;
- mjui0_update_section(sim, SECT_RENDERING);
- }
- break;
- case '[': // cycle down fixed cameras
- if ((sim->m_ || sim->is_passive_) && sim->ncam_) {
- sim->cam.type = mjCAMERA_FIXED;
- // camera = {0 or 1} are reserved for the free and tracking cameras
- if (sim->camera <= 2) {
- sim->camera = 2 + sim->ncam_-1;
- } else {
- sim->camera -= 1;
- }
- sim->cam.fixedcamid = sim->camera - 2;
- mjui0_update_section(sim, SECT_RENDERING);
- }
- break;
- case mjKEY_F6: // cycle frame visualisation
- if (sim->m_ || sim->is_passive_) {
- sim->opt.frame = (sim->opt.frame + 1) % mjNFRAME;
- mjui0_update_section(sim, SECT_RENDERING);
- }
- break;
- case mjKEY_F7: // cycle label visualisation
- if (sim->m_ || sim->is_passive_) {
- sim->opt.label = (sim->opt.label + 1) % mjNLABEL;
- mjui0_update_section(sim, SECT_RENDERING);
- }
- break;
- case mjKEY_ESCAPE: // free camera
- sim->cam.type = mjCAMERA_FREE;
- sim->camera = 0;
- mjui0_update_section(sim, SECT_RENDERING);
- break;
- case '-': // slow down
- if (!sim->is_passive_) {
- int numclicks = sizeof(sim->percentRealTime) / sizeof(sim->percentRealTime[0]);
- if (sim->real_time_index < numclicks-1 && !state->shift) {
- sim->real_time_index++;
- sim->speed_changed = true;
- }
- }
- break;
- case '=': // speed up
- if (!sim->is_passive_ && sim->real_time_index > 0 && !state->shift) {
- sim->real_time_index--;
- sim->speed_changed = true;
- }
- break;
- case mjKEY_TAB: // toggle left/right UI
- if (!state->shift) {
- // toggle left UI
- sim->ui0_enable = !sim->ui0_enable;
- UiModify(&sim->ui0, state, &sim->platform_ui->mjr_context());
- } else {
- // toggle right UI
- sim->ui1_enable = !sim->ui1_enable;
- UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context());
- }
- break;
- }
- return;
- }
- // 3D scroll
- if (state->type==mjEVENT_SCROLL && state->mouserect==3) {
- // emulate vertical mouse motion = 2% of window height
- if (sim->m_ && !sim->is_passive_) {
- mjv_moveCamera(sim->m_, mjMOUSE_ZOOM, 0, -zoom_increment*state->sy, &sim->scn, &sim->cam);
- } else {
- mjv_moveCameraFromState(
- &sim->scnstate_, mjMOUSE_ZOOM, 0, -zoom_increment*state->sy, &sim->scn, &sim->cam);
- }
- return;
- }
- // 3D press
- if (state->type==mjEVENT_PRESS && state->mouserect==3) {
- // set perturbation
- int newperturb = 0;
- if (state->control && sim->pert.select>0 && (sim->m_ || sim->is_passive_)) {
- // right: translate; left: rotate
- if (state->right) {
- newperturb = mjPERT_TRANSLATE;
- } else if (state->left) {
- newperturb = mjPERT_ROTATE;
- }
- if (newperturb && !sim->pert.active) {
- sim->pending_.newperturb = newperturb;
- }
- }
- // handle double-click
- if (state->doubleclick && (sim->m_ || sim->is_passive_)) {
- sim->pending_.select = true;
- std::memcpy(&sim->pending_.select_state, state, sizeof(sim->pending_.select_state));
- // stop perturbation on select
- sim->pert.active = 0;
- sim->pending_.newperturb = 0;
- }
- return;
- }
- // 3D release
- if (state->type==mjEVENT_RELEASE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
- // stop perturbation
- sim->pert.active = 0;
- sim->pending_.newperturb = 0;
- return;
- }
- // 3D move
- if (state->type==mjEVENT_MOVE && state->dragrect==3 && (sim->m_ || sim->is_passive_)) {
- // determine action based on mouse button
- mjtMouse action;
- if (state->right) {
- action = state->shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
- } else if (state->left) {
- action = state->shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
- } else {
- action = mjMOUSE_ZOOM;
- }
- // move perturb or camera
- mjrRect r = state->rect[3];
- if (sim->pert.active) {
- if (!sim->is_passive_) {
- mjv_movePerturb(
- sim->m_, sim->d_, action, state->dx / r.height, -state->dy / r.height,
- &sim->scn, &sim->pert);
- } else {
- mjv_movePerturbFromState(
- &sim->scnstate_, action, state->dx / r.height, -state->dy / r.height,
- &sim->scn, &sim->pert);
- }
- } else {
- if (!sim->is_passive_) {
- mjv_moveCamera(
- sim->m_, action, state->dx / r.height, -state->dy / r.height,
- &sim->scn, &sim->cam);
- } else {
- mjv_moveCameraFromState(
- &sim->scnstate_, action, state->dx / r.height, -state->dy / r.height,
- &sim->scn, &sim->cam);
- }
- }
- return;
- }
- // Dropped files
- if (state->type == mjEVENT_FILESDROP && state->dropcount > 0 && !sim->is_passive_) {
- while (sim->droploadrequest.load()) {}
- mju::strcpy_arr(sim->dropfilename, state->droppaths[0]);
- sim->droploadrequest.store(true);
- return;
- }
- // Redraw
- if (state->type == mjEVENT_REDRAW) {
- sim->Render();
- return;
- }
- }
- } // namespace
- namespace mujoco {
- namespace mju = ::mujoco::sample_util;
- Simulate::Simulate(std::unique_ptr<PlatformUIAdapter> platform_ui,
- mjvCamera* cam, mjvOption* opt, mjvPerturb* pert,
- bool is_passive)
- : is_passive_(is_passive),
- cam(*cam),
- opt(*opt),
- pert(*pert),
- platform_ui(std::move(platform_ui)),
- uistate(this->platform_ui->state()) {
- mjv_defaultScene(&scn);
- mjv_defaultSceneState(&scnstate_);
- }
- // synchronize model and data
- // operations which require holding the mutex, prevents racing with physics thread
- void Simulate::Sync() {
- MutexLock lock(this->mtx);
- if (!m_) {
- return;
- }
- if (this->exitrequest.load()) {
- return;
- }
- bool update_profiler = this->profiler && (this->pause_update || this->run);
- bool update_sensor = this->sensor && (this->pause_update || this->run);
- for (int i = 0; i < m_->njnt; ++i) {
- std::optional<std::pair<mjtNum, mjtNum>> range;
- if (m_->jnt_limited[i]) {
- range.emplace(m_->jnt_range[2*i], m_->jnt_range[2*i + 1]);
- }
- if (jnt_range_[i] != range) {
- pending_.ui_update_joint = true;
- jnt_range_[i].swap(range);
- }
- }
- for (int i = 0; i < m_->nu; ++i) {
- std::optional<std::pair<mjtNum, mjtNum>> range;
- if (m_->actuator_ctrllimited[i]) {
- range.emplace(m_->actuator_ctrlrange[2*i], m_->actuator_ctrlrange[2*i + 1]);
- }
- if (actuator_ctrlrange_[i] != range) {
- pending_.ui_remake_ctrl = true;
- actuator_ctrlrange_[i].swap(range);
- }
- }
- for (int i = 0; i < m_->nq; ++i) {
- if (qpos_[i] != qpos_prev_[i]) {
- d_->qpos[i] = qpos_[i];
- } else {
- qpos_[i] = d_->qpos[i];
- }
- if (qpos_prev_[i] != qpos_[i]) {
- pending_.ui_update_joint = true;
- qpos_prev_[i] = qpos_[i];
- }
- }
- for (int i = 0; i < m_->nu; ++i) {
- if (ctrl_[i] != ctrl_prev_[i]) {
- d_->ctrl[i] = ctrl_[i];
- } else {
- ctrl_[i] = d_->ctrl[i];
- }
- if (ctrl_prev_[i] != ctrl_[i]) {
- pending_.ui_update_ctrl = true;
- ctrl_prev_[i] = ctrl_[i];
- }
- }
- if (is_passive_) {
- // synchronize m_->opt with changes made via the UI
- #define X(name) \
- if (IsDifferent(scnstate_.model.opt.name, mjopt_prev_.name)) { \
- pending_.ui_update_physics = true; \
- Copy(m_->opt.name, scnstate_.model.opt.name); \
- }
- X(timestep);
- X(apirate);
- X(impratio);
- X(tolerance);
- X(noslip_tolerance);
- X(ccd_tolerance);
- X(gravity);
- X(wind);
- X(magnetic);
- X(density);
- X(viscosity);
- X(o_margin);
- X(o_solref);
- X(o_solimp);
- X(o_friction);
- X(integrator);
- X(cone);
- X(jacobian);
- X(solver);
- X(iterations);
- X(noslip_iterations);
- X(ccd_iterations);
- X(disableflags);
- X(enableflags);
- X(disableactuator);
- X(sdf_initpoints);
- X(sdf_iterations);
- #undef X
- // synchronize number of mjWARN_VGEOMFULL warnings
- if (scnstate_.data.warning[mjWARN_VGEOMFULL].number > warn_vgeomfull_prev_) {
- d_->warning[mjWARN_VGEOMFULL].number +=
- scnstate_.data.warning[mjWARN_VGEOMFULL].number - warn_vgeomfull_prev_;
- }
- }
- if (pending_.save_xml) {
- char err[200];
- if (!pending_.save_xml->empty() && !mj_saveLastXML(pending_.save_xml->c_str(), m_, err, 200)) {
- std::printf("Save XML error: %s", err);
- }
- pending_.save_xml = std::nullopt;
- }
- if (pending_.save_mjb) {
- if (!pending_.save_mjb->empty()) {
- mj_saveModel(m_, pending_.save_mjb->c_str(), nullptr, 0);
- }
- pending_.save_mjb = std::nullopt;
- }
- if (pending_.print_model) {
- if (!pending_.print_model->empty()) {
- mj_printModel(m_, pending_.print_model->c_str());
- }
- pending_.print_model = std::nullopt;
- }
- if (pending_.print_data) {
- if (!pending_.print_data->empty()) {
- mj_printData(m_, d_, pending_.print_data->c_str());
- }
- pending_.print_data = std::nullopt;
- }
- if (pending_.reset) {
- mj_resetData(m_, d_);
- mj_forward(m_, d_);
- load_error[0] = '\0';
- update_profiler = true;
- update_sensor = true;
- scrub_index = 0;
- pending_.ui_update_simulation = true;
- pending_.reset = false;
- }
- if (pending_.align) {
- AlignAndScaleView(this, m_);
- pending_.align = false;
- }
- if (pending_.copy_pose) {
- CopyPose(this, m_, d_);
- pending_.copy_pose = false;
- }
- if (pending_.load_from_history) {
- LoadScrubState(this);
- update_profiler = true;
- update_sensor = true;
- pending_.load_from_history = false;
- }
- if (pending_.load_key) {
- mj_resetDataKeyframe(m_, d_, this->key);
- mj_forward(m_, d_);
- update_profiler = true;
- update_sensor = true;
- pending_.load_key = false;
- }
- if (pending_.save_key) {
- mj_setKeyframe(m_, d_, this->key);
- pending_.save_key = false;
- }
- if (pending_.zero_ctrl) {
- mju_zero(d_->ctrl, m_->nu);
- pending_.zero_ctrl = false;
- }
- // perturbation onset: reset reference
- if (pending_.newperturb) {
- mjv_initPerturb(m_, d_, &this->scn, &this->pert);
- this->pert.active = pending_.newperturb;
- pending_.newperturb = 0;
- }
- if (pending_.select) {
- // determine selection mode
- int selmode;
- if (pending_.select_state.button==mjBUTTON_LEFT) {
- selmode = 1;
- } else if (pending_.select_state.control) {
- selmode = 3;
- } else {
- selmode = 2;
- }
- // find geom and 3D click point, get corresponding body
- mjrRect r = pending_.select_state.rect[3];
- mjtNum selpnt[3];
- int selgeom, selflex, selskin;
- int selbody = mjv_select(m_, d_, &this->opt,
- static_cast<mjtNum>(r.width) / r.height,
- (pending_.select_state.x - r.left) / r.width,
- (pending_.select_state.y - r.bottom) / r.height,
- &this->scn, selpnt, &selgeom, &selflex, &selskin);
- // set lookat point, start tracking is requested
- if (selmode==2 || selmode==3) {
- // copy selpnt if anything clicked
- if (selbody>=0) {
- mju_copy3(this->cam.lookat, selpnt);
- }
- // switch to tracking camera if dynamic body clicked
- if (selmode==3 && selbody>0) {
- // mujoco camera
- this->cam.type = mjCAMERA_TRACKING;
- this->cam.trackbodyid = selbody;
- this->cam.fixedcamid = -1;
- // UI camera
- this->camera = 1;
- pending_.ui_update_rendering = true;
- }
- }
- // set body selection
- else {
- if (selbody>=0) {
- // record selection
- this->pert.select = selbody;
- this->pert.flexselect = selflex;
- this->pert.skinselect = selskin;
- // compute localpos
- mjtNum tmp[3];
- mju_sub3(tmp, selpnt, d_->xpos + 3*this->pert.select);
- mju_mulMatTVec(this->pert.localpos, d_->xmat + 9*this->pert.select, tmp, 3, 3);
- } else {
- this->pert.select = 0;
- this->pert.flexselect = -1;
- this->pert.skinselect = -1;
- }
- }
- pending_.select = false;
- }
- // update scene
- if (!is_passive_) {
- mjv_updateScene(m_, d_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
- } else {
- mjv_updateSceneState(m_, d_, &this->opt, &scnstate_);
- // append geoms from user_scn to scnstate_ scratch space
- if (user_scn) {
- int ngeom = user_scn->ngeom;
- int maxgeom = scnstate_.scratch.maxgeom - scnstate_.scratch.ngeom;
- if (ngeom > maxgeom) {
- mj_warning(d_, mjWARN_VGEOMFULL, scnstate_.scratch.maxgeom);
- ngeom = maxgeom;
- }
- if (ngeom > 0) {
- std::memcpy(scnstate_.scratch.geoms + scnstate_.scratch.ngeom,
- user_scn->geoms,
- sizeof(mjvGeom) * ngeom);
- scnstate_.scratch.ngeom += ngeom;
- }
- }
- // pick up rendering flags changed via user_scn
- if (user_scn) {
- for (int i = 0; i < mjNRNDFLAG; ++i) {
- if (user_scn->flags[i] != user_scn_flags_prev_[i]) {
- scn.flags[i] = user_scn->flags[i];
- pending_.ui_update_rendering = true;
- }
- }
- Copy(user_scn->flags, scn.flags);
- Copy(user_scn_flags_prev_, user_scn->flags);
- }
- mjopt_prev_ = scnstate_.model.opt;
- warn_vgeomfull_prev_ = scnstate_.data.warning[mjWARN_VGEOMFULL].number;
- }
- // update settings
- UpdateSettings(this, m_);
- // update watch
- if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) {
- UpdateWatch(this, m_, d_);
- }
- // update info text
- if (this->info) {
- UpdateInfoText(this, m_, d_, this->info_title, this->info_content);
- }
- if (update_profiler) { UpdateProfiler(this, m_, d_); }
- if (update_sensor) { UpdateSensor(this, m_, d_); }
- // clear timers once profiler info has been copied
- ClearTimers(d_);
- if (this->run || this->is_passive_) {
- // clear old perturbations, apply new
- mju_zero(d_->xfrc_applied, 6*m_->nbody);
- mjv_applyPerturbPose(m_, d_, &this->pert, 0); // mocap bodies only
- mjv_applyPerturbForce(m_, d_, &this->pert);
- } else {
- mjv_applyPerturbPose(m_, d_, &this->pert, 1); // mocap and dynamic bodies
- }
- }
- //------------------------- Tell the render thread to load a file and wait -------------------------
- void Simulate::LoadMessage(const char* displayed_filename) {
- mju::strcpy_arr(this->filename, displayed_filename);
- {
- MutexLock lock(mtx);
- this->loadrequest = 3;
- }
- }
- void Simulate::Load(mjModel* m, mjData* d, const char* displayed_filename) {
- this->mnew_ = m;
- this->dnew_ = d;
- mju::strcpy_arr(this->filename, displayed_filename);
- {
- MutexLock lock(mtx);
- this->loadrequest = 2;
- // Wait for the render thread to be done loading
- // so that we know the old model and data's memory can
- // be free'd by the other thread (sometimes python)
- cond_loadrequest.wait(lock, [this]() { return this->loadrequest == 0; });
- }
- }
- void Simulate::LoadMessageClear(void) {
- {
- MutexLock lock(mtx);
- this->loadrequest = 0;
- }
- }
- //------------------------------------- load mjb or xml model --------------------------------------
- void Simulate::LoadOnRenderThread() {
- this->m_ = this->mnew_;
- this->d_ = this->dnew_;
- ncam_ = this->m_->ncam;
- nkey_ = this->m_->nkey;
- body_parentid_.resize(this->m_->nbody);
- std::memcpy(body_parentid_.data(), this->m_->body_parentid,
- sizeof(this->m_->body_parentid[0]) * this->m_->nbody);
- jnt_type_.resize(this->m_->njnt);
- std::memcpy(jnt_type_.data(), this->m_->jnt_type,
- sizeof(this->m_->jnt_type[0]) * this->m_->njnt);
- jnt_group_.resize(this->m_->njnt);
- std::memcpy(jnt_group_.data(), this->m_->jnt_group,
- sizeof(this->m_->jnt_group[0]) * this->m_->njnt);
- jnt_qposadr_.resize(this->m_->njnt);
- std::memcpy(jnt_qposadr_.data(), this->m_->jnt_qposadr,
- sizeof(this->m_->jnt_qposadr[0]) * this->m_->njnt);
- jnt_range_.clear();
- jnt_range_.reserve(this->m_->njnt);
- for (int i = 0; i < this->m_->njnt; ++i) {
- if (this->m_->jnt_limited[i]) {
- jnt_range_.push_back(
- std::make_pair(this->m_->jnt_range[2 * i], this->m_->jnt_range[2 * i + 1]));
- } else {
- jnt_range_.push_back(std::nullopt);
- }
- }
- jnt_names_.clear();
- jnt_names_.reserve(this->m_->njnt);
- for (int i = 0; i < this->m_->njnt; ++i) {
- jnt_names_.emplace_back(this->m_->names + this->m_->name_jntadr[i]);
- }
- actuator_group_.resize(this->m_->nu);
- std::memcpy(actuator_group_.data(), this->m_->actuator_group,
- sizeof(this->m_->actuator_group[0]) * this->m_->nu);
- actuator_ctrlrange_.clear();
- actuator_ctrlrange_.reserve(this->m_->nu);
- for (int i = 0; i < this->m_->nu; ++i) {
- if (this->m_->actuator_ctrllimited[i]) {
- actuator_ctrlrange_.push_back(std::make_pair(
- this->m_->actuator_ctrlrange[2 * i], this->m_->actuator_ctrlrange[2 * i + 1]));
- } else {
- actuator_ctrlrange_.push_back(std::nullopt);
- }
- }
- actuator_names_.clear();
- actuator_names_.reserve(this->m_->nu);
- for (int i = 0; i < this->m_->nu; ++i) {
- actuator_names_.emplace_back(this->m_->names + this->m_->name_actuatoradr[i]);
- }
- qpos_.resize(this->m_->nq);
- std::memcpy(qpos_.data(), this->d_->qpos, sizeof(this->d_->qpos[0]) * this->m_->nq);
- qpos_prev_ = qpos_;
- ctrl_.resize(this->m_->nu);
- std::memcpy(ctrl_.data(), this->d_->ctrl, sizeof(this->d_->ctrl[0]) * this->m_->nu);
- ctrl_prev_ = ctrl_;
- // allocate history buffer: smaller of {2000 states, 100 MB}
- if (!this->is_passive_) {
- constexpr int kMaxHistoryBytes = 1e8;
- // get state size, size of history buffer
- state_size_ = mj_stateSize(this->m_, mjSTATE_INTEGRATION);
- int state_bytes = state_size_ * sizeof(mjtNum);
- int history_length = mjMIN(INT_MAX / state_bytes, 2000);
- int history_bytes = mjMIN(state_bytes * history_length, kMaxHistoryBytes);
- nhistory_ = history_bytes / state_bytes;
- // allocate history buffer, reset cursor and UI slider
- history_.clear();
- history_.resize(nhistory_ * state_size_);
- history_cursor_ = 0;
- scrub_index = 0;
- // fill buffer with initial state
- mj_getState(this->m_, this->d_, history_.data(), mjSTATE_INTEGRATION);
- for (int i = 1; i < nhistory_; ++i) {
- mju_copy(&history_[i * state_size_], history_.data(), state_size_);
- }
- }
- // re-create scene and context
- mjv_makeScene(this->m_, &this->scn, kMaxGeom);
- if (this->is_passive_) {
- mjopt_prev_ = m_->opt;
- opt_prev_ = opt;
- cam_prev_ = cam;
- warn_vgeomfull_prev_ = d_->warning[mjWARN_VGEOMFULL].number;
- mjv_makeSceneState(this->m_, this->d_, &this->scnstate_, kMaxGeom);
- }
- this->platform_ui->RefreshMjrContext(this->m_, 50*(this->font+1));
- UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- if (!this->platform_ui->IsGPUAccelerated()) {
- this->scn.flags[mjRND_SHADOW] = 0;
- this->scn.flags[mjRND_REFLECTION] = 0;
- }
- if (this->user_scn) {
- Copy(this->user_scn->flags, this->scn.flags);
- Copy(this->user_scn_flags_prev_, this->scn.flags);
- }
- // clear perturbation state
- this->pert.active = 0;
- this->pert.select = 0;
- this->pert.flexselect = -1;
- this->pert.skinselect = -1;
- // align and scale view unless reloading the same file
- if (this->filename[0] &&
- mju::strcmp_arr(this->filename, this->previous_filename)) {
- AlignAndScaleView(this, this->m_);
- mju::strcpy_arr(this->previous_filename, this->filename);
- }
- // update scene
- if (!is_passive_) {
- mjv_updateScene(this->m_, this->d_,
- &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
- } else {
- mjv_updateSceneState(this->m_, this->d_, &this->opt, &this->scnstate_);
- }
- // set window title to model name
- if (this->m_->names) {
- char title[200] = "MuJoCo : ";
- mju::strcat_arr(title, this->m_->names);
- platform_ui->SetWindowTitle(title);
- }
- // set keyframe range and divisions
- this->ui0.sect[SECT_SIMULATION].item[5].slider.range[0] = 0;
- this->ui0.sect[SECT_SIMULATION].item[5].slider.range[1] = mjMAX(0, this->m_->nkey - 1);
- this->ui0.sect[SECT_SIMULATION].item[5].slider.divisions = mjMAX(1, this->m_->nkey - 1);
- // set scrubber range and divisions
- this->ui0.sect[SECT_SIMULATION].item[11].slider.range[0] = 1 - nhistory_;
- this->ui0.sect[SECT_SIMULATION].item[11].slider.divisions = nhistory_;
- // rebuild UI sections
- MakeUiSections(this, this->m_, this->d_);
- // full ui update
- UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- UpdateSettings(this, this->m_);
- // clear request
- this->loadrequest = 0;
- cond_loadrequest.notify_all();
- // set real time index
- int numclicks = sizeof(this->percentRealTime) / sizeof(this->percentRealTime[0]);
- float min_error = 1e6;
- float desired = mju_log(100*this->m_->vis.global.realtime);
- for (int click=0; click<numclicks; click++) {
- float error = mju_abs(mju_log(this->percentRealTime[click]) - desired);
- if (error < min_error) {
- min_error = error;
- this->real_time_index = click;
- }
- }
- this->mnew_ = nullptr;
- this->dnew_ = nullptr;
- }
- //------------------------------------------- rendering --------------------------------------------
- // render the ui to the window
- void Simulate::Render() {
- // update rendering context buffer size if required
- if (this->platform_ui->EnsureContextSize()) {
- UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- // get 3D rectangle and reduced for profiler
- mjrRect rect = this->uistate.rect[3];
- mjrRect smallrect = rect;
- if (this->profiler) {
- smallrect.width = rect.width - rect.width/4;
- }
- // no model
- if (!this->is_passive_ && !this->m_) {
- // blank screen
- mjr_rectangle(rect, 0.2f, 0.3f, 0.4f, 1);
- // label
- if (this->loadrequest) {
- mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, "LOADING...", nullptr,
- &this->platform_ui->mjr_context());
- } else {
- char intro_message[Simulate::kMaxFilenameLength];
- mju::sprintf_arr(intro_message,
- "MuJoCo version %s\nDrag-and-drop model file here", mj_versionString());
- mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, intro_message, 0,
- &this->platform_ui->mjr_context());
- }
- // show last loading error
- if (this->load_error[0]) {
- mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0,
- &this->platform_ui->mjr_context());
- }
- // render uis
- if (this->ui0_enable) {
- mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- }
- if (this->ui1_enable) {
- mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- // finalize
- this->platform_ui->SwapBuffers();
- return;
- }
- // update UI sections from last sync
- if (pending_.ui_update_simulation) {
- if (this->ui0_enable && this->ui0.sect[SECT_SIMULATION].state) {
- mjui0_update_section(this, SECT_SIMULATION);
- }
- pending_.ui_update_simulation = false;
- }
- if (this->ui0_enable && this->ui0.sect[SECT_WATCH].state) {
- mjui0_update_section(this, SECT_WATCH);
- }
- if (pending_.ui_update_physics) {
- if (this->ui0_enable && this->ui0.sect[SECT_PHYSICS].state) {
- mjui0_update_section(this, SECT_PHYSICS);
- }
- pending_.ui_update_physics = false;
- }
- if (is_passive_) {
- if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state &&
- (cam_prev_.type != cam.type ||
- cam_prev_.fixedcamid != cam.fixedcamid ||
- cam_prev_.trackbodyid != cam.trackbodyid ||
- opt_prev_.label != opt.label || opt_prev_.frame != opt.frame ||
- IsDifferent(opt_prev_.flags, opt.flags))) {
- pending_.ui_update_rendering = true;
- }
- if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state &&
- (IsDifferent(opt_prev_.geomgroup, opt.geomgroup) ||
- IsDifferent(opt_prev_.sitegroup, opt.sitegroup) ||
- IsDifferent(opt_prev_.jointgroup, opt.jointgroup) ||
- IsDifferent(opt_prev_.tendongroup, opt.tendongroup) ||
- IsDifferent(opt_prev_.actuatorgroup, opt.actuatorgroup) ||
- IsDifferent(opt_prev_.flexgroup, opt.flexgroup) ||
- IsDifferent(opt_prev_.skingroup, opt.skingroup))) {
- mjui0_update_section(this, SECT_GROUP);
- }
- opt_prev_ = opt;
- cam_prev_ = cam;
- }
- if (pending_.ui_update_rendering) {
- if (this->ui0_enable && this->ui0.sect[SECT_RENDERING].state) {
- mjui0_update_section(this, SECT_RENDERING);
- }
- pending_.ui_update_rendering = false;
- }
- if (pending_.ui_update_joint) {
- if (this->ui1_enable && this->ui1.sect[SECT_JOINT].state) {
- mjui_update(SECT_JOINT, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- pending_.ui_update_joint = false;
- }
- if (pending_.ui_remake_ctrl) {
- if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) {
- this->ui1.nsect = SECT_CONTROL;
- MakeControlSection(this);
- this->ui1.nsect = NSECT1;
- UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- pending_.ui_remake_ctrl = false;
- }
- if (pending_.ui_update_ctrl) {
- if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) {
- mjui_update(SECT_CONTROL, -1, &this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- pending_.ui_update_ctrl = false;
- }
- // render scene
- mjr_render(rect, &this->scn, &this->platform_ui->mjr_context());
- // show last loading error
- if (this->load_error[0]) {
- mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->load_error, 0,
- &this->platform_ui->mjr_context());
- }
- // show pause/loading label
- if (!this->run || this->loadrequest) {
- char label[30] = {'\0'};
- if (this->loadrequest) {
- std::snprintf(label, sizeof(label), "LOADING...");
- } else if (this->scrub_index == 0) {
- std::snprintf(label, sizeof(label), "PAUSE");
- } else {
- std::snprintf(label, sizeof(label), "PAUSE (%d)", this->scrub_index);
- }
- mjr_overlay(mjFONT_BIG, mjGRID_TOP, smallrect, label, nullptr,
- &this->platform_ui->mjr_context());
- }
- // get desired and actual percent-of-real-time
- float desiredRealtime = this->percentRealTime[this->real_time_index];
- float actualRealtime = 100 / this->measured_slowdown;
- // if running, check for misalignment of more than 10%
- float realtime_offset = mju_abs(actualRealtime - desiredRealtime);
- bool misaligned = this->run && realtime_offset > 0.1 * desiredRealtime;
- // make realtime overlay label
- char rtlabel[30] = {'\0'};
- if (desiredRealtime != 100.0 || misaligned) {
- // print desired realtime
- int labelsize = std::snprintf(rtlabel, sizeof(rtlabel), "%g%%", desiredRealtime);
- // if misaligned, append to label
- if (misaligned) {
- std::snprintf(rtlabel+labelsize, sizeof(rtlabel)-labelsize, " (%-4.1f%%)", actualRealtime);
- }
- }
- // show real-time overlay
- if (rtlabel[0]) {
- mjr_overlay(mjFONT_BIG, mjGRID_TOPLEFT, smallrect, rtlabel, nullptr,
- &this->platform_ui->mjr_context());
- }
- // show ui 0
- if (this->ui0_enable) {
- mjui_render(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- }
- // show ui 1
- if (this->ui1_enable) {
- mjui_render(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- }
- // show help
- if (this->help) {
- mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, rect, help_title, help_content,
- &this->platform_ui->mjr_context());
- }
- // show info
- if (this->info) {
- mjr_overlay(mjFONT_NORMAL, mjGRID_BOTTOMLEFT, rect, this->info_title, this->info_content,
- &this->platform_ui->mjr_context());
- }
- // show profiler
- if (this->profiler) {
- ShowProfiler(this, rect);
- }
- // show sensor
- if (this->sensor) {
- ShowSensor(this, smallrect);
- }
- // take screenshot, save to file
- if (this->screenshotrequest.exchange(false)) {
- const unsigned int h = uistate.rect[0].height;
- const unsigned int w = uistate.rect[0].width;
- std::unique_ptr<unsigned char[]> rgb(new unsigned char[3*w*h]);
- if (!rgb) {
- mju_error("could not allocate buffer for screenshot");
- }
- mjr_readPixels(rgb.get(), nullptr, uistate.rect[0], &this->platform_ui->mjr_context());
- // flip up-down
- for (int r = 0; r < h/2; ++r) {
- unsigned char* top_row = &rgb[3*w*r];
- unsigned char* bottom_row = &rgb[3*w*(h-1-r)];
- std::swap_ranges(top_row, top_row+3*w, bottom_row);
- }
- // save as PNG
- // TODO(b/241577466): Parse the stem of the filename and use a .PNG extension.
- // Unfortunately, if we just yank ".xml"/".mjb" from the filename and append .PNG, the macOS
- // file dialog does not automatically open that location. Thus, we defer to a default
- // "screenshot.png" for now.
- const std::string path = GetSavePath("screenshot.png");
- if (!path.empty()) {
- if (lodepng::encode(path, rgb.get(), w, h, LCT_RGB)) {
- mju_error("could not save screenshot");
- } else {
- std::printf("saved screenshot: %s\n", path.c_str());
- }
- }
- }
- // finalize
- this->platform_ui->SwapBuffers();
- }
- void Simulate::RenderLoop() {
- // Set timer callback (milliseconds)
- mjcb_time = Timer;
- // init abstract visualization
- mjv_defaultCamera(&this->cam);
- mjv_defaultOption(&this->opt);
- InitializeProfiler(this);
- InitializeSensor(this);
- // make empty scene
- if (!is_passive_) {
- mjv_defaultScene(&this->scn);
- mjv_makeScene(nullptr, &this->scn, kMaxGeom);
- }
- if (!this->platform_ui->IsGPUAccelerated()) {
- this->scn.flags[mjRND_SHADOW] = 0;
- this->scn.flags[mjRND_REFLECTION] = 0;
- }
- // select default font
- int fontscale = ComputeFontScale(*this->platform_ui);
- this->font = fontscale/50 - 1;
- // make empty context
- this->platform_ui->RefreshMjrContext(nullptr, fontscale);
- // init state and uis
- std::memset(&this->uistate, 0, sizeof(mjuiState));
- std::memset(&this->ui0, 0, sizeof(mjUI));
- std::memset(&this->ui1, 0, sizeof(mjUI));
- auto [buf_width, buf_height] = this->platform_ui->GetFramebufferSize();
- this->uistate.nrect = 1;
- this->uistate.rect[0].width = buf_width;
- this->uistate.rect[0].height = buf_height;
- this->ui0.spacing = mjui_themeSpacing(this->spacing);
- this->ui0.color = mjui_themeColor(this->color);
- this->ui0.predicate = UiPredicate;
- this->ui0.rectid = 1;
- this->ui0.auxid = 0;
- this->ui1.spacing = mjui_themeSpacing(this->spacing);
- this->ui1.color = mjui_themeColor(this->color);
- this->ui1.predicate = UiPredicate;
- this->ui1.rectid = 2;
- this->ui1.auxid = 1;
- // set GUI adapter callbacks
- this->uistate.userdata = this;
- this->platform_ui->SetEventCallback(UiEvent);
- this->platform_ui->SetLayoutCallback(UiLayout);
- // populate uis with standard sections, open some sections initially
- this->ui0.userdata = this;
- this->ui1.userdata = this;
- mjui_add(&this->ui0, defFile);
- mjui_add(&this->ui0, this->def_option);
- mjui_add(&this->ui0, this->def_simulation);
- this->ui0.sect[0].state = 1;
- this->ui0.sect[1].state = 1;
- this->ui0.sect[2].state = 1;
- mjui_add(&this->ui0, this->def_watch);
- UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context());
- UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context());
- // set VSync to initial value
- this->platform_ui->SetVSync(this->vsync);
- frames_ = 0;
- last_fps_update_ = mj::Simulate::Clock::now();
- // run event loop
- while (!this->platform_ui->ShouldCloseWindow() && !this->exitrequest.load()) {
- {
- const MutexLock lock(this->mtx);
- // load model (not on first pass, to show "loading" label)
- if (this->loadrequest==1) {
- this->LoadOnRenderThread();
- } else if (this->loadrequest == 2) {
- this->loadrequest = 1;
- }
- // poll and handle events
- this->platform_ui->PollEvents();
- // upload assets if requested
- bool upload_notify = false;
- if (hfield_upload_ != -1) {
- mjr_uploadHField(m_, &platform_ui->mjr_context(), hfield_upload_);
- hfield_upload_ = -1;
- upload_notify = true;
- }
- if (mesh_upload_ != -1) {
- mjr_uploadMesh(m_, &platform_ui->mjr_context(), mesh_upload_);
- mesh_upload_ = -1;
- upload_notify = true;
- }
- if (texture_upload_ != -1) {
- mjr_uploadTexture(m_, &platform_ui->mjr_context(), texture_upload_);
- texture_upload_ = -1;
- upload_notify = true;
- }
- if (upload_notify) {
- cond_upload_.notify_all();
- }
- // update scene, doing a full sync if in fully managed mode
- if (!this->is_passive_) {
- Sync();
- } else {
- scnstate_.data.warning[mjWARN_VGEOMFULL].number += mjv_updateSceneFromState(
- &scnstate_, &this->opt, &this->pert, &this->cam, mjCAT_ALL, &this->scn);
- }
- } // MutexLock (unblocks simulation thread)
- // render while simulation is running
- this->Render();
- // update FPS stat, at most 5 times per second
- auto now = mj::Simulate::Clock::now();
- double interval = Seconds(now - last_fps_update_).count();
- ++frames_;
- if (interval > 0.2) {
- last_fps_update_ = now;
- fps_ = frames_ / interval;
- frames_ = 0;
- }
- }
- const MutexLock lock(this->mtx);
- mjv_freeScene(&this->scn);
- if (is_passive_) {
- mjv_freeSceneState(&scnstate_);
- }
- this->exitrequest.store(2);
- }
- // add state to history buffer
- void Simulate::AddToHistory() {
- if (history_.empty()) {
- return;
- }
- // circular increment of cursor
- history_cursor_ = (history_cursor_ + 1) % nhistory_;
- // add state at cursor
- mjtNum* state = &history_[state_size_ * history_cursor_];
- mj_getState(m_, d_, state, mjSTATE_INTEGRATION);
- }
- // inject Brownian noise
- void Simulate::InjectNoise() {
- // no noise, return
- if (ctrl_noise_std <= 0) {
- return;
- }
- // convert rate and scale to discrete time (Ornstein–Uhlenbeck)
- mjtNum rate = mju_exp(-m_->opt.timestep / ctrl_noise_rate);
- mjtNum scale = ctrl_noise_std * mju_sqrt(1-rate*rate);
- for (int i=0; i<m_->nu; i++) {
- mjtNum bottom = 0, top = 0, midpoint = 0, halfrange = 1;
- if (m_->actuator_ctrllimited[i]) {
- bottom = m_->actuator_ctrlrange[2*i];
- top = m_->actuator_ctrlrange[2*i+1];
- midpoint = 0.5 * (top + bottom); // target of exponential decay
- halfrange = 0.5 * (top - bottom); // scales noise
- }
- // exponential convergence to midpoint at ctrl_noise_rate
- d_->ctrl[i] = rate * d_->ctrl[i] + (1-rate) * midpoint;
- // add noise
- d_->ctrl[i] += scale * halfrange * mju_standardNormal(nullptr);
- // clip to range if limited
- if (m_->actuator_ctrllimited[i]) {
- d_->ctrl[i] = mju_clip(d_->ctrl[i], bottom, top);
- }
- }
- }
- void Simulate::UpdateHField(int hfieldid) {
- MutexLock lock(this->mtx);
- if (!m_ || hfieldid < 0 || hfieldid >= m_->nhfield) {
- return;
- }
- hfield_upload_ = hfieldid;
- cond_upload_.wait(lock, [this]() { return hfield_upload_ == -1; });
- }
- void Simulate::UpdateMesh(int meshid) {
- MutexLock lock(this->mtx);
- if (!m_ || meshid < 0 || meshid >= m_->nmesh) {
- return;
- }
- mesh_upload_ = meshid;
- cond_upload_.wait(lock, [this]() { return mesh_upload_ == -1; });
- }
- void Simulate::UpdateTexture(int texid) {
- MutexLock lock(this->mtx);
- if (!m_ || texid < 0 || texid >= m_->ntex) {
- return;
- }
- texture_upload_ = texid;
- cond_upload_.wait(lock, [this]() { return texture_upload_ == -1; });
- }
- } // namespace mujoco
|