Build a Humanoid Robot Software Stack in 6 Months

# Ubuntu 24.04 setup
sudo apt install ros-humble-desktop ros-humble-moveit \
  ros-humble-gazebo-ros2-control python3-colcon-common-extensions

# Create workspace
mkdir -p ~/humanoid_ws/src
cd ~/humanoid_ws
colcon build
source install/setup.bash

<!-- humanoid.urdf.xacro -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="humanoid">
  
  <!-- Base parameters -->
  <xacro:property name="torso_height" value="0.8"/>
  <xacro:property name="leg_length" value="0.9"/>
  
  <!-- Torso -->
  <link name="base_link">
    <inertial>
      <mass value="15.0"/>
      <inertia ixx="0.5" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.3 0.4 ${torso_height}"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.3 0.4 ${torso_height}"/>
      </geometry>
    </collision>
  </link>
  
  <!-- Hip joint (ball joint as 3 revolute) -->
  <joint name="left_hip_yaw" type="revolute">
    <parent link="base_link"/>
    <child link="left_hip_roll_link"/>
    <origin xyz="0 0.15 -0.4" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="100" velocity="2.0" lower="-0.5" upper="0.5"/>
  </joint>
  
  <!-- Add more joints: hip_roll, hip_pitch, knee, ankle... -->
  
</robot>

# Install Gazebo with physics plugin
sudo apt install ros-humble-gazebo-ros-pkgs

# Launch simulation
ros2 launch humanoid_description gazebo.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg_share = get_package_share_directory('humanoid_description')
    urdf_file = os.path.join(pkg_share, 'urdf', 'humanoid.urdf.xacro')
    
    # Process xacro to URDF
    robot_desc = Command(['xacro ', urdf_file])
    
    return LaunchDescription([
        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            arguments=['-entity', 'humanoid', '-topic', 'robot_description'],
        ),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': robot_desc}]
        ),
    ])

// ik_solver.cpp
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>

class HumanoidIK {
private:
    KDL::Chain leg_chain_;
    std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver_;
    
public:
    HumanoidIK() {
        // Build kinematic chain for leg (hip -> ankle)
        leg_chain_.addSegment(KDL::Segment(
            KDL::Joint(KDL::Joint::RotZ),  // Hip yaw
            KDL::Frame::DH(0, M_PI_2, 0, 0)
        ));
        // Add more segments...
        
        // Levenberg-Marquardt solver for numerical IK
        ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_LMA>(leg_chain_);
    }
    
    bool solve(const KDL::Frame& target_pose, KDL::JntArray& joint_positions) {
        KDL::JntArray seed(leg_chain_.getNrOfJoints());
        // Use current joint positions as seed for faster convergence
        
        int result = ik_solver_->CartToJnt(seed, target_pose, joint_positions);
        return result >= 0;  // Success if non-negative
    }
};

# zmp_controller.py
import numpy as np
from scipy.linalg import solve_continuous_are

class ZMPBalanceController:
    def __init__(self, height=1.0, gravity=9.81):
        self.h = height  # COM height
        self.g = gravity
        
        # LQR gains for inverted pendulum model
        A = np.array([[0, 1], [self.g/self.h, 0]])
        B = np.array([[0], [1]])
        Q = np.diag([100, 1])  # Penalize position error more than velocity
        R = np.array([[1]])
        
        # Solve algebraic Riccati equation
        P = solve_continuous_are(A, B, Q, R)
        self.K = np.linalg.inv(R) @ B.T @ P
        
    def compute_zmp_force(self, com_position, com_velocity, desired_zmp):
        """
        Returns required ground reaction force to achieve desired ZMP.
        
        ZMP equation: p = x - (h/g) * x_ddot
        where p = ZMP position, x = COM position, h = height
        """
        state = np.array([com_position - desired_zmp, com_velocity])
        control = -self.K @ state
        
        # Convert to ankle torque
        ankle_torque = control[0] * self.h
        return ankle_torque
    
    def is_stable(self, zmp_position, support_polygon):
        """Check if ZMP is inside foot support polygon."""
        # Simplified: check if within foot bounds
        return (support_polygon[0] < zmp_position < support_polygon[1])

