examples cleanup
This commit is contained in:
Rooholla-KhorramBakht
parent
f72dc3e583
commit
dd93a5ffd4
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -231,7 +231,7 @@ class Go2Model:
|
|||
|
||||
def getGroundReactionForce(self, tau_est, body_acceleration=None):
|
||||
if body_acceleration is None:
|
||||
grf = {key:np.linalg.pinv(self.ef_J_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_J_.keys()}
|
||||
grf = {key:np.linalg.pinv(self.ef_Jw_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_Jw_.keys()}
|
||||
else:
|
||||
raise NotImplementedError("Ground reaction force with body dynamics is not implemented")
|
||||
return grf
|
||||
3
Makefile
3
Makefile
|
|
@ -1,3 +1,6 @@
|
|||
realsense:
|
||||
@cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense .
|
||||
|
||||
hesai:
|
||||
@cd deploy && docker build --no-cache --tag go2py_hesai:latest -f docker/Dockerfile.hesai .
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
# FROM isaac_ros_dev-aarch64
|
||||
FROM ros:humble
|
||||
ENV DEBIAN_FRONTEND=noninteractive
|
||||
SHELL ["/bin/bash", "-c"]
|
||||
# uodate and install dependencies
|
||||
RUN apt-get update && apt-get install -y \
|
||||
ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
|
||||
# ros-humble-realsense2-camera \
|
||||
# ros-humble-pointcloud-to-laserscan \
|
||||
libyaml-cpp-dev \
|
||||
libboost-all-dev\
|
||||
build-essential \
|
||||
cmake \
|
||||
git \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
|
||||
RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
|
||||
git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
|
||||
cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
|
||||
|
||||
COPY scripts/build-librealsense.sh /opt/realsense/build-librealsense.sh
|
||||
COPY scripts/install-realsense-dependencies.sh /opt/realsense/install-realsense-dependencies.sh
|
||||
|
||||
RUN chmod +x /opt/realsense/install-realsense-dependencies.sh && /opt/realsense/install-realsense-dependencies.sh
|
||||
RUN chmod +x /opt/realsense/build-librealsense.sh && /opt/realsense/build-librealsense.sh --no_cuda
|
||||
|
||||
# Copy hotplug script which will get invoked whenever a devices plugged or un-plugged
|
||||
RUN mkdir -p /opt/realsense/
|
||||
COPY scripts/hotplug-realsense.sh /opt/realsense/hotplug-realsense.sh
|
||||
|
||||
# Copy custom udev rules file
|
||||
COPY udev_rules/99-realsense-libusb-custom.rules /etc/udev/rules.d/99-realsense-libusb-custom.rules
|
||||
|
||||
|
||||
# copy the go2py ros2 nodes
|
||||
# COPY ros2_nodes/lidar_node /hesai_ws/src/lidar_node
|
||||
# RUN cd /hesai_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
|
||||
|
||||
# Copy the script to start the nodes
|
||||
# COPY docker/scripts /root/scripts
|
||||
# COPY launch_files /root/launch
|
||||
# set the entrypoint to bash
|
||||
ENTRYPOINT ["/bin/bash"]
|
||||
# ENTRYPOINT ["/bin/bash", "/root/scripts/hesai_start.sh"]
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
#!/bin/bash
|
||||
# Builds the Intel Realsense library librealsense on a Jetson Nano Development Kit
|
||||
# Copyright (c) 2016-21 Jetsonhacks
|
||||
# MIT License
|
||||
|
||||
LIBREALSENSE_DIRECTORY=${HOME}/librealsense
|
||||
INSTALL_DIR=$PWD
|
||||
NVCC_PATH=/usr/local/cuda/bin/nvcc
|
||||
|
||||
USE_CUDA=true
|
||||
|
||||
function usage ()
|
||||
{
|
||||
echo "Usage: ./build-librealsense.sh [-n | -no_cuda] [-v | -version <version>] [-j | --jobs <number of jobs>] [-h | --help] "
|
||||
echo "-n | --no_cuda Build with no CUDA (Defaults to with CUDA)"
|
||||
echo "-v | --version Version of librealsense to build
|
||||
(defaults to latest release)"
|
||||
echo "-j | --jobs Number of concurrent jobs (Default 1 on <= 4GB RAM
|
||||
#of cores-1 otherwise)"
|
||||
echo "-h | --help This message"
|
||||
exit 2
|
||||
}
|
||||
|
||||
PARSED_ARGUMENTS=$(getopt -a -n build-librealsense.sh -o nv:j:h --longoptions version:,no_cuda,jobs:,help -- "$@" )
|
||||
VALID_ARGUMENTS=$?
|
||||
|
||||
if [ "$VALID_ARGUMENTS" != "0" ]; then
|
||||
echo ""
|
||||
usage
|
||||
fi
|
||||
|
||||
eval set -- "$PARSED_ARGUMENTS"
|
||||
|
||||
LIBREALSENSE_VERSION=""
|
||||
USE_CUDA=true
|
||||
NUM_PROCS=""
|
||||
|
||||
while :
|
||||
do
|
||||
case "$1" in
|
||||
-n | --build_no_cuda) USE_CUDA=false ; shift ;;
|
||||
-v | --version ) LIBREALSENSE_VERSION="$2" ; shift 2 ;;
|
||||
-j | --jobs) NUM_PROCS="$2" ;
|
||||
shift 2 ;
|
||||
re_isanum='^[0-9]+$'
|
||||
if ! [[ $NUM_PROCS =~ $re_isanum ]] ; then
|
||||
echo "Number of jobs must be a positive, whole number"
|
||||
usage
|
||||
else
|
||||
if [ $NUM_PROCS -eq "0" ]; then
|
||||
echo "Number of jobs must be a positive, whole number"
|
||||
fi
|
||||
fi ;
|
||||
;;
|
||||
-h | --help ) usage ; shift ;;
|
||||
# -- means the end of arguments
|
||||
--) shift; break ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# From lukechilds gist discussion: https://gist.github.com/lukechilds/a83e1d7127b78fef38c2914c4ececc3c
|
||||
# We use wget instead of curl here
|
||||
# Sample usage:
|
||||
# VERSION_STRINGS=$(get_latest_release IntelRealSense/librealsense)
|
||||
|
||||
function get_latest_release () {
|
||||
# redirect wget to standard out and grep out the tag_name
|
||||
wget -qO- https://api.github.com/repos/$1/releases/latest |
|
||||
grep -Po '"tag_name": "\K.*?(?=")'
|
||||
}
|
||||
|
||||
if [[ $LIBREALSENSE_VERSION == "" ]] ; then
|
||||
echo "Getting latest librealsense version number"
|
||||
LIBREALSENSE_VERSION=$(get_latest_release IntelRealSense/librealsense)
|
||||
fi
|
||||
|
||||
echo "Build with CUDA: "$USE_CUDA
|
||||
echo "Librealsense Version: $LIBREALSENSE_VERSION"
|
||||
|
||||
red=`tput setaf 1`
|
||||
green=`tput setaf 2`
|
||||
reset=`tput sgr0`
|
||||
# e.g. echo "${red}The red tail hawk ${green}loves the green grass${reset}"
|
||||
|
||||
if [ ! -d "$LIBREALSENSE_DIRECTORY" ] ; then
|
||||
# clone librealsense
|
||||
cd ${HOME}
|
||||
echo "${green}Cloning librealsense${reset}"
|
||||
git clone https://github.com/IntelRealSense/librealsense.git
|
||||
fi
|
||||
|
||||
# Is the version of librealsense current enough?
|
||||
cd $LIBREALSENSE_DIRECTORY
|
||||
VERSION_TAG=$(git tag -l $LIBREALSENSE_VERSION)
|
||||
if [ ! $VERSION_TAG ] ; then
|
||||
echo ""
|
||||
tput setaf 1
|
||||
echo "==== librealsense Version Mismatch! ============="
|
||||
tput sgr0
|
||||
echo ""
|
||||
echo "The installed version of librealsense is not current enough for these scripts."
|
||||
echo "This script needs librealsense tag version: "$LIBREALSENSE_VERSION "but it is not available."
|
||||
echo "Please upgrade librealsense or remove the librealsense folder before attempting to install again."
|
||||
echo ""
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Checkout version the last tested version of librealsense
|
||||
git checkout $LIBREALSENSE_VERSION
|
||||
|
||||
# Install the dependencies
|
||||
cd $INSTALL_DIR
|
||||
|
||||
cd $LIBREALSENSE_DIRECTORY
|
||||
git checkout $LIBREALSENSE_VERSION
|
||||
|
||||
# Now compile librealsense and install
|
||||
mkdir build
|
||||
cd build
|
||||
# Build examples, including graphical ones
|
||||
echo "${green}Configuring Make system${reset}"
|
||||
# Build with CUDA (default), the CUDA flag is USE_CUDA, ie -DUSE_CUDA=true
|
||||
export CUDACXX=$NVCC_PATH
|
||||
export PATH=${PATH}:/usr/local/cuda/bin
|
||||
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/cuda/lib64
|
||||
|
||||
/usr/bin/cmake ../ -DBUILD_EXAMPLES=true -DFORCE_RSUSB_BACKEND=true -DBUILD_WITH_CUDA="$USE_CUDA" -DCMAKE_BUILD_TYPE=release -DBUILD_PYTHON_BINDINGS=bool:true
|
||||
|
||||
# The library will be installed in /usr/local/lib, header files in /usr/local/include
|
||||
# The demos, tutorials and tests will located in /usr/local/bin.
|
||||
echo "${green}Building librealsense, headers, tools and demos${reset}"
|
||||
|
||||
# If user didn't set # of jobs and we have > 4GB memory then
|
||||
# set # of jobs to # of cores-1, otherwise 1
|
||||
if [[ $NUM_PROCS == "" ]] ; then
|
||||
TOTAL_MEMORY=$(free | awk '/Mem\:/ { print $2 }')
|
||||
if [ $TOTAL_MEMORY -gt 4051048 ] ; then
|
||||
NUM_CPU=$(nproc)
|
||||
NUM_PROCS=$(($NUM_CPU - 1))
|
||||
else
|
||||
NUM_PROCS=1
|
||||
fi
|
||||
fi
|
||||
|
||||
time make -j$NUM_PROCS
|
||||
if [ $? -eq 0 ] ; then
|
||||
echo "librealsense make successful"
|
||||
else
|
||||
# Try to make again; Sometimes there are issues with the build
|
||||
# because of lack of resources or concurrency issues
|
||||
echo "librealsense did not build " >&2
|
||||
echo "Retrying ... "
|
||||
# Single thread this time
|
||||
time make
|
||||
if [ $? -eq 0 ] ; then
|
||||
echo "librealsense make successful"
|
||||
else
|
||||
# Try to make again
|
||||
echo "librealsense did not successfully build" >&2
|
||||
echo "Please fix issues and retry build"
|
||||
exit 1
|
||||
fi
|
||||
fi
|
||||
echo "${green}Installing librealsense, headers, tools and demos${reset}"
|
||||
sudo make install
|
||||
|
||||
if grep -Fxq 'export PYTHONPATH=$PYTHONPATH:/usr/local/lib' ~/.bashrc ; then
|
||||
echo "PYTHONPATH already exists in .bashrc file"
|
||||
else
|
||||
echo 'export PYTHONPATH=$PYTHONPATH:/usr/local/lib' >> ~/.bashrc
|
||||
echo "PYTHONPATH added to ~/.bashrc. Pyhon wrapper is now available for importing pyrealsense2"
|
||||
fi
|
||||
|
||||
cd $LIBREALSENSE_DIRECTORY
|
||||
echo "${green}Applying udev rules${reset}"
|
||||
# Copy over the udev rules so that camera can be run from user space
|
||||
sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
|
||||
sudo udevadm control --reload-rules && udevadm trigger
|
||||
|
||||
echo "${green}Library Installed${reset}"
|
||||
echo " "
|
||||
echo " -----------------------------------------"
|
||||
echo "The library is installed in /usr/local/lib"
|
||||
echo "The header files are in /usr/local/include"
|
||||
echo "The demos and tools are located in /usr/local/bin"
|
||||
echo " "
|
||||
echo " -----------------------------------------"
|
||||
echo " "
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#!/usr/bin/env bash
|
||||
|
||||
# Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved.
|
||||
#
|
||||
# NVIDIA CORPORATION and its licensors retain all intellectual property
|
||||
# and proprietary rights in and to this software, related documentation
|
||||
# and any modifications thereto. Any use, reproduction, disclosure or
|
||||
# distribution of this software and related documentation without an express
|
||||
# license agreement from NVIDIA CORPORATION is strictly prohibited.
|
||||
|
||||
|
||||
# This script is triggered when a corresponding udev rule is matched with action
|
||||
function usage() {
|
||||
echo "Usage: hotplug-realsense.sh -a <action=add/remove> -d <dev_path> [options]"
|
||||
echo "-a | --action Action determines whether the device has been added or removed. Valid values are 'add' or 'remove'."
|
||||
echo "-d | --dev-path Device path that will be used to mount the device."
|
||||
echo "-M | --major-version The kernel major number for the device. This will be used when action=add."
|
||||
echo "-m | --minor-version The kernel minor number for the device. This will be used when action=add."
|
||||
echo "-p | --parent-path Parent node path of the device pointed by the -d flag."
|
||||
echo "-h | --help Display this message."
|
||||
}
|
||||
|
||||
function check_mandatory_param() {
|
||||
VAR=$1
|
||||
MSG=$2
|
||||
if [[ -z ${VAR} ]] ; then
|
||||
echo "${MSG}"
|
||||
usage
|
||||
exit 1
|
||||
fi
|
||||
}
|
||||
|
||||
function timestamp() {
|
||||
echo [$EPOCHREALTIME] [$(date)]
|
||||
}
|
||||
|
||||
ARGUMENTS=$(getopt -n hotplug-realsense.sh -o a:d:M:m:p:h -l action:,dev-path:,major-version:,minor-version:,parent-path:,help -- "$@" )
|
||||
|
||||
if [[ $? -ne 0 ]]; then
|
||||
usage
|
||||
fi
|
||||
|
||||
eval set -- "$ARGUMENTS"
|
||||
|
||||
while [ : ]; do
|
||||
case "$1" in
|
||||
-a | --action)
|
||||
ACTION=$2 ; shift 2 ;;
|
||||
-d | --dev-path)
|
||||
DEV_PATH=$2 ; shift 2 ;;
|
||||
-M | --major-version)
|
||||
MAJOR_VERSION=$2 ; shift 2 ;;
|
||||
-m | --minor-version)
|
||||
MINOR_VERSION=$2 ; shift 2 ;;
|
||||
-p | --parent-path)
|
||||
PARENT_PATH=/dev/$2 ; shift 2 ;;
|
||||
-h | --help)
|
||||
usage ; shift ;;
|
||||
--) shift; break ;;
|
||||
esac
|
||||
done
|
||||
|
||||
check_mandatory_param ${ACTION:-""} "Please provide valid value for action"
|
||||
check_mandatory_param ${DEV_PATH:-""} "Please provide valid value for device path"
|
||||
|
||||
if [[ "${ACTION}" == "add" ]]; then
|
||||
check_mandatory_param ${MAJOR_VERSION:-""} "Please provide valid value for major number"
|
||||
check_mandatory_param ${MINOR_VERSION:-""} "Please provide valid value for minor number"
|
||||
sudo mknod -m a=rw ${DEV_PATH} c ${MAJOR_VERSION} ${MINOR_VERSION}
|
||||
sudo chown root:plugdev ${DEV_PATH}
|
||||
echo $(timestamp) "Added ${DEV_PATH} with major version: ${MAJOR_VERSION} and minor version: ${MINOR_VERSION} to docker" >> /tmp/docker_usb.log
|
||||
elif [[ "$ACTION" == "remove" ]]; then
|
||||
sudo rm ${DEV_PATH} ${PARENT_PATH}
|
||||
echo $(timestamp) "Removed ${DEV_PATH} ${PARENT_PATH} from docker" >> /tmp/docker_usb.log
|
||||
else
|
||||
echo "Cannot recognize action=${ACTION}"
|
||||
usage
|
||||
exit 1
|
||||
fi
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#!/bin/bash
|
||||
# install-realsense-dependencies.sh
|
||||
# Install dependencies for the Intel Realsense library librealsense2 on a Jetson Nano Developer Kit
|
||||
# Copyright (c) 2016-19 Jetsonhacks
|
||||
# MIT License
|
||||
|
||||
red=`tput setaf 1`
|
||||
green=`tput setaf 2`
|
||||
reset=`tput sgr0`
|
||||
# e.g. echo "${red}red text ${green}green text${reset}"
|
||||
echo "${green}Adding Universe repository and updating${reset}"
|
||||
apt-add-repository universe
|
||||
apt-get update
|
||||
echo "${green}Adding dependencies, graphics libraries and tools${reset}"
|
||||
apt-get install libssl-dev libusb-1.0-0-dev pkg-config -y
|
||||
|
||||
# Graphics libraries - for SDK's OpenGL-enabled examples
|
||||
apt-get install libgtk-3-dev libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev -y
|
||||
|
||||
# QtCreator for development; not required for librealsense core library
|
||||
apt-get install qtcreator -y
|
||||
|
||||
# Add Python 3 support
|
||||
apt-get install -y python3 python3-dev
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
##Version=0.1##
|
||||
# Device rules for Intel RealSense devices (D435 D435i D455)
|
||||
# For D435
|
||||
ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
|
||||
ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b07", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"
|
||||
# For D435i
|
||||
ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
|
||||
ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b3a", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"
|
||||
# For D455
|
||||
ACTION=="add" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a add -d '%N' -M '%M' -m '%m'"
|
||||
ACTION=="remove" SUBSYSTEMS=="usb", ATTRS{idVendor}=="8086", ATTRS{idProduct}=="0b5c", MODE:="0666", GROUP:="plugdev", RUN+="/opt/realsense/hotplug-realsense.sh -a remove -d '%N' -p '%P'"
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
# Based on https://github.com/stereolabs/zed-docker
|
||||
|
||||
# Download dependencies for zed SDK installation RUN file
|
||||
sudo apt-get update -y || true
|
||||
sudo apt-get install --no-install-recommends lsb-release wget less zstd udev sudo apt-transport-https -y
|
||||
|
||||
# Download zed SDK installation RUN file to /tmp directory
|
||||
cd /tmp
|
||||
|
||||
wget -q --no-check-certificate -O ZED_SDK_Linux.run https://stereolabs.sfo2.digitaloceanspaces.com/zedsdk/QA/JP5.1.2/ZED_SDK_Tegra_L4T35.4_v4.0.6.zstd.run
|
||||
chmod +x ZED_SDK_Linux.run ; ./ZED_SDK_Linux.run silent skip_od_module skip_python skip_drivers
|
||||
|
||||
# Symlink required to use the streaming features on Jetson inside a container, based on
|
||||
# https://github.com/stereolabs/zed-docker/blob/fd514606174d8bb09f21a229f1099205b284ecb6/4.X/l4t/devel/Dockerfile#L27C5-L27C95
|
||||
sudo ln -sf /usr/lib/aarch64-linux-gnu/tegra/libv4l2.so.0 /usr/lib/aarch64-linux-gnu/libv4l2.so
|
||||
|
||||
# Cleanup
|
||||
sudo rm -rf /usr/local/zed/resources/*
|
||||
rm -rf ZED_SDK_Linux.run
|
||||
sudo rm -rf /var/lib/apt/lists/*
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
# Based on https://github.com/stereolabs/zed-docker
|
||||
|
||||
# Extract ubuntu release year from /etc/lsb-release
|
||||
# Expects "/etc/lsb-release" to contain a line similar to "DISTRIB_RELEASE=20.04"
|
||||
export UBUNTU_RELEASE_YEAR="$(grep -o -P 'DISTRIB_RELEASE=.{0,2}' /etc/lsb-release | cut -d= -f2)"
|
||||
|
||||
# Extract cuda major and minor version from nvcc --version
|
||||
# Expects "nvcc --version" to contain a line similar to "release 11.8"
|
||||
export CUDA_MAJOR="$(nvcc --version | grep -o -P ' release .{0,4}' | cut -d. -f1 | cut -d ' ' -f3)"
|
||||
export CUDA_MINOR="$(nvcc --version | grep -o -P ' release .{0,4}' | cut -d. -f2)"
|
||||
|
||||
|
||||
# Download dependencies for zed SDK installation RUN file
|
||||
sudo apt-get update -y || true
|
||||
sudo apt-get install --no-install-recommends lsb-release wget less udev sudo zstd build-essential cmake libpng-dev libgomp1 -y
|
||||
|
||||
# Download zed SDK installation RUN file to /tmp directory
|
||||
cd /tmp
|
||||
wget -q -O ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run https://download.stereolabs.com/zedsdk/${ZED_SDK_MAJOR}.${ZED_SDK_MINOR}/cu${CUDA_MAJOR}${CUDA_MINOR%.*}/ubuntu${UBUNTU_RELEASE_YEAR}
|
||||
chmod +x ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run ; ./ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run -- silent skip_od_module skip_python skip_cuda
|
||||
|
||||
# Symlink required for zed SDK, based on
|
||||
# https://github.com/stereolabs/zed-docker/blob/fd514606174d8bb09f21a229f1099205b284ecb6/4.X/ubuntu/devel/Dockerfile#L24
|
||||
sudo ln -sf /lib/x86_64-linux-gnu/libusb-1.0.so.0 /usr/lib/x86_64-linux-gnu/libusb-1.0.so
|
||||
|
||||
# Cleanup
|
||||
rm ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run
|
||||
sudo rm -rf /var/lib/apt/lists/*
|
||||
|
|
@ -12,11 +12,10 @@
|
|||
echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
|
||||
source /opt/ros/${ROS_DISTRO}/setup.bash
|
||||
|
||||
# sudo apt-get update
|
||||
# rosdep update
|
||||
sudo apt-get update
|
||||
rosdep update
|
||||
|
||||
# Restart udev daemon
|
||||
sudo service udev restart
|
||||
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||||
|
||||
$@
|
||||
|
|
|
|||
|
|
@ -1,5 +1,12 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Lowlevel Control"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
|
|
@ -24,9 +31,30 @@
|
|||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"{'q': array([-0.04851204, 1.25695562, -2.80253339, 0.05441612, 1.24787235,\n",
|
||||
" -2.78187132, -0.34375334, 1.27221012, -2.80291271, 0.32734287,\n",
|
||||
" 1.27893162, -2.81329131]),\n",
|
||||
" 'dq': array([ 0.03875524, 0.0155021 , 0.0323522 , 0.07363495, 0.02712867,\n",
|
||||
" -0.0161761 , 0.01937762, 0.03487971, 0.00808805, -0.03487971,\n",
|
||||
" 0. , 0.02628616]),\n",
|
||||
" 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n",
|
||||
" 'tau_est': array([-0.04947656, 0.04947656, -0.04741504, -0.04947656, -0.04947656,\n",
|
||||
" 0. , 0. , 0.04947656, -0.04741504, 0.04947656,\n",
|
||||
" -0.07421485, 0.04741504]),\n",
|
||||
" 'temperature': array([34., 30., 29., 32., 30., 30., 36., 31., 29., 37., 31., 30.])}"
|
||||
]
|
||||
},
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"q = robot.getJointStates()['q']"
|
||||
"robot.getJointStates()"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -40,59 +68,26 @@
|
|||
"for i in range(1000000):\n",
|
||||
" # q = np.zeros(12) \n",
|
||||
" dq = np.zeros(12)\n",
|
||||
" kp = np.ones(12)*20\n",
|
||||
" kd = np.ones(12)*0.3\n",
|
||||
" kp = np.ones(12)*0.0\n",
|
||||
" kd = np.ones(12)*0.0\n",
|
||||
" tau = np.zeros(12)\n",
|
||||
" tau[0] = -0.0\n",
|
||||
" tau[0] = 0.0\n",
|
||||
" robot.setCommands(q, dq, kp, kd, tau)\n",
|
||||
" time.sleep(0.01) "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import time\n",
|
||||
"dataset = []\n",
|
||||
"for i in range(10000):\n",
|
||||
" sample = robot.getJointStates()\n",
|
||||
" dataset.append(sample['q'])\n",
|
||||
" time.sleep(0.01)"
|
||||
"# Highlevel Control"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from scipy.spatial.transform import Rotation"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"Rotation.from_matrix(np.eye(3)).as_quat()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
|
||||
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"ename": "NotImplementedError",
|
||||
"evalue": "DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.",
|
||||
|
|
@ -100,7 +95,7 @@
|
|||
"traceback": [
|
||||
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
|
||||
"\u001b[0;31mNotImplementedError\u001b[0m Traceback (most recent call last)",
|
||||
"Cell \u001b[0;32mIn[1], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01minterface\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mdds\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GO2Real\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mGO2Real\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmode\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mhighlevel\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n",
|
||||
"Cell \u001b[0;32mIn[4], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01minterface\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mdds\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GO2Real\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mGO2Real\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmode\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mhighlevel\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n",
|
||||
"File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/interface/dds.py:35\u001b[0m, in \u001b[0;36mGO2Real.__init__\u001b[0;34m(self, mode, vx_max, vy_max, ωz_max)\u001b[0m\n\u001b[1;32m 33\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m=\u001b[39m mode\n\u001b[1;32m 34\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124mhighlevel\u001b[39m\u001b[38;5;124m'\u001b[39m:\n\u001b[0;32m---> 35\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mNotImplementedError\u001b[39;00m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mDDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m 36\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msimulated \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mFalse\u001b[39;00m\n\u001b[1;32m 37\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mprestanding_q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([ \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.26186061\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 38\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.25883281\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 39\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27193761\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m, \n\u001b[1;32m 40\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27148342\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m])\n",
|
||||
"\u001b[0;31mNotImplementedError\u001b[0m: DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface."
|
||||
]
|
||||
|
|
@ -111,13 +106,6 @@
|
|||
"import time\n",
|
||||
"robot = GO2Real(mode='highlevel')"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Lowlevel Interface Test"
|
||||
"# Lowlevel Control"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -16,7 +16,7 @@
|
|||
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
|
||||
"import time\n",
|
||||
"ros2_init()\n",
|
||||
"robot = GO2Real(mode='highlevel')\n",
|
||||
"robot = GO2Real(mode='lowlevel')\n",
|
||||
"ros2_exec_manager = ROS2ExecutorManager()\n",
|
||||
"ros2_exec_manager.add_node(robot)\n",
|
||||
"ros2_exec_manager.start()"
|
||||
|
|
@ -104,7 +104,7 @@
|
|||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Highlevel Interface Test"
|
||||
"# Highlevel Control"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -130,13 +130,6 @@
|
|||
"source": [
|
||||
"robot.getJointStates()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
@ -2,45 +2,47 @@
|
|||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()\n",
|
||||
"remote = KeyboardRemote()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.sitDownReset()\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
"robot = Go2Sim()\n",
|
||||
"robot.standUpReset()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getJointStates()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import mujoco\n",
|
||||
"import time\n",
|
||||
"robot.standUpReset()\n",
|
||||
"for i in range(100000):\n",
|
||||
" state = robot.getJointStates()\n",
|
||||
" tau = 20*np.eye(12)@(robot.q0 - state['q']).reshape(12,1)\n",
|
||||
" robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
|
||||
" robot.step()"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Simulated"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.robot.model import Go2Model\n",
|
||||
"from Go2Py.estimation.contact import HysteresisContactDetector\n",
|
||||
"import pinocchio as pin \n",
|
||||
"import numpy as np\n",
|
||||
"import time"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"model = Go2Model()\n",
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 15,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"dict_keys(['M', 'Minv', 'nle', 'g', 'J_w', 'J_b'])\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"# Get robot robot states\n",
|
||||
"state = robot.getJointStates()\n",
|
||||
"\n",
|
||||
"# The pose and velocity generally is fed from an external state estimator,\n",
|
||||
"# here we just set it to identity\n",
|
||||
"T = np.eye(4)\n",
|
||||
"vel = np.zeros(6)\n",
|
||||
"\n",
|
||||
"# Compute the dynamics\n",
|
||||
"model.updateAllPose(state['q'], state['dq'], T, vel)\n",
|
||||
"\n",
|
||||
"# Get dynamic and kinematic models\n",
|
||||
"info = model.getInfo()\n",
|
||||
"print(info.keys())"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
|
|
@ -1,5 +1,12 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Test in Simulation"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
|
|
@ -7,7 +14,9 @@
|
|||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"import numpy as np"
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -17,7 +26,8 @@
|
|||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()\n",
|
||||
"robot.standUpReset()"
|
||||
"remote = KeyboardRemote()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -26,15 +36,37 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import mujoco\n",
|
||||
"robot.sitDownReset()\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Test on Real Robot"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"import time\n",
|
||||
"q,dq = robot.getJointStates()\n",
|
||||
"robot.standUpReset()\n",
|
||||
"for i in range(100000):\n",
|
||||
" state = robot.getJointStates()\n",
|
||||
" tau = 20*np.eye(12)@(robot.q0 - state['q']).reshape(12,1)\n",
|
||||
" robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), tau)\n",
|
||||
" robot.step()"
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -43,8 +75,9 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"n = robot.model.nv\n",
|
||||
"full_M = np.zeros((n, n))"
|
||||
"robot = GO2Real(mode='lowlevel')\n",
|
||||
"remote = KeyboardRemote()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -53,11 +86,7 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import mujoco\n",
|
||||
"nv = robot.model.nv\n",
|
||||
"M = np.zeros((nv, nv))\n",
|
||||
"mujoco.mj_fullM(robot.model, M, robot.data.qM)\n",
|
||||
"nle = robot.data.qfrc_bias.reshape(nv,1)"
|
||||
"fsm = FSM(robot, remote, safety)"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -66,21 +95,7 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"id = mujoco.mj_name2id(robot.model, mujoco.mjtObj.mjOBJ_SITE,'FR_foot')\n",
|
||||
"nv = robot.model.nv\n",
|
||||
"jacp = np.zeros((3, nv))\n",
|
||||
"jacr = np.zeros((3, nv))\n",
|
||||
"mujoco.mj_jacSite(robot.model, robot.data, jacp, jacr, id)\n",
|
||||
"jacp"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getSiteJacobian('FR_foot')"
|
||||
"fsm.close()"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
|
@ -100,7 +115,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.9.18"
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
@ -2,31 +2,7 @@
|
|||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"import numpy as np\n",
|
||||
"robot = GO2Real(mode='lowlevel')"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"from Go2Py.control.walk_these_ways import *\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
|
|
@ -60,7 +36,7 @@
|
|||
" omega = -robot.getRemoteState().rx*2.2\n",
|
||||
" self.command_profile.x_vel_cmd = vx*1.5\n",
|
||||
" self.command_profile.y_vel_cmd = vy*1.5\n",
|
||||
" self.command_profile.yaw_vel_cmd = omega\n"
|
||||
" self.command_profile.yaw_vel_cmd = omega"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -110,6 +86,113 @@
|
|||
" "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Test In Simulation"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.control.walk_these_ways import *"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"remote = KeyboardRemote()\n",
|
||||
"robot.standUpReset()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"controller.command_profile.pitch_cmd=0.0\n",
|
||||
"controller.command_profile.body_height_cmd=0.0\n",
|
||||
"controller.command_profile.footswing_height_cmd=0.08\n",
|
||||
"controller.command_profile.roll_cmd=0.0\n",
|
||||
"controller.command_profile.stance_width_cmd=0.2\n",
|
||||
"controller.command_profile.x_vel_cmd=-0.2\n",
|
||||
"controller.command_profile.y_vel_cmd=0.01\n",
|
||||
"controller.command_profile.setGaitType(\"trotting\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"checkpoint = \"../Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Test on Real Robot"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.control.walk_these_ways import *"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"import numpy as np\n",
|
||||
"robot = GO2Real(mode='lowlevel')"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
|
|
@ -126,13 +209,9 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
|
||||
"\n",
|
||||
"checkpoint = \"../Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)\n",
|
||||
"fsm.control_dT = 1./56."
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
|
|
@ -140,7 +219,18 @@
|
|||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
"source": [
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
@ -1,160 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.model import Go2Model\n",
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"[0.3868 0. 0. ]\n",
|
||||
"0.3868\n",
|
||||
"0.093\n",
|
||||
"0.09550000000000002\n",
|
||||
"0.213\n",
|
||||
"0.213\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"model = Go2Model()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 26,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"The task space position of the robot feet are:\n",
|
||||
"{'base_link': array([0. , 0. , 0.34]), 'FR_foot': array([ 0.19400515, -0.142 , 0.0372495 ]), 'FL_foot': array([0.19811581, 0.142 , 0.03813916]), 'RR_foot': array([-0.19303592, -0.142 , 0.03357947]), 'RL_foot': array([-0.1897387, 0.142 , 0.0330523])}\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"T0 = np.eye(4) \n",
|
||||
"T0[2,-1]=0.34\n",
|
||||
"q0 = robot.standing_q\n",
|
||||
"x0 = model.forwardKinematics(T0, q0)\n",
|
||||
"x0 = {key:x[0:3,-1] for key,x in zip(x0.keys(),x0.values())}\n",
|
||||
"print('The task space position of the robot feet are:')\n",
|
||||
"print(x0)\n",
|
||||
"x0 = np.hstack([x0[key] for key in ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']])\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 28,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/plain": [
|
||||
"array([ 0.00000000e+00, -2.22044605e-16, 2.22044605e-16, 0.00000000e+00,\n",
|
||||
" 2.22044605e-16, -4.44089210e-16, 0.00000000e+00, 1.11022302e-16,\n",
|
||||
" 0.00000000e+00, 0.00000000e+00, -1.11022302e-16, 0.00000000e+00])"
|
||||
]
|
||||
},
|
||||
"execution_count": 28,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"model.inverseKinematics(T0,x0)-q0"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 52,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"dT = 1/50.\n",
|
||||
"amplitudes = [0.01*(i+1) for i in range(8)]\n",
|
||||
"frequencies = [0.5*(i+1) for i in range(40)]\n",
|
||||
"durations = []\n",
|
||||
"\n",
|
||||
"for f in frequencies:\n",
|
||||
" durations.append(len(amplitudes)*[int(f+1)*(2*np.pi/f)])\n",
|
||||
"durations = np.array(durations).reshape(-1).tolist()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 55,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"x = []\n",
|
||||
"for i,a in enumerate(amplitudes):\n",
|
||||
" for j,f in enumerate(frequencies):\n",
|
||||
" for N in range(int(durations[i*j]//dT)):\n",
|
||||
" t = N/durations[i*j]\n",
|
||||
" x_s = a*np.cos(2*np.pi*f*t)\n",
|
||||
" y_s = a*np.sin(2*np.pi*f*t)\n",
|
||||
" x.append(x_s)\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import matplotlib.pyplot as = [] # List of Pose\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"(plan, fraction) = move_group.compute_cartesian_path, eef_step = 0.01, jump_threshold = 0.0)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.10"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,377 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from cyclonedds.domain import DomainParticipant\n",
|
||||
"from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_\n",
|
||||
"from cyclonedds.topic import Topic\n",
|
||||
"from cyclonedds.pub import DataWriter"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"participant = DomainParticipant()\n",
|
||||
"topic = Topic(participant, \"rt/go2/lowcmd\", Go2pyLowCmd_)\n",
|
||||
"writer = DataWriter(participant, topic)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import numpy as np\n",
|
||||
"msg = Go2pyLowCmd_(np.zeros(12),\n",
|
||||
"np.zeros(12),np.zeros(12),np.zeros(12),np.zeros(12))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import time\n",
|
||||
"while True:\n",
|
||||
" writer.write(msg)\n",
|
||||
" time.sleep(0.001)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.std_msgs.msg.dds_ import String_\n",
|
||||
"participant = DomainParticipant()\n",
|
||||
"topic = Topic(participant, \"rt/test\", String_)\n",
|
||||
"writer = DataWriter(participant, topic)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"msg = String_(\"Hello\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import time\n",
|
||||
"while True:\n",
|
||||
" writer.write(msg)\n",
|
||||
" time.sleep(0.001)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from cyclonedds.domain import DomainParticipant\n",
|
||||
"from cyclonedds.topic import Topic\n",
|
||||
"from cyclonedds.sub import DataReader\n",
|
||||
"from cyclonedds.util import duration\n",
|
||||
"from Go2Py.unitree_go.msg.dds_ import LowState_"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"participant = DomainParticipant()\n",
|
||||
"topic = Topic(participant, 'rt/lowstate', LowState_)\n",
|
||||
"reader = DataReader(participant, topic)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"for msg in reader.take_iter(timeout=duration(milliseconds=100.)):\n",
|
||||
" print(msg)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"reader.take(timeout=duration(milliseconds=100.))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import struct\n",
|
||||
"import threading\n",
|
||||
"import time\n",
|
||||
"import numpy as np\n",
|
||||
"import numpy.linalg as LA\n",
|
||||
"from scipy.spatial.transform import Rotation as R\n",
|
||||
"\n",
|
||||
"from cyclonedds.domain import DomainParticipant\n",
|
||||
"from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_\n",
|
||||
"from cyclonedds.topic import Topic\n",
|
||||
"from cyclonedds.pub import DataWriter\n",
|
||||
"\n",
|
||||
"from cyclonedds.domain import DomainParticipant\n",
|
||||
"from cyclonedds.topic import Topic\n",
|
||||
"from cyclonedds.sub import DataReader\n",
|
||||
"from cyclonedds.util import duration\n",
|
||||
"from Go2Py.unitree_go.msg.dds_ import LowState_\n",
|
||||
"from threading import Thread"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"class GO2Real():\n",
|
||||
" def __init__(\n",
|
||||
" self,\n",
|
||||
" mode = 'lowlevel', # 'highlevel' or 'lowlevel'\n",
|
||||
" vx_max=0.5,\n",
|
||||
" vy_max=0.4,\n",
|
||||
" ωz_max=0.5,\n",
|
||||
" ):\n",
|
||||
" assert mode in ['highlevel', 'lowlevel'], \"mode should be either 'highlevel' or 'lowlevel'\"\n",
|
||||
" self.mode = mode\n",
|
||||
" if self.mode == 'highlevel':\n",
|
||||
" raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')\n",
|
||||
"\n",
|
||||
" self.highcmd_topic_name = \"rt/go2/twist_cmd\"\n",
|
||||
" self.lowcmd_topic_name = \"rt/go2/lowcmd\"\n",
|
||||
" self.lowstate_topic_name = \"rt/lowstate\"\n",
|
||||
"\n",
|
||||
" self.participant = DomainParticipant()\n",
|
||||
" self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_)\n",
|
||||
" self.lowstate_reader = DataReader(self.participant, self.lowstate_topic)\n",
|
||||
" \n",
|
||||
" self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_)\n",
|
||||
" self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic)\n",
|
||||
"\n",
|
||||
" self.state = None\n",
|
||||
" self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode]\n",
|
||||
" self.lowstate_thread = Thread(target = self.lowstate_update)\n",
|
||||
" self.running = True\n",
|
||||
" self.lowstate_thread.start()\n",
|
||||
"\n",
|
||||
" def lowstate_update(self):\n",
|
||||
" \"\"\"\n",
|
||||
" Retrieve the state of the robot\n",
|
||||
" \"\"\"\n",
|
||||
" while self.running:\n",
|
||||
" for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=100.)):\n",
|
||||
" self.state = msg\n",
|
||||
"\n",
|
||||
" def getIMU(self):\n",
|
||||
" accel = self.state.imu_state.accelerometer\n",
|
||||
" gyro = self.state.imu_state.gyroscope\n",
|
||||
" quat = self.state.imu_state.quaternion\n",
|
||||
" rpy = self.state.imu_state.rpy\n",
|
||||
" temp = self.state.imu_state.temperature\n",
|
||||
" return {'accel':accel, 'gyro':gyro, 'quat':quat, \"rpy\":rpy, 'temp':temp}\n",
|
||||
"\n",
|
||||
" def getFootContacts(self):\n",
|
||||
" \"\"\"Returns the raw foot contact forces\"\"\"\n",
|
||||
" footContacts = self.state.foot_force \n",
|
||||
" return np.array(footContacts)\n",
|
||||
"\n",
|
||||
" def getJointStates(self):\n",
|
||||
" \"\"\"Returns the joint angles (q) and velocities (dq) of the robot\"\"\"\n",
|
||||
" motor_state = np.array([[self.state.motor_state[i].q,\n",
|
||||
" robot.state.motor_state[i].dq,\n",
|
||||
" robot.state.motor_state[i].ddq,\n",
|
||||
" robot.state.motor_state[i].tau_est,\n",
|
||||
" robot.state.motor_state[i].temperature] for i in range(12)])\n",
|
||||
" return {'q':motor_state[:,0], \n",
|
||||
" 'dq':motor_state[:,1],\n",
|
||||
" 'ddq':motor_state[:,2],\n",
|
||||
" 'tau_est':motor_state[:,3],\n",
|
||||
" 'temperature':motor_state[:,4]}\n",
|
||||
"\n",
|
||||
" def getRemoteState(self):\n",
|
||||
" \"\"\"A method to get the state of the wireless remote control. \n",
|
||||
" Returns a xRockerBtn object: \n",
|
||||
" - head: [head1, head2]\n",
|
||||
" - keySwitch: xKeySwitch object\n",
|
||||
" - lx: float\n",
|
||||
" - rx: float\n",
|
||||
" - ry: float\n",
|
||||
" - L2: float\n",
|
||||
" - ly: float\n",
|
||||
" \"\"\"\n",
|
||||
" wirelessRemote = self.state.wireless_remote[:24]\n",
|
||||
" binary_data = bytes(wirelessRemote)\n",
|
||||
"\n",
|
||||
" format_str = \"<2BH5f\"\n",
|
||||
" data = struct.unpack(format_str, binary_data)\n",
|
||||
"\n",
|
||||
" head = list(data[:2])\n",
|
||||
" lx = data[3]\n",
|
||||
" rx = data[4]\n",
|
||||
" ry = data[5]\n",
|
||||
" L2 = data[6]\n",
|
||||
" ly = data[7]\n",
|
||||
"\n",
|
||||
" _btn = bin(data[2])[2:].zfill(16)\n",
|
||||
" btn = [int(char) for char in _btn]\n",
|
||||
" btn.reverse()\n",
|
||||
"\n",
|
||||
" keySwitch = xKeySwitch(*btn)\n",
|
||||
" rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly)\n",
|
||||
" return rockerBtn\n",
|
||||
"\n",
|
||||
" def getCommandFromRemote(self):\n",
|
||||
" \"\"\"Do not use directly for control!!!\"\"\"\n",
|
||||
" rockerBtn = self.getRemoteState()\n",
|
||||
"\n",
|
||||
" lx = rockerBtn.lx\n",
|
||||
" ly = rockerBtn.ly\n",
|
||||
" rx = rockerBtn.rx\n",
|
||||
"\n",
|
||||
" v_x = ly * self.vx_max\n",
|
||||
" v_y = lx * self.vy_max\n",
|
||||
" ω = rx * self.ωz_max\n",
|
||||
" \n",
|
||||
" return v_x, v_y, ω\n",
|
||||
"\n",
|
||||
" def getBatteryState(self):\n",
|
||||
" \"\"\"Returns the battery percentage of the robot\"\"\"\n",
|
||||
" batteryState = self.state.bms_state\n",
|
||||
" return batteryState.soc\n",
|
||||
"\n",
|
||||
" def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):\n",
|
||||
" self.cmd_watchdog_timer = time.time()\n",
|
||||
" _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)\n",
|
||||
" self.highcmd.header.stamp = self.get_clock().now().to_msg()\n",
|
||||
" self.highcmd.header.frame_id = \"base_link\"\n",
|
||||
" self.highcmd.twist.linear.x = _v_x\n",
|
||||
" self.highcmd.twist.linear.y = _v_y\n",
|
||||
" self.highcmd.twist.angular.z = _ω_z\n",
|
||||
" self.highcmd_publisher.publish(self.highcmd)\n",
|
||||
"\n",
|
||||
" def setCommandsLow(self, q, dq, kp, kd, tau_ff):\n",
|
||||
" assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, \"q, dq, kp, kd, tau_ff should have size 12\"\n",
|
||||
" lowcmd = Go2pyLowCmd_(\n",
|
||||
" q,\n",
|
||||
" dq, \n",
|
||||
" kp,\n",
|
||||
" kd,\n",
|
||||
" tau_ff\n",
|
||||
" )\n",
|
||||
" self.lowcmd_writer.write(lowcmd)\n",
|
||||
"\n",
|
||||
" def close(self):\n",
|
||||
" self.running = False\n",
|
||||
"\n",
|
||||
" def check_calf_collision(self, q):\n",
|
||||
" self.pin_robot.update(q)\n",
|
||||
" in_collision = self.pin_robot.check_calf_collision(q)\n",
|
||||
" return in_collision\n",
|
||||
"\n",
|
||||
" def clip_velocity(self, v_x, v_y, ω_z):\n",
|
||||
" _v = np.array([[v_x], [v_y]])\n",
|
||||
" _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]\n",
|
||||
"\n",
|
||||
" if _scale > 1.0:\n",
|
||||
" scale = 1.0 / _scale\n",
|
||||
" else:\n",
|
||||
" scale = 1.0\n",
|
||||
"\n",
|
||||
" return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getBatteryState()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"robot = GO2Real()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getIMU()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
|
|
@ -1,69 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real\n",
|
||||
"import time\n",
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = GO2Real(mode='lowlevel')\n",
|
||||
"remote = KeyboardRemote()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm = FSM(robot, remote, safety)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "b1-env",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
|
|
@ -1,156 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Joystick Control\n",
|
||||
"As the first step after successfully communicating with the robot's onboard controller, we use a USB joystick (a Logitech Extreme 3D Pro) to command the robot with desired $x$, $y$, and $\\Psi$ velocities.\n",
|
||||
"\n",
|
||||
"First, use the Pygame-based joystick class provided with B1Py to connect to the joystick:"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
|
||||
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"from Go2Py.joy import Logitech3DPro\n",
|
||||
"# joy = Logitech3DPro(joy_id=0) "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"***Note:*** Initially when you instantiate the joystick, the class performs an offset calibration to maker sure the neutral configuration of the device reads zero. It is important to leave the device untouched during the couple of seconds after calling the cell above. The prompts tells you when you can continue. \n",
|
||||
"\n",
|
||||
"Then as explained in [hardware interface documentation](unitree_locomotion_controller_interface.ipynb), instantiate and communication link to high-level controller on the robot. Note that the robot should be standing and ready to walk before we can control it. Furthermore, the the highlevel node in b1py_node ROS2 package (in `deploy/ros2_ws`) should be launched on the robot. "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n",
|
||||
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
|
||||
"import time\n",
|
||||
"ros2_init()\n",
|
||||
"robot = GO2Real(mode='highlevel')\n",
|
||||
"ros2_exec_manager = ROS2ExecutorManager()\n",
|
||||
"ros2_exec_manager.add_node(robot)\n",
|
||||
"ros2_exec_manager.start()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"Finally, read the joystick values and send them to the robot in a loop:"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"Duration = 20\n",
|
||||
"N = Duration//0.01\n",
|
||||
"for i in range(500):\n",
|
||||
" cmd = joy.readAnalog()\n",
|
||||
" vx = cmd['x']\n",
|
||||
" vy = cmd['y']\n",
|
||||
" w = cmd['z'] / 2\n",
|
||||
" body_height = cmd['aux'] / 10\n",
|
||||
" robot.setCommands(vx,vy,w, bodyHeight=body_height)\n",
|
||||
" time.sleep(0.01)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 50,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"odometry = []\n",
|
||||
"imus = []\n",
|
||||
"joints = []\n",
|
||||
"foot_contacts = []\n",
|
||||
"stamps = []\n",
|
||||
"\n",
|
||||
"for i in range(60*200):\n",
|
||||
" time.sleep(1/200)\n",
|
||||
" odom = robot.getOdometry()\n",
|
||||
" contact = robot.getFootContacts()\n",
|
||||
" joint = robot.getJointStates()\n",
|
||||
" imu = robot.getIMU()\n",
|
||||
" imus.append(imu)\n",
|
||||
" joints.append(joint)\n",
|
||||
" foot_contacts.append(contact)\n",
|
||||
" stamps.append(time.time())\n",
|
||||
" odometry.append(odom)\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 51,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import pickle\n",
|
||||
"with open('data/april16-seq2.pkl', 'wb') as f:\n",
|
||||
" pickle.dump(\n",
|
||||
" {\n",
|
||||
" 'imu':imus,\n",
|
||||
" 'odometry':odometry,\n",
|
||||
" 'joints':joints,\n",
|
||||
" 'foot_contacts':foot_contacts,\n",
|
||||
" 'stamps':stamps\n",
|
||||
" }, f\n",
|
||||
" )"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.10"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
|
|
@ -1,206 +0,0 @@
|
|||
import isaacgym
|
||||
|
||||
assert isaacgym
|
||||
import matplotlib.pyplot as plt
|
||||
import torch
|
||||
from tqdm import trange
|
||||
|
||||
from Go2Py.sim.gym.envs import *
|
||||
from Go2Py.sim.gym.envs.base.legged_robot_config import Cfg
|
||||
from Go2Py.sim.gym.envs.go2.go2_config import config_go2
|
||||
from Go2Py.sim.gym.envs.go2.velocity_tracking import VelocityTrackingEasyEnv
|
||||
|
||||
|
||||
def run_env(render=False, headless=False):
|
||||
# prepare environment
|
||||
config_go2(Cfg)
|
||||
|
||||
Cfg.commands.num_lin_vel_bins = 30
|
||||
Cfg.commands.num_ang_vel_bins = 30
|
||||
Cfg.curriculum_thresholds.tracking_ang_vel = 0.7
|
||||
Cfg.curriculum_thresholds.tracking_lin_vel = 0.8
|
||||
Cfg.curriculum_thresholds.tracking_contacts_shaped_vel = 0.9
|
||||
Cfg.curriculum_thresholds.tracking_contacts_shaped_force = 0.9
|
||||
|
||||
Cfg.commands.distributional_commands = True
|
||||
|
||||
Cfg.env.priv_observe_motion = False
|
||||
Cfg.env.priv_observe_gravity_transformed_motion = True
|
||||
Cfg.domain_rand.randomize_friction_indep = False
|
||||
Cfg.env.priv_observe_friction_indep = False
|
||||
Cfg.domain_rand.randomize_friction = True
|
||||
Cfg.env.priv_observe_friction = False
|
||||
Cfg.domain_rand.friction_range = [0.0, 0.0]
|
||||
Cfg.domain_rand.randomize_restitution = True
|
||||
Cfg.env.priv_observe_restitution = False
|
||||
Cfg.domain_rand.restitution_range = [0.0, 1.0]
|
||||
Cfg.domain_rand.randomize_base_mass = True
|
||||
Cfg.env.priv_observe_base_mass = False
|
||||
Cfg.domain_rand.added_mass_range = [-1.0, 3.0]
|
||||
Cfg.domain_rand.randomize_gravity = True
|
||||
Cfg.domain_rand.gravity_range = [-1.0, 1.0]
|
||||
Cfg.domain_rand.gravity_rand_interval_s = 2.0
|
||||
Cfg.domain_rand.gravity_impulse_duration = 0.5
|
||||
Cfg.env.priv_observe_gravity = True
|
||||
Cfg.domain_rand.randomize_com_displacement = False
|
||||
Cfg.domain_rand.com_displacement_range = [-0.15, 0.15]
|
||||
Cfg.env.priv_observe_com_displacement = False
|
||||
Cfg.domain_rand.randomize_ground_friction = True
|
||||
Cfg.env.priv_observe_ground_friction = False
|
||||
Cfg.env.priv_observe_ground_friction_per_foot = False
|
||||
Cfg.domain_rand.ground_friction_range = [0.3, 2.0]
|
||||
Cfg.domain_rand.randomize_motor_strength = True
|
||||
Cfg.domain_rand.motor_strength_range = [0.9, 1.1]
|
||||
Cfg.env.priv_observe_motor_strength = False
|
||||
Cfg.domain_rand.randomize_motor_offset = True
|
||||
Cfg.domain_rand.motor_offset_range = [-0.02, 0.02]
|
||||
Cfg.env.priv_observe_motor_offset = False
|
||||
Cfg.domain_rand.push_robots = False
|
||||
Cfg.domain_rand.randomize_Kp_factor = False
|
||||
Cfg.env.priv_observe_Kp_factor = False
|
||||
Cfg.domain_rand.randomize_Kd_factor = False
|
||||
Cfg.env.priv_observe_Kd_factor = False
|
||||
Cfg.env.priv_observe_body_velocity = False
|
||||
Cfg.env.priv_observe_body_height = False
|
||||
Cfg.env.priv_observe_desired_contact_states = False
|
||||
Cfg.env.priv_observe_contact_forces = False
|
||||
Cfg.env.priv_observe_foot_displacement = False
|
||||
|
||||
Cfg.env.num_privileged_obs = 3
|
||||
Cfg.env.num_observation_history = 30
|
||||
Cfg.reward_scales.feet_contact_forces = 0.0
|
||||
|
||||
Cfg.domain_rand.rand_interval_s = 4
|
||||
Cfg.commands.num_commands = 15
|
||||
Cfg.env.observe_two_prev_actions = True
|
||||
Cfg.env.observe_yaw = True
|
||||
Cfg.env.num_observations = 71
|
||||
Cfg.env.num_scalar_observations = 71
|
||||
Cfg.env.observe_gait_commands = True
|
||||
Cfg.env.observe_timing_parameter = False
|
||||
Cfg.env.observe_clock_inputs = True
|
||||
|
||||
Cfg.domain_rand.tile_height_range = [-0.0, 0.0]
|
||||
Cfg.domain_rand.tile_height_curriculum = False
|
||||
Cfg.domain_rand.tile_height_update_interval = 1000, 3000
|
||||
Cfg.domain_rand.tile_height_curriculum_step = 0.01
|
||||
Cfg.terrain.border_size = 0.0
|
||||
|
||||
Cfg.commands.resampling_time = 10
|
||||
|
||||
Cfg.reward_scales.feet_slip = -0.04
|
||||
Cfg.reward_scales.action_smoothness_1 = -0.1
|
||||
Cfg.reward_scales.action_smoothness_2 = -0.1
|
||||
Cfg.reward_scales.dof_vel = -1e-4
|
||||
Cfg.reward_scales.dof_pos = -0.05
|
||||
Cfg.reward_scales.jump = 10.0
|
||||
Cfg.reward_scales.base_height = 0.0
|
||||
Cfg.rewards.base_height_target = 0.30
|
||||
Cfg.reward_scales.estimation_bonus = 0.0
|
||||
|
||||
Cfg.reward_scales.feet_impact_vel = -0.0
|
||||
|
||||
# rewards.footswing_height = 0.09
|
||||
Cfg.reward_scales.feet_clearance = -0.0
|
||||
Cfg.reward_scales.feet_clearance_cmd = -15.
|
||||
|
||||
# reward_scales.feet_contact_forces = -0.01
|
||||
|
||||
Cfg.rewards.reward_container_name = "CoRLRewards"
|
||||
Cfg.rewards.only_positive_rewards = False
|
||||
Cfg.rewards.only_positive_rewards_ji22_style = True
|
||||
Cfg.rewards.sigma_rew_neg = 0.02
|
||||
|
||||
Cfg.reward_scales.hop_symmetry = 0.0
|
||||
Cfg.rewards.kappa_gait_probs = 0.07
|
||||
Cfg.rewards.gait_force_sigma = 100.
|
||||
Cfg.rewards.gait_vel_sigma = 10.
|
||||
|
||||
Cfg.reward_scales.tracking_contacts_shaped_force = 4.0
|
||||
Cfg.reward_scales.tracking_contacts_shaped_vel = 4.0
|
||||
|
||||
Cfg.reward_scales.collision = -5.0
|
||||
|
||||
Cfg.commands.lin_vel_x = [-1.0, 1.0]
|
||||
Cfg.commands.lin_vel_y = [-0.6, 0.6]
|
||||
Cfg.commands.ang_vel_yaw = [-1.0, 1.0]
|
||||
Cfg.commands.body_height_cmd = [-0.25, 0.15]
|
||||
Cfg.commands.gait_frequency_cmd_range = [1.5, 4.0]
|
||||
Cfg.commands.gait_phase_cmd_range = [0.0, 1.0]
|
||||
Cfg.commands.gait_offset_cmd_range = [0.0, 1.0]
|
||||
Cfg.commands.gait_bound_cmd_range = [0.0, 1.0]
|
||||
Cfg.commands.gait_duration_cmd_range = [0.5, 0.5]
|
||||
Cfg.commands.footswing_height_range = [0.03, 0.25]
|
||||
|
||||
Cfg.reward_scales.lin_vel_z = -0.02
|
||||
Cfg.reward_scales.ang_vel_xy = -0.001
|
||||
Cfg.reward_scales.base_height = 0.0
|
||||
Cfg.reward_scales.feet_air_time = 0.0
|
||||
|
||||
Cfg.commands.limit_vel_x = [-5.0, 5.0]
|
||||
Cfg.commands.limit_vel_y = [-0.6, 0.6]
|
||||
Cfg.commands.limit_vel_yaw = [-5.0, 5.0]
|
||||
Cfg.commands.limit_body_height = [-0.25, 0.15]
|
||||
Cfg.commands.limit_gait_frequency = [1.5, 4.0]
|
||||
Cfg.commands.limit_gait_phase = [0.0, 1.0]
|
||||
Cfg.commands.limit_gait_offset = [0.0, 1.0]
|
||||
Cfg.commands.limit_gait_bound = [0.0, 1.0]
|
||||
Cfg.commands.limit_gait_duration = [0.5, 0.5]
|
||||
Cfg.commands.limit_footswing_height = [0.03, 0.25]
|
||||
|
||||
Cfg.commands.num_bins_vel_x = 21
|
||||
Cfg.commands.num_bins_vel_y = 1
|
||||
Cfg.commands.num_bins_vel_yaw = 21
|
||||
Cfg.commands.num_bins_body_height = 1
|
||||
Cfg.commands.num_bins_gait_frequency = 1
|
||||
Cfg.commands.num_bins_gait_phase = 1
|
||||
Cfg.commands.num_bins_gait_offset = 1
|
||||
Cfg.commands.num_bins_gait_bound = 1
|
||||
Cfg.commands.num_bins_gait_duration = 1
|
||||
Cfg.commands.num_bins_footswing_height = 1
|
||||
|
||||
Cfg.normalization.friction_range = [0, 1]
|
||||
Cfg.normalization.ground_friction_range = [0, 1]
|
||||
|
||||
Cfg.commands.exclusive_phase_offset = False
|
||||
Cfg.commands.pacing_offset = False
|
||||
Cfg.commands.binary_phases = True
|
||||
Cfg.commands.gaitwise_curricula = True
|
||||
|
||||
# 5 times per second
|
||||
|
||||
Cfg.env.num_envs = 3
|
||||
Cfg.domain_rand.push_interval_s = 1
|
||||
Cfg.terrain.num_rows = 3
|
||||
Cfg.terrain.num_cols = 5
|
||||
Cfg.terrain.border_size = 0
|
||||
Cfg.domain_rand.randomize_friction = True
|
||||
Cfg.domain_rand.friction_range = [1.0, 1.01]
|
||||
Cfg.domain_rand.randomize_base_mass = True
|
||||
Cfg.domain_rand.added_mass_range = [0., 6.]
|
||||
Cfg.terrain.terrain_noise_magnitude = 0.0
|
||||
# Cfg.asset.fix_base_link = True
|
||||
|
||||
Cfg.domain_rand.lag_timesteps = 6
|
||||
Cfg.domain_rand.randomize_lag_timesteps = True
|
||||
Cfg.control.control_type = "P"
|
||||
|
||||
env = VelocityTrackingEasyEnv(sim_device='cuda:0', headless=False, cfg=Cfg)
|
||||
env.reset()
|
||||
|
||||
if render and headless:
|
||||
img = env.render(mode="rgb_array")
|
||||
plt.imshow(img)
|
||||
plt.show()
|
||||
print("Show the first frame and exit.")
|
||||
exit()
|
||||
|
||||
for i in trange(1000, desc="Running"):
|
||||
actions = 0. * torch.ones(env.num_envs, env.num_actions, device=env.device)
|
||||
obs, rew, done, info = env.step(actions)
|
||||
# breakpoint()
|
||||
print("Done")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
run_env(render=True, headless=False)
|
||||
|
|
@ -1,73 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.dds import GO2Real "
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = GO2Real()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import time\n",
|
||||
"while True:\n",
|
||||
" time.sleep(0.1)\n",
|
||||
" print(robot.getRemoteState())"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.remote import UnitreeRemote"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"remote = UnitreeRemote(robot)"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "b1py",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
|
|
@ -1,147 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.robot.model import Go2Model\n",
|
||||
"import pinocchio as pin \n",
|
||||
"import numpy as np\n",
|
||||
"import time"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"[0.3868 0. 0. ]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"model = Go2Model()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.step()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"omega = robot.getIMU()['gyro']\n",
|
||||
"def hat(w):\n",
|
||||
" return np.array([[0., -w[2], w[1]],\n",
|
||||
" [w[2], 0., -w[0]],\n",
|
||||
" [-w[1], w[0], 0.]])"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.standUpReset()\n",
|
||||
"while True:\n",
|
||||
" time.sleep(0.001)\n",
|
||||
" robot.setCommands(robot.standing_q, np.zeros(12), np.ones(12)*100, np.ones(12)*0.1, np.zeros(12))\n",
|
||||
" robot.step() \n",
|
||||
" trans, quat = robot.getPose()\n",
|
||||
" state = robot.getJointStates()\n",
|
||||
" quat = pin.Quaternion(np.hstack([quat[1:],quat[0]]))\n",
|
||||
" R = quat.matrix()\n",
|
||||
" T = np.eye(4)\n",
|
||||
" T[0:3,0:3] = R\n",
|
||||
" vel = np.zeros(6)\n",
|
||||
" model.update(state['q'], state['dq'],T,vel)\n",
|
||||
"\n",
|
||||
" J = model.getInfo()['J']['FR_foot'][:,6:]\n",
|
||||
" nle = model.getInfo()['nle'][6:]\n",
|
||||
" # tau = (robot.data.qfrc_smooth+robot.data.qfrc_constraint)[6:]\n",
|
||||
" tau = state['tau_est'].squeeze()\n",
|
||||
" print(np.linalg.pinv(J.T)[0:3]@(tau - nle))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"question: What exactly is the difference between the local world aligned and local jacobians in the pinocchio? Why it fits my expectations for the FR3 and not the Go2?"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getJointStates()\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"J[3:,:].T"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.10"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
|
|
@ -1,140 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Lowlevel Interface Test"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
|
||||
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
|
||||
"from Go2Py.robot.interface.ros2 import ROS2TFInterface\n",
|
||||
"import time\n",
|
||||
"ros2_init()\n",
|
||||
"vicon = ROS2TFInterface('vicon/World', 'vicon/GO2/GO2', 'vicon')\n",
|
||||
"robot = GO2Real(mode='highlevel')\n",
|
||||
"ros2_exec_manager = ROS2ExecutorManager()\n",
|
||||
"ros2_exec_manager.add_node(robot)\n",
|
||||
"ros2_exec_manager.add_node(vicon)\n",
|
||||
"ros2_exec_manager.start()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"for i in range(1000):\n",
|
||||
" time.sleep(0.01)\n",
|
||||
" imu = robot.getIMU()\n",
|
||||
" accel = imu['accel']\n",
|
||||
" gyro = imu['gyro']\n",
|
||||
" q = imu['quat']\n",
|
||||
" pose = vicon.get_pose()\n",
|
||||
" stamp = time.time()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import time\n",
|
||||
"import numpy as np\n",
|
||||
"im = []\n",
|
||||
"acc = []\n",
|
||||
"gy = []\n",
|
||||
"quaternion = []\n",
|
||||
"pos = []\n",
|
||||
"stamps = []\n",
|
||||
"for i in range(2000):\n",
|
||||
" time.sleep(0.01)\n",
|
||||
" imu = robot.getIMU()\n",
|
||||
" accel = imu['accel']\n",
|
||||
" gyro = imu['gyro']\n",
|
||||
" q = imu['quat']\n",
|
||||
" pose = vicon.get_pose()\n",
|
||||
" stamp = time.time()\n",
|
||||
" im.append(imu)\n",
|
||||
" acc.append(accel)\n",
|
||||
" gy.append(gyro)\n",
|
||||
" quaternion.append(q)\n",
|
||||
" pos.append(pose)\n",
|
||||
" stamps.append(stamp)\n",
|
||||
"\n",
|
||||
"imu = np.array(im)\n",
|
||||
"accel = np.array(acc)\n",
|
||||
"gyro = np.array(gy)\n",
|
||||
"q = np.array(quaternion)\n",
|
||||
"pose = np.array(pos)\n",
|
||||
"stamp = np.array(stamps)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"import pickle \n",
|
||||
"with open('vicon2gt_5.pkl', 'wb') as f:\n",
|
||||
" pickle.dump(\n",
|
||||
" {\n",
|
||||
" 'imu':imu,\n",
|
||||
" 'accel':accel,\n",
|
||||
" 'gyro':gyro,\n",
|
||||
" 'q':q,\n",
|
||||
" 'pose':pose,\n",
|
||||
" 'stamp':stamp\n",
|
||||
" }\n",
|
||||
" ,f\n",
|
||||
" )"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,4 +0,0 @@
|
|||
from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager
|
||||
import time
|
||||
ros2_init()
|
||||
robot = GO2Real(mode='highlevel')
|
||||
|
|
@ -1,263 +0,0 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Basic Test"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()\n",
|
||||
"robot.standUpReset()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.control.walk_these_ways import *\n",
|
||||
"# checkpoint_path = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"checkpoint_path = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
|
||||
"\n",
|
||||
"\n",
|
||||
"cfg = loadParameters(checkpoint_path)\n",
|
||||
"policy = Policy(checkpoint_path)\n",
|
||||
"command_profile = CommandInterface()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"agent = WalkTheseWaysAgent(cfg, command_profile, robot)\n",
|
||||
"agent = HistoryWrapper(agent)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 5,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"control_dt = cfg[\"control\"][\"decimation\"] * cfg[\"sim\"][\"dt\"]\n",
|
||||
"simulation_dt = robot.dt\n",
|
||||
"obs = agent.reset()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"frq: 15.967047985229458 Hz\n",
|
||||
"frq: 42.47353444522081 Hz\n",
|
||||
"frq: 43.69977078558033 Hz\n",
|
||||
"frq: 40.20806211954177 Hz\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"robot.reset()\n",
|
||||
"obs = agent.reset()\n",
|
||||
"for i in range(50000):\n",
|
||||
" policy_info = {}\n",
|
||||
" action = policy(obs, policy_info)\n",
|
||||
" if i % (control_dt // simulation_dt) == 0:\n",
|
||||
" obs, ret, done, info = agent.step(action)\n",
|
||||
" robot.step()\n",
|
||||
" command_profile.yaw_vel_cmd = 0.0\n",
|
||||
" command_profile.x_vel_cmd = 0.0\n",
|
||||
" command_profile.y_vel_cmd = 0.0\n",
|
||||
" time.sleep(robot.dt/4)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# Full System With FSM"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.fsm import FSM\n",
|
||||
"from Go2Py.robot.remote import KeyboardRemote\n",
|
||||
"from Go2Py.robot.safety import SafetyHypervisor\n",
|
||||
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||
"from Go2Py.control.walk_these_ways import *\n",
|
||||
"import numpy as np"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot = Go2Sim()"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"class walkTheseWaysController:\n",
|
||||
" def __init__(self, robot, remote, checkpoint):\n",
|
||||
" self.remote = remote\n",
|
||||
" self.robot = robot\n",
|
||||
" self.cfg = loadParameters(checkpoint)\n",
|
||||
" self.policy = Policy(checkpoint)\n",
|
||||
" self.command_profile = CommandInterface()\n",
|
||||
" self.agent = WalkTheseWaysAgent(self.cfg, self.command_profile, self.robot)\n",
|
||||
" self.agent = HistoryWrapper(self.agent)\n",
|
||||
" self.init()\n",
|
||||
"\n",
|
||||
" def init(self):\n",
|
||||
" self.obs = self.agent.reset()\n",
|
||||
" self.policy_info = {}\n",
|
||||
" self.command_profile.yaw_vel_cmd = 0.0\n",
|
||||
" self.command_profile.x_vel_cmd = 0.0\n",
|
||||
" self.command_profile.y_vel_cmd = 0.0\n",
|
||||
" self.command_profile.stance_width_cmd=0.25\n",
|
||||
" self.command_profile.footswing_height_cmd=0.08\n",
|
||||
" self.command_profile.step_frequency_cmd = 3.0\n",
|
||||
" self.command_profile.body_height_cmd = 0.5\n",
|
||||
"\n",
|
||||
" def update(self, robot, remote):\n",
|
||||
" action = self.policy(self.obs, self.policy_info)\n",
|
||||
" self.obs, self.ret, self.done, self.info = self.agent.step(action)\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"remote = KeyboardRemote()\n",
|
||||
"robot.standUpReset()\n",
|
||||
"safety_hypervisor = SafetyHypervisor(robot)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 5,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
|
||||
"# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
|
||||
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
|
||||
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"controller.command_profile.pitch_cmd=0.0\n",
|
||||
"controller.command_profile.body_height_cmd=0.0\n",
|
||||
"controller.command_profile.footswing_height_cmd=0.08\n",
|
||||
"controller.command_profile.roll_cmd=0.0\n",
|
||||
"controller.command_profile.stance_width_cmd=0.2\n",
|
||||
"controller.command_profile.x_vel_cmd=-0.2\n",
|
||||
"controller.command_profile.y_vel_cmd=0.01\n",
|
||||
"controller.command_profile.setGaitType(\"trotting\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"controller.command_profile.pitch_cmd=0.0\n",
|
||||
"controller.command_profile.body_height_cmd=0.0\n",
|
||||
"controller.command_profile.footswing_height_cmd=0.08\n",
|
||||
"controller.command_profile.roll_cmd=0.0\n",
|
||||
"controller.command_profile.stance_width_cmd=0.2\n",
|
||||
"controller.command_profile.x_vel_cmd=-0.2\n",
|
||||
"controller.command_profile.y_vel_cmd=0.01\n",
|
||||
"controller.command_profile.setGaitType(\"trotting\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"fsm.close()"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "b1-env",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 2
|
||||
}
|
||||
Loading…
Reference in New Issue