Hello, I'm just starting Simulation with MuJoCo. It's great!! Is there a posibility to directly asign joint angles to hinge joints through d->qpos? And further: How do I determine the right joints to Control? Thank You! Best, Marc Tuscher
Welcome! You can change the numbers in d->qpos programmatically between time steps. This will change the joint angles. The change will affect everything else next time you call mj_step or mj_forward. Please be careful with quaternion joints. The forward kinematics will normalize all quaternions automatically, but still, setting their values in qpos has to be done carefully in order to get the desired effect. As for controlling joints, you can define actuators and attach them to joints (and then specify control signals in d->ctrl) or specify applied torques directly in d->qfrc_applied. To find out which elements of d->qfrc_applied to set: if the desired joint is n, m->jnt_dofadr[n] is the address for that joint in the vector of torques. How do you know what n to use? The function mj_name2id converts names to ids for all MuJoCo objects. Alternatively, you can count the joints in the XML (starting from 0), or save the model in text format (with mj_printModel) which produces a human-readable roadmap with the names and ids of everything.
Thanks for your quick reply. This helped me a lot. But I couldn't completely figure out the right way to access d->qpos so far. To convert from radian angles to quaternions I would use mju_axisAngle2Quat(). How can I move Joint1 (first hinge joint) in the example model 45 degree around the y-axis ? Code: <mujoco model="example"> <compiler coordinate="global"/> <default> <geom rgba=".8 .6 .4 1"/> </default> <asset> <texture type="skybox" builtin="gradient" rgb1="1 1 1" rgb2=".6 .8 1" width="256" height="256"/> </asset> <worldbody> <light pos="0 1 1" dir="0 -1 -1" diffuse="1 1 1"/> <body> <geom type="capsule" fromto="0 0 1 0 0 0.6" size="0.06"/> <joint type="ball" pos="0 0 1"/> <body> <geom type="capsule" fromto="0 0 0.6 0.3 0 0.6" size="0.04"/> <joint type="hinge" pos="0 0 0.6" axis="0 1 0"/> <joint type="hinge" pos="0 0 0.6" axis="1 0 0"/> <body> <geom type="ellipsoid" pos="0.4 0 0.6" size="0.1 0.08 0.02"/> <site name="end1" pos="0.5 0 0.6" type="sphere" size="0.01"/> <joint type="hinge" pos="0.3 0 0.6" axis="0 1 0"/> <joint type="hinge" pos="0.3 0 0.6" axis="0 0 1"/> </body> </body> </body> <body> <geom type="cylinder" fromto="0.5 0 0.2 0.5 0 0" size="0.07"/> <site name="end2" pos="0.5 0 0.2" type="sphere" size="0.01"/> <joint type="free"/> </body> </worldbody> <tendon> <spatial limited="true" range="0 0.6" width="0.005"> <site site="end1"/> <site site="end2"/> </spatial> </tendon> </mujoco>
The first hinge joint is after the ball joint, which occupies qpos[0-3] since it is a quaternion. So the index you need is 4. Note that a hinge joint has only one element in qpos; it is a scalar joint, not a quaternion. To set it to 45 deg, you need to convert to radians: d->qpos[4] = 45.0/180.0*mjPI; The more civilized way of doing this is to give it a name in the XML, say "Hinge1", and then do: int id = mj_name2id(m, mjOBJ_JOINT, "Hinge1"); if( id>=0 ) d->qpos[id] = 45.0/180.0*mjPI; else mju_error("Joint 'Hinge1' not found");
Thanks for your great support. I got this working. There's just one issue I'm not sure being a bug or wanted by developers: I excpected int id = mj_name2id(m, mjOBJ_JOINT, "HINGE1"); printf("Hinge: %d\n", id); setting id = 4, but instead it sets id = 1. So if I set d->qpos[id] = 45/180*mjPI, I set the second part of the quaternion ball joint to this value.
Oh, sorry, my mistake. This gives you the id of the joint -- which is indeed 1. The address of the qpos element(s) corresponding to a given joint is m->jnt_qposadr[id]. So you need: d->qpos[m->jnt_qposadr[id]] = 45/180*mjPI;
Thanks for your support. I got this working. In reality my robot is driven by 3 position servos, so I'm using actuator/position at the corresponding joints in my MJCF to model this. What is the right way to assign scalar angles to these actuators?
d->ctrl contains the control signals to the actuators. When you have a position actuator, its control signal has the meaning of reference position/angle. So just assign the reference positions in d->ctrl.
I'm assigning the reference positions to d->ctrl like this: d->ctrl[0] = theta1; //first actuator at joint J1 First of all: Is this the right way to assign the angles? If so, I expect the following equation [1] to be correct after calling mj_step or mj_forward. [1] d->qpos[m->jnt_qposadr[J1]] = theta1; Is this assumption correct? I'm quite sure about this being a problem related to correctly creating a PD controller with actuator/position and actuator/velocity, but I'm not really familier with modelling PD controllers. Is there a simple way to get into it?
The thing you expect is not what happens. d->ctrl is a control signal. When you have a position servo in the model, the control signal is interpreted as reference position. The servo then computes the difference between the reference position and the current position, multiplies this difference by the feedback gain, and this yields a force that is applied to the joints. The effects of this force are then integrated through the dynamics, along with any other forces acting on the system. So in the long run the position servo should bring d->qpos to the reference positions you specified, unless of course it ends up fighting some other force (gravity or some constraint). But this will not happen in one time step.
Hello, This post helps me to understand how to find joint id and position address, velocity address using XML. Now I wonder how to find the controller address corresponding to that joint, especially when the number of actuation is less then number of dof, and also you have a free joint which also makes you nu<nv<nq I have tried to use for example in the given humanoid.xml model, Code: int abdomen_z_act = mj_name2id(m, mjOBJ_ACTUATOR, "abdomen_z"); but I am not sure how to track controller address from this ID. How should I use mj_name2id properly to get the variable 'ctladr' to use in the following code, as you did in d->qpos[m->jnt_qposadr[J1]] Code: int ctrl; d->ctrl[ctrladr] = blahblah A separate question I have is when I try to use mjPI, VS says that "mjPI: undeclared identifier". How should I resolve this issue? This looks a simple global variable syntax issue so I would put less priority on this question...
So I think I 've resolved the issue. The int abdomen_z_act = mj_name2id(m, mjOBJ_ACTUATOR, "abdomen_z"); d->ctrl[abdomen_z_act] seems work. Thanks! The separate question is also resolved by rebuilding everything.