Skip to content

Commit 23ca511

Browse files
madhephaestusmadhephaestus
authored
Merge pull request #2 from CommonWealthRobotics/kh/AddManagerClassAndExamples
adding to the manager class
2 parents 8f8afc8 + cdb4ebb commit 23ca511

5 files changed

Lines changed: 145 additions & 54 deletions

File tree

‎.github/workflows/release.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ on:
77

88
jobs:
99
linux:
10-
runs-on: ubuntu-latest
10+
runs-on: ubuntu-20.04
1111
steps:
1212
- name: Checkout
1313
uses: actions/checkout@v1

‎build-linux.sh‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,8 @@ echo "JavaCPP configs:"
4545
java -jar $SCRIPT_DIR/javacpp-platform-$JAVACPP_VER-bin/javacpp.jar -Dcompiler.includepath=$BUILDDIR/include/ -print properties.includepath
4646

4747
java -jar $SCRIPT_DIR/javacpp-platform-$JAVACPP_VER-bin/javacpp.jar org/mujoco/MuJoCoConfig.java
48-
java -jar $SCRIPT_DIR/javacpp-platform-$JAVACPP_VER-bin/javacpp.jar -copylibs -copyresources -Xcompiler "-I$JAVADIR" -Xcompiler "-L$JAVADIR" org/mujoco/MuJoCoLib.java
48+
echo "Start compile \n\n"
49+
java -jar $SCRIPT_DIR/javacpp-platform-$JAVACPP_VER-bin/javacpp.jar -copylibs -copyresources -Xcompiler "-no-pie" -Xcompiler "-I$JAVADIR" -Xcompiler "-L$JAVADIR" org/mujoco/MuJoCoLib.java
4950
LIBPATH=$PWD/../resources/$TYPE/
5051
mkdir -p $SCRIPT_DIR/src/main/resources/
5152

‎src/main/java/org/mujoco/IMujocoController.java‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
package org.mujoco;
2+
3+
import org.mujoco.MuJoCoLib.mjData_;
4+
import org.mujoco.MuJoCoLib.mjModel_;
5+
6+
public interface IMujocoController {
7+
public void controlStep(mjData_ data,mjModel_ model);
8+
}

‎src/main/java/org/mujoco/MuJoCoModelManager.java‎

Lines changed: 91 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,20 +7,29 @@
77
import java.net.URL;
88

99
import org.bytedeco.javacpp.BytePointer;
10+
import org.bytedeco.javacpp.IntPointer;
1011
import org.mujoco.MuJoCoLib.mjData;
1112
import org.mujoco.MuJoCoLib.mjData_;
1213
import org.mujoco.MuJoCoLib.mjModel;
1314
import org.mujoco.MuJoCoLib.mjModel_;
15+
import org.mujoco.MuJoCoLib.mjOption_;
1416
import org.mujoco.MuJoCoLib.mjVFS;
1517

1618
public 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
}

‎src/test/java/mujoco/java/MuJoColibTest.java‎

Lines changed: 43 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -8,80 +8,77 @@
88
import java.io.File;
99

1010
import org.bytedeco.javacpp.BytePointer;
11+
import org.bytedeco.javacpp.IntPointer;
12+
import org.bytedeco.javacpp.Pointer;
1113
import org.junit.Test;
14+
import org.mujoco.IMujocoController;
1215
import org.mujoco.MuJoCoLib;
1316
import org.mujoco.MuJoCoLib.mjData;
1417
import org.mujoco.MuJoCoLib.mjData_;
1518
import org.mujoco.MuJoCoLib.mjModel;
1619
import org.mujoco.MuJoCoLib.mjModel_;
20+
import org.mujoco.MuJoCoLib.mjOption_;
1721
import org.mujoco.MuJoCoLib.mjVFS;
1822
import org.mujoco.MuJoCoModelManager;
1923

