From a216ddf755467171bd927422450647d2be1ac4cb Mon Sep 17 00:00:00 2001 From: Ramy Wahib Date: Thu, 30 Jul 2026 23:31:16 -0400 Subject: [PATCH 1/4] isaacsim visualization --- ...nt_command_core.cpp.tmp.11367.889a3665d704 | 304 ++++++++++++++++++ .../wato_bimanual_arm/live_arm_isaacsim.py | 283 ++++++++++++++++ 2 files changed, 587 insertions(+) create mode 100644 autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 create mode 100644 autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py diff --git a/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 b/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 new file mode 100644 index 00000000..dba44b51 --- /dev/null +++ b/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 @@ -0,0 +1,304 @@ +#include "joint_command_core.hpp" + +#include +#include +#include +#include + + +// Parses one joint's YAML block into a JointConfig. +JointConfig JointCommandCore::loadJointConfig(const YAML::Node& joint_node) { + JointConfig joint; + joint.motor_id = static_cast(joint_node["can_id"].as()); + joint.lower_limit = joint_node["lower_limit"].as(); + joint.upper_limit = joint_node["upper_limit"].as(); + joint.direction = joint_node["direction"].as(); + joint.zero_offset = joint_node["zero_offset"].as(); + joint.limit_range = joint_node["limit_range"].as(); + return joint; +} + +// Loads all 6 joints for an arm side, resets safety/state to defaults. +bool JointCommandCore::loadFromYaml(const YAML::Node& config, const std::string& arm_side) { + joints_.clear(); + + if (!config[arm_side]) { + return false; + } + + const YAML::Node arm = config[arm_side]; + const std::vector> joint_paths = { + {"shoulder", "pitch"}, {"shoulder", "roll"}, {"shoulder", "yaw"}, + {"elbow", "pitch"}, {"elbow", "roll"}, {"wrist", "pitch"}, + }; + + for (const auto& [group, joint_name] : joint_paths) { + const YAML::Node joint_node = arm[group][joint_name]; + if (!joint_node) { + return false; + } + joints_.push_back(loadJointConfig(joint_node)); + } + + const bool ok = joints_.size() == 6; + if (!ok) { + return false; + } + + safety_.assign(joints_.size(), JointSafetyConfig{}); + // Seed at 0 (the assumed safe starting pose an operator positions the arm at before + // startup) and mark ready immediately, so the very first ArmPose message received is + // ALSO velocity/delta rate-limited relative to that pose, not just position-clamped. + // Previously this started false, letting the first command bypass all rate limiting + // and jump straight to its target -- visible as a sudden snap before smooth tracking. + prev_targets_.assign(joints_.size(), 0.0); + prev_velocities_.assign(joints_.size(), 0.0); + have_prev_targets_ = true; + return true; +} + +// Overwrites the assumed-0 start pose with real measured motor angles. +size_t JointCommandCore::seedPrevTargetsFromFeedback(const std::map& motor_positions) { + std::vector seeded(joints_.size(), 0.0); + size_t matched = 0; + for (size_t i = 0; i < joints_.size(); ++i) { + const auto it = motor_positions.find(static_cast(joints_[i].motor_id)); + if (it == motor_positions.end()) { + continue; // motor not reporting (e.g. unwired wrist) -> leave at 0 + } + // Inverse of applyCalibration (motor = direction * (cmd - zero_offset)): + // cmd = zero_offset + motor / direction + const double dir = + (joints_[i].direction == 0) ? 1.0 : static_cast(joints_[i].direction); + seeded[i] = joints_[i].zero_offset + it->second / dir; + ++matched; + } + prev_targets_ = std::move(seeded); + // Real feedback carries no velocity sample here, so the ramp starts from rest each time it's + // re-seeded (e.g. after a hand-move) -- safe, since starting from 0 can only under-accelerate, + // never overshoot. + prev_velocities_.assign(joints_.size(), 0.0); + have_prev_targets_ = true; + return matched; +} + +// Hard-limit clip, and arm-frame<->motor-frame conversion. +double JointCommandCore::clampAngle(double angle, const JointConfig& joint) { + if (!joint.limit_range) { + return angle; + } + return std::clamp(angle, joint.lower_limit, joint.upper_limit); +} + +double JointCommandCore::applyCalibration(double angle, const JointConfig& joint) { + return static_cast(joint.direction) * (angle - joint.zero_offset); +} + +// Applies a joint's YAML overrides on top of a base/default config. +JointSafetyConfig JointCommandCore::loadJointSafetyConfig(const YAML::Node& joint_node, + const JointSafetyConfig& base) { + JointSafetyConfig cfg = base; + if (!joint_node) { + return cfg; + } + + if (joint_node["enable_position_clamp"]) { + cfg.enable_position_clamp = joint_node["enable_position_clamp"].as(); + } + if (joint_node["enable_velocity_limit"]) { + cfg.enable_velocity_limit = joint_node["enable_velocity_limit"].as(); + } + if (joint_node["enable_delta_limit"]) { + cfg.enable_delta_limit = joint_node["enable_delta_limit"].as(); + } + if (joint_node["enable_low_pass"]) { + cfg.enable_low_pass = joint_node["enable_low_pass"].as(); + } + if (joint_node["velocity_max"]) { + cfg.velocity_max = joint_node["velocity_max"].as(); + } + if (joint_node["delta_max"]) { + cfg.delta_max = joint_node["delta_max"].as(); + } + if (joint_node["low_pass_alpha"]) { + cfg.low_pass_alpha = joint_node["low_pass_alpha"].as(); + } + cfg.low_pass_alpha = std::clamp(cfg.low_pass_alpha, 0.0, 1.0); + if (joint_node["mit_kp"]) { + cfg.mit_kp = joint_node["mit_kp"].as(); + } + if (joint_node["mit_kd"]) { + cfg.mit_kd = joint_node["mit_kd"].as(); + } + if (joint_node["enable_trapezoidal_limit"]) { + cfg.enable_trapezoidal_limit = joint_node["enable_trapezoidal_limit"].as(); + } + if (joint_node["accel_max"]) { + cfg.accel_max = joint_node["accel_max"].as(); + } + return cfg; +} + +// Resolves global defaults then per-joint overrides for all 6 joints. +bool JointCommandCore::loadSafetyFromYaml(const YAML::Node& safety_cfg, double control_rate_hz) { + if (joints_.empty()) { + return false; + } + + control_rate_hz_ = control_rate_hz; + JointSafetyConfig defaults; + if (safety_cfg["global"]) { + defaults = loadJointSafetyConfig(safety_cfg["global"], defaults); + } + + const std::vector> joint_paths = { + {"shoulder", "pitch"}, {"shoulder", "roll"}, {"shoulder", "yaw"}, + {"elbow", "pitch"}, {"elbow", "roll"}, {"wrist", "pitch"}, + }; + + safety_.assign(joints_.size(), defaults); + for (size_t i = 0; i < joint_paths.size(); ++i) { + const auto& [group, joint_name] = joint_paths[i]; + const YAML::Node joint_node = safety_cfg["joints"][group][joint_name]; + safety_[i] = loadJointSafetyConfig(joint_node, defaults); + } + return true; +} + +// Generic rate-limit-toward-target, and exponential smoothing. +double JointCommandCore::clampStep(double target, double previous, double delta_max) { + return previous + std::clamp(target - previous, -delta_max, delta_max); +} + +double JointCommandCore::applyLowPass(double target, double previous, double alpha) { + return alpha * previous + (1.0 - alpha) * target; +} + +// Accelerate/cruise/decelerate ramp onto a (possibly moving) target, no overshoot. +// Reactive trapezoidal profile, re-planned every call: accelerate at accel_max toward +// velocity_max, then decelerate at accel_max so as to land exactly on target with zero +// overshoot, regardless of whether target itself is still moving next tick. prev_vel is +// updated in place (signed, degrees/second) so the ramp's own speed carries into the next tick. +double JointCommandCore::stepTrapezoidal(double target, double prev_pos, double& prev_vel, + double velocity_max, double accel_max, double dt) { + const double error = target - prev_pos; + const double direction = (error > 0.0) ? 1.0 : (error < 0.0 ? -1.0 : 0.0); + const double accel_step = accel_max * dt; + + double desired_vel; + if (direction == 0.0 || (prev_vel != 0.0 && (prev_vel > 0.0) != (direction > 0.0))) { + // Already there, or still moving the wrong way (target reversed under us) -- brake to a + // stop before committing to the new direction, so we never yank straight through zero. + const double braked = std::abs(prev_vel) - accel_step; + desired_vel = (braked > 0.0) ? std::copysign(braked, prev_vel) : 0.0; + } else { + const double stopping_distance = (prev_vel * prev_vel) / (2.0 * accel_max); + if (std::abs(error) <= stopping_distance) { + // Close enough that cruising another tick would overshoot -- decelerate. + desired_vel = direction * std::max(0.0, std::abs(prev_vel) - accel_step); + } else { + // Free to speed up toward cruise. + desired_vel = direction * std::min(velocity_max, std::abs(prev_vel) + accel_step); + } + } + + double step = desired_vel * dt; + if (std::abs(step) >= std::abs(error)) { + // Never overshoot within a single tick; land exactly on target and stop. + step = error; + desired_vel = 0.0; + } + + prev_vel = desired_vel; + return prev_pos + step; +} + +std::vector +JointCommandCore::armPoseToMotorCmds(const common_msgs::msg::ArmPose& pose, int8_t control_type) { + if (joints_.size() != 6) { + throw std::runtime_error("JointCommandCore is not configured for 6 joints"); + } + + if (pose.shoulder.position.size() < 3 || pose.elbow.position.size() < 2 || + pose.wrist.position.size() < 1) { + throw std::runtime_error("ArmPose must contain 3 shoulder, 2 elbow, and 1 wrist positions"); + } + + const std::vector source_angles = { + pose.shoulder.position[0], pose.shoulder.position[1], pose.shoulder.position[2], + pose.elbow.position[0], pose.elbow.position[1], pose.wrist.position[0], + }; + + std::vector commands; + commands.reserve(joints_.size()); + std::vector next_targets(joints_.size(), 0.0); + + if (safety_.size() != joints_.size()) { + safety_.assign(joints_.size(), JointSafetyConfig{}); + } + if (prev_targets_.size() != joints_.size()) { + prev_targets_.assign(joints_.size(), 0.0); + have_prev_targets_ = true; + } + if (prev_velocities_.size() != joints_.size()) { + prev_velocities_.assign(joints_.size(), 0.0); + } + + for (size_t i = 0; i < joints_.size(); ++i) { + const JointSafetyConfig& safety = safety_[i]; + + double target = source_angles[i]; + if (safety.enable_position_clamp) { + target = clampAngle(target, joints_[i]); + } + + if (have_prev_targets_) { + if (safety.enable_trapezoidal_limit && control_rate_hz_ > 0.0) { + // Replaces the plain velocity clamp and low-pass below: both of those re-discount the + // ramp's own speed (a low-pass after a velocity clamp silently cuts steady-state speed + // by ~(1-alpha) -- see JointCommand.md), which is the bug this profile fixes. + target = stepTrapezoidal(target, prev_targets_[i], prev_velocities_[i], + std::abs(safety.velocity_max), std::abs(safety.accel_max), + 1.0 / control_rate_hz_); + } else if (safety.enable_velocity_limit && control_rate_hz_ > 0.0) { + const double velocity_step = std::abs(safety.velocity_max) / control_rate_hz_; + target = clampStep(target, prev_targets_[i], velocity_step); + } + if (safety.enable_delta_limit) { + target = clampStep(target, prev_targets_[i], std::abs(safety.delta_max)); + } + if (!safety.enable_trapezoidal_limit && safety.enable_low_pass) { + target = applyLowPass(target, prev_targets_[i], safety.low_pass_alpha); + } + } + + if (safety.enable_position_clamp) { + target = clampAngle(target, joints_[i]); + } + next_targets[i] = target; + + const double calibrated_deg = applyCalibration(target, joints_[i]); + + common_msgs::msg::MotorCmd cmd; + cmd.motor_id = joints_[i].motor_id; + cmd.control_type = control_type; + if (control_type == common_msgs::msg::MotorCmd::MIT_CONTROL) { + // can_node's MIT path expects position in RADIANS (CubeMars manual MIT protocol), + // unlike POSITION_LOOP's PositionDeg which is degrees -- see can_node.cpp packMitValue. + // velocity/torque feed-forward left at 0 (pure position+PD hold via kp/kd). + constexpr double kDegToRad = 3.14159265358979323846 / 180.0; + cmd.position = static_cast(calibrated_deg * kDegToRad); + cmd.velocity = 0.0f; + cmd.torque = 0.0f; + cmd.kp = static_cast(safety.mit_kp); + cmd.kd = static_cast(safety.mit_kd); + } else { + cmd.position = static_cast(calibrated_deg); + } + commands.push_back(cmd); + } + + prev_targets_ = std::move(next_targets); + have_prev_targets_ = true; + return commands; +} diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py new file mode 100644 index 00000000..ebb3f275 --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py @@ -0,0 +1,283 @@ +"""Real-time Isaac Sim visualization of the physical bimanual test stand (Isaac Lab +counterpart to live_arm_mjviser.py, same directory). + +Drives one of BIMANUAL_ARM_CFG's two joint chains directly from live +/interfacing/motorFeedback data, same --arm-side/--urdf-side split as +live_arm_mjviser.py: --arm-side picks which hardware_mapping.yaml section / real motors +to read (only "left" is wired/calibrated today); --urdf-side picks which URDF chain to +animate with that feedback. Defaults to --urdf-side right (the unsuffixed joint1..joint6 +chain), matching live_arm_mjviser.py's own validated default invocation in +ARM_BRINGUP.md (--arm-side left --urdf-side right) -- confirmed by direct observation +against the real test stand that the suffixed joint1L..joint6l chain is NOT the visually +correct one here, despite bimanual_arm_cfg.py's docstring claiming otherwise. Read-only: +never calls set_joint_position_target or publishes MotorCmd -- each tick it force-writes +joint state via write_joint_state_to_sim(), the same direct-overwrite semantics +live_arm_mjviser.py uses on data.qpos (no PD lag/interpolation, exact live mirror). It +cannot move the real arm. + +Zero position matches live_arm_mjviser.py: both compute +zero_offset + direction*motor_deg + display_offset from hardware_mapping.yaml's +calibrated zero_offset per joint. This also matches BIMANUAL_ARM_CFG's default/rest pose +for BOTH chains (see bimanual_arm_cfg.py's _load_zero_offsets_deg()), so joints with no +live feedback yet (e.g. wrist_pitch, not wired today) sit at the calibrated zero instead +of a stale Physics Inspector snapshot. task_space_real.py's own sim-zero is matched the +same way, on the OTHER chain (RIGHT_ARM_JOINTS, which it always drives for real hardware +output regardless of which chain looks right in this viewer). + +rclpy can't be imported inside env_isaaclab (its compiled extension targets the system +ROS Python, not conda's -- see udp_to_ros_bridge.py for the same constraint in the +opposite direction). So this script never imports rclpy: it reads motor feedback from a +UDP socket instead, fed by feedback_to_udp_bridge.py running under system ROS Python. + +Terminal 1 (system python, ROS sourced): + source /opt/ros/jazzy/setup.bash + source /home/rwahib/wato/humanoid/autonomy/install/setup.bash + /usr/bin/python3 autonomy/behaviour/joint_command/scripts/feedback_to_udp_bridge.py + +Terminal 2 (env_isaaclab): + conda activate env_isaaclab + cd /home/rwahib/wato/humanoid + python autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py +""" + +import argparse +import os +import socket +import struct +import sys + +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Live Isaac Sim mirror of the real bimanual arm.") +parser.add_argument("--arm-side", default="left", choices=["left", "right"], + help="hardware side: which hardware_mapping.yaml section / real motors to read " + "(only 'left' is wired/calibrated today)") +parser.add_argument("--urdf-side", default="right", choices=["left", "right"], + help="which URDF chain to drive with that feedback: 'right' -> unsuffixed " + "joint1..joint6 (default, matches live_arm_mjviser.py's validated " + "ARM_BRINGUP.md invocation), 'left' -> suffixed joint1L..joint6l " + "(the chain task_space_real.py drives for real hardware output).") +parser.add_argument("--flip", nargs="*", default=[], metavar="LABEL", + help="hardware_mapping labels whose sign to invert (e.g. shoulder_roll) -- " + "same as live_arm_mjviser.py's --flip.") +parser.add_argument("--offset", nargs="*", default=[], metavar="LABEL=DEG", + help="viewer-only constant added to a joint's displayed angle, in degrees " + "(e.g. shoulder_yaw=90) -- same as live_arm_mjviser.py's --offset. " + "Never touches real commands.") +parser.add_argument("--host", type=str, default="127.0.0.1", help="feedback_to_udp_bridge.py host") +parser.add_argument("--port", type=int, default=5006, help="feedback_to_udp_bridge.py port") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +# Import bimanual_arm_cfg from keyboard teleoperation (same robot model/zero convention +# as task_space_real.py and live_arm_mjviser.py). +_KEYBOARD_TELEOP_DIR = os.path.abspath( + os.path.join(os.path.dirname(__file__), "../../Teleop/keyboard_based_teleoperation") +) +sys.path.insert(0, _KEYBOARD_TELEOP_DIR) + +import yaml # noqa: E402 + +from bimanual_arm_cfg import ( # noqa: E402 + BIMANUAL_ARM_CFG, + _HARDWARE_MAPPING_PATH, + apply_joint_limits, + resolve_joint_name, +) +import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.assets import AssetBaseCfg # noqa: E402 +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg # noqa: E402 +from isaaclab.utils import configclass # noqa: E402 +import math # noqa: E402 + + +# hardware_mapping.yaml label -> BIMANUAL_ARM_CFG URDF joint, per side. Same dict as +# live_arm_mjviser.py's LABEL_TO_URDF_JOINT (duplicated rather than imported from that +# script, since it pulls in rclpy/mujoco/viser at import time). +LABEL_TO_URDF_JOINT = { + "left": { + "shoulder_pitch": "joint1L", + "shoulder_roll": "joint2l", + "shoulder_yaw": "joint3l", + "elbow_pitch": "joint4l", + "elbow_roll": "joint5l", + "wrist_pitch": "joint6l", + }, + "right": { + "shoulder_pitch": "joint1", + "shoulder_roll": "joint2", + "shoulder_yaw": "joint3", + "elbow_pitch": "joint4", + "elbow_roll": "joint5", + "wrist_pitch": "joint6", + }, +} + + +def load_can_id_map( + mapping_path: str, + hw_side: str, + urdf_side: str, + flip_labels: set = frozenset(), + offset_labels: dict = None, +) -> dict: + """hardware_mapping.yaml -> {can_id: {label, urdf_joint, direction, zero_offset, display_offset, + lower_limit, upper_limit}}. + + Same fields, --flip/--offset semantics, and hw_side/urdf_side split as + live_arm_mjviser.py's load_can_id_map. + """ + with open(mapping_path) as f: + config = yaml.safe_load(f)[hw_side] + + label_to_joint = LABEL_TO_URDF_JOINT[urdf_side] + can_id_map = {} + for group, joints in config.items(): + for name, cfg in joints.items(): + label = f"{group}_{name}" + urdf_joint = label_to_joint.get(label) + if urdf_joint is None: + continue + # MUST negate zero_offset together with direction, not direction alone: + # zero_offset was computed as -home_pos/direction during calibration so that + # zero_offset + direction*home_pos == 0 at the real motor's physical zero pose. + # Flipping direction alone breaks that identity and shifts the displayed pose + # by 2*zero_offset at the real zero -- negating both preserves + # joint_deg(home_pos) == 0 while correctly reversing the sense of motion + # elsewhere (verified: joint_deg_new(raw) == -joint_deg_old(raw) identically). + flip = label in flip_labels + direction = int(cfg["direction"]) * (-1 if flip else 1) + zero_offset = float(cfg["zero_offset"]) * (-1 if flip else 1) + can_id_map[int(cfg["can_id"])] = { + "label": label, + "urdf_joint": urdf_joint, + "direction": direction, + "zero_offset": zero_offset, + "display_offset": float((offset_labels or {}).get(label, 0.0)), + "lower_limit": float(cfg["lower_limit"]), + "upper_limit": float(cfg["upper_limit"]), + } + return can_id_map + + +@configclass +class BareSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)), + ) + robot = BIMANUAL_ARM_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene, + can_id_map: dict, sock: socket.socket) -> None: + robot = scene["robot"] + sim_dt = sim.get_physics_dt() + + scene.update(sim_dt) + apply_joint_limits(robot) + + name_to_id = {name: i for i, name in enumerate(robot.data.joint_names)} + urdf_joint_ids = { + cfg["urdf_joint"]: name_to_id[resolve_joint_name(robot, cfg["urdf_joint"])] + for cfg in can_id_map.values() + } + + print(f"Tracking {len(can_id_map)} motors from {_HARDWARE_MAPPING_PATH} " + f"(hardware={args_cli.arm_side} arm -> driving URDF {args_cli.urdf_side} arm):") + for can_id, cfg in sorted(can_id_map.items()): + flip = " (flipped)" if cfg["label"] in set(args_cli.flip) else "" + off = f" (offset {cfg['display_offset']:+g}°)" if cfg["display_offset"] else "" + print(f" 0x{can_id:02X} -> {cfg['label']:<14} -> {cfg['urdf_joint']}{flip}{off}") + + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + print("[INFO] Live Isaac Sim mirror running -- move the real arm to see it track here.") + + # Diagnostic only (no filtering/rejection -- this stays a raw, unfiltered mirror by + # design, same as live_arm_mjviser.py). Logs a jump's RAW inputs so a future glitch + # can be traced to its actual source: if the raw `position` printed here is already + # anomalous, it's upstream (CAN/encoder); if raw looks sane but joint_deg doesn't, + # it's a bug in this script's own transform. + _JUMP_WARN_DEG = 15.0 + _last_deg = {} + + while simulation_app.is_running(): + while True: + try: + data, _ = sock.recvfrom(1024) + except BlockingIOError: + break + if len(data) != struct.calcsize("=id"): + continue + motor_id, position = struct.unpack("=id", data) + cfg = can_id_map.get(motor_id) + if cfg is None: + continue + joint_deg = cfg["zero_offset"] + cfg["direction"] * position + cfg["display_offset"] + prev = _last_deg.get(motor_id) + if prev is not None and abs(joint_deg - prev) > _JUMP_WARN_DEG: + print(f"[WARN] {cfg['label']} (motor {motor_id}) jumped {prev:.2f}deg -> " + f"{joint_deg:.2f}deg (raw position={position:.3f}, packet_bytes={len(data)})") + _last_deg[motor_id] = joint_deg + joint_id = urdf_joint_ids[cfg["urdf_joint"]] + joint_position[0, joint_id] = math.radians(joint_deg) + + # Direct state overwrite (not set_joint_position_target): matches + # live_arm_mjviser.py's `data.qpos[...] = ...` -- an exact live mirror with no + # PD lag, since this is read-only visualization, not a commanded target. + robot.write_joint_state_to_sim(joint_position, joint_vel) + scene.write_data_to_sim() + sim.step() + scene.update(sim_dt) + + +def main() -> None: + offset_labels = {} + for item in args_cli.offset: + label, _, val = item.partition("=") + offset_labels[label.strip()] = float(val) + + can_id_map = load_can_id_map( + _HARDWARE_MAPPING_PATH, args_cli.arm_side, args_cli.urdf_side, + set(args_cli.flip), offset_labels + ) + if not can_id_map: + raise RuntimeError( + f"No joints resolved for arm_side={args_cli.arm_side!r} in {_HARDWARE_MAPPING_PATH}" + ) + + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.bind((args_cli.host, args_cli.port)) + sock.setblocking(False) + print(f"[UDP] Listening for motor feedback on {args_cli.host}:{args_cli.port} " + f"(run feedback_to_udp_bridge.py to feed this)") + + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + sim.set_camera_view([2.5, 2.5, 2.0], [0.0, 0.0, 0.8]) + + scene_cfg = BareSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + sim.reset() + + print("[INFO]: Setup complete...") + run_simulator(sim, scene, can_id_map, sock) + + +if __name__ == "__main__": + main() + simulation_app.close() From eea05217e70f6af3b560603f87f34f387b52fbc0 Mon Sep 17 00:00:00 2001 From: Ramy Wahib Date: Thu, 30 Jul 2026 23:32:37 -0400 Subject: [PATCH 2/4] removing trash --- ...nt_command_core.cpp.tmp.11367.889a3665d704 | 304 ------------------ 1 file changed, 304 deletions(-) delete mode 100644 autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 diff --git a/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 b/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 deleted file mode 100644 index dba44b51..00000000 --- a/autonomy/behaviour/joint_command/src/joint_command_core.cpp.tmp.11367.889a3665d704 +++ /dev/null @@ -1,304 +0,0 @@ -#include "joint_command_core.hpp" - -#include -#include -#include -#include - - -// Parses one joint's YAML block into a JointConfig. -JointConfig JointCommandCore::loadJointConfig(const YAML::Node& joint_node) { - JointConfig joint; - joint.motor_id = static_cast(joint_node["can_id"].as()); - joint.lower_limit = joint_node["lower_limit"].as(); - joint.upper_limit = joint_node["upper_limit"].as(); - joint.direction = joint_node["direction"].as(); - joint.zero_offset = joint_node["zero_offset"].as(); - joint.limit_range = joint_node["limit_range"].as(); - return joint; -} - -// Loads all 6 joints for an arm side, resets safety/state to defaults. -bool JointCommandCore::loadFromYaml(const YAML::Node& config, const std::string& arm_side) { - joints_.clear(); - - if (!config[arm_side]) { - return false; - } - - const YAML::Node arm = config[arm_side]; - const std::vector> joint_paths = { - {"shoulder", "pitch"}, {"shoulder", "roll"}, {"shoulder", "yaw"}, - {"elbow", "pitch"}, {"elbow", "roll"}, {"wrist", "pitch"}, - }; - - for (const auto& [group, joint_name] : joint_paths) { - const YAML::Node joint_node = arm[group][joint_name]; - if (!joint_node) { - return false; - } - joints_.push_back(loadJointConfig(joint_node)); - } - - const bool ok = joints_.size() == 6; - if (!ok) { - return false; - } - - safety_.assign(joints_.size(), JointSafetyConfig{}); - // Seed at 0 (the assumed safe starting pose an operator positions the arm at before - // startup) and mark ready immediately, so the very first ArmPose message received is - // ALSO velocity/delta rate-limited relative to that pose, not just position-clamped. - // Previously this started false, letting the first command bypass all rate limiting - // and jump straight to its target -- visible as a sudden snap before smooth tracking. - prev_targets_.assign(joints_.size(), 0.0); - prev_velocities_.assign(joints_.size(), 0.0); - have_prev_targets_ = true; - return true; -} - -// Overwrites the assumed-0 start pose with real measured motor angles. -size_t JointCommandCore::seedPrevTargetsFromFeedback(const std::map& motor_positions) { - std::vector seeded(joints_.size(), 0.0); - size_t matched = 0; - for (size_t i = 0; i < joints_.size(); ++i) { - const auto it = motor_positions.find(static_cast(joints_[i].motor_id)); - if (it == motor_positions.end()) { - continue; // motor not reporting (e.g. unwired wrist) -> leave at 0 - } - // Inverse of applyCalibration (motor = direction * (cmd - zero_offset)): - // cmd = zero_offset + motor / direction - const double dir = - (joints_[i].direction == 0) ? 1.0 : static_cast(joints_[i].direction); - seeded[i] = joints_[i].zero_offset + it->second / dir; - ++matched; - } - prev_targets_ = std::move(seeded); - // Real feedback carries no velocity sample here, so the ramp starts from rest each time it's - // re-seeded (e.g. after a hand-move) -- safe, since starting from 0 can only under-accelerate, - // never overshoot. - prev_velocities_.assign(joints_.size(), 0.0); - have_prev_targets_ = true; - return matched; -} - -// Hard-limit clip, and arm-frame<->motor-frame conversion. -double JointCommandCore::clampAngle(double angle, const JointConfig& joint) { - if (!joint.limit_range) { - return angle; - } - return std::clamp(angle, joint.lower_limit, joint.upper_limit); -} - -double JointCommandCore::applyCalibration(double angle, const JointConfig& joint) { - return static_cast(joint.direction) * (angle - joint.zero_offset); -} - -// Applies a joint's YAML overrides on top of a base/default config. -JointSafetyConfig JointCommandCore::loadJointSafetyConfig(const YAML::Node& joint_node, - const JointSafetyConfig& base) { - JointSafetyConfig cfg = base; - if (!joint_node) { - return cfg; - } - - if (joint_node["enable_position_clamp"]) { - cfg.enable_position_clamp = joint_node["enable_position_clamp"].as(); - } - if (joint_node["enable_velocity_limit"]) { - cfg.enable_velocity_limit = joint_node["enable_velocity_limit"].as(); - } - if (joint_node["enable_delta_limit"]) { - cfg.enable_delta_limit = joint_node["enable_delta_limit"].as(); - } - if (joint_node["enable_low_pass"]) { - cfg.enable_low_pass = joint_node["enable_low_pass"].as(); - } - if (joint_node["velocity_max"]) { - cfg.velocity_max = joint_node["velocity_max"].as(); - } - if (joint_node["delta_max"]) { - cfg.delta_max = joint_node["delta_max"].as(); - } - if (joint_node["low_pass_alpha"]) { - cfg.low_pass_alpha = joint_node["low_pass_alpha"].as(); - } - cfg.low_pass_alpha = std::clamp(cfg.low_pass_alpha, 0.0, 1.0); - if (joint_node["mit_kp"]) { - cfg.mit_kp = joint_node["mit_kp"].as(); - } - if (joint_node["mit_kd"]) { - cfg.mit_kd = joint_node["mit_kd"].as(); - } - if (joint_node["enable_trapezoidal_limit"]) { - cfg.enable_trapezoidal_limit = joint_node["enable_trapezoidal_limit"].as(); - } - if (joint_node["accel_max"]) { - cfg.accel_max = joint_node["accel_max"].as(); - } - return cfg; -} - -// Resolves global defaults then per-joint overrides for all 6 joints. -bool JointCommandCore::loadSafetyFromYaml(const YAML::Node& safety_cfg, double control_rate_hz) { - if (joints_.empty()) { - return false; - } - - control_rate_hz_ = control_rate_hz; - JointSafetyConfig defaults; - if (safety_cfg["global"]) { - defaults = loadJointSafetyConfig(safety_cfg["global"], defaults); - } - - const std::vector> joint_paths = { - {"shoulder", "pitch"}, {"shoulder", "roll"}, {"shoulder", "yaw"}, - {"elbow", "pitch"}, {"elbow", "roll"}, {"wrist", "pitch"}, - }; - - safety_.assign(joints_.size(), defaults); - for (size_t i = 0; i < joint_paths.size(); ++i) { - const auto& [group, joint_name] = joint_paths[i]; - const YAML::Node joint_node = safety_cfg["joints"][group][joint_name]; - safety_[i] = loadJointSafetyConfig(joint_node, defaults); - } - return true; -} - -// Generic rate-limit-toward-target, and exponential smoothing. -double JointCommandCore::clampStep(double target, double previous, double delta_max) { - return previous + std::clamp(target - previous, -delta_max, delta_max); -} - -double JointCommandCore::applyLowPass(double target, double previous, double alpha) { - return alpha * previous + (1.0 - alpha) * target; -} - -// Accelerate/cruise/decelerate ramp onto a (possibly moving) target, no overshoot. -// Reactive trapezoidal profile, re-planned every call: accelerate at accel_max toward -// velocity_max, then decelerate at accel_max so as to land exactly on target with zero -// overshoot, regardless of whether target itself is still moving next tick. prev_vel is -// updated in place (signed, degrees/second) so the ramp's own speed carries into the next tick. -double JointCommandCore::stepTrapezoidal(double target, double prev_pos, double& prev_vel, - double velocity_max, double accel_max, double dt) { - const double error = target - prev_pos; - const double direction = (error > 0.0) ? 1.0 : (error < 0.0 ? -1.0 : 0.0); - const double accel_step = accel_max * dt; - - double desired_vel; - if (direction == 0.0 || (prev_vel != 0.0 && (prev_vel > 0.0) != (direction > 0.0))) { - // Already there, or still moving the wrong way (target reversed under us) -- brake to a - // stop before committing to the new direction, so we never yank straight through zero. - const double braked = std::abs(prev_vel) - accel_step; - desired_vel = (braked > 0.0) ? std::copysign(braked, prev_vel) : 0.0; - } else { - const double stopping_distance = (prev_vel * prev_vel) / (2.0 * accel_max); - if (std::abs(error) <= stopping_distance) { - // Close enough that cruising another tick would overshoot -- decelerate. - desired_vel = direction * std::max(0.0, std::abs(prev_vel) - accel_step); - } else { - // Free to speed up toward cruise. - desired_vel = direction * std::min(velocity_max, std::abs(prev_vel) + accel_step); - } - } - - double step = desired_vel * dt; - if (std::abs(step) >= std::abs(error)) { - // Never overshoot within a single tick; land exactly on target and stop. - step = error; - desired_vel = 0.0; - } - - prev_vel = desired_vel; - return prev_pos + step; -} - -std::vector -JointCommandCore::armPoseToMotorCmds(const common_msgs::msg::ArmPose& pose, int8_t control_type) { - if (joints_.size() != 6) { - throw std::runtime_error("JointCommandCore is not configured for 6 joints"); - } - - if (pose.shoulder.position.size() < 3 || pose.elbow.position.size() < 2 || - pose.wrist.position.size() < 1) { - throw std::runtime_error("ArmPose must contain 3 shoulder, 2 elbow, and 1 wrist positions"); - } - - const std::vector source_angles = { - pose.shoulder.position[0], pose.shoulder.position[1], pose.shoulder.position[2], - pose.elbow.position[0], pose.elbow.position[1], pose.wrist.position[0], - }; - - std::vector commands; - commands.reserve(joints_.size()); - std::vector next_targets(joints_.size(), 0.0); - - if (safety_.size() != joints_.size()) { - safety_.assign(joints_.size(), JointSafetyConfig{}); - } - if (prev_targets_.size() != joints_.size()) { - prev_targets_.assign(joints_.size(), 0.0); - have_prev_targets_ = true; - } - if (prev_velocities_.size() != joints_.size()) { - prev_velocities_.assign(joints_.size(), 0.0); - } - - for (size_t i = 0; i < joints_.size(); ++i) { - const JointSafetyConfig& safety = safety_[i]; - - double target = source_angles[i]; - if (safety.enable_position_clamp) { - target = clampAngle(target, joints_[i]); - } - - if (have_prev_targets_) { - if (safety.enable_trapezoidal_limit && control_rate_hz_ > 0.0) { - // Replaces the plain velocity clamp and low-pass below: both of those re-discount the - // ramp's own speed (a low-pass after a velocity clamp silently cuts steady-state speed - // by ~(1-alpha) -- see JointCommand.md), which is the bug this profile fixes. - target = stepTrapezoidal(target, prev_targets_[i], prev_velocities_[i], - std::abs(safety.velocity_max), std::abs(safety.accel_max), - 1.0 / control_rate_hz_); - } else if (safety.enable_velocity_limit && control_rate_hz_ > 0.0) { - const double velocity_step = std::abs(safety.velocity_max) / control_rate_hz_; - target = clampStep(target, prev_targets_[i], velocity_step); - } - if (safety.enable_delta_limit) { - target = clampStep(target, prev_targets_[i], std::abs(safety.delta_max)); - } - if (!safety.enable_trapezoidal_limit && safety.enable_low_pass) { - target = applyLowPass(target, prev_targets_[i], safety.low_pass_alpha); - } - } - - if (safety.enable_position_clamp) { - target = clampAngle(target, joints_[i]); - } - next_targets[i] = target; - - const double calibrated_deg = applyCalibration(target, joints_[i]); - - common_msgs::msg::MotorCmd cmd; - cmd.motor_id = joints_[i].motor_id; - cmd.control_type = control_type; - if (control_type == common_msgs::msg::MotorCmd::MIT_CONTROL) { - // can_node's MIT path expects position in RADIANS (CubeMars manual MIT protocol), - // unlike POSITION_LOOP's PositionDeg which is degrees -- see can_node.cpp packMitValue. - // velocity/torque feed-forward left at 0 (pure position+PD hold via kp/kd). - constexpr double kDegToRad = 3.14159265358979323846 / 180.0; - cmd.position = static_cast(calibrated_deg * kDegToRad); - cmd.velocity = 0.0f; - cmd.torque = 0.0f; - cmd.kp = static_cast(safety.mit_kp); - cmd.kd = static_cast(safety.mit_kd); - } else { - cmd.position = static_cast(calibrated_deg); - } - commands.push_back(cmd); - } - - prev_targets_ = std::move(next_targets); - have_prev_targets_ = true; - return commands; -} From 60e59d6abf3f0bc2dfedfac47b541f12fb9d4a09 Mon Sep 17 00:00:00 2001 From: Ramy Wahib Date: Fri, 31 Jul 2026 11:16:33 -0400 Subject: [PATCH 3/4] polishing --- .../wato_bimanual_arm/live_arm_isaacsim.py | 57 +++++++------------ 1 file changed, 22 insertions(+), 35 deletions(-) diff --git a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py index ebb3f275..0a7be5f1 100644 --- a/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py +++ b/autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py @@ -1,33 +1,20 @@ -"""Real-time Isaac Sim visualization of the physical bimanual test stand (Isaac Lab +"""Real-time Isaac Sim viewer for the physical bimanual test stand (Isaac Lab counterpart to live_arm_mjviser.py, same directory). -Drives one of BIMANUAL_ARM_CFG's two joint chains directly from live -/interfacing/motorFeedback data, same --arm-side/--urdf-side split as -live_arm_mjviser.py: --arm-side picks which hardware_mapping.yaml section / real motors -to read (only "left" is wired/calibrated today); --urdf-side picks which URDF chain to -animate with that feedback. Defaults to --urdf-side right (the unsuffixed joint1..joint6 -chain), matching live_arm_mjviser.py's own validated default invocation in -ARM_BRINGUP.md (--arm-side left --urdf-side right) -- confirmed by direct observation -against the real test stand that the suffixed joint1L..joint6l chain is NOT the visually -correct one here, despite bimanual_arm_cfg.py's docstring claiming otherwise. Read-only: -never calls set_joint_position_target or publishes MotorCmd -- each tick it force-writes -joint state via write_joint_state_to_sim(), the same direct-overwrite semantics -live_arm_mjviser.py uses on data.qpos (no PD lag/interpolation, exact live mirror). It -cannot move the real arm. - -Zero position matches live_arm_mjviser.py: both compute -zero_offset + direction*motor_deg + display_offset from hardware_mapping.yaml's -calibrated zero_offset per joint. This also matches BIMANUAL_ARM_CFG's default/rest pose -for BOTH chains (see bimanual_arm_cfg.py's _load_zero_offsets_deg()), so joints with no -live feedback yet (e.g. wrist_pitch, not wired today) sit at the calibrated zero instead -of a stale Physics Inspector snapshot. task_space_real.py's own sim-zero is matched the -same way, on the OTHER chain (RIGHT_ARM_JOINTS, which it always drives for real hardware -output regardless of which chain looks right in this viewer). - -rclpy can't be imported inside env_isaaclab (its compiled extension targets the system -ROS Python, not conda's -- see udp_to_ros_bridge.py for the same constraint in the -opposite direction). So this script never imports rclpy: it reads motor feedback from a -UDP socket instead, fed by feedback_to_udp_bridge.py running under system ROS Python. +Drives one of BIMANUAL_ARM_CFG's two joint chains from live /interfacing/motorFeedback. +--arm-side picks which hardware_mapping.yaml motors to read (only "left" is +wired/calibrated today); --urdf-side picks which URDF chain to animate. Defaults to +--urdf-side right (unsuffixed joint1..joint6) -- confirmed against the real test stand +that this is the visually correct chain, NOT joint1L..joint6l as bimanual_arm_cfg.py's +docstring claims. Read-only: force-writes joint state via write_joint_state_to_sim() +each tick (no PD lag, exact mirror); never commands the real arm. + +Zero position: zero_offset + direction*motor_deg + display_offset per joint, matching +live_arm_mjviser.py and BIMANUAL_ARM_CFG's rest pose, so unwired joints (e.g. +wrist_pitch) sit at calibrated zero rather than a stale snapshot. + +For better compatibility with everyone's system, feedback comes via UDP from feedback_to_udp_bridge.py (system ROS +Python) instead of a direct subscription. Terminal 1 (system python, ROS sourced): source /opt/ros/jazzy/setup.bash @@ -40,6 +27,7 @@ python autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/live_arm_isaacsim.py """ + import argparse import os import socket @@ -141,13 +129,12 @@ def load_can_id_map( urdf_joint = label_to_joint.get(label) if urdf_joint is None: continue - # MUST negate zero_offset together with direction, not direction alone: - # zero_offset was computed as -home_pos/direction during calibration so that - # zero_offset + direction*home_pos == 0 at the real motor's physical zero pose. - # Flipping direction alone breaks that identity and shifts the displayed pose - # by 2*zero_offset at the real zero -- negating both preserves - # joint_deg(home_pos) == 0 while correctly reversing the sense of motion - # elsewhere (verified: joint_deg_new(raw) == -joint_deg_old(raw) identically). + # Must negate zero_offset together with direction, not direction alone: zero_offset was + # derived as -home_pos/direction so zero_offset + direction*home_pos == 0 at the real + # zero pose. Flipping direction alone breaks that and shifts the zero pose by + # 2*zero_offset. Negating both preserves joint_deg(home_pos)==0 while correctly + # reversing motion sense (verified: joint_deg_new(raw) == -joint_deg_old(raw)). + flip = label in flip_labels direction = int(cfg["direction"]) * (-1 if flip else 1) zero_offset = float(cfg["zero_offset"]) * (-1 if flip else 1) From a5e19a96754c4216d9741d71d2c7cb6b2728ffb6 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Fri, 31 Jul 2026 00:18:34 -0400 Subject: [PATCH 4/4] Fix clang-format CI issue in foc.cpp (#170) Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> --- embedded/STM32/app/src/foc.cpp | 453 ++++++++++++++------------------- 1 file changed, 196 insertions(+), 257 deletions(-) diff --git a/embedded/STM32/app/src/foc.cpp b/embedded/STM32/app/src/foc.cpp index 84185ffe..cd17b39a 100644 --- a/embedded/STM32/app/src/foc.cpp +++ b/embedded/STM32/app/src/foc.cpp @@ -1,5 +1,5 @@ -/** For setup +/** For setup * - initialize PWM * - initialize angle/position encoder (MT6835) * - intialize current sensing @@ -7,15 +7,15 @@ * - intialize motor configurations (PID, mode) + motor.initFOC() * - intialize CANFD and enable RX ISR * -*/ + */ -#include -#include #include "SimpleFOCDrivers.h" #include "encoders/mt6835/MagneticSensorMT6835.h" #include "stm32g4xx_hal.h" +#include +#include -BLDCMotor motor = BLDCMotor(7); +BLDCMotor motor = BLDCMotor(7); bool setup_success = true; float target_voltage = 0.0f; @@ -23,21 +23,14 @@ float target_voltage = 0.0f; #define M0_IN1_PIN PA_8 #define M0_IN2_PIN PA_9 #define M0_IN3_PIN PA_10 -#define M0_EN_PIN PB_5 +#define M0_EN_PIN PB_5 #define SENSOR_CS_PIN PB_6 BLDCDriver3PWM driver = BLDCDriver3PWM(M0_IN1_PIN, M0_IN2_PIN, M0_IN3_PIN, M0_EN_PIN); -SPISettings mt6835_spi_settings( - 1000000, - MT6835_BITORDER, - SPI_MODE3 -); +SPISettings mt6835_spi_settings(1000000, MT6835_BITORDER, SPI_MODE3); -MagneticSensorMT6835 sensor( - SENSOR_CS_PIN, - mt6835_spi_settings -); +MagneticSensorMT6835 sensor(SENSOR_CS_PIN, mt6835_spi_settings); FDCAN_HandleTypeDef hfdcan1; @@ -48,247 +41,208 @@ struct can_rx_message { FDCAN_RxHeaderTypeDef header; uint8_t data[64]; - }; - can_rx_message can_rx_queue[CAN_RX_QUEUE_SIZE]; volatile uint8_t can_rx_head = 0; volatile uint8_t can_rx_tail = 0; volatile uint32_t can_rx_dropped = 0; - bool initCANFD() { - /* - * Select PCLK1 as the FDCAN clock. - * - * The bit timings below assume PCLK1/FDCAN clock = 170 MHz. - */ - RCC_PeriphCLKInitTypeDef peripheral_clock = {}; - - peripheral_clock.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - peripheral_clock.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1; + /* + * Select PCLK1 as the FDCAN clock. + * + * The bit timings below assume PCLK1/FDCAN clock = 170 MHz. + */ + RCC_PeriphCLKInitTypeDef peripheral_clock = {}; + + peripheral_clock.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; + peripheral_clock.FdcanClockSelection = RCC_FDCANCLKSOURCE_PCLK1; + + if (HAL_RCCEx_PeriphCLKConfig(&peripheral_clock) != HAL_OK) { + return false; + } - if (HAL_RCCEx_PeriphCLKConfig(&peripheral_clock) != HAL_OK) - { - return false; - } + /* Enable peripheral and GPIO clocks. */ + __HAL_RCC_FDCAN_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + + /* + * PA11: FDCAN1_RX + * PA12: FDCAN1_TX + */ + GPIO_InitTypeDef gpio = {}; + + gpio.Pin = GPIO_PIN_11 | GPIO_PIN_12; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = GPIO_AF9_FDCAN1; + + HAL_GPIO_Init(GPIOA, &gpio); + + /* Configure the FDCAN peripheral. */ + hfdcan1.Instance = FDCAN1; + + hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1; + hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS; + hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + + hfdcan1.Init.AutoRetransmission = ENABLE; + hfdcan1.Init.TransmitPause = DISABLE; + hfdcan1.Init.ProtocolException = DISABLE; + + /* + * Nominal/arbitration rate: + * + * 170 MHz / [10 × (1 + 27 + 6)] + * = 500 kbit/s + */ + hfdcan1.Init.NominalPrescaler = 10; + hfdcan1.Init.NominalSyncJumpWidth = 6; + hfdcan1.Init.NominalTimeSeg1 = 27; + hfdcan1.Init.NominalTimeSeg2 = 6; + + /* + * Data-phase rate: + * + * 170 MHz / [5 × (1 + 13 + 3)] + * = 2 Mbit/s + */ + hfdcan1.Init.DataPrescaler = 5; + hfdcan1.Init.DataSyncJumpWidth = 3; + hfdcan1.Init.DataTimeSeg1 = 13; + hfdcan1.Init.DataTimeSeg2 = 3; + + hfdcan1.Init.StdFiltersNbr = 1; + hfdcan1.Init.ExtFiltersNbr = 0; + hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; + + if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) { + return false; + } - /* Enable peripheral and GPIO clocks. */ - __HAL_RCC_FDCAN_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); + /* + * Accept all 11-bit standard identifiers. + * + * Mask = 0 means no identifier bits need to match. + */ + FDCAN_FilterTypeDef filter = {}; + + filter.IdType = FDCAN_STANDARD_ID; + filter.FilterIndex = 0; + filter.FilterType = FDCAN_FILTER_MASK; + filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; + filter.FilterID1 = 0x000; + filter.FilterID2 = 0x000; + + if (HAL_FDCAN_ConfigFilter(&hfdcan1, &filter) != HAL_OK) { + return false; + } - /* - * PA11: FDCAN1_RX - * PA12: FDCAN1_TX - */ - GPIO_InitTypeDef gpio = {}; + /* + * Reject nonmatching messages and remote frames. + * All standard data frames match the filter above. + */ + if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_REJECT_REMOTE, + FDCAN_REJECT_REMOTE) != HAL_OK) { + return false; + } - gpio.Pin = GPIO_PIN_11 | GPIO_PIN_12; - gpio.Mode = GPIO_MODE_AF_PP; - gpio.Pull = GPIO_NOPULL; - gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - gpio.Alternate = GPIO_AF9_FDCAN1; + /* Enable RX FIFO 0 new-message notification. */ + if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK) { + return false; + } - HAL_GPIO_Init(GPIOA, &gpio); + /* + * Recommended when transmitting FD frames using BRS. + * TDC offset = DataPrescaler × DataTimeSeg1 = 5 × 13. + */ + if (HAL_FDCAN_ConfigTxDelayCompensation(&hfdcan1, 65, 0) != HAL_OK) { + return false; + } - /* Configure the FDCAN peripheral. */ - hfdcan1.Instance = FDCAN1; + if (HAL_FDCAN_EnableTxDelayCompensation(&hfdcan1) != HAL_OK) { + return false; + } - hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1; - hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS; - hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + /* + * Enable the interrupt in the ARM NVIC. + * Priority 5 keeps it below very high-priority control interrupts. + */ + HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); - hfdcan1.Init.AutoRetransmission = ENABLE; - hfdcan1.Init.TransmitPause = DISABLE; - hfdcan1.Init.ProtocolException = DISABLE; + if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { + return false; + } - /* - * Nominal/arbitration rate: - * - * 170 MHz / [10 × (1 + 27 + 6)] - * = 500 kbit/s - */ - hfdcan1.Init.NominalPrescaler = 10; - hfdcan1.Init.NominalSyncJumpWidth = 6; - hfdcan1.Init.NominalTimeSeg1 = 27; - hfdcan1.Init.NominalTimeSeg2 = 6; + return true; +} - /* - * Data-phase rate: - * - * 170 MHz / [5 × (1 + 13 + 3)] - * = 2 Mbit/s - */ - hfdcan1.Init.DataPrescaler = 5; - hfdcan1.Init.DataSyncJumpWidth = 3; - hfdcan1.Init.DataTimeSeg1 = 13; - hfdcan1.Init.DataTimeSeg2 = 3; - - hfdcan1.Init.StdFiltersNbr = 1; - hfdcan1.Init.ExtFiltersNbr = 0; - hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; - - if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) - { - return false; - } +extern "C" void FDCAN1_IT0_IRQHandler() { + HAL_FDCAN_IRQHandler(&hfdcan1); +} - /* - * Accept all 11-bit standard identifiers. - * - * Mask = 0 means no identifier bits need to match. - */ - FDCAN_FilterTypeDef filter = {}; - - filter.IdType = FDCAN_STANDARD_ID; - filter.FilterIndex = 0; - filter.FilterType = FDCAN_FILTER_MASK; - filter.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; - filter.FilterID1 = 0x000; - filter.FilterID2 = 0x000; - - if (HAL_FDCAN_ConfigFilter(&hfdcan1, &filter) != HAL_OK) - { - return false; - } +extern "C" void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* hfdcan, + uint32_t rx_fifo0_interrupts) { + if (hfdcan->Instance != FDCAN1) { + return; + } - /* - * Reject nonmatching messages and remote frames. - * All standard data frames match the filter above. - */ - if (HAL_FDCAN_ConfigGlobalFilter( - &hfdcan1, - FDCAN_REJECT, - FDCAN_REJECT, - FDCAN_REJECT_REMOTE, - FDCAN_REJECT_REMOTE) != HAL_OK) - { - return false; - } + if ((rx_fifo0_interrupts & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U) { + return; + } - /* Enable RX FIFO 0 new-message notification. */ - if (HAL_FDCAN_ActivateNotification( - &hfdcan1, - FDCAN_IT_RX_FIFO0_NEW_MESSAGE, - 0) != HAL_OK) - { - return false; - } + /* + * Drain all currently available messages from hardware FIFO 0. + */ + while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0U) { + const uint8_t head = can_rx_head; + const uint8_t next = (head + 1U) & CAN_RX_QUEUE_MASK; /* - * Recommended when transmitting FD frames using BRS. - * TDC offset = DataPrescaler × DataTimeSeg1 = 5 × 13. + * Read into the current empty ring-buffer entry. */ - if (HAL_FDCAN_ConfigTxDelayCompensation( - &hfdcan1, - 65, - 0) != HAL_OK) - { - return false; - } - - if (HAL_FDCAN_EnableTxDelayCompensation(&hfdcan1) != HAL_OK) - { - return false; + if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &can_rx_queue[head].header, + can_rx_queue[head].data) != HAL_OK) { + break; } /* - * Enable the interrupt in the ARM NVIC. - * Priority 5 keeps it below very high-priority control interrupts. + * If next == tail, the software queue is full. + * The hardware message has still been drained, but it is discarded. */ - HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 5, 0); - HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); - - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) - { - return false; - } - - return true; -} - - -extern "C" void FDCAN1_IT0_IRQHandler() -{ - HAL_FDCAN_IRQHandler(&hfdcan1); -} - -extern "C" void HAL_FDCAN_RxFifo0Callback( - FDCAN_HandleTypeDef *hfdcan, - uint32_t rx_fifo0_interrupts) -{ - if (hfdcan->Instance != FDCAN1) - { - return; - } - - if ((rx_fifo0_interrupts & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) == 0U) - { - return; + if (next == can_rx_tail) { + can_rx_dropped++; + continue; } /* - * Drain all currently available messages from hardware FIFO 0. + * Ensure message data is written before publishing the new head. */ - while (HAL_FDCAN_GetRxFifoFillLevel( - hfdcan, - FDCAN_RX_FIFO0) > 0U) - { - const uint8_t head = can_rx_head; - const uint8_t next = - (head + 1U) & CAN_RX_QUEUE_MASK; - - /* - * Read into the current empty ring-buffer entry. - */ - if (HAL_FDCAN_GetRxMessage( - hfdcan, - FDCAN_RX_FIFO0, - &can_rx_queue[head].header, - can_rx_queue[head].data) != HAL_OK) - { - break; - } - - /* - * If next == tail, the software queue is full. - * The hardware message has still been drained, but it is discarded. - */ - if (next == can_rx_tail) - { - can_rx_dropped++; - continue; - } - - /* - * Ensure message data is written before publishing the new head. - */ - __DMB(); - can_rx_head = next; - } + __DMB(); + can_rx_head = next; + } } -bool popCANMessage(can_rx_message &message) -{ - const uint8_t tail = can_rx_tail; +bool popCANMessage(can_rx_message& message) { + const uint8_t tail = can_rx_tail; - if (tail == can_rx_head) - { - return false; - } + if (tail == can_rx_head) { + return false; + } - message = can_rx_queue[tail]; + message = can_rx_queue[tail]; - __DMB(); + __DMB(); - can_rx_tail = (tail + 1U) & CAN_RX_QUEUE_MASK; + can_rx_tail = (tail + 1U) & CAN_RX_QUEUE_MASK; - return true; + return true; } - /* * Example protocol: * @@ -302,52 +256,40 @@ bool popCANMessage(can_rx_message &message) * 1000 -> +1.000 V * -500 -> -0.500 V */ -void processCANMessage(const can_rx_message &message) -{ - if (message.header.IdType != FDCAN_STANDARD_ID) - { - return; - } +void processCANMessage(const can_rx_message& message) { + if (message.header.IdType != FDCAN_STANDARD_ID) { + return; + } - if (message.header.Identifier != 0x100) - { - return; - } + if (message.header.Identifier != 0x100) { + return; + } - if (message.header.DataLength != FDCAN_DLC_BYTES_2) - { - return; - } + if (message.header.DataLength != FDCAN_DLC_BYTES_2) { + return; + } - int16_t target_millivolts = - static_cast( - static_cast(message.data[0]) | - (static_cast(message.data[1]) << 8U) - ); + int16_t target_millivolts = static_cast(static_cast(message.data[0]) | + (static_cast(message.data[1]) << 8U)); - float requested_voltage = - static_cast(target_millivolts) / 1000.0f; + float requested_voltage = static_cast(target_millivolts) / 1000.0f; - /* Clamp the command to the configured safety limit. */ - if (requested_voltage > motor.voltage_limit) - { - requested_voltage = motor.voltage_limit; - } - else if (requested_voltage < -motor.voltage_limit) - { - requested_voltage = -motor.voltage_limit; - } + /* Clamp the command to the configured safety limit. */ + if (requested_voltage > motor.voltage_limit) { + requested_voltage = motor.voltage_limit; + } else if (requested_voltage < -motor.voltage_limit) { + requested_voltage = -motor.voltage_limit; + } - target_voltage = requested_voltage; + target_voltage = requested_voltage; } -void setup() -{ +void setup() { Serial.begin(115200); SimpleFOCDebug::enable(&Serial); - + // need to check with motor driver specs driver.voltage_power_supply = 12.0; // used for initial testing @@ -355,7 +297,7 @@ void setup() // might need to be changed driver.pwm_frequency = 20000; - if (!driver.init()){ + if (!driver.init()) { Serial.println("Driver init failed!"); setup_success = false; return; @@ -372,7 +314,7 @@ void setup() motor.torque_controller = TorqueControlType::voltage; motor.init(); - if (!motor.initFOC()){ + if (!motor.initFOC()) { Serial.println("Motor init failed!"); setup_success = false; return; @@ -384,27 +326,24 @@ void setup() setup_success = false; return; } - + Serial.println("PWM, driver, and motor initialized."); delay(1000); - } - /** For loop * -call loopFOC, move * -check ringbuffer for messages * -send telemetry data back - * + * */ -void loop() -{ +void loop() { if (!setup_success) { return; } can_rx_message message; - if (popCANMessage(message)){ + if (popCANMessage(message)) { processCANMessage(message); }