146 lines
6.7 KiB
Python
146 lines
6.7 KiB
Python
"""Metric 1 x 1 m, four-wheel differential laboratory chassis.
|
|
|
|
Geometry is explicit. Mass/friction/drive gains are provisional simulation
|
|
parameters, NOT a calibrated braking or terrain model of the owner's vehicles.
|
|
"""
|
|
|
|
PROFILE = {
|
|
"schema_version": "missioncore.virtual-rover/v1",
|
|
"length_m": 1.0,
|
|
"width_m": 1.0,
|
|
"wheel_radius_m": 0.25,
|
|
"wheel_collision": "physx-convex-cylinder",
|
|
"wheelbase_m": 0.5,
|
|
"body_contact_height_m": 0.37,
|
|
"track_width_m": 0.9,
|
|
"camera_forward_m": 0.38, # Lens ahead of the mast's front face (x=0.33).
|
|
"camera_pitch_degrees": -10.0,
|
|
"rear_range_forward_m": -0.38,
|
|
"range_sensor_profile": "front-360-rear-160-first-hit-v2",
|
|
"camera_focal_length_mm": 14.0, # Wide pinhole view retains ground on slopes.
|
|
"camera_horizontal_aperture_mm": 36.0,
|
|
"mass_kg": 24.0,
|
|
"calibration": "geometry-only",
|
|
"max_step_m": 0.10,
|
|
"max_slope_degrees": 25.0,
|
|
"stop_tilt_degrees": 30.0,
|
|
"drive_damping": 1000.0,
|
|
"brake_stiffness": 2000.0,
|
|
"brake_damping": 100.0,
|
|
"solver_position_iterations": 32,
|
|
"solver_velocity_iterations": 8,
|
|
"contact_offset_m": 0.005,
|
|
"wheel_names": ["front_left", "rear_left", "front_right", "rear_right"],
|
|
}
|
|
|
|
|
|
class DifferentialDrive:
|
|
"""Velocity drive plus physical wheel-position hold when braking.
|
|
|
|
Holding uses joint drive forces, never teleportation or a frozen chassis.
|
|
The same actuator is used by capability qualification and the live scene.
|
|
"""
|
|
|
|
def __init__(self, articulation):
|
|
self.robot = articulation
|
|
self.indices = articulation.get_dof_indices(PROFILE["wheel_names"])
|
|
self.holding = None
|
|
|
|
def command(self, velocity, yaw_rate=0.0):
|
|
hold = abs(velocity) + abs(yaw_rate) < 1e-5
|
|
if hold != self.holding:
|
|
if hold:
|
|
position = self.robot.get_dof_positions(dof_indices=self.indices)
|
|
self.robot.set_dof_position_targets(position, dof_indices=self.indices)
|
|
self.robot.set_dof_gains(
|
|
stiffnesses=PROFILE["brake_stiffness"] if hold else 0.0,
|
|
dampings=PROFILE["brake_damping"] if hold else PROFILE["drive_damping"],
|
|
dof_indices=self.indices,
|
|
)
|
|
self.holding = hold
|
|
half_track, radius = PROFILE["track_width_m"] / 2, PROFILE["wheel_radius_m"]
|
|
left = (velocity - yaw_rate * half_track) / radius
|
|
right = (velocity + yaw_rate * half_track) / radius
|
|
self.robot.set_dof_velocity_targets([[left, left, right, right]], dof_indices=self.indices)
|
|
|
|
|
|
def create_rover(stage, spawn, heading, ground_normal=(0, 0, 1), root_path="/World/Rover"):
|
|
import carb.settings
|
|
from omni.physx.bindings._physx import SETTING_COLLISION_APPROXIMATE_CYLINDERS
|
|
from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics, UsdShade
|
|
|
|
# Official PhysX cylinder approximation avoids the pathological contact
|
|
# cost measured on the Forest reconstruction. This changes collision
|
|
# representation, not wheel radius, chassis dimensions or terrain limits.
|
|
carb.settings.get_settings().set_bool(SETTING_COLLISION_APPROXIMATE_CYLINDERS, True)
|
|
|
|
tire = UsdShade.Material.Define(stage, "/World/Materials/RoverTire")
|
|
tire_physics = UsdPhysics.MaterialAPI.Apply(tire.GetPrim())
|
|
tire_physics.CreateStaticFrictionAttr(1.0)
|
|
tire_physics.CreateDynamicFrictionAttr(0.9)
|
|
tire_physics.CreateRestitutionAttr(0)
|
|
|
|
root = UsdGeom.Xform.Define(stage, root_path)
|
|
root.AddTranslateOp().Set(Gf.Vec3d(*spawn))
|
|
tilt = Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(*ground_normal)).GetQuat()
|
|
yaw = Gf.Rotation(Gf.Vec3d(0, 0, 1), heading).GetQuat()
|
|
root.AddOrientOp().Set(Gf.Quatf(tilt * yaw))
|
|
UsdPhysics.ArticulationRootAPI.Apply(root.GetPrim())
|
|
articulation = PhysxSchema.PhysxArticulationAPI.Apply(root.GetPrim())
|
|
articulation.CreateEnabledSelfCollisionsAttr(False)
|
|
articulation.CreateSolverPositionIterationCountAttr(PROFILE["solver_position_iterations"])
|
|
articulation.CreateSolverVelocityIterationCountAttr(PROFILE["solver_velocity_iterations"])
|
|
|
|
def contact(prim):
|
|
UsdPhysics.CollisionAPI.Apply(prim)
|
|
collision = PhysxSchema.PhysxCollisionAPI.Apply(prim)
|
|
collision.CreateContactOffsetAttr(PROFILE["contact_offset_m"])
|
|
collision.CreateRestOffsetAttr(0.0)
|
|
|
|
body = UsdGeom.Xform.Define(stage, root_path + "/chassis")
|
|
body.AddTranslateOp().Set(Gf.Vec3d(0, 0, 0.12))
|
|
UsdPhysics.RigidBodyAPI.Apply(body.GetPrim())
|
|
PhysxSchema.PhysxRigidBodyAPI.Apply(body.GetPrim()).CreateMaxDepenetrationVelocityAttr(1.0)
|
|
UsdPhysics.MassAPI.Apply(body.GetPrim()).CreateMassAttr(20)
|
|
hull = UsdGeom.Cube.Define(stage, root_path + "/chassis/hull")
|
|
hull.CreateSizeAttr(1)
|
|
hull.AddScaleOp().Set(Gf.Vec3f(0.8, 0.78, 0.24))
|
|
hull.CreateDisplayColorAttr([Gf.Vec3f(0.65, 0.67, 0.61)])
|
|
contact(hull.GetPrim())
|
|
# Mast is visual geometry under the rigid chassis (not a separate body).
|
|
sensor = UsdGeom.Cube.Define(stage, root_path + "/chassis/sensor")
|
|
sensor.CreateSizeAttr(1)
|
|
sensor.AddTranslateOp().Set(Gf.Vec3d(0.25, 0, 0.3))
|
|
sensor.AddScaleOp().Set(Gf.Vec3f(0.16, 0.16, 0.6))
|
|
sensor.CreateDisplayColorAttr([Gf.Vec3f(0.12, 0.13, 0.13)])
|
|
for name in PROFILE["wheel_names"]:
|
|
x = PROFILE["wheelbase_m"] / 2 * (1 if name.startswith("front") else -1)
|
|
y = 0.45 if name.endswith("left") else -0.45
|
|
path = root_path + "/" + name
|
|
wheel = UsdGeom.Cylinder.Define(stage, path)
|
|
wheel.CreateAxisAttr("Y")
|
|
wheel.CreateRadiusAttr(PROFILE["wheel_radius_m"])
|
|
wheel.CreateHeightAttr(0.1)
|
|
wheel.AddTranslateOp().Set(Gf.Vec3d(x, y, 0))
|
|
wheel.CreateDisplayColorAttr([Gf.Vec3f(0.08, 0.08, 0.08)])
|
|
UsdPhysics.RigidBodyAPI.Apply(wheel.GetPrim())
|
|
PhysxSchema.PhysxRigidBodyAPI.Apply(wheel.GetPrim()).CreateMaxDepenetrationVelocityAttr(1.0)
|
|
contact(wheel.GetPrim())
|
|
UsdShade.MaterialBindingAPI.Apply(wheel.GetPrim()).Bind(
|
|
tire, UsdShade.Tokens.weakerThanDescendants, "physics"
|
|
)
|
|
UsdPhysics.MassAPI.Apply(wheel.GetPrim()).CreateMassAttr(1)
|
|
joint = UsdPhysics.RevoluteJoint.Define(stage, root_path + "/joints/" + name)
|
|
joint.CreateBody0Rel().SetTargets([body.GetPath()])
|
|
joint.CreateBody1Rel().SetTargets([wheel.GetPath()])
|
|
joint.CreateLocalPos0Attr(Gf.Vec3f(x, y, -0.12))
|
|
joint.CreateLocalPos1Attr(Gf.Vec3f(0))
|
|
joint.CreateAxisAttr("Y")
|
|
drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular")
|
|
drive.CreateTypeAttr("force")
|
|
drive.CreateStiffnessAttr(0)
|
|
drive.CreateDampingAttr(PROFILE["drive_damping"])
|
|
drive.CreateMaxForceAttr(50)
|
|
drive.CreateTargetVelocityAttr(0)
|
|
return PROFILE
|