initial commit

.gitignore

@@ -0,0 +1,5 @@
+__pycache__/
+log/
+**/*.egg-info/
+.vscode
+**/MUJOCO_LOG.TXT

example/run_g1_benchmark.py

@@ -0,0 +1,62 @@
+from spark_pipeline import G1BenchmarkPipeline, G1BenchmarkPipelineConfig
+from spark_utils import update_class_attributes
+
+if __name__ == "__main__":
+
+    cfg = G1BenchmarkPipelineConfig()
+    cfg.max_num_steps = 2000
+    # --------------------- configure safe control algorithm --------------------- #
+    safe_control_algo = 'ssa' # 'ssa', 'sss', 'cbf', 'pfm', 'sma'
+    params = {
+        'ssa': dict(
+                    class_name = "SafeSetAlgorithm",
+                    eta = 1.0,
+                    safety_buffer = 0.1, # positive to allow hold state
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0, # waist
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, # left arm
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, # right arm
+                        10.0, 10.0, 10.0 # locomotion
+                    ]
+                ),
+        'sss': dict(
+                    class_name = "SublevelSafeSetAlgorithm",
+                    lambda_SSS = 1.0,
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0
+                    ]
+                ),
+        'cbf': dict(
+                    class_name = "ControlBarrierFunction",
+                    lambda_cbf = 1,
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0
+                    ]
+                ),
+        'pfm': dict(
+                    class_name = "PotentialFieldMethod",
+                    lambda_pfm = 0.1
+                ),
+        'sma': dict(
+                    class_name = "SlidingModeAlgorithm",
+                    c_2 = 1.0
+                )
+    }
+
+    update_class_attributes(cfg.algo.safe_controller.safe_algo, params[safe_control_algo])
+
+    # ------------------------------- run pipeline ------------------------------- #
+    pipeline = G1BenchmarkPipeline(cfg)
+    pipeline.run()
+
+
+

example/run_g1_safe_teleop_sim.py

@@ -0,0 +1,66 @@
+from spark_pipeline import G1SafeTeleopSimPipeline, G1SafeTeleopSimPipelineConfig
+from spark_utils import update_class_attributes
+
+if __name__ == "__main__":
+
+    cfg = G1SafeTeleopSimPipelineConfig()
+
+    # ------------------------- change g1 model if needed ------------------------ #
+    cfg.robot.cfg.class_name = "G1BasicConfig" # "G1BasicConfig", "G1RightArmConfig", etc.
+
+    # --------------------- configure safe control algorithm --------------------- #
+    safe_control_algo = 'ssa' # 'ssa', 'sss', 'cbf', 'pfm', 'sma'
+    params = {
+        'ssa': dict(
+                    class_name = "SafeSetAlgorithm",
+                    eta = 1.0,
+                    safety_buffer = 0.1, # positive to allow hold state
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0, # waist
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, # left arm
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, # right arm
+                        10.0, 10.0, 10.0 # locomotion
+                    ]
+                ),
+        'sss': dict(
+                    class_name = "SublevelSafeSetAlgorithm",
+                    lambda_SSS = 1.0,
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0
+                    ]
+                ),
+        'cbf': dict(
+                    class_name = "ControlBarrierFunction",
+                    lambda_cbf = 10,
+                    slack_weight = 1e3,
+                    control_weight = [
+                        1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
+                        1.0, 1.0, 1.0
+                    ]
+                ),
+        'pfm': dict(
+                    class_name = "PotentialFieldMethod",
+                    lambda_pfm = 0.1
+                ),
+        'sma': dict(
+                    class_name = "SlidingModeAlgorithm",
+                    c_2 = 1.0
+                )
+    }
+
+    update_class_attributes(cfg.algo.safe_controller.safe_algo, params[safe_control_algo])
+
+    # --------------------------- other configurations --------------------------- #
+    cfg.env.task.enable_ros = False # if ROS is needed for receiving task info
+    cfg.env.agent.obstacle_debug["manual_movement_step_size"] = 0.1 # Tune step size for keyboard controlled obstacles
+
+    # ------------------------------- run pipeline ------------------------------- #
+    pipeline = G1SafeTeleopSimPipeline(cfg)
+    pipeline.run()

