manipulation

The mecanum_base.urdf can be procedurally generated using the parametric_mecanum_chassis_urdf_generator tool in Anzu. This allows you to customize the physical dimensions and mass properties of the chassis.

Key parameters include:

• --wheelbase: Distance between front and rear axles.
• --track: Distance between left and right wheels.
• --chassis_tube_radius and --chassis_tube_mass: Dimensions and mass of the chassis tubes.
• --axle_radius and --axle_mass: Dimensions and mass of the axles.
• --hub_radius, --hub_thickness, and --hub_mass: Dimensions and mass of the wheel hubs.
• --roller_length, --roller_diameter, --roller_mass, and --num_rollers: Specifications for the mecanum rollers.
• --model_name: The name of the generated model.
• --filename: The output path for the resulting .urdf file.

bazel run //models/robomaster:parametric_mecanum_chassis_urdf_generator -- --wheelbase=0.2 --track=0.205 --chassis_tube_radius=0.02 --chassis_tube_mass=0.5 --axle_radius=0.02 --axle_mass=0.5 --suspension_joint_range=0.5 --hub_radius=0.045 --hub_thickness=0.04 --hub_mass=0.125 --wheel_effort_limit=1.0 --roller_length=0.06 --roller_diameter=0.015 --roller_mass=0.01 --num_rollers=12 --model_name=mecanum_base --filename=/tmp/mecanum_base.urdf

When designing robot manipulation tasks, you must define poses relative to both the object and the world frame.

Grasp Pose Calculation The grasp pose in the world frame (${^WX^G}$) is computed by combining the object's pose in the world frame (${^W}X^{O}$) and the grasp pose in the object frame (${^OX^G}$):

$${{^WX^G} = {}{^W}X^{O}} {^OX^G}$$

Approach Poses To avoid collisions during movement, define 'pre-pick' or 'pre-place' approach poses. An approach pose (${X_{WGApproach}}$) is a nearby position with sufficient clearance (e.g., 0.1m offset) to allow the robot to move into the final grasp/place pose without colliding with the environment or the object itself.

def design_grasp_pose(X_WO: RigidTransform) -> tuple[RigidTransform, RigidTransform]:
    """
    X_WO: Object pose in world frame
    Returns:
        X_OG: Grasp pose in object frame
        X_WG: Grasp pose in world frame
    """
    X_OG = RigidTransform()
    X_WG = RigidTransform()
    return X_OG, X_WG

In this project, pointclouds are represented using pydrake.all.PointCloud. For computational efficiency, point data is typically stored in two separate 3xN numpy arrays:

• XYZ positions: A 3xN np.float32 array where each column is an $[x, y, z]^T$ coordinate in meters.
• RGB colors: A 3xN np.uint8 array where each column contains RGB information.

Note that while numpy operations often prefer row-wise data, the PointCloud object and many manipulation functions expect column-wise (transposed) arrays. Use the .T operator to switch between these representations.

You can use the ToPointCloud helper to convert these numpy arrays into a PointCloud object.

import numpy as np
from pydrake.all import PointCloud, Fields, BaseField

def ToPointCloud(xyzs, rgbs=None):
    if rgbs:
        # xyzs and rgbs should be 3xN arrays
        cloud = PointCloud(xyzs.shape[1], Fields(BaseField.kXYZs | BaseField.kRGBs))
        cloud.mutable_rgbs()[:] = rgbs
    else:
        cloud = PointCloud(xyzs.shape[1])
    cloud.mutable_xyzs()[:] = xyzs
    return cloud

This strategy uses an OsqpSolver within a LeafSystem to calculate the optimal contact forces required to rotate an object (e.g., a box) to a desired angle.

Key components of the implementation:

• State Tracking: Uses a continuous state to track the integral of the angle error for PID control.
• Optimization Problem: A MathematicalProgram is constructed to minimize the squared error between the commanded torque (derived from PID) and the contact force, subject to friction cone constraints (mu_A, mu_C) and contact maintenance constraints.
• Constraints:
  • Friction cone: $|f_{tangential}| \≤ μ imes f_{normal}$
  • Contact maintenance: $f_{normal} \ge 0.1$
• Control Loop: The solver finds the optimal force $f_{C_W}$, which is then applied as negative actuation to the finger.

# Inside RotateBoxOnTopCorner.CalcOutput
prog = MathematicalProgram()
f_C_W = prog.NewContinuousVariables(2, "f_C_W")  # force from finger to box
f_C_C = np.matmul(R_CW, f_C_W)

# PID control on f_CC[1]
kp = 20.0
ki = 0.5
kd = 2.0 * np.sqrt(kp)
integral_error = context.get_continuous_state_vector().GetAtIndex(0)
pid = kp * (theta_desired - theta) + ki * integral_error - kd * theta_dot
prog.AddQuadraticCost((w * f_C_C[0] - pid) ** 2, True)

# Friction and contact constraints
prog.AddConstraint(f_C_W[0] <= mu_A * (box_mass * g - f_C_W[1]))
prog.AddConstraint(-f_C_W[0] <= mu_A * (box_mass * g - f_C_W[1]))
prog.AddConstraint(f_C_C[1] >= 0.1)  # Ensure contact
prog.AddConstraint(f_C_C[0] <= mu_C * f_C_C[1])
prog.AddConstraint(-f_C_C[0] <= mu_C * f_C_C[1])

result = self.solver.Solve(prog)

Instead of manually adding models and welding frames in Python code, you can use a YAML-based Model Directive file. This allows you to define all objects, their file locations, and their relative poses (including welds and default free body poses) in a single structured format. You can load these using LoadModelDirectivesFromString and apply them with ProcessModelDirectives.

from pydrake.multibody.parsing import LoadModelDirectivesFromString, ProcessModelDirectives

model_directive = """
    directives:
    - add_model:
        name: table_top
        file: file:///path/to/table_top.sdf
    - add_weld:
        parent: world
        child: table_top::table_top_center
    - add_model:
        name: cracker_box
        file: package://drake_models/ycb/003_cracker_box.sdf
        default_free_body_pose:
            base_link_cracker:
                translation: [0,0,0.8]
                rotation: !Rpy { deg: [42, 33, 18] }
    """

# Inside your setup function:
directives = LoadModelDirectivesFromString(model_directive)
models = ProcessModelDirectives(directives, plant, parser)
plant.Finalize()