Code: void setState(const mjtNum* ptr, const mjModel* m, mjData* d) { mju_copy(d->qpos, ptr, m->nq); ptr += m->nq; mju_copy(d->qvel, ptr, m->nv); } void setCtrl(const mjtNum* ptr, const mjModel* m, mjData* d) { mju_copy(d->ctrl, ptr, m->nu); } // simple controller void mycontroller(const mjModel* m, mjData* d) { double x[4] = {0.0, 0.0, 0.0, 0.0}; //in radians double u[2] = {1.0, 0.0}; //control signals //this works //mju_scl(d->ctrl, d->qvel, -0.1, m->nv); //mj_step(m,d); const mjtNum* state = x; const mjtNum* ctrl = u; setState(state, m, d); mj_step1(m,d); setCtrl(ctrl, m, d); mj_step2(m,d); } I am trying out various methods mentioned in the tutorial section of mujoco. This is a two link manipulator with two actuators. I am calling the above function from my main using mycontroller(m,d). I am able to set joint position on manipulator (using setState), it does not move at all under the control signal (using setCtrl) u = [1.0, 0.0]. Is this not the right way to use mj_step1 and mj_step2? (it does move when mj_step1 and mj_step2 are commented and mju_scl - mj_step is run).
I am assuming you are calling "mycontroller" in a loop? The problem is that you are setting the simulation state to (0,0,0,0) in each call, so the simulation cannot advance. In contrast, the code you commented out (using mj_step) does not set the state to (0,0,0,0). In general, another reason for not moving might be that the simulation is going unstable and resetting itself automatically (in which case a warning is printed in the error log). But that does not seem to be the case here.
You were right. It solved the problem. It would immensely helpful if the tutorial put some more code examples. Suppose the xml file contains two 2- link manipulators that are interacting. How to individually address the state and controls of them? Is there a way to address joints by their name given in xml files?
There is only one mjModel, containing all joints and states in the order in which they appear in the XML. So the two 2-link manipulations will simply appear one after the other. To find the index of any object by name (assuming the object was named in the XML) use the function mj_name2id(model, type, name). The MuJoCo Pro chapter in the documentation contains more code snippets, and is progressing rapidly. The Tutorial is on hold for now. It may even disappear at some point, because the Programming Guide in the MuJoCo Pro chapter serves a similar purpose but is much more in-depth.
I not sure but I guess setState and setCtrl are TWO types of controllers? The setState seems like a position controller, while the setCtrl seems like a torque controller or something. Well, I just want to confirm that we can only use setState function here to control the manipulator (in other words, do position control). It seems reasonable for me based on my experience (that's how I control a real industrial robot). Thanks and wish somebody can share some thoughts.
Setting the state of the simulation (qpos and qvel) is equivalent to teleporting the system to some arbitrary state and bypassing the physics and actuation altogether. This should only be used to set an initial state. The only reason to use it at each step is to re-create a prerecorded animation sequence and render it. If you look at playlog.m in the HAPTIX mex toolkit, it uses that mechanism (over the socket API) to play back a log file. For simulation and control, use ctrl. Each actuator has a scalar control input which is an element of ctrl. How this input is interpreted depends on the actuator type. For direct drive actuators, ctrl is a torque. For position actuators, ctrl is a reference position. See documentation of MuJoCo's general actuation model here: http://www.mujoco.org/book/overview.html#Actuation and documentation of actuator shortcuts (used for creating common actuator types) here: http://www.mujoco.org/book/modeling.html#CActuator
Thank you so much for the reply and the link. They are very useful! I usually do high level trajectory planning and generate joint velocity command, then the robot itself will run low-level tracking and give torque command to motors. It indeed bypasses the physics and actuation. Here, it seems we should give any command from d -> ctrl when do the torque control (use motor shortcut is good enough for hinge joint and direct drive). I still have three questions related to this: If I use motor shortcut and do torque control, I will give d->ctrl a torque(say it's 0.1 Nm), by setting the gear='10', the joint torque executed on the joint is 1 Nm. Is this correct? Joint stiffness: I understand that the stiffness is like a spring to pull the joint back to its equilibrium position. My question is when do torque control (d->ctrl = "some torque"), is the equilibrium position always equals to the initial joint position? The influence of joint damping/ stiffness to torque control: The torque control calculate desired torque first, and then set d->ctrl = "desired torque" to the model. Now, what if the damping/stiffness values are positive? I am almost sure they will change the torque value of the joint. Should we set them as zero when doing torque control? Look forward to your reply, thanks so much!
1. Yes, the gear parameter scales the force. 2. If you model a position actuator, then ctrl has the meaning of equilibrium position, and you can change it at each time step. Again, the meaning of ctrl depends on the actuator model you defined. It can be force, torque, desired position, desired velocity, rate of change of pressure, as well as other things we don't even have names for -- in case you created some custom actuation model. 3. When simulating a servo, you have a choice: you can do it yourself in your code, include a torque actuator in the model, and send torque commands to MuJoCo. Or, you can include a servo actuator in the model, and let MuJoCo do the servo simulation for you. The latter is usually the better option, but it is up to you. The Darwin model on the Forum (under Resources) is an example of using position servo actuators.