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
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)?