google-deepmind / google-deepmind/dm_control

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

Aperta
#480 1 commento 0 reazioni 0 assegnatari Vedi su GitHub
Lingua principale
Python
Stelle
4.7k
Fork
764
Metriche di merge delle PR
Nessuna PR unita negli ultimi 30g

Descrizione

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

Guida per i contributori

Apri la guida per i contributori

Valutazione

Questa issue non è ancora stata valutata.

Ricevi le nuove issue nella tua casella

Un breve riepilogo di issue GitHub adatte ai principianti.