update docstring; add new robostack script

Author: zzx9636

Host_Setup/RoboStack/robotstack.md

@@ -1,6 +1,13 @@
 # Install ROBOSTACK ROS-Noetic 
 [RoboStack](https://robostack.github.io/index.html) is a bundling of the Robot Operating System (ROS) by Open Robotics for Linux, Mac and Windows using the Conda package manager. Traditionaly, ROS is only available to Linux and does not work well with Conda.
 
+# Update: 2023.2.5
+RoboStack has released a new version of ROS Noetic. It uses a more recent version of Boost, which allows use to use newer version of Jax, PyTorch, and HPP-FCL. This means your algorithms will run (potentially) faster and more stable. If you want to try out, please follow the instructions below.
+```
+./ros_conda_install_unix_new.sh
+```
+It will create a new environment called ```ros_base2```. You can activate it by ```conda activate ros_base2```. Your old environment ```ros_base``` will still be there.
+
 ## Mac OS and Linux
 If you are using Mac OS or Linux, we provide you a simple script to automate the process. Simply open a terminal, then run
 

Host_Setup/RoboStack/ros_conda_install_unix.sh

@@ -89,6 +89,9 @@ mamba --version
 
 # make sure we are in base 
 echo -e "${BLUE}Setup RoboStack Env${NC}"
+for i in $(seq ${CONDA_SHLVL}); do
+    conda deactivate
+done
 conda activate base
 
 mamba create -n ros_base ros-noetic-desktop python=3.9 -c robostack -c robostack-experimental \
@@ -103,8 +106,6 @@ mamba install compilers cmake pkg-config make ninja -c conda-forge --override-ch
 
 mamba install catkin_tools -c conda-forge -c robostack -c robostack-experimental --yes
 
-mamba install networkx shapely -c conda-forge --yes
-
 # reload environment to activate required scripts before running anything
 # on Windows, please restart the Anaconda Prompt / Command Prompt!
 conda deactivate

Host_Setup/RoboStack/ros_conda_install_unix_new.sh

@@ -0,0 +1,166 @@
+#!/bin/bash
+
+# define color
+RED='\033[0;31m'
+GREEN='\033[0;32m'
+BLUE='\033[0;34m'
+NC='\033[0m' # No Color
+
+cur_dir=$(pwd)
+
+OS=$(uname -s)
+ARCH=$(uname -m)
+
+# Check if conda installed
+if test -z "$(which $CONDA_EXE)" ; then
+  echo -e "${RED} Conda is not installed, install Mambaforge ${NC}"
+  # Mac OS
+  if [[ $OS == 'Darwin' ]]; then
+    if [[ $ARCH == 'arm64' ]]; then
+      echo -e "${GREEN} You are using Mac OS with Apple Silicon${NC}"
+      curl -L https://github.com/conda-forge/miniforge/releases/latest/download/Mambaforge-MacOSX-arm64.sh > Mambaforge-Installer.sh
+    elif [[ $ARCH == 'x86_64' ]]; then  
+      echo -e "${GREEN} You are using Mac OS with X86${NC}"
+      curl -L https://github.com/conda-forge/miniforge/releases/latest/download/Mambaforge-MacOSX-x86_64.sh > Mambaforge-Installer.sh
+    else
+      echo -e "${RED} Unrecognized arch type, Mac OS with ${ARCH}${NC}"
+      return 1 2>/dev/null
+      exit 1
+    fi
+
+  elif [[ $OS == "Linux" ]]; then
+    # Linux
+    if [[ $ARCH == 'aarch64' ]]; then
+      echo -e "${GREEN}You are using Linux with aarch64${NC}"
+      curl -L https://github.com/conda-forge/miniforge/releases/latest/download/Mambaforge-Linux-aarch64.sh > Mambaforge-Installer.sh
+    elif [[ $ARCH == 'x86_64' ]]; then  
+      echo -e "${GREEN}You are using Linux with X86${NC}"
+      curl -L https://github.com/conda-forge/miniforge/releases/latest/download/Mambaforge-Linux-x86_64.sh > Mambaforge-Installer.sh
+    else
+      echo -e "${RED} Unrecognized arch type, Linux with ${ARCH}${NC}"
+      return 1 2>/dev/null
+      exit 1
+    fi
+
+  else
+    echo -e "${RED} OS: ${OS} not supported ${NC}"
+    exit 1
+  fi
+
+  # Install Mambaforge
+  chmod +x Mambaforge-Installer.sh
+  ./Mambaforge-Installer.sh -b -f
+
+  cd ~/mambaforge/bin
+  ./conda config --set auto_activate_base false
+  ./conda init --all
+  ./mamba init --all
+
+  # manually enable conda and mamba
+  eval "$(./conda shell.bash hook)"
+  cd ~/mambaforge/etc/profile.d/
+  . "mamba.sh"
+
+  cd $cur_dir
+  rm Mambaforge-Installer.sh
+
+  echo -e "${Green} Finish install Mambaforge${NC}"
+
+else
+  echo -e "${BLUE}Found Conda in $CONDA_EXE${NC}, install Mamba to base environment"
+  eval "$($CONDA_EXE shell.bash hook)"
+  conda activate base
+  conda install mamba -c conda-forge --yes
+  mamba init --all
+
+  # manually enable mamba
+  cd $(conda info --base)/etc/profile.d/
+  . "mamba.sh"
+fi
+
+cd $cur_dir
+
+echo -e "Current Conda is running on ${GREEN}$CONDA_EXE${NC}"
+
+conda --version
+mamba --version
+
+# make sure we are in base 
+echo -e "${BLUE}Setup RoboStack Env${NC}"
+for i in $(seq ${CONDA_SHLVL}); do
+    conda deactivate
+done
+conda activate base
+
+mamba create -n ros_base2 ros-noetic-desktop python=3.9 \
+             -c robostack-staging -c conda-forge \
+             --no-channel-priority --override-channels --yes
+
+
+echo -e "${BLUE}Finish Settting up RoboStack Env${NC}"
+
+conda activate ros_base2
+
+mamba install compilers cmake pkg-config make ninja -c conda-forge --override-channels --yes
+
+mamba install catkin_tools -c conda-forge -c robostack-staging --yes
+
+
+# reload environment to activate required scripts before running anything
+# on Windows, please restart the Anaconda Prompt / Command Prompt!
+conda deactivate
+conda activate ros_base2
+
+# if you want to use rosdep, also do:
+mamba install rosdep -c conda-forge -c robostack-staging --yes
+rosdep init  # note: do not use sudo!
+rosdep update
+
+echo -e "${BLUE}Install Dependency${NC}"
+
+mamba install numpy scipy matplotlib jupyter notebook networkx shapely -c conda-forge --yes
+
+echo -e "${BLUE}Install PySpline${NC}"
+# Mac OS
+if [[ $OS == 'Darwin' ]]; then
+  if [[ $ARCH == 'arm64' ]]; then
+    echo -e "${GREEN} You are using Mac OS with Apple Silicon${NC}"
+    # this avoid segfault when importing hppfcl 
+    # mamba install eigenpy=2.7.10 --yes -c conda-forge 
+    pip install osx_arm/pyspline-1.5.2-py3-none-any.whl
+  elif [[ $ARCH == 'x86_64' ]]; then  
+    echo -e "${GREEN} You are using Mac OS with X86${NC}"
+    pip install osx/pyspline-1.5.2-py3-none-any.whl
+  else
+    echo -e "${RED} Unrecognized arch type, Mac OS with ${ARCH}. Cannot Install PySpline, Please compile from source${NC}"
+    exit 1
+  fi
+
+elif [[ $OS == "Linux" ]]; then
+  # Linux
+  if [[ $ARCH == 'aarch64' ]]; then
+    echo -e "${GREEN}You are using Linux with aarch64${NC}"
+    pip install linux_aarch64/pyspline-1.5.2-py3-none-linux_aarch64.whl
+  elif [[ $ARCH == 'x86_64' ]]; then  
+    echo -e "${GREEN}You are using Linux with X86${NC}"
+    pip install linux/pyspline-1.5.2-py3-none-linux_x86_64.whl
+  else
+    echo -e "${RED} Unrecognized arch type, Linux with ${ARCH}. Cannot Install PySpline, Please compile from source${NC}"
+    return 1 2>/dev/null
+    exit 1
+  fi
+else
+  echo -e "${RED} OS: ${OS} not supported ${NC}"
+  return 1 2>/dev/null
+  exit 1
+fi
+
+echo -e "${BLUE}Install Jax${NC}"
+# Instal Jax https://github.com/google/jax#conda-installation
+mamba install jax=0.4.2 'jaxlib=0.4.2=cpu*' -c conda-forge --yes
+
+echo -e "${BLUE}Install Hpp-FCL${NC}"
+
+mamba install -c conda-forge hpp-fcl --yes
+
+echo -e "${Green}Finished! Reopen a new terminal to see if everything works. ${NC}"

ROS_Core/src/Labs/Lab0/assets/example.png

@@ -46,4 +46,3 @@ def reset(self):
         Reset the buffer to None
         '''
         self.writeFromNonRT(None)
-        self.new_data_available = False

ROS_Core/src/Labs/Lab0/scripts/controller/utils/realtime_buffer.py

@@ -2,7 +2,7 @@
 from geometry_msgs.msg import PoseStamped
 from tf.transformations import quaternion_from_euler
 
-def get_ros_param(param_name, default):
+def get_ros_param(param_name: str, default):
     '''
     Read a parameter from the ROS parameter server. If the parameter does not exist, return the default value.
     Args:
@@ -27,25 +27,3 @@ def get_ros_param(param_name, default):
             rospy.logwarn("Parameter '%s' not found, using default: %s", param_name, default)
             return default
 
-
-def state_to_pose_stamped(state, t, frame_id='map'):
-    '''
-    Convert a State Vector object to a PoseStamped message
-    state: [x,y,v,yaw,delta]
-    t: float time in seconds
-    frame_id: string
-    '''
-
-    pose = PoseStamped()
-    pose.header.frame_id = frame_id
-    pose.header.stamp =rospy.Time.from_sec(t)
-    pose.pose.position.x = state[0]
-    pose.pose.position.y = state[1]
-    pose.pose.position.z = 0.0
-    
-    q = quaternion_from_euler(0.0, 0.0, state[3])
-    pose.pose.orientation.x = q[0]
-    pose.pose.orientation.y = q[1]
-    pose.pose.orientation.z = q[2]
-    pose.pose.orientation.w = q[3]
-    return pose

ROS_Core/src/Labs/Lab0/scripts/controller/utils/ros_utility.py

@@ -1,12 +1,19 @@
 import numpy as np
 from scipy.spatial.transform import Rotation as R
 from tf.transformations import euler_from_quaternion
+from nav_msgs.msg import Odometry
 
 class State2D():
     '''
     2D vehicle state
     '''
-    def __init__(self, odom_msg = None) -> None:
+    def __init__(self, odom_msg: Odometry = None) -> None:
+        '''
+        Constructor of the State2D class
+        Parameters:
+            odom_msg: nav_msgs.msg.Odometry, 
+                odometry message to initialize the state
+        '''
 
         self.t = 0 # time stamp
         self.x = 0 #x position
@@ -25,9 +32,12 @@ def __init__(self, odom_msg = None) -> None:
         if odom_msg is not None:
             self.from_odom_msg(odom_msg)
 
-    def from_odom_msg(self, odom_msg):
+    def from_odom_msg(self, odom_msg: Odometry) -> None:
         '''
         Construct state from odometry message
+        Parameters:
+            odom_msg: nav_msgs.msg.Odometry, 
+                odometry message to initialize the state
         '''
         self.t = odom_msg.header.stamp.to_sec()
         self.x = odom_msg.pose.pose.position.x
@@ -46,11 +56,12 @@ def from_odom_msg(self, odom_msg):
 
         self.initialized = True
 
-    def from_SE3(self, pose_base_to_world, twist_base, t):
+    def from_SE3(self, pose_base_to_world: np.ndarray, twist_base: np.ndarray, t: float) -> None:
         '''
         Construct state from SE3 pose and twist
-            pose_base_to_world: SE3 pose from world to base
-            twist_base: twist of the base
+            pose_base_to_world: np array, SE3 pose from world to base
+            twist_base: np array, twist of the base
+            t: float, time stamp
         '''
         self.t = t
         self.x = pose_base_to_world[0,3]
@@ -69,9 +80,16 @@ def from_SE3(self, pose_base_to_world, twist_base, t):
 
         self.initialized = True
 
-    def from_state(self, x, y, v_long, psi, w, t):
+    def from_state(self, x: float, y: float, v_long: float, psi: float, w: float, t: float) -> None:
         '''
         Construct state from a state object
+        Parameters:
+            x: float, x position
+            y: float, y position
+            v_long: float, longitudinal velocity
+            psi: float, pose angle around z axis
+            w: float, angular velocity around z axis
+            t: float, time stamp
         '''
         self.t = t
         self.x = x
@@ -88,13 +106,13 @@ def from_state(self, x, y, v_long, psi, w, t):
 
         self.initialized = True
 
-    def state_vector(self, delta):
+    def state_vector(self, delta) -> np.ndarray:
         '''
         Return the state vector
         '''
         return np.array([self.x, self.y, self.v_long, self.psi, delta])
 
-    def transformation_matrix(self):
+    def transformation_matrix(self) -> np.ndarray:
         '''
         Retrun Translation matrix from world to base
         '''
@@ -103,7 +121,7 @@ def transformation_matrix(self):
                     [0, 0, 1]])
         return T
 
-    def __str__(self):
+    def __str__(self) -> str:
         return f"State at [{np.round(self.x,3)}, {np.round(self.y,3)}] "+ \
             f"with pose {np.round(np.rad2deg(self.psi),3)} deg\n" + \
             f"Speed: {np.round(self.v,3)} pointing to {np.round(np.rad2deg(self.v_dir),3)} deg; Omega: {np.round(self.w,3)} \n"+ \