example/run_g1_unsafe_teleop_sim.py

@@ -0,0 +1,17 @@
+from spark_pipeline import G1SafeTeleopSimPipeline, G1UnsafeTeleopSimPipelineConfig
+
+if __name__ == "__main__":
+
+    cfg = G1UnsafeTeleopSimPipelineConfig()
+
+    # ------------------------- change g1 model if needed ------------------------ #
+
+    # --------------------- configure safe control algorithm --------------------- #
+
+    # --------------------------- other configurations --------------------------- #
+    cfg.env.task.enable_ros = False # if ROS is needed for receiving task info
+    cfg.env.agent.obstacle_debug["manual_movement_step_size"] = 0.1 # Tune step size for keyboard controlled obstacles
+
+    # ------------------------------- run pipeline ------------------------------- #
+    pipeline = G1SafeTeleopSimPipeline(cfg)
+    pipeline.run()

install.sh

@@ -0,0 +1,116 @@
+#!/bin/bash
+
+# Initialize Conda
+CONDA_BASE=$(conda info --base)
+source "$CONDA_BASE/etc/profile.d/conda.sh"
+
+# Function to report errors and exit
+function handle_error {
+    echo "Error: $1"
+    exit 1
+}
+
+# Default values
+INSTALL_ROS=false
+ENV_NAME=""
+
+# Parse arguments
+while [[ $# -gt 0 ]]; do
+    case "$1" in
+        --ros)
+            INSTALL_ROS=true
+            shift
+            ;;
+        --name)
+            shift
+            # Check if environment name is provided
+            if [[ -z "$1" || "$1" == "--"* ]]; then
+                handle_error "You must provide an environment name after --name."
+            fi
+            ENV_NAME="$1"
+            shift
+            ;;
+        *)
+            handle_error "Unknown option: $1"
+            ;;
+    esac
+done
+
+# Check if ENV_NAME was provided
+if [[ -z "$ENV_NAME" ]]; then
+    handle_error "You must provide an environment name using --name env_name"
+fi
+
+# Step 1: Create Conda environment
+echo "Step 1: Creating Conda environment: $ENV_NAME"
+conda create --name $ENV_NAME "python=3.10" -y || handle_error "Failed to create Conda environment $ENV_NAME using 'conda create --name $ENV_NAME python=3.10'"
+
+# Step 2: Activate the environment
+echo "Step 2: Activating Conda environment: $ENV_NAME"
+conda activate $ENV_NAME || handle_error "Failed to activate Conda environment $ENV_NAME using 'conda activate $ENV_NAME'"
+
+# Step 3: Install Pinocchio
+echo "Step 3: Installing Pinocchio"
+conda install -c conda-forge pinocchio=3.2.0 -y || handle_error "Failed to install Pinocchio using 'conda install -c conda-forge pinocchio=3.2.0'"
+
+# Step 4: Install numpy=2.0
+echo "Step 4: Installing numpy=2.0"
+conda install numpy=2.0 -y || handle_error "Failed to install numpy=2.0 using 'conda install numpy=2.0'"
+
+# Step 5: Install core Python packages
+echo "Step 5: Installing core Python packages"
+
+# Declare the array of core Python packages
+core_packages=("scipy" "numba" "casadi" "matplotlib" "mujoco" "pyyaml" "osqp" "tensorboardX")
+
+# Iterate through each package in the array
+for package in "${core_packages[@]}"; do
+    echo "Installing $package..."
+    pip install "$package" || handle_error "Failed to install Python package $package using 'pip install $package'"
+done
+
+# Step 6: Install spark_* modules
+echo "Step 6: Installing spark_* modules"
+
+# Declare the array of spark modules
+spark_modules=(
+    "module/spark_agent"
+    "module/spark_policy"
+    "module/spark_robot"
+    "module/spark_safe"
+    "module/spark_task"
+    "module/spark_utils"
+    "pipeline/"
+    "wrapper/spark_algo"
+    "wrapper/spark_env"
+)
+
+# Iterate through each module in the array
+for module in "${spark_modules[@]}"; do
+    echo "Installing module: $module"
+    cd "$module" || handle_error "Failed to enter directory $module"
+    pip install -e . || handle_error "Failed to install module $module using 'pip install -e .'"
+    cd - || handle_error "Failed to return to parent directory from $module"
+done
+
+echo "All spark_* modules installed successfully!"
+
+# Step 7: Conditionally install ROS dependencies
+if $INSTALL_ROS; then
+    echo "Step 7: Installing ROS dependencies"
+
+    # Declare the array of ROS packages
+    ros_packages=("yaml" "rospkg" "catkin_pkg")
+
+    # Iterate through each package in the array
+    for package in "${ros_packages[@]}"; do
+        echo "Installing $package..."
+        conda install conda-forge::"$package" -y || handle_error "Failed to install ROS package $package using 'conda install -c conda-forge $package'"
+    done
+
+    echo "ROS dependencies installed successfully!"
+else
+    echo "Step 7: Skipping ROS dependencies installation. Use '--ros' if ROS-related utilities are needed."
+fi
+
+echo "Installation process completed successfully!"

