Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update inverse kinematics to support multiple sites #399

Open
wants to merge 2 commits into
base: main
Choose a base branch
from

Conversation

vittorione94
Copy link
Contributor

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
Copy link
Member

saran-t commented May 18, 2023

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
Copy link
Contributor

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

@vittorione94
Copy link
Contributor Author

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

@hartikainen
Copy link
Contributor

hartikainen commented Jun 8, 2023

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].

Copy link
Member

@sbohez sbohez left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Few small comments but looks good otherwise!

# 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)):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Switch to

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

# 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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

# 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])
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@@ -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:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why only when there are multiple site names?


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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please remove this comment block.

Copy link
Collaborator

@nimrod-gileadi nimrod-gileadi left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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


def qpos_from_site_pose(physics,
site_name,
sites_names,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@@ -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


Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove

"""
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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add newline at the end of the file.

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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@@ -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):
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

target_pos = target_pos.reshape((-1, 3))
target_quat = None

_SITES_NAMES = []
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

"lshoulder", "rshoulder", "lhip", "rhip",
]

for name in body_names:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

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

_MAX_STEPS = 5000
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

max_steps

@@ -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],
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants