Skip to content

Commit

Permalink
Add printing of the names of the links
Browse files Browse the repository at this point in the history
  • Loading branch information
hephaestus committed Dec 28, 2023
1 parent 628453a commit e9da6d6
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 56 deletions.
65 changes: 56 additions & 9 deletions src/main/java/org/mujoco/MuJoCoModelManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import java.net.URL;

import org.bytedeco.javacpp.BytePointer;
import org.bytedeco.javacpp.IntPointer;
import org.mujoco.MuJoCoLib.mjData;
import org.mujoco.MuJoCoLib.mjData_;
import org.mujoco.MuJoCoLib.mjModel;
Expand All @@ -19,10 +20,16 @@ public class MuJoCoModelManager {

private mjModel m;
private mjData d;
private mjModel_ maccessable;
private mjData_ daccessable;
private mjModel_ model;
private mjData_ data;
private mjOption_ opt;
private IMujocoController controller = null;

private BytePointer modelNames;

private IntPointer jointNameIndexes;

private IntPointer bodyNameIndex;
public MuJoCoModelManager(File config){
loadFromFile(config);
}
Expand All @@ -47,9 +54,37 @@ private void loadFromFile(File config) {
setModel(new mjModel_(m));
setData(new mjData_(d));
setOpt(new mjOption_(getModel().opt()));

setModelNames(model.names());
jointNameIndexes = model.name_jntadr();
bodyNameIndex = model.name_bodyadr();
}
public double getCurrentSimulationTimeSeconds() {
return data.time();
}
public int getNumberOfJoints() {
return model.nu();
}
public String getJointName(int i) {
if(i<0)
throw new IndexOutOfBoundsException("Joint index must be positive or zero");
if(i>=getNumberOfJoints()) {
throw new IndexOutOfBoundsException("Joint index must be less than "+i);
}
BytePointer byp = modelNames.getPointer(jointNameIndexes.getPointer(i).get());
return byp.getString();
}
public int getNumberOfBodys() {
return model.nbody();
}
public String getBodyName(int i) {
if(i<0)
throw new IndexOutOfBoundsException("Body index must be positive or zero");
if(i>=getNumberOfJoints()) {
throw new IndexOutOfBoundsException("Body index must be less than "+i);
}
BytePointer byp = modelNames.getPointer(bodyNameIndex.getPointer(i).get());
return byp.getString();
}

public double getTimestepSeconds() {
return getOpt().timestep();
}
Expand All @@ -65,33 +100,33 @@ public void close() {
* @return the maccessable
*/
public mjModel_ getModel() {
return maccessable;
return model;
}

/**
* @param maccessable the maccessable to set
*/
private void setModel(mjModel_ maccessable) {
this.maccessable = maccessable;
this.model = maccessable;
}

/**
* @return the daccessable
*/
public mjData_ getData() {
return daccessable;
return data;
}

/**
* @param daccessable the daccessable to set
*/
public void setData(mjData_ daccessable) {
this.daccessable = daccessable;
this.data = daccessable;
}
public void step() {
stepOne();
if(controller!=null)
controller.controlStep(daccessable, maccessable);
controller.controlStep(data, model);
stepTwo();
}
public void stepOne() {
Expand Down Expand Up @@ -124,4 +159,16 @@ public IMujocoController getController() {
public void setController(IMujocoController controller) {
this.controller = controller;
}
/**
* @return the modelNames
*/
public BytePointer getModelNames() {
return modelNames;
}
/**
* @param modelNames the modelNames to set
*/
public void setModelNames(BytePointer modelNames) {
this.modelNames = modelNames;
}
}
78 changes: 31 additions & 47 deletions src/test/java/mujoco/java/MuJoColibTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
import java.io.File;

import org.bytedeco.javacpp.BytePointer;
import org.bytedeco.javacpp.IntPointer;
import org.bytedeco.javacpp.Pointer;
import org.junit.Test;
import org.mujoco.IMujocoController;
import org.mujoco.MuJoCoLib;
Expand All @@ -21,31 +23,54 @@

public class MuJoColibTest {
IMujocoController controller = (d, m) -> {
// apply controls https://mujoco.readthedocs.io/en/stable/programming/simulation.html#simulation-loop
if( m.nu()==m.nv() )
/**
* This illustrates two concepts. First, we are checking
* if the number of controls mjModel.nu equals the number
* of DoFs mjModel.nv. In general, the same callback may
* be used with multiple models depending on how the user
* code is structured, and so it is a good idea to check
* the model dimensions in the callback. Second, MuJoCo
* has a library of BLAS-like functions that are very
* useful; indeed a large part of the code base consists
* of calling such functions internally. The mju_scl
* function above scales the velocity vector mjData.qvel
* by a constant feedback gain and copies the result into
* the control vector mjData.ctrl.
*/
// apply controls
// https://mujoco.readthedocs.io/en/stable/programming/simulation.html#simulation-loop
if (m.nu() == m.nv())
MuJoCoLib.mju_scl(d.ctrl(), d.qvel(), -0.1, m.nv());
};

@Test
public void managerTest() throws InterruptedException {
System.out.println("managerTest");
String filename = "model/humanoid/humanoid.xml";
File file = new File(filename);
if(!file.exists()) {
if (!file.exists()) {
fail("File is missing from the disk");
}
MuJoCoModelManager m = new MuJoCoModelManager(file);
mjModel_ model = m.getModel();
mjData_ data = m.getData();
System.out.println("Run ModelManager for 10 seconds");


for (int i = 0; i < m.getNumberOfJoints(); i++) {
System.out.println(i + " link = " + m.getJointName(i));
}
for (int i = 0; i < m.getNumberOfBodys(); i++) {
System.out.println(i + " Body = " + m.getBodyName(i));
}

m.setController(controller);
while (data.time() < 10) {
while (m.getCurrentSimulationTimeSeconds() < 10) {
m.step();
// sleep
Thread.sleep(m.getTimestepMilliSeconds());
}
m.close();
}

@Test
public void mujocoJNILoadTest() {
System.out.println("mujocoJNILoadTest");
Expand All @@ -54,47 +79,6 @@ public void mujocoJNILoadTest() {
MuJoCoLib lib = new MuJoCoLib();

System.out.println("Starting " + MuJoCoLib.mj_versionString().getString());
int error_sz = 1000;
BytePointer error = new BytePointer(error_sz);
String filename = "model/humanoid/humanoid.xml";
File file = new File(filename);
if(!file.exists()) {
fail("File is missing from the disk");
}
filename = file.getAbsolutePath();
mjModel m = MuJoCoLib.mj_loadXML(
filename, null, error,
error_sz);

if(m==null) {
String message = "Model failed to load from "+filename+"\n"+error.getString();
System.err.println(message);
fail(message);
}
System.out.println("Humanoid model loaded " + filename);
mjData d = MuJoCoLib.mj_makeData(m);
try {
mjModel_ Maccessable = new mjModel_(m);
try (mjData_ accessable = new mjData_(d)) {
System.out.println("Run model for 10 seconds");
while (accessable.time() < 10) {
MuJoCoLib.mj_step1(m, d);
// apply controls https://mujoco.readthedocs.io/en/stable/programming/simulation.html
controller.controlStep(accessable, Maccessable);
MuJoCoLib.mj_step2(m, d);
double timestep = new mjOption_(Maccessable.opt()).timestep()*1000;
Thread.sleep((long) timestep);

}

}
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
System.out.println("Clean up data objects");

MuJoCoLib.mj_deleteData(d);
MuJoCoLib.mj_deleteModel(m);
}
}

0 comments on commit e9da6d6

Please sign in to comment.