module/spark_agent/setup.py

@@ -0,0 +1,13 @@
+from setuptools import setup, find_packages
+
+setup(
+    name="spark_agent",
+    version="1.0.0",
+    description="Interface for the real robot and the simulation.",
+    packages=find_packages(),
+    include_package_data=True,
+    install_requires=[
+        "numpy",  # Add your library dependencies here
+    ],
+    python_requires=">=3.8",
+)

module/spark_agent/spark_agent/__init__.py

@@ -0,0 +1,4 @@
+from .base.base_agent_config import BaseAgentConfig
+from .base.base_agent import BaseAgent
+from .simulation.mujoco.mujoco_agent import MujocoAgent
+from .simulation.mujoco.g1_basic_mujoco_agent import G1BasicMujocoAgent

module/spark_agent/spark_agent/base/base_agent.py

@@ -0,0 +1,67 @@
+from abc import ABC, abstractmethod
+import numpy as np
+from spark_robot import RobotConfig
+
+class BaseAgent(ABC):
+    """
+    An abstract base class for defining robot agents.
+    """
+
+    def __init__(self, robot_cfg: RobotConfig) -> None:
+        super().__init__()
+        self.robot_cfg = robot_cfg
+        self.num_dof = len(self.robot_cfg.DoFs)
+        self.num_control = len(self.robot_cfg.Control)
+        self.num_obstacle_agent = 0
+
+        self.dof_pos_cmd = None
+        self.dof_vel_cmd = None
+        self.dof_acc_cmd = None
+
+        self.dof_pos_fbk = None
+        self.dof_vel_fbk = None
+        self.dof_acc_fbk = None
+
+    @abstractmethod
+    def reset(self) -> None:
+        """
+        Reset the robot agent.
+        """
+        pass
+
+    @abstractmethod
+    def compose_state(self) -> np.ndarray:
+        """
+        Compose the state of the robot agent that matches the robot config.
+        """
+        pass
+
+    @abstractmethod
+    def send_control(self, control: np.ndarray) -> None:
+        """
+        Execute control on the robot agent.
+        """
+        pass
+
+    @abstractmethod
+    def post_control_processing(self, **kwargs) -> None:
+        """
+        Processes following commands: sim step, state refresh, render, etc.
+        """
+        pass
+
+    def step(self, control: np.ndarray, **kwargs) -> None:
+        """
+        Step the robot agent with a given action.
+        Action should be defined in RobotConfig
+        """
+
+        self.send_control(control, **kwargs)
+        self.post_control_processing(**kwargs)
+
+    @abstractmethod
+    def get_feedback(self) -> None:
+        """
+        Get the current state of the robot agent.
+        """
+        pass