"""juno kinematic chain spec shared by the CAD assembly and URDF/SRDF. Stdlib-only (no build123d) so robot-description generators can import it cheaply. `juno.py` consumes the same offsets and CAD pose angles, keeping the STEP assembly and the URDF tree in lockstep. Design ledger (URDF): - Units: chain offsets are millimeters in each parent part's local frame; URDF emission converts to meters/radians. Frames follow REP-103: +X forward, +Y robot-left, +Z up. The pelvis waist-yaw joint center is the robot origin; every child link frame sits at its joint center with axes parallel to the parent at zero joint angle (pure translations). - Source: offsets and axes mirror the `revolute_attach()` calls in `juno.py` (the build123d assembly), which is the CAD source of truth. - Positive-motion conventions (verified against the CAD athletic stance): pitch joints rotate about +Y (positive swings the distal segment backward: knee flexion positive, hip/shoulder flexion forward negative); roll joints rotate about +X (positive moves the distal segment toward +Y, robot-left); yaw joints rotate about +Z (positive turns toward robot-left). Side-dependent signs (abduction, internal rotation) are encoded per side in limits and poses. - Joint limits, efforts, and velocities are ASSUMED plausible values for a 1.4 m research-humanoid concept (G1-class actuators); the CAD does not model hard stops. Documented here, not measured. """ from __future__ import annotations # Lateral offsets (mm) from juno.py. HIP_Y_MM = 90.0 SHOULDER_Y_MM = 148.0 # Parent-local joint origin offsets (mm); see juno.py chain-offset docstring. WAIST_YAW_ORIGIN_MM = (0.0, 0.0, 0.0) NECK_YAW_ORIGIN_MM = (0.0, 0.0, 324.0) NECK_PITCH_ORIGIN_MM = (0.0, 0.0, 46.0) HIP_YAW_DROP_Z_MM = -120.0 HIP_ROLL_ORIGIN_MM = (0.0, 0.0, -64.0) HIP_PITCH_ORIGIN_MM = (0.0, 0.0, -78.0) KNEE_ORIGIN_MM = (0.0, 0.0, -290.0) ANKLE_PITCH_ORIGIN_MM = (0.0, 0.0, -290.0) ANKLE_ROLL_ORIGIN_MM = (0.0, 0.0, -30.0) SHOULDER_PITCH_RAISE_Z_MM = 290.0 SHOULDER_ROLL_Y_MM = 34.0 SHOULDER_ROLL_Z_MM = -72.0 SHOULDER_YAW_ORIGIN_MM = (0.0, 0.0, -24.0) ELBOW_ORIGIN_MM = (0.0, 0.0, -156.0) WRIST_ROLL_ORIGIN_MM = (0.0, 0.0, -150.0) WRIST_PITCH_ORIGIN_MM = (0.0, 0.0, -28.0) X_AXIS = (1.0, 0.0, 0.0) Y_AXIS = (0.0, 1.0, 0.0) Z_AXIS = (0.0, 0.0, 1.0) # Sole plane below the foot link origin (foot part bbox bottom; the rubber # sole pads are modeled 26 mm under the ankle-roll center). SOLE_BELOW_FOOT_MM = 26.0 # Standing pelvis height at the all-zero pose: the URDF roots a frame-only # `base_footprint` link at ground level and lifts the pelvis by this fixed # offset so the zero pose stands exactly on z = 0. Posed states with bent # knees (athletic_ready, squat) keep the pelvis fixed and hover slightly. BASE_FOOTPRINT_TO_PELVIS_Z_MM = -( HIP_YAW_DROP_Z_MM + HIP_ROLL_ORIGIN_MM[2] + HIP_PITCH_ORIGIN_MM[2] + KNEE_ORIGIN_MM[2] + ANKLE_PITCH_ORIGIN_MM[2] + ANKLE_ROLL_ORIGIN_MM[2] - SOLE_BELOW_FOOT_MM ) SIDES = ("left", "right") def side_sign(side: str) -> float: """+1 for the robot-left side, -1 for the robot-right side.""" return 1.0 if side == "left" else -1.0 # CAD athletic ready stance (degrees), shared with juno.py and the SRDF # `athletic_ready` group state. HIP_PITCH_DEG = -16.0 KNEE_DEG = 32.0 ANKLE_PITCH_DEG = -16.0 HIP_ROLL_ABDUCT_DEG = 3.0 # left +, right -; ankle roll compensates HIP_YAW_DEG = 0.0 WAIST_YAW_DEG = 0.0 SHOULDER_PITCH_DEG = -8.0 # negative swings the arm forward SHOULDER_ROLL_ABDUCT_DEG = 8.0 # elbows out SHOULDER_YAW_INTERNAL_DEG = 8.0 ELBOW_DEG = -20.0 # negative flexes the forearm forward WRIST_ROLL_DEG = 0.0 WRIST_PITCH_DEG = -2.0 NECK_YAW_DEG = 0.0 NECK_PITCH_DEG = -2.0 # ---------------------------------------------------------------- limits # ASSUMED actuator classes (Nm rated torque / rad/s no-load class speed) # for a G1-class concept humanoid; the CAD does not model actuators' data. LEG_PRIMARY_EFFORT_NM = 120.0 # hip yaw/roll/pitch, knee LEG_PRIMARY_VELOCITY_RAD_S = 15.0 ANKLE_EFFORT_NM = 50.0 ANKLE_VELOCITY_RAD_S = 12.0 WAIST_EFFORT_NM = 60.0 WAIST_VELOCITY_RAD_S = 8.0 SHOULDER_EFFORT_NM = 40.0 SHOULDER_VELOCITY_RAD_S = 12.0 ELBOW_EFFORT_NM = 35.0 ELBOW_VELOCITY_RAD_S = 12.0 WRIST_EFFORT_NM = 15.0 WRIST_VELOCITY_RAD_S = 15.0 NECK_EFFORT_NM = 8.0 NECK_VELOCITY_RAD_S = 8.0 # ASSUMED joint travel (degrees), humanoid-plausible and containing the CAD # athletic stance. Asymmetric ranges are authored for the LEFT side with # the convention noted; the right side mirrors lower/upper. WAIST_YAW_RANGE_DEG = (-150.0, 150.0) NECK_YAW_RANGE_DEG = (-90.0, 90.0) NECK_PITCH_RANGE_DEG = (-35.0, 45.0) # negative looks up, positive down HIP_YAW_RANGE_DEG = (-60.0, 60.0) HIP_ROLL_LEFT_RANGE_DEG = (-20.0, 45.0) # +abduction for the left leg HIP_PITCH_RANGE_DEG = (-120.0, 30.0) # negative flexes forward KNEE_RANGE_DEG = (-5.0, 140.0) # positive flexes backward ANKLE_PITCH_RANGE_DEG = (-30.0, 50.0) # negative toes-up ANKLE_ROLL_LEFT_RANGE_DEG = (-25.0, 25.0) SHOULDER_PITCH_RANGE_DEG = (-180.0, 60.0) # negative swings forward SHOULDER_ROLL_LEFT_RANGE_DEG = (-25.0, 160.0) # +abduction for the left arm SHOULDER_YAW_RANGE_DEG = (-120.0, 120.0) ELBOW_RANGE_DEG = (-145.0, 5.0) # negative flexes forward WRIST_ROLL_RANGE_DEG = (-170.0, 170.0) WRIST_PITCH_RANGE_DEG = (-70.0, 70.0) def _mirror_range(range_deg: tuple[float, float], side: str) -> tuple[float, float]: if side == "left": return range_deg lower, upper = range_deg return (-upper, -lower) def central_joints() -> list[dict]: """The three non-sided joints in root-to-leaf order.""" return [ { "name": "waist_yaw", "parent": "pelvis", "child": "torso", "origin_mm": WAIST_YAW_ORIGIN_MM, "axis": Z_AXIS, "range_deg": WAIST_YAW_RANGE_DEG, "effort_nm": WAIST_EFFORT_NM, "velocity_rad_s": WAIST_VELOCITY_RAD_S, }, { "name": "neck_yaw", "parent": "torso", "child": "neck_collar", "origin_mm": NECK_YAW_ORIGIN_MM, "axis": Z_AXIS, "range_deg": NECK_YAW_RANGE_DEG, "effort_nm": NECK_EFFORT_NM, "velocity_rad_s": NECK_VELOCITY_RAD_S, }, { "name": "neck_pitch", "parent": "neck_collar", "child": "head", "origin_mm": NECK_PITCH_ORIGIN_MM, "axis": Y_AXIS, "range_deg": NECK_PITCH_RANGE_DEG, "effort_nm": NECK_EFFORT_NM, "velocity_rad_s": NECK_VELOCITY_RAD_S, }, ] def leg_joints(side: str) -> list[dict]: s = side_sign(side) return [ { "name": f"hip_yaw_{side}", "parent": "pelvis", "child": f"hip_bracket_{side}", "origin_mm": (0.0, s * HIP_Y_MM, HIP_YAW_DROP_Z_MM), "axis": Z_AXIS, "range_deg": HIP_YAW_RANGE_DEG, "effort_nm": LEG_PRIMARY_EFFORT_NM, "velocity_rad_s": LEG_PRIMARY_VELOCITY_RAD_S, }, { "name": f"hip_roll_{side}", "parent": f"hip_bracket_{side}", "child": f"hip_carrier_{side}", "origin_mm": HIP_ROLL_ORIGIN_MM, "axis": X_AXIS, "range_deg": _mirror_range(HIP_ROLL_LEFT_RANGE_DEG, side), "effort_nm": LEG_PRIMARY_EFFORT_NM, "velocity_rad_s": LEG_PRIMARY_VELOCITY_RAD_S, }, { "name": f"hip_pitch_{side}", "parent": f"hip_carrier_{side}", "child": f"thigh_{side}", "origin_mm": HIP_PITCH_ORIGIN_MM, "axis": Y_AXIS, "range_deg": HIP_PITCH_RANGE_DEG, "effort_nm": LEG_PRIMARY_EFFORT_NM, "velocity_rad_s": LEG_PRIMARY_VELOCITY_RAD_S, }, { "name": f"knee_{side}", "parent": f"thigh_{side}", "child": f"shin_{side}", "origin_mm": KNEE_ORIGIN_MM, "axis": Y_AXIS, "range_deg": KNEE_RANGE_DEG, "effort_nm": LEG_PRIMARY_EFFORT_NM, "velocity_rad_s": LEG_PRIMARY_VELOCITY_RAD_S, }, { "name": f"ankle_pitch_{side}", "parent": f"shin_{side}", "child": f"ankle_link_{side}", "origin_mm": ANKLE_PITCH_ORIGIN_MM, "axis": Y_AXIS, "range_deg": ANKLE_PITCH_RANGE_DEG, "effort_nm": ANKLE_EFFORT_NM, "velocity_rad_s": ANKLE_VELOCITY_RAD_S, }, { "name": f"ankle_roll_{side}", "parent": f"ankle_link_{side}", "child": f"foot_{side}", "origin_mm": ANKLE_ROLL_ORIGIN_MM, "axis": X_AXIS, "range_deg": _mirror_range(ANKLE_ROLL_LEFT_RANGE_DEG, side), "effort_nm": ANKLE_EFFORT_NM, "velocity_rad_s": ANKLE_VELOCITY_RAD_S, }, ] def arm_joints(side: str) -> list[dict]: s = side_sign(side) return [ { "name": f"shoulder_pitch_{side}", "parent": "torso", "child": f"shoulder_pod_{side}", "origin_mm": (0.0, s * SHOULDER_Y_MM, SHOULDER_PITCH_RAISE_Z_MM), "axis": Y_AXIS, "range_deg": SHOULDER_PITCH_RANGE_DEG, "effort_nm": SHOULDER_EFFORT_NM, "velocity_rad_s": SHOULDER_VELOCITY_RAD_S, }, { "name": f"shoulder_roll_{side}", "parent": f"shoulder_pod_{side}", "child": f"yaw_housing_{side}", "origin_mm": (0.0, s * SHOULDER_ROLL_Y_MM, SHOULDER_ROLL_Z_MM), "axis": X_AXIS, "range_deg": _mirror_range(SHOULDER_ROLL_LEFT_RANGE_DEG, side), "effort_nm": SHOULDER_EFFORT_NM, "velocity_rad_s": SHOULDER_VELOCITY_RAD_S, }, { "name": f"shoulder_yaw_{side}", "parent": f"yaw_housing_{side}", "child": f"bicep_{side}", "origin_mm": SHOULDER_YAW_ORIGIN_MM, "axis": Z_AXIS, "range_deg": SHOULDER_YAW_RANGE_DEG, "effort_nm": SHOULDER_EFFORT_NM, "velocity_rad_s": SHOULDER_VELOCITY_RAD_S, }, { "name": f"elbow_{side}", "parent": f"bicep_{side}", "child": f"forearm_{side}", "origin_mm": ELBOW_ORIGIN_MM, "axis": Y_AXIS, "range_deg": ELBOW_RANGE_DEG, "effort_nm": ELBOW_EFFORT_NM, "velocity_rad_s": ELBOW_VELOCITY_RAD_S, }, { "name": f"wrist_roll_{side}", "parent": f"forearm_{side}", "child": f"wrist_carrier_{side}", "origin_mm": WRIST_ROLL_ORIGIN_MM, "axis": Z_AXIS, "range_deg": WRIST_ROLL_RANGE_DEG, "effort_nm": WRIST_EFFORT_NM, "velocity_rad_s": WRIST_VELOCITY_RAD_S, }, { "name": f"wrist_pitch_{side}", "parent": f"wrist_carrier_{side}", "child": f"hand_{side}", "origin_mm": WRIST_PITCH_ORIGIN_MM, "axis": Y_AXIS, "range_deg": WRIST_PITCH_RANGE_DEG, "effort_nm": WRIST_EFFORT_NM, "velocity_rad_s": WRIST_VELOCITY_RAD_S, }, ] def all_joints() -> list[dict]: """All 27 movable joints in root-to-leaf order (legs then arms per side).""" joints = central_joints() for side in SIDES: joints.extend(leg_joints(side)) for side in SIDES: joints.extend(arm_joints(side)) return joints def all_links() -> list[str]: """All 28 link names, root first, in joint emission order.""" links = ["pelvis"] for joint in all_joints(): links.append(joint["child"]) return links # ----------------------------------------------------------------- poses def _athletic_ready_deg() -> dict[str, float]: pose = { "waist_yaw": WAIST_YAW_DEG, "neck_yaw": NECK_YAW_DEG, "neck_pitch": NECK_PITCH_DEG, } for side in SIDES: s = side_sign(side) pose.update( { f"hip_yaw_{side}": HIP_YAW_DEG, f"hip_roll_{side}": s * HIP_ROLL_ABDUCT_DEG, f"hip_pitch_{side}": HIP_PITCH_DEG, f"knee_{side}": KNEE_DEG, f"ankle_pitch_{side}": ANKLE_PITCH_DEG, f"ankle_roll_{side}": -s * HIP_ROLL_ABDUCT_DEG, f"shoulder_pitch_{side}": SHOULDER_PITCH_DEG, f"shoulder_roll_{side}": s * SHOULDER_ROLL_ABDUCT_DEG, f"shoulder_yaw_{side}": -s * SHOULDER_YAW_INTERNAL_DEG, f"elbow_{side}": ELBOW_DEG, f"wrist_roll_{side}": WRIST_ROLL_DEG, f"wrist_pitch_{side}": WRIST_PITCH_DEG, } ) return pose def _wave_right_deg() -> dict[str, float]: """Friendly right-hand wave on top of the athletic lower body. Right upper arm raised out to the side just above horizontal (shoulder roll), full external shoulder yaw so the 90 deg elbow carries the forearm vertically up, slight wrist-pitch flick, head turned toward the waving hand. """ pose = _athletic_ready_deg() pose.update( { "shoulder_pitch_right": 0.0, "shoulder_roll_right": -115.0, "shoulder_yaw_right": -90.0, "elbow_right": -90.0, "wrist_roll_right": 0.0, "wrist_pitch_right": -15.0, "neck_yaw": -20.0, "neck_pitch": -5.0, } ) return pose def _squat_deg() -> dict[str, float]: """Deep flat-footed squat: pitch sum stays zero so the soles stay level.""" pose = _athletic_ready_deg() for side in SIDES: pose.update( { f"hip_pitch_{side}": -55.0, f"knee_{side}": 85.0, f"ankle_pitch_{side}": -30.0, } ) return pose def _t_pose_deg() -> dict[str, float]: pose = {name: 0.0 for name in (joint["name"] for joint in all_joints())} pose["shoulder_roll_left"] = 90.0 pose["shoulder_roll_right"] = -90.0 return pose def named_poses_deg() -> dict[str, dict[str, float]]: """SRDF group-state poses (degrees) for the whole_body group.""" zero = {name: 0.0 for name in (joint["name"] for joint in all_joints())} return { "zero": zero, "athletic_ready": _athletic_ready_deg(), "t_pose": _t_pose_deg(), "wave_right": _wave_right_deg(), "squat": _squat_deg(), }