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

Add mujoco environments #63

Merged
merged 43 commits into from
Dec 3, 2024
Merged
Changes from 1 commit
Commits
Show all changes
43 commits
Select commit Hold shift + click to select a range
039543a
Disable mujoco gui by default
mlaux1 Sep 12, 2024
c991f03
Merge pull request #58 from dfki-ric/fix/disable_mj_gui
mlaux1 Sep 12, 2024
f507997
Add separate requirements for mujoco and dev installations
mlaux1 Sep 12, 2024
5dd7be4
Add flake8 as dev dependency
mlaux1 Sep 12, 2024
3873659
Update pipline
mlaux1 Sep 12, 2024
144ff0f
Merge pull request #60 from dfki-ric/fix/split-dependencies
mlaux1 Sep 12, 2024
f0c9def
Change relative body of equality constraints
xkiixkii Sep 10, 2024
452dfed
Remove initial position
xkiixkii Sep 10, 2024
702589e
Add default position and orientation for pose
xkiixkii Sep 10, 2024
07635ca
Refactor asset manager into modules
xkiixkii Sep 10, 2024
299e1cb
Add new robots
xkiixkii Sep 10, 2024
19a85e9
Support mocap body control
xkiixkii Sep 17, 2024
b8cc066
Add mocap body control
xkiixkii Sep 17, 2024
c9b287e
Load init pose from init_pose and add pillow object
xkiixkii Sep 17, 2024
fb100dc
Add mocap control and arm descriptions
xkiixkii Sep 17, 2024
1d09e86
Add new robots
xkiixkii Sep 17, 2024
fc5d275
Add mocap body and rotation utils
xkiixkii Sep 17, 2024
c60c0e9
Set None as initial value of Pose
xkiixkii Sep 19, 2024
8500d28
Use pose defined in mjcf if pos or quat is not specified in given pose
xkiixkii Sep 19, 2024
a697981
Change relative weld body
xkiixkii Sep 30, 2024
14bce13
Register all mujoco grasp environments
xkiixkii Sep 30, 2024
e8b7dfa
Add mujoco environment scene base
xkiixkii Sep 30, 2024
268e668
Use gymnasium style render method
xkiixkii Sep 30, 2024
86f7b6e
Adapt initial pose for UR5ShadowGraspPillow
xkiixkii Sep 30, 2024
11bf6e8
Use only GraspEnv for all mujoco grasp environments
xkiixkii Sep 30, 2024
fa91ecb
Reset equality constraint after reset mocap pose
xkiixkii Sep 30, 2024
dc2bc13
Add tests for mocap control
xkiixkii Sep 30, 2024
9718608
Update README
xkiixkii Sep 30, 2024
2eb34e2
Add mjSpec utils
xkiixkii Oct 1, 2024
65fc70e
Use native mujoco viewer for "human" render mode
xkiixkii Oct 4, 2024
a5a02ef
Update dependencies
xkiixkii Oct 4, 2024
49f5012
Fix annotation issue
xkiixkii Oct 4, 2024
b15f926
Update dependencies
xkiixkii Oct 4, 2024
24a2cb0
Set render mode to "human"
xkiixkii Oct 4, 2024
54e96fc
Update README
xkiixkii Oct 4, 2024
e994b4a
Update docstring
xkiixkii Oct 4, 2024
73c4248
Add docstring
xkiixkii Oct 4, 2024
65f2a02
Update setup
xkiixkii Oct 4, 2024
f112f61
Merge pull request #61 from xkiixkii/development
mlaux1 Dec 2, 2024
0b35208
Ignore mujoco log files
mlaux1 Dec 3, 2024
9d1c02b
Require specific mujoco version
mlaux1 Dec 3, 2024
2412b69
Fix urls in contribution guide
mlaux1 Dec 3, 2024
bb6c9f3
Bump to version 0.4.2
mlaux1 Dec 3, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add docstring
xkiixkii committed Oct 4, 2024
commit 73c42484593a1812af68e0b42fb589b6bb908dec
63 changes: 57 additions & 6 deletions deformable_gym/helpers/mj_mocap_control.py
Original file line number Diff line number Diff line change
@@ -8,6 +8,24 @@


