google-deepmind / google-deepmind/dm_control

Attach a class-defined body to another class-defined body using joints

Open
#480 1 comment 0 reactions 0 assignees View on GitHub
Dominant language
Python
Stars
4.7k
Forks
764
PR merge metrics
No merged PRs in 30d

Description

Hello,
In order attach multiple links together, each having its own collision/visual mesh, inertia, joint, etc. I decided to construct a class that defines such a link, and it is then called multiple times, where each of its objects are initialized accordingly.

The issue that i am facing is that, when attaching such a child-body (`Child`) to the parent body, where the parent-body (`Parent-2`) has a joint w.r.t. its parent (`Parent-1`), I am unable to place the `Child` inside the `XML`definition of `Parent-2` where the joint is defined, preventing the effects of the joint (`Joint 1-to-2`) between `Parent-1` and `Parent-2` to effect the `Child` body.

Below, I define a class for constructing bodies with joints:
```
class Link_w_Joints:
#===================#
# C O N S T A N T #
#===================#

#===============================#
# I N I T I A L I Z A T I O N #
#===============================#
# joint axis default value [0, 0, 1]: https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-joint
def __init__(self, col_meshes, com_pos, dclass_col, inertia, joint_damping, joint_frictionloss, joint_name, joint_range, mass, name, viz_meshes, dclass_viz = None, joint_axis = [0, 0, 1]):
# Make a MujoCo Root
self.mjcf_model = mjcf.RootElement(model=name)
# Add a body element:
self.link_body = self.mjcf_model.worldbody.add('body',
name = name)
# Add Inertial properties
self.link_body.add('inertial',
pos = com_pos,
mass = mass,
fullinertia = inertia)
# Add Joints
self.link_body.add('joint',
name = joint_name,
range = joint_range,
damping = joint_damping,
axis = joint_axis,
frictionloss = joint_frictionloss,
dclass = 'rand')
# Add the Geometry (https://github.com/google-deepmind/dm_control/blob/ed424509c0a0e8ddf7a43824924de483026ad9cc/dm_control/locomotion/soccer/humanoid.py#L50)
# For visualization
self.link_body.add('geom',
dclass = name+'\\viz' if dclass_viz is None else dclass_viz,
mesh = viz_meshes)
# For Collision
for x in range(len(col_meshes)):
self.link_body.add('geom',
dclass = dclass_col,
mesh = col_meshes[x])
```
and below, I call the class `Link_w_Joints` multiple-times and attach successive-links (from ground to tool) to the preceding-ones:
```
class WAM:
#===================#
# C O N S T A N T #
#===================#
_body_name = ['base_link',
'shoulder_yaw_link',
'shoulder_pitch_link']
_bhand_name = 'bhand'
_dclass_col = 'wam\\col'
_camera_pos = [0, 0.085, 0.02]
_camera_name = "camera"
_camera_fovy = 55
_col_mesh_names = [['shoulder_link_convex_decomposition_p1',
'shoulder_link_convex_decomposition_p2',
'shoulder_link_convex_decomposition_p3'],
['shoulder_pitch_link_convex']]
_com_pos = [[-0.00443422, -0.00066489, -0.12189039],
[-0.00236983, -0.01542114, 0.03105614]]
_inertial = [[0.13488033, 0.09046330, 0.11328369, -0.00012485, 0.00213041, -0.00068555],
[0.02140958, 0.01558906, 0.01377875, -0.00002461, 0.00027172, 0.00181920]]
_joint_damping = [1000,
1000]
_joint_frictionloss = 1000
_joint_name = ['wam\\J1',
'wam\\J2']
_joint_range = [[-2.6, 2.6],
[-2.0, 2.0]]
_mass = [10.76768767,
3.8749]
_pos = [[0, 0, 0.346],
[0, 0, 0]]
# quant=w x y z
_quat = [[1, 0, 0, 0],
[0.707107, -0.707107, 0, 0]]
_site_name = 'site'
_viz_mesh_names = ['shoulder_link_fine',
'shoulder_pitch_link_fine']
_wam_name = 'wam'
#===============================#
# I N I T I A L I Z A T I O N #
#===============================#
def __init__(self, name):
# Make a MujoCo Root
self.mjcf_model = mjcf.RootElement(model=name)
# Add 'base_link' element:
self.base_link = self.mjcf_model.worldbody.add('body',
name = self._wam_name+'\\'+self._body_name[0],
pos = (0, 0, 0))

# Attach site for 'shoulder_yaw_link'
self.shoulder_yaw_link_site = self.base_link.add('site',
pos = self._pos[0],
quat = self._quat[0],
dclass = self._site_name)
# Create link 'shoulder_yaw_link'
self.shoulder_yaw_link = Link_w_Joints(col_meshes = self._col_mesh_names[0],
com_pos = self._com_pos[0],
dclass_col = 'wam\\col',
inertia = self._inertial[0],
joint_damping = self._joint_damping[0],
joint_frictionloss = self._joint_frictionloss,
joint_name = self._joint_name[0],
joint_range = self._joint_range[0],
mass = self._mass[0],
name = self._wam_name+'\\'+self._body_name[1],
viz_meshes = self._viz_mesh_names[0])
# Attach 'shoulder_yaw_link'
self.shoulder_yaw_link_site.attach(self.shoulder_yaw_link.mjcf_model)

# Attach site for 'shoulder_pitch_link'
self.shoulder_pitch_link_site = self.shoulder_yaw_link.mjcf_model.worldbody.add('site',
pos = self._pos[1],
quat = self._quat[1],
dclass = self._site_name)
# Create link 'shoulder_pitch_link'
self.shoulder_pitch_link = Link_w_Joints(col_meshes = self._col_mesh_names[1],
com_pos = self._com_pos[1],
dclass_col = 'wam\\col',
inertia = self._inertial[1],
joint_damping = self._joint_damping[1],
joint_frictionloss = self._joint_frictionloss,
joint_name = self._joint_name[1],
joint_range = self._joint_range[1],
mass = self._mass[1],
name = self._wam_name+'\\'+self._body_name[2],
viz_meshes = self._viz_mesh_names[1])
# Attach 'shoulder_pitch_link'
self.shoulder_pitch_link_site.attach(self.shoulder_pitch_link.mjcf_model)
```
I have already identified the cause of said problem:

1. When attaching a body to a site, the `worldbody`-element of the `Child` is converted into a `body`-element and inserted into `Parent-2` [link](https://github.com/google-deepmind/dm_control/blob/ed424509c0a0e8ddf7a43824924de483026ad9cc/dm_control/mjcf/README.md#attachment-frames),
2. Because the same thing had happened when attaching `Parent-2` to `Parent-1`, the `Child` does not end up inside the `body-element` that holds the `joint`-definition, but in the outer-one that was defined when the `worldbody`-element of the child-body is converted into a `body`-element.

Below, you will find the output of the above script which shows the above-described behavior:
```



















```

What is desired is the following output (the problematic-lines have been commented `<-- -->`):
```


<--body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_yaw_link"-->






<--/body-->

<--body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_pitch_link\wam\shoulder_pitch_link"-->




<--/body-->




```
Can we make changes that does not create a new `body`-element, and, instead, appends the the `site`'s `pos` and `quat` information to the child's `body`-element, which is defined inside the child's `worldbody` element? This will also address the issue [here](https://github.com/google-deepmind/dm_control/issues/477#issue-2360208339) as well

Contributor guide

Open the contributing guide

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.