2024
public class MuJoColibTest {
25+
IMujocoController controller = (d, m) -> {
26+
/**
27+
* This illustrates two concepts. First, we are checking
28+
* if the number of controls mjModel.nu equals the number
29+
* of DoFs mjModel.nv. In general, the same callback may
30+
* be used with multiple models depending on how the user
31+
* code is structured, and so it is a good idea to check
32+
* the model dimensions in the callback. Second, MuJoCo
33+
* has a library of BLAS-like functions that are very
34+
* useful; indeed a large part of the code base consists
35+
* of calling such functions internally. The mju_scl
36+
* function above scales the velocity vector mjData.qvel
37+
* by a constant feedback gain and copies the result into
38+
* the control vector mjData.ctrl.
39+
*/
40+
// apply controls
41+
// https://mujoco.readthedocs.io/en/stable/programming/simulation.html#simulation-loop
42+
if (m.nu() == m.nv())
43+
MuJoCoLib.mju_scl(d.ctrl(), d.qvel(), -0.1, m.nv());
44+
};
45+
2146
@Test
2247
public void managerTest() throws InterruptedException {
48+
System.out.println("managerTest");
2349
String filename = "model/humanoid/humanoid.xml";
2450
File file = new File(filename);
25-
if(!file.exists()) {
51+
if (!file.exists()) {
2652
fail("File is missing from the disk");
2753
}
2854
MuJoCoModelManager m = new MuJoCoModelManager(file);
29-
mjModel_ model = m.getModel();
30-
mjData_ data = m.getData();
31-
System.out.println("Run model for 10 seconds");
32-
while (data.time() < 10) {
33-
m.stepOne();
34-
//apply controls
35-
m.stepTwo();
55+
System.out.println("Run ModelManager for 10 seconds");
56+
57+
58+
for (int i = 0; i < m.getNumberOfJoints(); i++) {
59+
System.out.println(i + " link = " + m.getJointName(i));
60+
}
61+
for (int i = 0; i < m.getNumberOfBodys(); i++) {
62+
System.out.println(i + " Body = " + m.getBodyName(i));
63+
}
64+
65+
m.setController(controller);
66+
while (m.getCurrentSimulationTimeSeconds() < 10) {
67+
m.step();
3668
// sleep
37-
Thread.sleep(1);
69+
Thread.sleep(m.getTimestepMilliSeconds());
3870
}
3971
m.close();
4072
}
73+
4174
@Test
4275
public void mujocoJNILoadTest() {
76+
System.out.println("mujocoJNILoadTest");
4377
System.out.println(System.getProperty("org.bytedeco.javacpp.logger.debug"));
4478
System.setProperty("org.bytedeco.javacpp.logger.debug", "true");
4579
MuJoCoLib lib = new MuJoCoLib();
4680

4781
System.out.println("Starting " + MuJoCoLib.mj_versionString().getString());
48-
int error_sz = 1000;
49-
BytePointer error = new BytePointer(error_sz);
50-
String filename = "model/humanoid/humanoid.xml";
51-
File file = new File(filename);
52-
if(!file.exists()) {
53-
fail("File is missing from the disk");
54-
}
55-
filename = file.getAbsolutePath();
56-
mjModel m = MuJoCoLib.mj_loadXML(
57-
filename, null, error,
58-
error_sz);
59-
60-
if(m==null) {
61-
String message = "Model failed to load from "+filename+"\n"+error.getString();
62-
System.err.println(message);
63-
fail(message);
64-
}
65-
System.out.println("Humanoid model loaded " + filename);
66-
mjData d = MuJoCoLib.mj_makeData(m);
67-
try {
68-
mjModel_ Maccessable = new mjModel_(m);
69-
try (mjData_ accessable = new mjData_(d)) {
70-
System.out.println("Run model for 10 seconds");
71-
while (accessable.time() < 10) {
72-
MuJoCoLib.mj_step(m, d);
73-
Thread.sleep(1);
74-
75-
}
76-
77-
}
78-
} catch (Exception e) {
79-
// TODO Auto-generated catch block
80-
e.printStackTrace();
81-
}
82-
System.out.println("Clean up data objects");
8382

84-
MuJoCoLib.mj_deleteData(d);
85-
MuJoCoLib.mj_deleteModel(m);
8683
}
8784
}

0 commit comments

Comments
 (0)