class MocapControl:
"""
A class to control the motion capture (mocap) object in a MuJoCo simulation. It handles attaching
the mocap body to a simulated object and managing equality constraints such as welds, allowing
for precise manipulation and control of the object in the environment.

Parameters:
-----------
body_name : str, optional
The name of the mocap body in the simulation, default is "mocap".
equality_name : str, optional
The name of the equality constraint (mjtEq.mjEQ_WELD) used to attach the mocap to another body. Default is "mocap".

Notes:
------
- This class assumes that only one mocap body is present in the simulation (`model.nmocap == 1`).
- The control vector `ctrl` should be of shape (6,) with the first 3 elements controlling position
and the last 3 controlling orientation as Euler angles.
"""

def __init__(
self, body_name: str = "mocap", equality_name: str = "mocap"
@@ -16,22 +34,45 @@ def __init__(
self.equality_name = equality_name

def eq_is_active(self, model: MjModel, data: MjData) -> bool:
"""
Checks if the equality constraint for the mocap body is currently active.
"""
eq_id = mju.name2id(model, self.equality_name, "equality")
return True if data.eq_active[eq_id] else False

def enable_eq(self, model: MjModel, data: MjData) -> None:
"""
Enables the equality constraint (weld) for the mocap body, effectively linking it to another body.
"""
mju.enable_equality_constraint(model, data, self.equality_name)

def disable_eq(self, model: MjModel, data: MjData) -> None:
"""
Disables the equality constraint for the mocap body.

"""
mju.disable_equality_constraint(model, data, self.equality_name)

def reset_eq(self, model: MjModel, data: MjData) -> None:
# the indication of eq_data is as follows:
# eq_data[:3]: anchor position
# eq_data[3:10]: relative position (3D position followed by 4D quaternion orientation)
# eq_data[10]: torquescale
# since mocap body is attached to the desired body, and no anchor is set,
# we need to set the relative position 0,0,0,1,0,0,0 and anchor position 0,0,0 and torquescale 1 by default
"""
Resets the equality constraint data, setting the relative position to zero and the quaternion
to identity. This ensures that the mocap body is aligned with the welded body without any offsets.

Raises:
-------
AssertionError
If the equality type is not 'weld', which is the only supported type.

Note:
-----
The indication of eq_data is as follows:
- eq_data[:3]: anchor position
- eq_data[3:10]: relative position (3D position followed by 4D quaternion orientation)
- eq_data[10]: torquescale
since mocap body is attached to the desired body, and no anchor is set,
we need to set the relative position 0,0,0,1,0,0,0 and anchor position 0,0,0 and torquescale 1 by default
"""

eq_id = mju.name2id(model, self.equality_name, "equality")
assert (
model.equality(eq_id).type == mjtEq.mjEQ_WELD
@@ -42,12 +83,18 @@ def reset_eq(self, model: MjModel, data: MjData) -> None:
mujoco.mj_forward(model, data)

def get_weld_body_id(self, model: MjModel) -> int:
"""
Returns the ID of the body that is welded to the mocap body.
"""
mocap_body_id = mju.name2id(model, self.body_name)
body1_id = model.equality(self.equality_name).obj1id
body2_id = model.equality(self.equality_name).obj2id
return body1_id if body1_id != mocap_body_id else body2_id

def attach_mocap2weld_body(self, model: MjModel, data: MjData) -> None:
"""
Attaches the mocap body to the welded body by aligning their positions and orientations, and resetting the equality constraint.
"""
weld_body_id = self.get_weld_body_id(model)
body_xpos = data.body(weld_body_id).xpos
body_xquat = data.body(weld_body_id).xquat
@@ -58,6 +105,10 @@ def attach_mocap2weld_body(self, model: MjModel, data: MjData) -> None:
self.reset_eq(model, data)

def set_ctrl(self, model: MjModel, data: MjData, ctrl: NDArray) -> None:
"""
Sets the control inputs for the mocap body. The control vector is expected to have 6 elements:
the first 3 control the position, and the last 3 control the orientation (Euler angles).
"""
assert len(ctrl) == 6, "mocap ctrl should be in shape (6,)."
assert model.nmocap == 1, "Only one mocap body is supported."