77import java .net .URL ;
88
99import org .bytedeco .javacpp .BytePointer ;
10+ import org .bytedeco .javacpp .IntPointer ;
1011import org .mujoco .MuJoCoLib .mjData ;
1112import org .mujoco .MuJoCoLib .mjData_ ;
1213import org .mujoco .MuJoCoLib .mjModel ;
1314import org .mujoco .MuJoCoLib .mjModel_ ;
15+ import org .mujoco .MuJoCoLib .mjOption_ ;
1416import org .mujoco .MuJoCoLib .mjVFS ;
1517
1618public class MuJoCoModelManager {
1719 MuJoCoLib lib = new MuJoCoLib ();
1820
1921 private mjModel m ;
2022 private mjData d ;
21- private mjModel_ maccessable ;
22- private mjData_ daccessable ;
23+ private mjModel_ model ;
24+ private mjData_ data ;
25+ private mjOption_ opt ;
26+ private IMujocoController controller = null ;
2327
28+ private BytePointer modelNames ;
29+
30+ private IntPointer jointNameIndexes ;
31+
32+ private IntPointer bodyNameIndex ;
2433 public MuJoCoModelManager (File config ){
2534 loadFromFile (config );
2635 }
@@ -44,8 +53,46 @@ private void loadFromFile(File config) {
4453 d = MuJoCoLib .mj_makeData (m );
4554 setModel (new mjModel_ (m ));
4655 setData (new mjData_ (d ));
56+ setOpt (new mjOption_ (getModel ().opt ()));
57+ setModelNames (model .names ());
58+ jointNameIndexes = model .name_jntadr ();
59+ bodyNameIndex = model .name_bodyadr ();
60+ }
61+ public double getCurrentSimulationTimeSeconds () {
62+ return data .time ();
63+ }
64+ public int getNumberOfJoints () {
65+ return model .njnt ();
66+ }
67+ public String getJointName (int i ) {
68+ if (i <0 )
69+ throw new IndexOutOfBoundsException ("Joint index must be positive or zero" );
70+ if (i >=getNumberOfJoints ()) {
71+ throw new IndexOutOfBoundsException ("Joint index must be less than " +i );
72+ }
73+ BytePointer byp = modelNames .getPointer (jointNameIndexes .getPointer (i ).get ());
74+ return byp .getString ();
75+ }
76+ public int getNumberOfBodys () {
77+ return model .nbody ();
78+ }
79+ public String getBodyName (int i ) {
80+ if (i <0 )
81+ throw new IndexOutOfBoundsException ("Body index must be positive or zero" );
82+ if (i >=getNumberOfBodys ()) {
83+ throw new IndexOutOfBoundsException ("Body index must be less than " +i );
84+ }
85+ BytePointer byp = modelNames .getPointer (bodyNameIndex .getPointer (i ).get ());
86+ return byp .getString ();
4787 }
4888
89+
90+ public double getTimestepSeconds () {
91+ return getOpt ().timestep ();
92+ }
93+ public long getTimestepMilliSeconds () {
94+ return (long )(getTimestepSeconds ()*1000 );
95+ }
4996 public void close () {
5097 MuJoCoLib .mj_deleteData (d );
5198 MuJoCoLib .mj_deleteModel (m );
@@ -55,31 +102,33 @@ public void close() {
55102 * @return the maccessable
56103 */
57104 public mjModel_ getModel () {
58- return maccessable ;
105+ return model ;
59106 }
60107
61108 /**
62109 * @param maccessable the maccessable to set
63110 */
64111 private void setModel (mjModel_ maccessable ) {
65- this .maccessable = maccessable ;
112+ this .model = maccessable ;
66113 }
67114
68115 /**
69116 * @return the daccessable
70117 */
71118 public mjData_ getData () {
72- return daccessable ;
119+ return data ;
73120 }
74121
75122 /**
76123 * @param daccessable the daccessable to set
77124 */
78125 public void setData (mjData_ daccessable ) {
79- this .daccessable = daccessable ;
126+ this .data = daccessable ;
80127 }
81128 public void step () {
82129 stepOne ();
130+ if (controller !=null )
131+ controller .controlStep (data , model );
83132 stepTwo ();
84133 }
85134 public void stepOne () {
@@ -88,4 +137,40 @@ public void stepOne() {
88137 public void stepTwo () {
89138 MuJoCoLib .mj_step2 (m , d );
90139 }
140+ /**
141+ * @return the opt
142+ */
143+ public mjOption_ getOpt () {
144+ return opt ;
145+ }
146+ /**
147+ * @param opt the opt to set
148+ */
149+ public void setOpt (mjOption_ opt ) {
150+ this .opt = opt ;
151+ }
152+ /**
153+ * @return the controller
154+ */
155+ public IMujocoController getController () {
156+ return controller ;
157+ }
158+ /**
159+ * @param controller the controller to set
160+ */
161+ public void setController (IMujocoController controller ) {
162+ this .controller = controller ;
163+ }
164+ /**
165+ * @return the modelNames
166+ */
167+ public BytePointer getModelNames () {
168+ return modelNames ;
169+ }
170+ /**
171+ * @param modelNames the modelNames to set
172+ */
173+ public void setModelNames (BytePointer modelNames ) {
174+ this .modelNames = modelNames ;
175+ }
91176}
0 commit comments