google-deepmind / google-deepmind/dm_control
Attach a class-defined body to another class-defined body using joints
- 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.