google-deepmind / google-deepmind/dm_control

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

Abierto
#480 1 comentario 0 reacciones 0 asignados Ver en GitHub
Lenguaje dominante
Python
Estrellas
4.7k
Forks
764
Métricas de merge de PR
Sin PR fusionados en 30 d

Descripción

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

Guía de contribución

Abrir la guía de contribución

Evaluación

Este issue todavía no se ha evaluado.

Recibe los nuevos issues en tu correo

Un resumen breve de issues de GitHub para principiantes.