I want to command the robot to move to a given joint configuration. I have used position servo in my model. Here is the link to the model file. I have set the default damping in all joints to 10 and have set high kp(1000) for the all the position servo. Even then there is an error of 0.1-0.3 in some joint angles. I have tried 2 methods 1) to set the desired joint angles to data->ctrl and let the simulation run 2) to set the data->qpos directly at first time step The first one seems to work with some error. But the second makes simulation unstable. Kindly let me know how to achieve a desired joint configurations. I have looked up at this thread. For example: when I command the left hand to go to [-0.65597575, -0.77090292, -0.85008688, 1.69193828, 0.70414396, 0.98353719, -0.98110516] the robot ends up at [-0.61080661, -0.62312414, -0.89644858, 1.69188066, 0.70416729, 0.97795999, -0.97552713] another ex: [-0.57907814, -0.55066804, -0.67528671, 1.76927182, 0.94890188, 0.71474327, -1.00205739] it ends up at [-0.56791072, -0.52178578, -1.52679502, 1.7688799 , 0.94832794, 0.71387267, -1.00146106] another ex joint configuration: [-0.84808738, -0.3352765 , -0.84940643, 1.42527668, 1.04024461, 0.96507808, -1.19109601] it ends up at [-0.7725542 , -0.21602743, -1.28876539, 1.42544359, 1.04010984, 0.96040377, -1.13495196]
There could be two reasons: 1. Gravity or some joint spring is exerting an additional force, which causes bias. Note that in the real world people normally use PID control. The integral term accumulates and eventually cancels any biases due to gravity etc. 2. To determine if your gain and damping settings are reasonable, you also need to take into account the mass of the system (at least approximately). You want to get close to critical damping, maybe a bit over-damped. If the system is too over-damped, it may take a long time to get to the target, which could be mistaken for bias (if the simulation is terminated early).
In my current system, I have no gravity. After setting ctrl I ran the simulation for 10k steps. It still doesn't reach to the commanded joint configuration. Is there a test to see if my system is over-damped with the current setting? Kindly suggest if there are any other solution to make it work.
Try much smaller damping (even though the system might oscillate) and see where it settles. Also, are there any springs defined in your model? If so, they will introduce a bias. When the system settles, does the actuation force go to 0 (mjData.qfrc_actuator)? If so, then you have somehow defined an actuation model whose reference point is not what you think it is... note that actuators have their own bias terms. In general, the best way to debug things is to build a very simple model that has the same problem, and then figure out where the problem is.
I tried with damping with very small damping value like 2, 0.001 and 0.00001. The oscillates settles to a value which is close to the above posted values. However I see that the mjData.qfrc_actuator is still very big like array([[ -4.51715189e+02, -1.47759790e+02, 1.22906564e+00, 7.67415754e-01, -8.59914578e-02, 5.59787041e+01, -1.66907898e-02]]) I don't know if this is good or bad. I think this means that the joint is trying to go but is not able to go to the target location in some degree of freedom. But I am not sure. I don't have any springs in my system. I have not touched the 'ref' variable in joints. I have tried replicating this problem for 2 dof arm in a plane. However I don't face this problem in simpler model. I don't how to simplify my robot model and do debugging in it. Next thing I will try is to remove everything other than the arm and command it to the desired location. Kindly let me know if there are anymore suggestions.
Sounds like something is generating a force that opposes your actuators. Can you look at all the other qfrc_XXX fields - are any of them big? In particular, I wonder if there is a hidden contact somewhere that generates contact forces...
I printed everything out for 1 target joint configuration. I got following results ('commanded jt config', array([[-0.65597575, -0.77090292, -0.85008688, 1.69193828, 0.70414396, 0.98353719, -0.98110516]])) ('stable jt config', array([[-0.60659 , -0.61230933, 0.40846823, 1.49856161, 2.31311306, 0.97197784, -2.83707231]])) ('qfrc_actuator', array([[-493.8575207 , -158.59358702, -12.58555054, 1.93376731, -16.08968985, 115.59344234, 18.55966994]])) ('qfrc_applied', array([[ 0., 0., 0., 0., 0., 0., 0.]])) ('qfrc_bias', array([[ 9.39551132e-11, -1.74973793e-10, -7.46991004e-11, 3.91374672e-10, -1.18213605e-10, -3.06296813e-10, -9.61548762e-12]])) ('qfrc_passive', array([[ 3.05079202e-06, -6.90189018e-06, -3.03463068e-04, -2.94608553e-04, -5.91914187e-04, 1.65568540e-05, 7.61537528e-04]])) ('qfrc_unc', array([[-493.85751765, -158.59359392, -12.58585401, 1.9334727 , -16.09028177, 115.5934589 , 18.56043148]])) ('qfrc_constraint', array([[ 493.85746931, 158.59355028, 12.5858443 , -1.93351482, 16.09028168, -115.59344349, -18.56042031]])) ('qfrc_inverse', array([[ 0., 0., 0., 0., 0., 0., 0.]])) qfrc_constraint seems big and opposing the qfrc_actuator. I have also uploaded a photo of the model using the mujoco visualizer with joints, contraints and contacts switched on. I am not of what is wrong in this model.
So you have an active constraint somewhere. You have to figure out what it is and remove it (unless of course it is there for a good reason). If you examine the array mjData.efc_type it will tell you the types of the active constraints. The number of active constraints is mjData.nefc.
Thanks for a quick response. I check mjData.efc_type. It shows a bunch of constraint of type 6 i.e. mjCNSTR_CONTACT_PYRAMIDAL and 1 of type mjCNSTR_LIMIT_JOINT. I command the robot to go to jt1 = np.array([-0.65597575, -0.77090292, -0.85008688, 1.69193828, 0.70414396, 0.98353719, -0.98110516]).reshape(1,7) in which all the joints are in the limits of robot joint angles and actuators. However when I check the data->qpos it does have 1 angle which is out of bounds. However the commanded locations in data->ctrl are still in bounds. I don't know why I am getting the first error. I haven't specied the contact type in mujoco and I have turned off the collision of different robot links with itself. Kindly suggest of how to remove these constraints and allow the robot to go to goal joint configuration.
There are many ways to enable contacts... you will have to read the documentation and look at the sample models and figure it out. You can find out which geoms participate in the active contact; that will help you locate it in the model. But if there is one contact that surprised you, others may show up in different robot configurations -- so you really need to understand how contacts work and make sure the model is defined the way you intended.