Issues using Inverse Kinematics on Spherical Joints (DeepMimic Humanoid)

Post Reply
Herou
Posts: 1
Joined: Fri May 20, 2022 5:53 pm

Issues using Inverse Kinematics on Spherical Joints (DeepMimic Humanoid)

Post by Herou »

Hi!
I am working on trying to apply Inverse Kinematics to DeepMimic's humanoid model (https://github.com/xbpeng/DeepMimic/blo ... noid3d.txt) in order to slightly alter the character's motion in real time.

In order to figure out how to do so I have reduced the problem into a small example - Trying to modify the Humanoid's left wrist position using Inverse Kinematics.
The humanoid has 8 Spherical Joints and 4 Revolute Joints

I wrote the following code in order to try to dynamically alter the humanoid's wrist position taking in new values from the user:

Code: Select all

endEffector = 14
curPos = self._get_joint_pose(phys_model, endEffector)[0]

if(self.first):
	self.first = False
 	self.c = [self._pybullet_client.addUserDebugParameter("lWrist x", -2, 2, curPos[0]), self._pybullet_client.addUserDebugParameter("lWrist y", -2, 2, curPos[1]), self._pybullet_client.addUserDebugParameter("lWrist z", -2, 2, curPos[2])]

targetPos = [self._pybullet_client.readUserDebugParameter(self.c[0]), self._pybullet_client.readUserDebugParameter(self.c[1]), self._pybullet_client.readUserDebugParameter(self.c[2])]

jointPoses = self._pybullet_client.calculateInverseKinematics(phys_model, endEffector, targetPos)

currentPointer = 0
for i in range(s.num_joints):
	jtype = s.joint_types[i]
	
	if jtype is JointType.BASE: #Root
		pass
      	elif jtype is JointType.FIXED: #R/L Wrist
        	pass
      	elif jtype is JointType.REVOLUTE: #R/L Knee ; R/L Elbow
        	self._pybullet_client.resetJointStateMultiDof(phys_model, i, [jointPoses[currentPointer]])
       		currentPointer += 1
      	elif jtype is JointType.SPHERE: #Chest ; Neck ; R/L Hip ; R/L Ankle ; R/L Shoulder
        	quat = self._pybullet_client.getQuaternionFromEuler([jointPoses[currentPointer], jointPoses[currentPointer+1], jointPoses[currentPointer+2]])
        	self._pybullet_client.resetJointStateMultiDof(phys_model, i, quat)
        	currentPointer += 3
However, I am not getting the expected results as the final outcome appears distorted/incorrect.

Image
Image

I believe the issue has to do with me not really knowing what to do with the outcome of the CalculateInverseKinematics function. I have tried looking at other examples, but I couldn't find anyone else using a combination of InverseKinematics and Spherical Joints. Has anyone tried doing something similar (i.e using InverseKinematics on Spherical joints/the DeepMimic Humanoid.urdf)?
Post Reply