# Usage in control loop
controller = ZMPBalanceController(height=1.0)

def balance_callback(imu_data, joint_states):
    com_pos = estimate_com_from_joints(joint_states)
    com_vel = estimate_com_velocity(imu_data)
    
    desired_zmp = [0.0, 0.0]  # Center of foot
    torque = controller.compute_zmp_force(com_pos[0], com_vel[0], desired_zmp[0])
    
    publish_ankle_torque(torque)

// whole_body_controller.rs
use nalgebra as na;

pub struct WholeBodyController {
    joint_limits: Vec<(f64, f64)>,
    max_joint_velocity: f64,
    control_rate: f64,  // 100 Hz typical
}

impl WholeBodyController {
    pub fn compute_joint_commands(
        &self,
        target_ee_pose: &na::Isometry3<f64>,  // End-effector pose
        current_joints: &[f64],
        balance_constraint: &BalanceConstraint,
    ) -> Vec<f64> {
        // 1. Solve IK for target pose
        let mut desired_joints = self.inverse_kinematics(target_ee_pose);
        
        // 2. Apply balance constraints (adjust hip/ankle to maintain ZMP)
        self.apply_balance_adjustment(&mut desired_joints, balance_constraint);
        
        // 3. Enforce joint limits and velocity limits
        for (i, joint) in desired_joints.iter_mut().enumerate() {
            let (min, max) = self.joint_limits[i];
            *joint = joint.clamp(min, max);
            
            // Velocity limiting
            let max_delta = self.max_joint_velocity / self.control_rate;
            let delta = *joint - current_joints[i];
            *joint = current_joints[i] + delta.clamp(-max_delta, max_delta);
        }
        
        desired_joints
    }
    
    fn apply_balance_adjustment(
        &self,
        joints: &mut [f64],
        balance: &BalanceConstraint
    ) {
        // Adjust hip and ankle joints to satisfy ZMP constraint
        // This is a simplified null-space projection
        let zmp_error = balance.desired_zmp - balance.current_zmp;
        joints[HIP_PITCH] += 0.1 * zmp_error;  // Proportional adjustment
        joints[ANKLE_PITCH] -= 0.05 * zmp_error;
    }
}

# vision_node.py
import torch
from sam2.build_sam import build_sam2
from sam2.sam2_image_predictor import SAM2ImagePredictor
import cv2

