Skip to content

Commit 6e646d8

Browse files
madhephaestusmadhephaestus
committed
update test to go over API
1 parent 03d1b86 commit 6e646d8

File tree

3 files changed

+134
-51
lines changed

3 files changed

+134
-51
lines changed

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,5 @@
44
import org.mujoco.MuJoCoLib.mjModel_;
55

66
public interface IMujocoController {
7-
public void controlStep(mjData_ data,mjModel_ model);
7+
public void controlStep(MuJoCoModelManager manager);
88
}

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

Lines changed: 99 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,7 @@ public double[] getGeometryPose(String name) {
149149
*/
150150
public double[] getGeometrySize(String name) {
151151
DoublePointer geomSize = model.geom_size();
152-
DoublePointer coords = geomSize.getPointer(getGeometryIndex(null) * 3);
152+
DoublePointer coords = geomSize.getPointer(getGeometryIndex(name) * 3);
153153
double x = coords.getPointer(0).get();
154154
double y = coords.getPointer(1).get();
155155
double z = coords.getPointer(2).get();
@@ -183,20 +183,14 @@ public void setActualtorCtrl(HashMap<String, Double> control) {
183183
private HashMap<String, Integer> ActuatorNameIndexMap = null;
184184

185185
public Set<String> getActuatorNames() {
186-
getActuatorIndex("");
187-
return ActuatorNameIndexMap.keySet();
186+
return getActuatorNameIndexMap().keySet();
188187
}
189188

190189
public int getActuatorIndex(String name) {
191190
check();
192191

193-
if (ActuatorNameIndexMap == null) {
194-
ActuatorNameIndexMap = new HashMap<>();
195-
for (int i = 0; i < getNumberOfActuators(); i++) {
196-
ActuatorNameIndexMap.put(getActuatorName(i), i);
197-
}
198-
}
199-
Integer i = ActuatorNameIndexMap.get(name);
192+
193+
Integer i = getActuatorNameIndexMap().get(name);
200194
if (i != null)
201195
return i;
202196
throw new RuntimeException("Actuator named " + name + " not found");
@@ -219,20 +213,14 @@ public String getActuatorName(int i) {
219213
}
220214

221215
public Set<String> getJointNames() {
222-
getJointIndex("");
223-
return jointNameIndexMap.keySet();
216+
return getJointNameIndexMap().keySet();
224217
}
225218

226219
public int getJointIndex(String name) {
227220
check();
228221

229-
if (jointNameIndexMap == null) {
230-
jointNameIndexMap = new HashMap<>();
231-
for (int i = 0; i < getNumberOfJoints(); i++) {
232-
jointNameIndexMap.put(getJointName(i), i);
233-
}
234-
}
235-
Integer i = jointNameIndexMap.get(name);
222+
223+
Integer i = getJointNameIndexMap().get(name);
236224
if (i != null)
237225
return i;
238226
throw new RuntimeException("Joint named " + name + " not found");
@@ -255,39 +243,28 @@ public String getJointName(int i) {
255243
}
256244

257245
public Set<String> getBodyNames() {
258-
getBodyIndex("");
259-
return bodyNameIndexMap.keySet();
246+
return getBodyNameIndexMap().keySet();
260247
}
261248

262249
public int getBodyIndex(String name) {
263250
check();
264251

265-
if (bodyNameIndexMap == null) {
266-
bodyNameIndexMap = new HashMap<>();
267-
for (int i = 0; i < getNumberOfBodys(); i++) {
268-
bodyNameIndexMap.put(getBodyName(i), i);
269-
}
270-
}
271-
Integer i = bodyNameIndexMap.get(name);
252+
253+
Integer i = getBodyNameIndexMap().get(name);
272254
if (i != null)
273255
return i;
274256
throw new RuntimeException("Body named " + name + " not found");
275257
}
276258

277259
public Set<String> getGeometryNames() {
278-
getGeometryIndex("");
279-
return geometryNameIndexMap.keySet();
260+
261+
return getGeometryNameIndexMap().keySet();
280262
}
281263

282264
public int getGeometryIndex(String name) {
283265
check();
284-
if (geometryNameIndexMap == null) {
285-
geometryNameIndexMap = new HashMap<>();
286-
for (int i = 0; i < getNumberOfGeometrys(); i++) {
287-
geometryNameIndexMap.put(getGeometryName(i), i);
288-
}
289-
}
290-
Integer i = geometryNameIndexMap.get(name);
266+
267+
Integer i = getGeometryNameIndexMap().get(name);
291268
if (i != null)
292269
return i;
293270
throw new RuntimeException("Geometry named " + name + " not found");
@@ -298,13 +275,13 @@ public MuJoCoGeomType getMuJoCoGeomType(String geom) {
298275
return MuJoCoGeomType.get(type);
299276
}
300277

301-
public int getBodyIndexOfGeometry(String geomName) {
278+
public int getBodyIndexForGeometry(String geomName) {
302279
int i = getGeometryIndex(geomName);
303280
return model.geom_bodyid().getPointer(i).get();
304281
}
305282

306-
public String getBodyNameOfGeometry(String geomName) {
307-
return getBodyName(getBodyIndexOfGeometry(geomName));
283+
public String getBodyNameForGeometry(String geomName) {
284+
return getBodyName(getBodyIndexForGeometry(geomName));
308285
}
309286

310287
public double getTimestepSeconds() {
@@ -347,14 +324,14 @@ public mjData_ getData() {
347324
/**
348325
* @param daccessable the daccessable to set
349326
*/
350-
public void setData(mjData_ daccessable) {
327+
private void setData(mjData_ daccessable) {
351328
this.data = daccessable;
352329
}
353330

354331
public void step() {
355332
stepOne();
356333
if (controller != null)
357-
controller.controlStep(data, model);
334+
controller.controlStep(this);
358335
stepTwo();
359336
}
360337

@@ -409,4 +386,84 @@ public BytePointer getModelNames() {
409386
private void setModelNames(BytePointer modelNames) {
410387
this.modelNames = modelNames;
411388
}
389+
390+
/**
391+
* @return the bodyNameIndexMap
392+
*/
393+
public HashMap<String, Integer> getBodyNameIndexMap() {
394+
if (bodyNameIndexMap == null) {
395+
setBodyNameIndexMap(new HashMap<>());
396+
for (int i = 0; i < getNumberOfBodys(); i++) {
397+
bodyNameIndexMap.put(getBodyName(i), i);
398+
}
399+
}
400+
return bodyNameIndexMap;
401+
}
402+
403+
/**
404+
* @param bodyNameIndexMap the bodyNameIndexMap to set
405+
*/
406+
public void setBodyNameIndexMap(HashMap<String, Integer> bodyNameIndexMap) {
407+
this.bodyNameIndexMap = bodyNameIndexMap;
408+
}
409+
410+
/**
411+
* @return the geometryNameIndexMap
412+
*/
413+
public HashMap<String, Integer> getGeometryNameIndexMap() {
414+
if (geometryNameIndexMap == null) {
415+
setGeometryNameIndexMap(new HashMap<>());
416+
for (int i = 0; i < getNumberOfGeometrys(); i++) {
417+
geometryNameIndexMap.put(getGeometryName(i), i);
418+
}
419+
}
420+
return geometryNameIndexMap;
421+
}
422+
423+
/**
424+
* @param geometryNameIndexMap the geometryNameIndexMap to set
425+
*/
426+
public void setGeometryNameIndexMap(HashMap<String, Integer> geometryNameIndexMap) {
427+
this.geometryNameIndexMap = geometryNameIndexMap;
428+
}
429+
430+
/**
431+
* @return the jointNameIndexMap
432+
*/
433+
public HashMap<String, Integer> getJointNameIndexMap() {
434+
if (jointNameIndexMap == null) {
435+
setJointNameIndexMap(new HashMap<>());
436+
for (int i = 0; i < getNumberOfJoints(); i++) {
437+
jointNameIndexMap.put(getJointName(i), i);
438+
}
439+
}
440+
return jointNameIndexMap;
441+
}
442+
443+
/**
444+
* @param jointNameIndexMap the jointNameIndexMap to set
445+
*/
446+
public void setJointNameIndexMap(HashMap<String, Integer> jointNameIndexMap) {
447+
this.jointNameIndexMap = jointNameIndexMap;
448+
}
449+
450+
/**
451+
* @return the actuatorNameIndexMap
452+
*/
453+
public HashMap<String, Integer> getActuatorNameIndexMap() {
454+
if (ActuatorNameIndexMap == null) {
455+
setActuatorNameIndexMap(new HashMap<>());
456+
for (int i = 0; i < getNumberOfActuators(); i++) {
457+
ActuatorNameIndexMap.put(getActuatorName(i), i);
458+
}
459+
}
460+
return ActuatorNameIndexMap;
461+
}
462+
463+
/**
464+
* @param actuatorNameIndexMap the actuatorNameIndexMap to set
465+
*/
466+
public void setActuatorNameIndexMap(HashMap<String, Integer> actuatorNameIndexMap) {
467+
ActuatorNameIndexMap = actuatorNameIndexMap;
468+
}
412469
}

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

Lines changed: 34 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import static org.junit.Assert.fail;
77

88
import java.io.File;
9+
import java.util.HashMap;
910

1011
import org.bytedeco.javacpp.BytePointer;
1112
import org.bytedeco.javacpp.IntPointer;
@@ -22,7 +23,9 @@
2223
import org.mujoco.MuJoCoModelManager;
2324

2425
public class MuJoColibTest {
25-
IMujocoController controller = (d, m) -> {
26+
IMujocoController controller = (manager) -> {
27+
mjModel_ m=manager.getModel();
28+
mjData_ d=manager.getData();
2629
/**
2730
* This illustrates two concepts. First, we are checking
2831
* if the number of controls mjModel.nu equals the number
@@ -39,8 +42,22 @@ public class MuJoColibTest {
3942
*/
4043
// apply controls
4144
// 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());
45+
46+
// if (m.nu() == m.nv())
47+
// MuJoCoLib.mju_scl(d.ctrl(), d.qvel(), -0.1, m.nv());
48+
HashMap<String, Double> setEfforts = new HashMap<String, Double>();
49+
HashMap<String, Double> positions = manager.getAllJointPositions();
50+
// this is a simple P controller
51+
double target =0;
52+
double kp = 0.3;
53+
for(String s:manager.getActuatorNames()) {
54+
double error = target-positions.get(s);
55+
double effort = error * kp;
56+
System.out.println("Actuator "+s+" position "+positions.get(s)+" effort "+effort);
57+
setEfforts.put(s,effort);
58+
}
59+
manager.setActualtorCtrl(setEfforts);
60+
4461
};
4562

4663
@Test
@@ -54,19 +71,28 @@ public void managerTest() throws InterruptedException {
5471
MuJoCoModelManager m = new MuJoCoModelManager(file);
5572
System.out.println("Run ModelManager for 10 seconds");
5673

57-
58-
for (int i = 0; i < m.getNumberOfJoints(); i++) {
59-
System.out.println(i + " link = " + m.getJointName(i));
74+
HashMap<String, Double> positions = m.getAllJointPositions();
75+
for(String s:m.getBodyNames()) {
76+
System.out.println("Body "+s+" pose "+m.getBodyPose(s));
77+
}
78+
for(String s:m.getJointNames()) {
79+
System.out.println("Joint "+s+" position "+positions.get(s));
80+
}
81+
for(String s:m.getActuatorNames()) {
82+
System.out.println("Actuator "+s+" position "+positions.get(s));
6083
}
61-
for (int i = 0; i < m.getNumberOfBodys(); i++) {
62-
System.out.println(i + " Body = " + m.getBodyName(i));
84+
for(String s:m.getGeometryNames()) {
85+
System.out.println("Geom "+s+" pose "+m.getGeometryPose(s)+" size "+m.getGeometrySize(s));
6386
}
6487

6588
m.setController(controller);
6689
while (m.getCurrentSimulationTimeSeconds() < 10) {
6790
m.step();
6891
// sleep
6992
Thread.sleep(m.getTimestepMilliSeconds());
93+
for(String s:m.getBodyNames()) {
94+
System.out.println("Body "+s+" pose "+m.getBodyPose(s));
95+
}
7096
}
7197
m.close();
7298
}

0 commit comments

Comments
 (0)