Skip to content

Commit 8d665ac

Browse files
committed
Revert "Add ability to restrict IK to certain dofs. (#53)"
This reverts commit c2a1f49.
1 parent c2a1f49 commit 8d665ac

9 files changed

+10
-117
lines changed

CHANGELOG.md

-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@ All notable changes to this project will be documented in this file.
99
- Added a new humanoid example using the Apollo robot.
1010
- Added `utils.get_subtree_body_ids` to get all bodies belonging to the subtree starting at a given body.
1111
- Example usage of this function can be found in the [ALOHA example script](examples/arm_aloha.py) where it is used to selectively apply gravity compensation torques to the left and right arm bodies.
12-
- Added `joint_names` parameter to `Configuration` to allow performing IK on a subset of joints.
1312

1413
### Changed
1514

mink/__init__.py

-2
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515
MinkError,
1616
NotWithinConfigurationLimits,
1717
UnsupportedFrame,
18-
InvalidJointName,
1918
)
2019
from .lie import SE3, SO3, MatrixLieGroup
2120
from .limits import (
@@ -71,7 +70,6 @@
7170
"NotWithinConfigurationLimits",
7271
"TargetNotSet",
7372
"InvalidMocapBody",
74-
"InvalidJointName",
7573
"SUPPORTED_FRAMES",
7674
"FRAME_TO_ENUM",
7775
"FRAME_TO_JAC_FUNC",

mink/configuration.py

+3-18
Original file line numberDiff line numberDiff line change
@@ -8,15 +8,14 @@
88
"""
99

1010
import logging
11-
from typing import Optional, Sequence
11+
from typing import Optional
1212

1313
import mujoco
1414
import numpy as np
1515

1616
from . import constants as consts
1717
from . import exceptions
1818
from .lie import SE3, SO3
19-
from .utils import get_dof_ids
2019

2120

2221
class Configuration:
@@ -39,26 +38,18 @@ def __init__(
3938
self,
4039
model: mujoco.MjModel,
4140
q: Optional[np.ndarray] = None,
42-
joint_names: Optional[Sequence[str]] = None,
4341
):
4442
"""Constructor.
4543
4644
Args:
4745
model: Mujoco model.
4846
q: Configuration to initialize from. If None, the configuration is
4947
initialized to the default configuration `qpos0`.
50-
joint_names: List of joints to be controlled. If None (default), all
51-
joints are used.
5248
"""
5349
self.model = model
5450
self.data = mujoco.MjData(model)
5551
self.update(q=q)
5652

57-
if joint_names is None:
58-
self.dof_ids = np.arange(model.nv)
59-
else:
60-
self.dof_ids = np.asarray(get_dof_ids(model, joint_names))
61-
6253
def update(self, q: Optional[np.ndarray] = None) -> None:
6354
"""Run forward kinematics.
6455
@@ -94,11 +85,9 @@ def check_limits(self, tol: float = 1e-6, safety_break: bool = True) -> None:
9485
"""
9586
for jnt in range(self.model.njnt):
9687
jnt_type = self.model.jnt_type[jnt]
97-
jnt_id = self.model.jnt_dofadr[jnt]
9888
if (
9989
jnt_type == mujoco.mjtJoint.mjJNT_FREE
10090
or not self.model.jnt_limited[jnt]
101-
or jnt_id not in self.dof_ids
10291
):
10392
continue
10493
padr = self.model.jnt_qposadr[jnt]
@@ -233,9 +222,7 @@ def integrate(self, velocity: np.ndarray, dt: float) -> np.ndarray:
233222
The new configuration after integration.
234223
"""
235224
q = self.data.qpos.copy()
236-
qvel = np.zeros((self.nv,))
237-
qvel[self.dof_ids] = velocity[self.dof_ids]
238-
mujoco.mj_integratePos(self.model, q, qvel, dt)
225+
mujoco.mj_integratePos(self.model, q, velocity, dt)
239226
return q
240227

241228
def integrate_inplace(self, velocity: np.ndarray, dt: float) -> None:
@@ -245,9 +232,7 @@ def integrate_inplace(self, velocity: np.ndarray, dt: float) -> None:
245232
velocity: The velocity in tangent space.
246233
dt: Integration duration in [s].
247234
"""
248-
qvel = np.zeros((self.nv,))
249-
qvel[self.dof_ids] = velocity[self.dof_ids]
250-
mujoco.mj_integratePos(self.model, self.data.qpos, qvel, dt)
235+
mujoco.mj_integratePos(self.model, self.data.qpos, velocity, dt)
251236
self.update()
252237

253238
# Aliases.

mink/exceptions.py

-12
Original file line numberDiff line numberDiff line change
@@ -96,15 +96,3 @@ def __init__(
9696
f"{lower} <= {value} <= {upper}"
9797
)
9898
super().__init__(message)
99-
100-
101-
class InvalidJointName(MinkError):
102-
"""Exception raised when a joint name is not found in the robot model."""
103-
104-
def __init__(self, joint_name: str, model: mujoco.MjModel):
105-
joint_names = [model.joint(i).name for i in range(model.njnt)]
106-
message = (
107-
f"Joint '{joint_name}' does not exist in the model. Available "
108-
f"joint names: {joint_names}"
109-
)
110-
super().__init__(message)

mink/limits/velocity_limit.py

+3-3
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
"""Joint velocity limit."""
22

3-
from typing import Mapping, Optional
3+
from typing import Mapping
44

55
import mujoco
66
import numpy as np
@@ -28,7 +28,7 @@ class VelocityLimit(Limit):
2828

2929
indices: np.ndarray
3030
limit: np.ndarray
31-
projection_matrix: Optional[np.ndarray]
31+
projection_matrix: np.ndarray
3232

3333
def __init__(
3434
self,
@@ -58,7 +58,7 @@ def __init__(
5858
f"Got: {max_vel.shape}"
5959
)
6060
index_list.extend(range(vadr, vadr + vdim))
61-
limit_list.extend(float(val) for val in max_vel)
61+
limit_list.extend(max_vel.tolist())
6262

6363
self.indices = np.array(index_list)
6464
self.indices.setflags(write=False)

mink/utils.py

+2-26
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
1-
from typing import Optional, Sequence
1+
from typing import Optional
22

33
import mujoco
44
import numpy as np
55

66
from . import constants as consts
7-
from .exceptions import InvalidKeyframe, InvalidMocapBody, InvalidJointName
7+
from .exceptions import InvalidKeyframe, InvalidMocapBody
88

99

1010
def move_mocap_to_frame(
@@ -56,30 +56,6 @@ def get_freejoint_dims(model: mujoco.MjModel) -> tuple[list[int], list[int]]:
5656
return q_ids, v_ids
5757

5858

59-
def get_dof_ids(model: mujoco.MjModel, joint_names: Sequence[str]) -> list[int]:
60-
"""Get DOF indices for a list of joint names.
61-
62-
Args:
63-
model: Mujoco model.
64-
joint_names: List of joint names.
65-
66-
Returns:
67-
List of DOF indices.
68-
69-
Raises:
70-
InvalidJointName: If any joint name is invalid.
71-
"""
72-
dof_ids: list[int] = []
73-
for joint_name in joint_names:
74-
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
75-
if jid == -1:
76-
raise InvalidJointName(joint_name, model)
77-
vadr = model.jnt_dofadr[jid]
78-
vdim = consts.dof_width(model.jnt_type[jid])
79-
dof_ids.extend(range(vadr, vadr + vdim))
80-
return dof_ids
81-
82-
8359
def custom_configuration_vector(
8460
model: mujoco.MjModel,
8561
key_name: Optional[str] = None,

tests/test_configuration.py

-28
Original file line numberDiff line numberDiff line change
@@ -117,34 +117,6 @@ def test_check_limits_freejoint(self):
117117
configuration.update(q=q)
118118
configuration.check_limits(safety_break=True) # Should not raise.
119119

120-
def test_raises_error_if_joint_name_is_invalid(self):
121-
joint_names = ["invalid_joint_name"]
122-
with self.assertRaises(mink.InvalidJointName):
123-
mink.Configuration(self.model, joint_names=joint_names)
124-
125-
def test_dof_subset(self):
126-
joint_names = ["elbow_joint", "shoulder_pan_joint"]
127-
configuration = mink.Configuration(self.model, joint_names=joint_names)
128-
129-
# Check that the DOF IDs are set correctly.
130-
expected_dof_ids = np.array(
131-
[self.model.joint(name).dofadr[0] for name in joint_names]
132-
)
133-
np.testing.assert_equal(configuration.dof_ids, expected_dof_ids)
134-
135-
# Integrate a velocity and check that only the specified joints move.
136-
configuration.update(self.q_ref)
137-
qvel = np.random.randn(self.model.nv) * 0.1
138-
dt = 1e-1
139-
configuration.integrate_inplace(qvel, dt)
140-
qpos = configuration.q.copy()
141-
142-
expected_qpos = qpos.copy()
143-
expected_qpos[expected_dof_ids] = (
144-
self.q_ref[expected_dof_ids] + qvel[expected_dof_ids] * dt
145-
)
146-
np.testing.assert_almost_equal(qpos, expected_qpos)
147-
148120

149121
if __name__ == "__main__":
150122
absltest.main()

tests/test_posture_task.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ class TestPostureTask(absltest.TestCase):
1818

1919
@classmethod
2020
def setUpClass(cls):
21-
cls.model = load_robot_description("talos_mj_description")
21+
cls.model = load_robot_description("stretch_mj_description")
2222

2323
def setUp(self):
2424
self.configuration = Configuration(self.model)

tests/test_utils.py

+1-26
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
from robot_descriptions.loaders.mujoco import load_robot_description
77

88
from mink import utils
9-
from mink.exceptions import InvalidKeyframe, InvalidMocapBody, InvalidJointName
9+
from mink.exceptions import InvalidKeyframe, InvalidMocapBody
1010

1111

1212
class TestUtils(absltest.TestCase):
@@ -108,31 +108,6 @@ def test_get_freejoint_dims(self):
108108
np.asarray(list(range(0, 6))),
109109
)
110110

111-
def test_get_dof_ids(self):
112-
xml_str = """
113-
<mujoco>
114-
<worldbody>
115-
<body>
116-
<joint type="ball" name="ball"/>
117-
<geom type="sphere" size=".1" mass=".1"/>
118-
<body>
119-
<joint type="hinge" name="hinge1" range="0 1.57"/>
120-
<geom type="sphere" size=".1" mass=".1"/>
121-
<body>
122-
<joint type="hinge" name="hinge2" range="0 1.57"/>
123-
<geom type="sphere" size=".1" mass=".1"/>
124-
</body>
125-
</body>
126-
</body>
127-
</worldbody>
128-
</mujoco>
129-
"""
130-
model = mujoco.MjModel.from_xml_string(xml_str)
131-
dof_ids = utils.get_dof_ids(model, ["ball", "hinge2"])
132-
np.testing.assert_allclose(dof_ids, [0, 1, 2, 4])
133-
with self.assertRaises(InvalidJointName):
134-
utils.get_dof_ids(model, ["invalid_joint_name"])
135-
136111
def test_get_subtree_geom_ids(self):
137112
xml_str = """
138113
<mujoco>

0 commit comments

Comments
 (0)