update inverse kinematics to support multiple sites #399

vittorione94 · 2023-05-18T12:59:54Z

I have updated the inverse kinematics script to support multiple sites. It still supports the previous script and all the tests run correctly. There is also a new visualise script that just visualise the output position in the humanoid.

saran-t · 2023-05-18T15:58:20Z

Could you please put test assets in their own directory, similar to https://github.com/deepmind/dm_control/tree/main/dm_control/mujoco/testing/assets ?

vaxenburg · 2023-05-19T14:30:54Z

Unlike the original single-site version, looks like the multiple site generalization doesn't support simultaneous position and orientation fitting, does it?

vittorione94 · 2023-05-19T16:01:56Z

@vaxenburg you're right the multiple site generalization doesn't support simultaneous position and orientation fitting.

hartikainen · 2023-06-08T14:33:34Z

I just tested this and can confirm that this works as expected at least with the default parameters taken from dm_control/utils/inverse_kinematics_test.py. One thing I noticed is that, when using joint_names argument to ik.qpos_from_site_pose, the line here errors: https://github.com/deepmind/dm_control/pull/399/files#diff-2301b5a4bd0ea589f2d0802b1fa08eae5de429ac2fa333fa510c1e5acbe61f25R229. update_nv[dof_indices] = update_joints should probably be changed to update_nv[dof_indices] = update_joints[dof_indices].

sbohez

Few small comments but looks good otherwise!

sbohez · 2023-11-06T14:05:47Z

dm_control/utils/inverse_kinematics.py

