Skip to content

Fix spherical joint handling #950

@eric-heiden

Description

@eric-heiden

We currently do not create any ball joints in MuJoCo for given spherical joints in Newton. We need to ensure to create 3 angular dofs for spherical joints in the ModelBuilder, and also handle armature and joint friction:

    def add_joint_ball(
        self,
        parent: int,
        child: int,
        parent_xform: Transform | None = None,
        child_xform: Transform | None = None,
        key: str | None = None,
        collision_filter_parent: bool = True,
        enabled: bool = True,
        armature: float | None = None,
        friction: float | None = None,
    ) -> int:
        """Adds a ball (spherical) joint to the model. Its position is defined by a 4D quaternion (xyzw) and its velocity is a 3D vector.

        Args:
            parent: The index of the parent body.
            child: The index of the child body.
            parent_xform (Transform): The transform of the joint in the parent body's local frame.
            child_xform (Transform): The transform of the joint in the child body's local frame.
            key: The key of the joint.
            collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies.
            enabled: Whether the joint is enabled.
            armature: Artificial inertia added around the joint axes. If None, the default value from :attr:`default_joint_armature` is used.
            friction: Friction coefficient for the joint axes. If None, the default value from :attr:`default_joint_cfg.friction` is used.
        Returns:
            The index of the added joint.

        """

        if armature is None:
            armature = self.default_joint_cfg.armature
        if friction is None:
            friction = self.default_joint_cfg.friction

        ax = ModelBuilder.JointDofConfig(
            axis=(1.0, 0.0, 0.0),
            armature=armature,
            friction=friction,
        )

        return self.add_joint(
            JointType.BALL,
            parent,
            child,
            parent_xform=parent_xform,
            child_xform=child_xform,
            angular_axes=[ax, ax, ax],
            key=key,
            collision_filter_parent=collision_filter_parent,
            enabled=enabled,
        )

In MuJoCoSolver.update_joint_dof_properties we run into a memory access error when launching update_dof_properties_kernel where the model has spherical joints since apparently we don't allocate joint_armature or joint_friction for those dofs.
We should enforce joint_armature or joint_friction are allocated correctly to have dimension joint_dof_count.

Another problem occurs in apply_mjc_qfrc_kernel where mj_data.qfrc_applied does not have the expected dimension we have in Control.joint_f.

Metadata

Metadata

Assignees

Labels

No labels
No labels

Type

Projects

Status

Todo

Relationships

None yet

Development

No branches or pull requests

Issue actions