class VisionPipeline:
    def __init__(self):
        self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
        
        # Load SAM2 model (Hiera-L backbone)
        checkpoint = "./checkpoints/sam2_hiera_large.pt"
        config = "sam2_hiera_l.yaml"
        self.predictor = SAM2ImagePredictor(build_sam2(config, checkpoint))
        
        # Depth estimation model
        self.depth_model = torch.hub.load("intel-isl/MiDaS", "DPT_Large")
        self.depth_model.to(self.device).eval()
        
    def process_frame(self, rgb_image):
        """Returns object masks and 3D positions."""
        # 1. Get object masks from SAM2
        self.predictor.set_image(rgb_image)
        
        # Detect objects (you'd use YOLO/DINO for bounding boxes first)
        boxes = self.detect_objects(rgb_image)  # Returns [[x1,y1,x2,y2], ...]
        
        masks, scores, _ = self.predictor.predict(
            box=boxes,
            multimask_output=False
        )
        
        # 2. Estimate depth
        depth_map = self.estimate_depth(rgb_image)
        
        # 3. Get 3D position of each object
        objects_3d = []
        for i, mask in enumerate(masks):
            # Get median depth in mask region
            masked_depth = depth_map[mask[0]]
            depth = torch.median(masked_depth).item()
            
            # Convert to 3D using camera intrinsics
            center_x, center_y = self.get_mask_center(mask[0])
            point_3d = self.pixel_to_3d(center_x, center_y, depth)
            
            objects_3d.append({
                'mask': mask[0],
                'position': point_3d,
                'confidence': scores[i]
            })
            
        return objects_3d
    
    def estimate_depth(self, rgb_image):
        """Monocular depth estimation using MiDaS."""
        # MiDaS expects RGB, normalize to [0,1]
        img_tensor = torch.from_numpy(rgb_image).permute(2, 0, 1).float() / 255.0
        img_tensor = img_tensor.unsqueeze(0).to(self.device)
        
        with torch.no_grad():
            depth = self.depth_model(img_tensor)
            
        return depth.squeeze().cpu()
    
    def pixel_to_3d(self, u, v, depth):
        """Convert pixel + depth to 3D point using camera intrinsics."""
        # Camera intrinsics (adjust for your camera)
        fx, fy = 525.0, 525.0  # Focal lengths
        cx, cy = 319.5, 239.5  # Principal point
        
        x = (u - cx) * depth / fx
        y = (v - cy) * depth / fy
        z = depth
        
        return [x, y, z]

# ROS2 node wrapper
def main():
    rclpy.init()
    node = rclpy.create_node('vision_pipeline')
    
    pipeline = VisionPipeline()
    
    def camera_callback(msg):
        # Convert ROS Image to numpy
        rgb = bridge.imgmsg_to_cv2(msg, "rgb8")
        
        # Process
        objects = pipeline.process_frame(rgb)
        
        # Publish detections
        publish_objects(objects)
    
    node.create_subscription(Image, '/camera/image_raw', camera_callback, 10)
    rclpy.spin(node)

# hand_eye_calibration.py
import numpy as np
from scipy.spatial.transform import Rotation

def calibrate_hand_eye(robot_poses, camera_poses):
    """
    Solves AX = XB problem where:
    A = robot base -> end-effector transforms
    B = camera -> marker transforms
    X = end-effector -> camera transform (what we want)
    """
    # Using Tsai-Lenz method
    n = len(robot_poses)
    
    # Build linear system for rotation
    M = np.zeros((3*n, 3))
    C = np.zeros(3*n)
    
    for i in range(n):
        Ra = Rotation.from_matrix(robot_poses[i][:3, :3])
        Rb = Rotation.from_matrix(camera_poses[i][:3, :3])
        
        # Convert to axis-angle
        alpha = Ra.as_rotvec()
        beta = Rb.as_rotvec()
        
        # Build equation
        M[3*i:3*i+3, :] = skew(alpha + beta)
        C[3*i:3*i+3] = beta - alpha
    
    # Solve for rotation
    rotation_vec, _, _, _ = np.linalg.lstsq(M, C, rcond=None)
    R_x = Rotation.from_rotvec(rotation_vec).as_matrix()
    
    # Solve for translation (after rotation is known)
    # ... similar linear system
    
    return build_transform(R_x, t_x)

def skew(v):
    """Skew-symmetric matrix from vector."""
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

<!-- pick_object.xml -->
<root main_tree_to_execute="PickAndPlace">
  <BehaviorTree ID="PickAndPlace">
    <Sequence>
      
      <!-- Phase 1: Perception -->
      <DetectObject name="target_object"/>
      <ComputeGraspPose pose="{grasp_pose}"/>
      
      <!-- Phase 2: Approach -->
      <Fallback>
        <PlanPath start="{current_pose}" goal="{pre_grasp_pose}"/>
        <ForceSuccess>
          <AlwaysFailure/>
        </ForceSuccess>
      </Fallback>
      
      <ExecuteTrajectory trajectory="{planned_path}"/>
      
      <!-- Phase 3: Grasp -->
      <Sequence>
        <MoveToGraspPose pose="{grasp_pose}"/>
        <CloseGripper force="20N"/>
        <CheckGraspSuccess/>
      </Sequence>
      
      <!-- Phase 4: Lift -->
      <MoveUp distance="0.2m"/>
      
    </Sequence>
  </BehaviorTree>
