Is there a simple way to get the joint configuration from a given end-effector configuration for manipulators like baxter? I am looking for a way to set one of the end-effector(left) at specific pose(position + orientation) in task space like near the cube for example. The link to the xml file of baxter robot is given below https://drive.google.com/file/d/0B62cBQ948cfVMHBVTFRlXzJlalE/view?usp=sharing I tried to see if I can work out something using Jacobian transpose. The dimension of Jacobian transpose at the start of simulation is (26, 795). I am not able to extract the required jacobian to map end-effector position of one hand to the joint coordinates of that hand and keeping everything else the same. Kindly help me in solving this problem
The inverse kinematics mapping is usually not unique, and even when it is there is no explicit formula. You have to solve some optimization problem to obtain the solution numerically. Most robotics textbooks have a chapter on inverse kinematics.
I am trying to use formulate the inverse kinematics problem as an optimization problem. However its difficult for me to extract the jacobian matrix which includes just one hand. I have referred here http://www.mujoco.org/book/programming.html#siJacobian. However its still not clear to me how to extract the jacobian for just one hand. Kindly let me know how to extract jacobian of 1 hand.
You can call the function mj_jacBody, with the index of your end-effector body, and pointers to the translation and rotation jacobians (3-by-nv matrices) where the function writes the data.
I tried to use the mj_jacBody and used the translation jacobian for my model. I implemented the minimization problem in the task space. However the solution to which it converges is not the correct one. I have tried to use the same formulation in another simulator pydart. https://drive.google.com/file/d/0B62cBQ948cfVMVVZb21DaEpEWlU/view?usp=sharing contains the model and my code I am using for solving ik. I am commanding the robot to go to top of the box. A video of the behavior I am getting is here. Kindly let me know if there is a better solution to the ik problem using the jacobian in mujoco.
There are two questions here: (1) is the Jacobian computed correctly; (2) what to do once you have the Jacobian. The answer to (1) is yes; you can check it against a finite different approximation if you want, but Jacobians in MuJoCo have been used extensively and there is no reason to suspect that code. The answer to (2) is entirely up to you. There are many reasons why an optimizer can converge to something other than what you want. The behavior in your simulation looks rather springy, suggesting that you should reduce the positional feedback gain and/or add damping (by changing the cost function you are optimizing, or by making some adhoc change to the output of the optimizer). I will not have the time to read and debug your code, but perhaps someone else can find the time. You could also try to do it with a simpler system - say a 2-link planar arm - to convince yourself that your overall approach makes sense. Is the behavior you are getting in pydart different?
The behavior in pydart is that the end-effector reaches the commanded position with the same problem formulation. Here is a video showing behaviour in pydart. I will try with 2dof arm and share the results.
I tried out for 2d reacher. The method works and the reacher is able to reach goal position within some error. Here is a video of the simulation. During working on the 2d reacher case, I realized I wasn't stepping the simulation enough for intermediate joint locations in optimization which led to wrong jacobian returned in those steps. Therefore I increase the number of steps to run the simulation for the intermediate solutions returned by the optimizer and the method works for baxter as well. Here is the video of the simulation. This is the link to the files of the working example. This method is slow because I have to run forward simulation for 2000 steps to calculate the next step of the optimization. I tried to set the joint angles directly by assigning data->qpos. However the arm moves randomly and does reach the desired goal location. Therefore kindly let me know if its possible to calculate jacobian given a joint configuration without running the forward simulation or if there is a faster way to set the joint angles to desired value. I also tried to play with the actuator properties like increasing the kp and increasing the dampling. However that doesn't seem to help. Thank you for all the support.
When qpos changes, there are certain computations that need to run before Jacobians can be computed. But you don't need all computations. The minimum is this: // set joint angles in data->qpos mj_kinematics(model, data); mj_comPos(model, data); mj_jacBody(model, data, jacpos, jacrot, bodyid); Note that for other Jacobians you need more computation. For example, the above code skips the entire constraint mechanism, so you will not get contacts and contact Jacobians.