-      # TODO(b/112141592): This does not take joint limits into consideration.
-      reg_strength = (
+      if len(site_ids) > 1:
+        for idx in range(len(site_ids)):


Switch to

for idx, site_id in enumerate(site_ids): ...

sbohez · 2023-11-06T14:11:33Z

dm_control/utils/inverse_kinematics.py

+      # clip joint angles to their respective limits
+      if len(sites_names) > 1:
+        joint_names = physics.named.data.qpos.axes.row.names
+        limited_joints = joint_names[1:]  # ignore root joint


You probably want to explicitly check which joints are actually limited using jnt_limited.

sbohez · 2023-11-06T14:14:13Z

dm_control/utils/inverse_kinematics.py

      # Compute the new Cartesian position of the site.
      mjlib.mj_fwdPosition(physics.model.ptr, physics.data.ptr)
+      site_xpos = np.squeeze(physics.named.data.site_xpos[sites_names])


Do we need the additional index here? I think np.squeeze is a view.

sbohez · 2023-11-06T14:16:59Z

dm_control/utils/inverse_kinematics.py

@@ -207,8 +231,18 @@ def qpos_from_site_pose(physics,
      # Update `physics.qpos`, taking quaternions into account.
      mjlib.mj_integratePos(physics.model.ptr, physics.data.qpos, update_nv, 1)

+      # clip joint angles to their respective limits
+      if len(sites_names) > 1:


Why only when there are multiple site names?

sbohez · 2023-11-06T14:20:08Z

dm_control/utils/visualize.py

+
+    env = control.Environment(physics, Task(mocap_qpos=[mocap_qpos]))
+
+    # env = control.Environment(physics, Task(mocap_qpos=[[-0.04934, -0.00198, 1.25512, 0.99691, 0.0161, -0.04859, -0.05959,


Please remove this comment block.

nimrod-gileadi

Thank you for your contribution. I left a couple of comments.

nimrod-gileadi · 2023-12-18T18:15:15Z

dm_control/utils/inverse_kinematics.py

@@ -35,7 +35,7 @@


 def qpos_from_site_pose(physics,
-                        site_name,
+                        sites_names,


This is a widely used function, that we don't want to break API compatibility on.

Can you instead add a new function, qpos_from_site_poses which takes a parameter (called site_names rather than sites_names, BTW)?

The existing qpos_from_site_pose function could call down to the new qpos_from_site_poses.

nimrod-gileadi · 2023-12-18T18:16:45Z

dm_control/utils/inverse_kinematics.py

@@ -170,24 +177,41 @@ def qpos_from_site_pose(physics,
      mjlib.mju_quat2Vel(err_rot, err_rot_quat, 1)
      err_norm += np.linalg.norm(err_rot) * rot_weight

+


nimrod-gileadi · 2023-12-18T18:18:09Z

dm_control/utils/inverse_kinematics.py

+  """
+  JJ_t = jac_joints.dot(jac_joints.T)
+  JJ_t += np.eye(JJ_t.shape[0]) * regularization_strength
+  return jac_joints.T.dot(np.linalg.inv(JJ_t)).dot(delta)


Add newline at the end of the file.

nimrod-gileadi · 2023-12-18T18:18:31Z

dm_control/utils/inverse_kinematics.py

+        for idx in range(len(site_ids)):
+          site_id = site_ids[idx]
+          mjlib.mj_jacSite(
+            physics.model.ptr, physics.data.ptr, jac_pos[idx], jac_rot, site_id)


indent 2 more spaces. Same for other line continuations below.

nimrod-gileadi · 2023-12-18T18:19:54Z

dm_control/utils/inverse_kinematics.py

@@ -46,12 +46,13 @@ def qpos_from_site_pose(physics,
                        max_update_norm=2.0,
                        progress_thresh=20.0,
                        max_steps=100,
-                        inplace=False):
+                        inplace=False,
+                        null_space_method=True):


What do you think about not using a boolean variable, and instead having an enum for regularization_method that can be NULLSPACE_METHOD or DAMPED_LEAST_SQUARES? That would allow us to add more methosd in the future without breaking the API.

BTW: I think it's confusing that there's a function called nullspace_method and a variable called null_space_method. If we stick with the boolean, name the variable use_nullspace_method.

Same below.

nimrod-gileadi · 2023-12-18T18:24:33Z

dm_control/utils/inverse_kinematics_test.py

+      target_pos = target_pos.reshape((-1, 3))
+      target_quat = None
+
+      _SITES_NAMES = []


This naming is reserved for module-wide constants. Name it site_names.

nimrod-gileadi · 2023-12-18T18:25:44Z

dm_control/utils/inverse_kinematics_test.py

+        "lshoulder", "rshoulder", "lhip",  "rhip",
+      ]
+
+      for name in body_names:


Maybe just use a list comprehension, e.g.:

site_names = [f"tracking[{name}]" for name in body_names]

nimrod-gileadi · 2023-12-18T18:26:25Z

dm_control/utils/inverse_kinematics_test.py

+      for name in body_names:
+          _SITES_NAMES.append("tracking[" + name + "]")
+
+      _MAX_STEPS = 5000


nimrod-gileadi · 2023-12-18T18:30:50Z

dm_control/utils/inverse_kinematics_test.py

@@ -90,7 +139,7 @@ def testQposFromSitePose(self, target, inplace):
    while True:
      result = ik.qpos_from_site_pose(
          physics=physics2,
-          site_name=_SITE_NAME,
+          sites_names=[_SITE_NAME],


The tests should prove that the existing functions continue to work.

Maybe add a parameterized annotation that will run both qpos_from_site_pose and qpos_from_site_poses on the same input.

update inverse kinematics to support multiple sites

38bbd04

created directory for test assets

a14bace

sbohez reviewed Nov 6, 2023

View reviewed changes

yuvaltassa requested a review from nimrod-gileadi December 18, 2023 14:51

nimrod-gileadi requested changes Dec 18, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

update inverse kinematics to support multiple sites #399

update inverse kinematics to support multiple sites #399

vittorione94 commented May 18, 2023

saran-t commented May 18, 2023

vaxenburg commented May 19, 2023

vittorione94 commented May 19, 2023

hartikainen commented Jun 8, 2023 •

edited

sbohez left a comment

sbohez Nov 6, 2023

sbohez Nov 6, 2023

sbohez Nov 6, 2023

sbohez Nov 6, 2023

sbohez Nov 6, 2023

nimrod-gileadi left a comment

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023

nimrod-gileadi Dec 18, 2023


		env = control.Environment(physics, Task(mocap_qpos=[mocap_qpos]))

		# env = control.Environment(physics, Task(mocap_qpos=[[-0.04934, -0.00198, 1.25512, 0.99691, 0.0161, -0.04859, -0.05959,

		@@ -170,24 +177,41 @@ def qpos_from_site_pose(physics,
		mjlib.mju_quat2Vel(err_rot, err_rot_quat, 1)
		err_norm += np.linalg.norm(err_rot) * rot_weight

update inverse kinematics to support multiple sites #399

Are you sure you want to change the base?

update inverse kinematics to support multiple sites #399

Conversation

vittorione94 commented May 18, 2023

saran-t commented May 18, 2023

vaxenburg commented May 19, 2023

vittorione94 commented May 19, 2023

hartikainen commented Jun 8, 2023 • edited

sbohez left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nimrod-gileadi left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hartikainen commented Jun 8, 2023 •

edited