</root>

// behavior_nodes.cpp
#include <behaviortree_cpp_v3/behavior_tree.h>

class DetectObject : public BT::SyncActionNode {
public:
    DetectObject(const std::string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config) {}
    
    static BT::PortsList providedPorts() {
        return { BT::OutputPort<std::string>("object_id") };
    }
    
    BT::NodeStatus tick() override {
        // Call vision service
        auto client = node_->create_client<ObjectDetection>("detect_objects");
        auto request = std::make_shared<ObjectDetection::Request>();
        
        auto future = client->async_send_request(request);
        if (rclcpp::spin_until_future_complete(node_, future) == 
            rclcpp::FutureReturnCode::SUCCESS) {
            
            auto response = future.get();
            if (!response->objects.empty()) {
                setOutput("object_id", response->objects[0].id);
                return BT::NodeStatus::SUCCESS;
            }
        }
        
        return BT::NodeStatus::FAILURE;
    }
};

class MoveToGraspPose : public BT::AsyncActionNode {
    // Runs in separate thread to not block behavior tree
public:
    MoveToGraspPose(const std::string& name, const BT::NodeConfiguration& config)
        : BT::AsyncActionNode(name, config) {}
    
    BT::NodeStatus tick() override {
        geometry_msgs::msg::Pose target_pose;
        if (!getInput("pose", target_pose)) {
            return BT::NodeStatus::FAILURE;
        }
        
        // Send to motion controller
        auto action_client = rclcpp_action::create_client<MoveToTarget>(
            node_, "move_to_target");
        
        auto goal = MoveToTarget::Goal();
        goal.target_pose = target_pose;
        
        auto future = action_client->async_send_goal(goal);
        // Wait for completion...
        
        return BT::NodeStatus::SUCCESS;
    }
};

# humanoid_system.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription

def generate_launch_description():
    return LaunchDescription([
        
        # Layer 1: Motor control (1kHz)
        Node(
            package='motor_control',
            executable='ethercat_driver',
            parameters=[{'control_frequency': 1000}]
        ),
        
        # Layer 2: Whole-body controller (100Hz)
        Node(
            package='humanoid_control',
            executable='whole_body_controller',
            parameters=[{'control_frequency': 100}]
        ),
        
        # Layer 3: Motion planner
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([
                get_package_share_directory('moveit2'),
                '/launch/moveit.launch.py'
            ])
        ),
        
        # Layer 4: Vision pipeline (30 FPS)
        Node(
            package='humanoid_vision',
            executable='vision_pipeline',
            parameters=[{'camera_topic': '/camera/image_raw'}]
        ),
        
        # Layer 5: Behavior tree executor
        Node(
            package='humanoid_behavior',
            executable='behavior_executor',
            parameters=[{'tree_file': 'pick_and_place.xml'}]
        ),
        
        # Monitoring and safety
        Node(
            package='humanoid_safety',
            executable='safety_monitor',
            parameters=[{'emergency_stop_topic': '/estop'}]
        ),
    ])

# Launch system
ros2 launch humanoid_bringup system.launch.py

# Apply external force in Gazebo
ros2 run gazebo_tools apply_force --force 30 --duration 2

# Place object in simulation
ros2 run gazebo_tools spawn_object --name "cube" --pose "[0.5, 0, 0.8]"

# Run pick task
ros2 action send_goal /pick_object humanoid_msgs/action/PickObject "{object_id: 'cube'}"

# Command walking
ros2 topic pub /cmd_vel geometry_msgs/Twist \
  "{linear: {x: 0.2}, angular: {z: 0.0}}"