diff --git a/.gitignore b/.gitignore index 33bc90c7..258144fe 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,138 @@ +# Created by .ignore support plugin (hsz.mobi) +### Python template +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +pip-wheel-metadata/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +.python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ +.idea/ +wandb/ +.DS_Store *.DS_Store -*.pyc -**/__pycache__ +*.hdf5 +scripts/*.npz +scripts/*.pkl \ No newline at end of file diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 5b34c5e2..00000000 --- a/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2020 Stanford Student Robotics - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/README.md b/README.md index 1ed2ac3a..c72907d8 100644 --- a/README.md +++ b/README.md @@ -1,41 +1,130 @@ -# Stanford Quadruped - -## Overview -This repository hosts the code for Stanford Pupper and Stanford Woofer, Raspberry Pi-based quadruped robots that can trot, walk, and jump. +# Stanford Quadruped Gym +This repo is adapted from the original Standford Quadruped repo at https://github.com/stanfordroboticsclub/StanfordQuadruped and we've added an additional simulator and gym-compatible environments + ![Pupper CC Max Morse](https://live.staticflickr.com/65535/49614690753_78edca83bc_4k.jpg) -Video of pupper in action: https://youtu.be/NIjodHA78UE -Project page: https://stanfordstudentrobotics.org/pupper +## Installation + +We assume you got an environment where `pip` and `python` point to the Python 3 binaries. This repo was tested on Python 3.6. + +```bash +git clone https://github.com/fgolemo/StanfordQuadruped.git +cd StanfordQuadruped +pip install -e .[sim] +``` + +#### Robot installation + +```bash +sudo pip install -e .[robot] +``` + +## Getting Started + +The new simulator lives in `stanford_quad/sim/simulator2.py` in the `PupperSim2` class. You can run the simulator in dev mode by running the script + + python scripts/07-debug-new-simulator.py + +## Gym Environments + +There are currently 11 environments. All come in 2 variants: `Headless` and `Graphical`. `Headless` is meant for training and launches the PyBullet sim without any GUI, `Graphical` is meant for local debugging, offers a GUI to inspect the robot and is significantly slower. + +You can try out one of the walking environments, by running: + + python scripts/08-run-walker-gym-env.py + +### Walking + +In all environments, the observations are the same: + +**Observation space**: +- 12 leg joints in the order + - front right hip + - front right upper leg + - front right lower leg + - front left hip/upper/lower leg + - back right hip/upper/lower leg + - back left hip/upper/lower leg +- 3 body orientation in euler angles +- 2 linear velocity (only along the plane, we don't care about z velocity + +The joints are normalized to be in [-1,1] but the orientation and velocity can be arbitrarily large. + +The **action space** in both environments is also 12-dimensional (corresponding to the same 12 joints as above in that order) and also normalized in [-1,1] but the effects are different between both environments (see below). + +In both environments, the goal is to walk/run as fast as possible straight forward. This means the **reward** is calculated as relative increase of x position with respect to the previous timestep (minus a small penalty term for high action values, same as in HalfCheetah-v2) + +#### Parameters + +There are several settings for the walking environment and a handful of combinations of these parameters have been given dedicated names. Here's the list of parameters and their meaning: + +- `debug` (bool): If True, this will turn ON the GUI. Usually the `[Headless|Graphical]` name in the environment specifies that this is False/True respectively. +- `steps` (int): default 120, Length of an episode. This corresponds to 2 seconds at 60Hz. +- `relative_action` (bool): If False, action commands correspond directly to the joint positions. If True, then at each step, the actions are added to the stable resting position, i.e. instead of `robot.move(action)`, it's `robot.move(REST_POSE+action)`. +- `action_scaling` (float): By default, the robot has a large movement range and very responsive joints, meaning the policy can pick the maximum negative joint position in one step and the maximum positive joint position in the next step. This causes a lot of jitter. In order to reduce this, this setting allows to restrict the movement range. Best used in combination with `relative_action`. +- `action_smoothing` (int): Another method to reduce jitter. If this is larger than 1, actions aren't applied to the robot directly anymore but instead go into a queue of this length. At each step, the mean of this queue is applied to the robot. +- `random_rot` (3-tuple of floats): This allows to specify the initial random rotation. The 3 elements in the triple correspond to rotation around the x/y/z axes respectively. The rotations are drawn from a normal distribution centered at 0 and this value specifies the variance on each axis. Values are in degrees. +- `reward_stability` (float): Specifies the coefficient of the IMU reward term that encourages stability. By default, it's 0. + +Based on our previous experiments, the following set of parameters seem to perform best (corresponding to the environment **`Pupper-Walk-Relative-ScaledNSmoothed3-RandomZRot-Headless-v0`**): + +```python +params = { + "debug": False, + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "action_smoothing": 3, + "random_rot": (0,0,15), + "reward_stability": 0 +} +``` + +#### Pupper-Walk-Absolute-[Headless|Graphical]-v0 + +In this environment, you have full absolute control over all joints and their resting position is set to 0 (which looks unnatural - the robot is standing fully straight, legs extended). Any action command is sent directly to the joints. + +#### Pupper-Walk-Relative-[Headless|Graphical]-v0 + +In this env, your actions are relative to the resting position (see image at the top - roughly that). Meaning an action of `[0]*12` will put the Pupper to a stable rest. Action clipping is done after summing the current action and the action corresponding to the resting position, which means the action space is asymmetric - e.g. if a given joint's resting position is at `0.7`, then the action space for that joint is `[-1.7,.3]`. This is intentional because it allows the Pupper to start with a stable position. + +#### Pupper-Walk-Relative-ScaledDown_[0.05|0.1|0.15|...|0.5]-[Headless|Graphical]-v0 + +Similar to the `Pupper-Walk-Relative` but here the actions are multiplied with a factor (in the environment name) to reduce the range of motion. + +#### Pupper-Walk-Relative-ScaledDown-RandomZRot-[Headless|Graphical]-v0 + +Like the `Pupper-Walk-Relative-ScaledDown_0.3` but with random initial z rotation (rotation is drawn from a normal distribution, centered at 0, variance of 15 degrees). + +#### Pupper-Walk-Relative-ScaledNSmoothed3-RandomZRot-[Headless|Graphical]-v0 + +Like `Pupper-Walk-Relative-ScaledDown-RandomZRot` but with an additional action smoothing of **3**. + +#### Pupper-Walk-Relative-ScaledNSmoothed5-RandomZRot-[Headless|Graphical]-v0 -Documentation & build guide: https://pupper.readthedocs.io/en/latest/ +Like `Pupper-Walk-Relative-ScaledDown-RandomZRot` but with an additional action smoothing of **5**. -## How it works -![Overview diagram](imgs/diagram1.jpg) -The main program is ```run_robot.py``` which is located in this directory. The robot code is run as a loop, with a joystick interface, a controller, and a hardware interface orchestrating the behavior. +#### Pupper-Walk-Relative-Smoothed5-RandomZRot-[Headless|Graphical]-v0 -The joystick interface is responsible for reading joystick inputs from a UDP socket and converting them into a generic robot ```command``` type. A separate program, ```joystick.py```, publishes these UDP messages, and is responsible for reading inputs from the PS4 controller over bluetooth. The controller does the bulk of the work, switching between states (trot, walk, rest, etc) and generating servo position targets. A detailed model of the controller is shown below. The third component of the code, the hardware interface, converts the position targets from the controller into PWM duty cycles, which it then passes to a Python binding to ```pigpiod```, which then generates PWM signals in software and sends these signals to the motors attached to the Raspberry Pi. -![Controller diagram](imgs/diagram2.jpg) -This diagram shows a breakdown of the robot controller. Inside, you can see four primary components: a gait scheduler (also called gait controller), a stance controller, a swing controller, and an inverse kinematics model. +Like `Pupper-Walk-Relative-ScaledNSmoothed5-RandomZRot` but with the actions only averaged in a list of 5, not scaled. -The gait scheduler is responsible for planning which feet should be on the ground (stance) and which should be moving forward to the next step (swing) at any given time. In a trot for example, the diagonal pairs of legs move in sync and take turns between stance and swing. As shown in the diagram, the gait scheduler can be thought of as a conductor for each leg, switching it between stance and swing as time progresses. +#### Pupper-Walk-Relative-RewardStable0.5-[Headless|Graphical]-v0 -The stance controller controls the feet on the ground, and is actually quite simple. It looks at the desired robot velocity, and then generates a body-relative target velocity for these stance feet that is in the opposite direction as the desired velocity. It also incorporates turning, in which case it rotates the feet relative to the body in the opposite direction as the desired body rotation. +Like `Pupper-Walk-Relative` but with the additional reward term for body orientation close to zero. Coefficient for the reward term is 0.5 -The swing controller picks up the feet that just finished their stance phase, and brings them to their next touchdown location. The touchdown locations are selected so that the foot moves the same distance forward in swing as it does backwards in stance. For example, if in stance phase the feet move backwards at -0.4m/s (to achieve a body velocity of +0.4m/s) and the stance phase is 0.5 seconds long, then we know the feet will have moved backwards -0.20m. The swing controller will then move the feet forwards 0.20m to put the foot back in its starting place. You can imagine that if the swing controller only put the leg forward 0.15m, then every step the foot would lag more and more behind the body by -0.05m. +#### Pupper-Walk-Relative-RewardStable0.5-ScaledDown3-[Headless|Graphical]-v0 -Both the stance and swing controllers generate target positions for the feet in cartesian coordinates relative the body center of mass. It's convenient to work in cartesian coordinates for the stance and swing planning, but we now need to convert them to motor angles. This is done by using an inverse kinematics model, which maps between cartesian body coordinates and motor angles. These motor angles, also called joint angles, are then populated into the ```state``` variable and returned by the model. +Like `Pupper-Walk-Relative-RewardStable0.5` but additionally with the actions scaled by 0.3 +#### Pupper-Walk-Relative-RewardStable0.5-ScaledDown-RandomZRot-[Headless|Graphical]-v0 -## How to Build Pupper -Main documentation: https://pupper.readthedocs.io/en/latest/ +Like `Pupper-Walk-Relative-RewardStable0.5-ScaledDown3` but with additional random initial rotation around the z axis -You can find the bill of materials, pre-made kit purchasing options, assembly instructions, software installation, etc at this website. +#### Pupper-Walk-Relative-RewardStable0.5-ScaledNSmoothed-RandomZRot-[Headless|Graphical]-v0 +Like `Pupper-Walk-Relative-RewardStable0.5-ScaledDown-RandomZRot` but additionally with the actions smoothed with queue length 3. -## Help -- Feel free to raise an issue (https://github.com/stanfordroboticsclub/StanfordQuadruped/issues/new/choose) or email me at nathankau [at] stanford [dot] edu -- We also have a Google group set up here: https://groups.google.com/forum/#!forum/stanford-quadrupeds diff --git a/pupper/HardwareConfig.py b/pupper/HardwareConfig.py deleted file mode 100644 index c09f39f7..00000000 --- a/pupper/HardwareConfig.py +++ /dev/null @@ -1,13 +0,0 @@ -""" -Per-robot configuration file that is particular to each individual robot, not just the type of robot. -""" -import numpy as np - - -MICROS_PER_RAD = 11.333 * 180.0 / np.pi # Must be calibrated -NEUTRAL_ANGLE_DEGREES = np.array( - [[ -0, 7, 2, 3], [ 17, 57, 46, 52], [-39, -35, -33, -64]] -) - -PS4_COLOR = {"red": 0, "blue": 0, "green": 255} -PS4_DEACTIVATED_COLOR = {"red": 0, "blue": 0, "green": 50} \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..2f7cc3f4 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,3 @@ +[tool.black] +line-length = 120 +target-version = ['py37'] diff --git a/robot.service b/robot.service index 2b18d5df..13b139d0 100644 --- a/robot.service +++ b/robot.service @@ -5,7 +5,7 @@ After=joystick.service [Service] ExecStartPre=-sudo pigpiod -ExecStart=/usr/bin/python3 /home/pi/StanfordQuadruped/run_robot.py +ExecStart=/usr/bin/python3 /home/pi/StanfordQuadruped/scripts/run_robot.py KillSignal=2 TimeoutStopSec=10 diff --git a/run_robot.py b/run_robot.py deleted file mode 100644 index 30b472ea..00000000 --- a/run_robot.py +++ /dev/null @@ -1,79 +0,0 @@ -import numpy as np -import time -from src.IMU import IMU -from src.Controller import Controller -from src.JoystickInterface import JoystickInterface -from src.State import State -from pupper.HardwareInterface import HardwareInterface -from pupper.Config import Configuration -from pupper.Kinematics import four_legs_inverse_kinematics - -def main(use_imu=False): - """Main program - """ - - # Create config - config = Configuration() - hardware_interface = HardwareInterface() - - # Create imu handle - if use_imu: - imu = IMU(port="/dev/ttyACM0") - imu.flush_buffer() - - # Create controller and user input handles - controller = Controller( - config, - four_legs_inverse_kinematics, - ) - state = State() - print("Creating joystick listener...") - joystick_interface = JoystickInterface(config) - print("Done.") - - last_loop = time.time() - - print("Summary of gait parameters:") - print("overlap time: ", config.overlap_time) - print("swing time: ", config.swing_time) - print("z clearance: ", config.z_clearance) - print("x shift: ", config.x_shift) - - # Wait until the activate button has been pressed - while True: - print("Waiting for L1 to activate robot.") - while True: - command = joystick_interface.get_command(state) - joystick_interface.set_color(config.ps4_deactivated_color) - if command.activate_event == 1: - break - time.sleep(0.1) - print("Robot activated.") - joystick_interface.set_color(config.ps4_color) - - while True: - now = time.time() - if now - last_loop < config.dt: - continue - last_loop = time.time() - - # Parse the udp joystick commands and then update the robot controller's parameters - command = joystick_interface.get_command(state) - if command.activate_event == 1: - print("Deactivating Robot") - break - - # Read imu data. Orientation will be None if no data was available - quat_orientation = ( - imu.read_orientation() if use_imu else np.array([1, 0, 0, 0]) - ) - state.quat_orientation = quat_orientation - - # Step the controller forward by dt - controller.run(state, command) - - # Update the pwm widths going to the servos - hardware_interface.set_actuator_postions(state.joint_angles) - - -main() diff --git a/scripts/03-pybullet-quadruped-example.py b/scripts/03-pybullet-quadruped-example.py new file mode 100644 index 00000000..356bb777 --- /dev/null +++ b/scripts/03-pybullet-quadruped-example.py @@ -0,0 +1,469 @@ +import pybullet as p +import time +import math +import pybullet_data + + + + +def drawInertiaBox(parentUid, parentLinkIndex, color): + dyn = p.getDynamicsInfo(parentUid, parentLinkIndex) + mass = dyn[0] + frictionCoeff = dyn[1] + inertia = dyn[2] + if (mass > 0): + Ixx = inertia[0] + Iyy = inertia[1] + Izz = inertia[2] + boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass) + boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass) + boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass) + + halfExtents = [boxScaleX, boxScaleY, boxScaleZ] + pts = [[halfExtents[0], halfExtents[1], halfExtents[2]], + [-halfExtents[0], halfExtents[1], halfExtents[2]], + [halfExtents[0], -halfExtents[1], halfExtents[2]], + [-halfExtents[0], -halfExtents[1], halfExtents[2]], + [halfExtents[0], halfExtents[1], -halfExtents[2]], + [-halfExtents[0], halfExtents[1], -halfExtents[2]], + [halfExtents[0], -halfExtents[1], -halfExtents[2]], + [-halfExtents[0], -halfExtents[1], -halfExtents[2]]] + + p.addUserDebugLine(pts[0], + pts[1], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[1], + pts[3], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[3], + pts[2], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[2], + pts[0], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + + p.addUserDebugLine(pts[0], + pts[4], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[1], + pts[5], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[2], + pts[6], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[3], + pts[7], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + + p.addUserDebugLine(pts[4 + 0], + pts[4 + 1], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[4 + 1], + pts[4 + 3], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[4 + 3], + pts[4 + 2], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + p.addUserDebugLine(pts[4 + 2], + pts[4 + 0], + color, + 1, + parentObjectUniqueId=parentUid, + parentLinkIndex=parentLinkIndex) + + +toeConstraint = True +useMaximalCoordinates = False +useRealTime = 0 + +#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance +fixedTimeStep = 1. / 100 +numSolverIterations = 50 + +if (useMaximalCoordinates): + fixedTimeStep = 1. / 500 + numSolverIterations = 200 + +speed = 10 +amplitude = 0.8 +jump_amp = 0.5 +maxForce = 3.5 +kneeFrictionForce = 0 +kp = 1 +kd = .5 +maxKneeForce = 1000 + +physId = p.connect(p.SHARED_MEMORY_GUI) +if (physId < 0): + p.connect(p.GUI) +#p.resetSimulation() + +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +angle = 0 # pick in range 0..0.2 radians +orn = p.getQuaternionFromEuler([0, angle, 0]) +p.loadURDF("plane.urdf", [0, 0, 0], orn) +p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) +p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, + "genericlogdata.bin", + maxLogDof=16, + logFlags=p.STATE_LOG_JOINT_TORQUES) +p.setTimeOut(4000000) + +p.setGravity(0, 0, 0) +p.setTimeStep(fixedTimeStep) + +orn = p.getQuaternionFromEuler([0, 0, 0.4]) +p.setRealTimeSimulation(0) +quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3], + orn, + useFixedBase=False, + useMaximalCoordinates=useMaximalCoordinates, + flags=p.URDF_USE_IMPLICIT_CYLINDER) +nJoints = p.getNumJoints(quadruped) + +jointNameToId = {} +for i in range(nJoints): + jointInfo = p.getJointInfo(quadruped, i) + jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] + +motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] +motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] +knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] +hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] +knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] +motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] +motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] +hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] +knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] +motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint'] +motor_front_leftL_link = jointNameToId['motor_front_leftL_link'] +knee_front_leftL_link = jointNameToId['knee_front_leftL_link'] +motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint'] +hip_rightR_link = jointNameToId['hip_rightR_link'] +knee_back_rightR_link = jointNameToId['knee_back_rightR_link'] +motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint'] +motor_back_rightL_link = jointNameToId['motor_back_rightL_link'] +knee_back_rightL_link = jointNameToId['knee_back_rightL_link'] +motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint'] +hip_leftR_link = jointNameToId['hip_leftR_link'] +knee_back_leftR_link = jointNameToId['knee_back_leftR_link'] +motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint'] +motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] +knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] + +#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0]) + +motordir = [-1, -1, -1, -1, 1, 1, 1, 1] +halfpi = 1.57079632679 +twopi = 4 * halfpi +kneeangle = -2.1834 + +dyn = p.getDynamicsInfo(quadruped, -1) +mass = dyn[0] +friction = dyn[1] +localInertiaDiagonal = dyn[2] + +print("localInertiaDiagonal", localInertiaDiagonal) + +#this is a no-op, just to show the API +p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal) + +#for i in range (nJoints): +# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001]) + +drawInertiaBox(quadruped, -1, [1, 0, 0]) +#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0]) + +for i in range(nJoints): + drawInertiaBox(quadruped, i, [0, 1, 0]) + +if (useMaximalCoordinates): + steps = 400 + for aa in range(steps): + p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL, + motordir[0] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL, + motordir[1] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL, + motordir[2] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL, + motordir[3] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL, + motordir[4] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL, + motordir[5] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL, + motordir[6] * halfpi * float(aa) / steps) + p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL, + motordir[7] * halfpi * float(aa) / steps) + + p.setJointMotorControl2(quadruped, knee_front_leftL_link, p.POSITION_CONTROL, + motordir[0] * (kneeangle + twopi) * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_front_leftR_link, p.POSITION_CONTROL, + motordir[1] * kneeangle * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_back_leftL_link, p.POSITION_CONTROL, + motordir[2] * kneeangle * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL, + motordir[3] * (kneeangle + twopi) * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL, + motordir[4] * (kneeangle) * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL, + motordir[5] * (kneeangle + twopi) * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL, + motordir[6] * (kneeangle + twopi) * float(aa) / steps) + p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL, + motordir[7] * kneeangle * float(aa) / steps) + + p.stepSimulation() + #time.sleep(fixedTimeStep) +else: + + p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi) + p.resetJointState(quadruped, knee_front_leftL_link, motordir[0] * kneeangle) + p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi) + p.resetJointState(quadruped, knee_front_leftR_link, motordir[1] * kneeangle) + + p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi) + p.resetJointState(quadruped, knee_back_leftL_link, motordir[2] * kneeangle) + p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi) + p.resetJointState(quadruped, knee_back_leftR_link, motordir[3] * kneeangle) + + p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi) + p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle) + p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi) + p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle) + + p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi) + p.resetJointState(quadruped, knee_back_rightL_link, motordir[6] * kneeangle) + p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi) + p.resetJointState(quadruped, knee_back_rightR_link, motordir[7] * kneeangle) + +#p.getNumJoints(1) + +if (toeConstraint): + cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link, + p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) + p.changeConstraint(cid, maxForce=maxKneeForce) + cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link, + p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) + p.changeConstraint(cid, maxForce=maxKneeForce) + cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link, + p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) + p.changeConstraint(cid, maxForce=maxKneeForce) + cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link, + p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1]) + p.changeConstraint(cid, maxForce=maxKneeForce) + +if (1): + p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_rightL_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + p.setJointMotorControl(quadruped, knee_back_rightR_link, p.VELOCITY_CONTROL, 0, + kneeFrictionForce) + +p.setGravity(0, 0, -10) + +legnumbering = [ + motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint, + motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint, + motor_back_rightL_joint, motor_back_rightR_joint +] + +for i in range(8): + print(legnumbering[i]) +#use the Minitaur leg numbering +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[0], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[0] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[1], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[1] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[2], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[2] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[3], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[3] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[4], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[4] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[5], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[5] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[6], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[6] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[7], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[7] * 1.57, + positionGain=kp, + velocityGain=kd, + force=maxForce) +#stand still +p.setRealTimeSimulation(useRealTime) + +t = 0.0 +t_end = t + 15 +ref_time = time.time() +while (t < t_end): + p.setGravity(0, 0, -10) + if (useRealTime): + t = time.time() - ref_time + else: + t = t + fixedTimeStep + if (useRealTime == 0): + p.stepSimulation() + time.sleep(fixedTimeStep) + +print("quadruped Id = ") +print(quadruped) +p.saveWorld("quadru.py") +logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped]) + +#jump +t = 0.0 +t_end = t + 100 +i = 0 +ref_time = time.time() + +while (1): + if (useRealTime): + t = time.time() - ref_time + else: + t = t + fixedTimeStep + if (True): + + target = math.sin(t * speed) * jump_amp + 1.57 + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[0], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[0] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[1], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[1] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[2], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[2] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[3], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[3] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[4], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[4] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[5], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[5] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[6], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[6] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + p.setJointMotorControl2(bodyIndex=quadruped, + jointIndex=legnumbering[7], + controlMode=p.POSITION_CONTROL, + targetPosition=motordir[7] * target, + positionGain=kp, + velocityGain=kd, + force=maxForce) + + if (useRealTime == 0): + p.stepSimulation() + time.sleep(fixedTimeStep) diff --git a/scripts/04-testing-pybullet-multithreading.py b/scripts/04-testing-pybullet-multithreading.py new file mode 100644 index 00000000..d4093504 --- /dev/null +++ b/scripts/04-testing-pybullet-multithreading.py @@ -0,0 +1,64 @@ +import threading +import time +import numpy as np + + +def make_sim(): + import pybullet_data + + + # OPTION 1 - NOT THREAD-SAFE + # import pybullet as p + # p.connect(p.DIRECT) + + # OPTION 2 - SEEMINGLY THREAD-SAFE + import pybullet + import pybullet_utils.bullet_client as bc + p = bc.BulletClient(connection_mode=pybullet.DIRECT) + + + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + + plane = p.loadURDF("plane.urdf") + sphereRadius = 0.05 + colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) + visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1, 0, 1, 1], radius=sphereRadius) + sphere = p.createMultiBody(0.1, colSphereId, visualShapeId, basePosition=[0, 0, sphereRadius + 0.00001]) + p.setGravity(0, 0, -10) + p.setRealTimeSimulation(0) + p.applyExternalForce(sphere, -1, [100, 0, 0], posObj=[0, 0, 0], flags=p.LINK_FRAME) + positions = [] + for _ in range(240): + p.stepSimulation() + positions.append(list(p.getBasePositionAndOrientation(sphere)[0])) + + return np.array(positions) + + +def worker(inst): + pos = make_sim() + np.savez(f"inst-{inst}.npz", positions=pos) + + +w1 = threading.Thread(target=worker, kwargs={"inst":0}) +w2 = threading.Thread(target=worker, kwargs={"inst":1}) +w3 = threading.Thread(target=worker, kwargs={"inst":2}) + +w1.start() +w2.start() +w3.start() + +w1.join() +w2.join() +w3.join() + +import matplotlib.pyplot as plt + +x = np.arange(0,240) +data = [] +for i in range(3): + trajectory = np.load(f"inst-{i}.npz")["positions"] + plt.plot(x, trajectory[:,0]) + +plt.show() + diff --git a/scripts/05-plot-joint-positions.py b/scripts/05-plot-joint-positions.py new file mode 100644 index 00000000..9f0b12b1 --- /dev/null +++ b/scripts/05-plot-joint-positions.py @@ -0,0 +1,38 @@ +import pickle +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import colors + +saved_states = pickle.load(open("saved-states.pkl", "rb")) +foot_locations = np.array([s.foot_locations for s in saved_states]) +joint_angles = np.array([s.joint_angles for s in saved_states]) + +# print (foot_locations[0,[0,2],:]) +# +# print (foot_locations.shape) +# +# foot_locations_x = foot_locations[50:,0,:] +# foot_locations_y = foot_locations[50:,2,:] +# +# print (foot_locations_x[:10]) +# print (foot_locations_y[:10]) +# +# norm = colors.Normalize(vmin=0, vmax=239) +# +# for i in range(4): +# plt.scatter(foot_locations_x,foot_locations_y,label=f"foot {i}") +# +# plt.legend() +# plt.show() + + +x = np.arange(0,len(joint_angles)) +for i in range(3): + plt.plot(x, joint_angles[:,i,0], label=f"joint {i}") + +plt.legend() +plt.tight_layout() +plt.show() + + + diff --git a/scripts/06-run-controller-live.py b/scripts/06-run-controller-live.py new file mode 100644 index 00000000..24abad8d --- /dev/null +++ b/scripts/06-run-controller-live.py @@ -0,0 +1,145 @@ +from collections import deque +import matplotlib.pyplot as plt +import numpy as np +from drawnow import drawnow +import cv2 +from stanford_quad.common.Command import Command +from stanford_quad.common.Controller import Controller +from stanford_quad.common.State import State +from stanford_quad.pupper.Config import Configuration +from stanford_quad.pupper.Kinematics import four_legs_inverse_kinematics +from stanford_quad.sim.IMU import IMU + +USE_IMU = False +DEFAULT_VEL = np.array([0.15, 0]) +DEFAULT_YAW_RATE = 0.0 + +# Create config +config = Configuration() +config.z_clearance = 0.02 + +# Create imu handle +if USE_IMU: + imu = IMU() + +# Create controller and user input handles +controller = Controller(config, four_legs_inverse_kinematics) +state = State() +command = Command() + +# Emulate the joystick inputs required to activate the robot +command.activate_event = 1 +controller.run(state, command) +command.activate_event = 0 +command.trot_event = 1 +controller.run(state, command) +command = Command() # zero it out + +print("resting pos:", state.joint_angles) + +# Apply a constant command. # TODO Add support for user input or an external commander +command.horizontal_velocity = DEFAULT_VEL +command.yaw_rate = DEFAULT_YAW_RATE + +# The joystick service is linux-only, so commenting out for mac +# print("Creating joystick listener...") +# joystick_interface = JoystickInterface(config) +# print("Done.") + +print("Summary of gait parameters:") +print("overlap time: ", config.overlap_time) +print("swing time: ", config.swing_time) +print("z clearance: ", config.z_clearance) +print("x shift: ", config.x_shift) + +SIM_FPS = 240 + +steps = 1000 + +# these values are from plotting foot locations and getting matplotlib recommendations for canvas +x_min = -0.14 +x_max = 0.14 + +y_min = -0.162 +y_max = -0.138 + +img_width = 800 +img_height = 400 + + +def coords_to_pix(coords): + x_norm = (coords[0] - x_min) / (x_max - x_min) + y_norm = (coords[1] - y_min) / (y_max - y_min) + x_pix = int(round(img_width * x_norm)) + y_pix = img_height - int(round(img_height * y_norm)) + return x_pix, y_pix + + +no_points = 20 +# I tried `foot_pts = [deque(maxlen=no_points)] * 4` but that didn't work +foot_pts = [deque(maxlen=no_points), deque(maxlen=no_points), deque(maxlen=no_points), deque(maxlen=no_points)] + +img_feet = np.zeros((img_height, img_width, 3), dtype=np.uint8) +joints_left = [] +joints_right = [] +joints = [joints_right, joints_left] +plt.plot([], [], label="right - joint 0") +plt.plot([], [], label="right - joint 1") +plt.plot([], [], label="right - joint 2") +plt.plot([], [], label="left - joint 0") +plt.plot([], [], label="left - joint 1") +plt.plot([], [], label="left - joint 2") +plt.legend(loc='center left') + +for steps in range(steps): + img_feet.fill(0) + + # Get IMU measurement if enabled + state.quat_orientation = IMU.read_orientation() if USE_IMU else np.array([1, 0, 0, 0]) + + # Step the controller forward by dt + controller.run(state, command) + + for foot_idx, color, name in zip( + range(4), + [[0, 0, 255], [0, 255, 0], [255, 0, 0], [255, 255, 0]], + ["front-right", "front-left", "back-right", "back-left"], + ): + + cv2.putText( + img_feet, + name, + (int(round(img_width / 2 - 70)), int(round(img_height - 100 + (foot_idx * 25)))), + cv2.FONT_HERSHEY_SIMPLEX, + fontScale=1, + color=color, + thickness=2, + ) + + foot_xy = state.foot_locations[[0, 2], foot_idx] + foot_pix = coords_to_pix(foot_xy) + cv2.circle(img_feet, foot_pix, 5, (255, 255, 255), -1) + foot_pts[foot_idx].appendleft(foot_pix) + + for i in range(1, len(foot_pts[foot_idx])): + thickness = int(np.sqrt(no_points / float(i + 1)) * 2.5) + cv2.line(img_feet, foot_pts[foot_idx][i - 1], foot_pts[foot_idx][i], color, thickness) + + cv2.imshow("feet", img_feet[:, :, ::-1]) + cv2.waitKey(1) + + for foot_idx, color, name in zip(range(2), [[0, 0, 255], [0, 255, 0]], ["front-right", "front-left"],): + + joints[foot_idx].append(np.rad2deg(state.joint_angles[:, foot_idx])) + joint_np = np.array(joints[foot_idx]) + + for joint_idx in range(3): + plot_id = joint_idx + (foot_idx * 3) + print ("plot id",plot_id) + print (joint_np[:,joint_idx]) + plt.gca().lines[plot_id].set_xdata(np.arange(len(joint_np))) + plt.gca().lines[plot_id].set_ydata(joint_np[:,joint_idx]) + + plt.gca().relim() + plt.gca().autoscale_view() + plt.pause(0.001) diff --git a/scripts/07-debug-new-simulator.py b/scripts/07-debug-new-simulator.py new file mode 100644 index 00000000..648a7954 --- /dev/null +++ b/scripts/07-debug-new-simulator.py @@ -0,0 +1,29 @@ +import time +import numpy as np +from stanford_quad.sim.simulator2 import PupperSim2, REST_POS + +sim = PupperSim2(debug=True) + +debugParams = [] + +rest_pos = REST_POS.T.flatten() +for i in range(len(sim.joint_indices)): + motor = sim.p.addUserDebugParameter("motor{}".format(i + 1), -np.pi, np.pi, rest_pos[i]) + debugParams.append(motor) + +print(sim.get_pos_orn_vel()) +print(sim.get_joint_states()) + +for step in range(100000): + motorPos = [] + for i in range(len(sim.joint_indices)): + pos = sim.p.readUserDebugParameter(debugParams[i]) + motorPos.append(pos) + sim.action(motorPos) + sim.step() + pos, orn, vel = sim.get_pos_orn_vel() + # print(pos, orn, vel) + # for i in range(40): + # print(sim.p.getLinkState(bodyUniqueId=sim.model, linkIndex=i)) + # quit() + sim.check_collision() diff --git a/scripts/08-run-walker-gym-env.py b/scripts/08-run-walker-gym-env.py new file mode 100644 index 00000000..4e3e7e4d --- /dev/null +++ b/scripts/08-run-walker-gym-env.py @@ -0,0 +1,21 @@ +import time + +import gym +import stanford_quad # need this unused import to get our custom environments +import numpy as np + +# env = gym.make("Pupper-Walk-Relative-ScaledDown-RandomZRot-Graphical-v0") +env = gym.make("Pupper-Walk-Relative-RewardStable0.5-Graphical-v0") + +for run in range(5): + env.reset() + time.sleep(2) + for step in range(121): + # action = env.action_space.sample() * 0.3 + action = np.array([0] * 12) + obs, rew, done, misc = env.step(action) + print( + f"action: {np.around(action,2)}, " f"obs: {np.around(obs,2)}, " f"rew: {np.around(rew,2)}, " f"done: {done}" + ) + + time.sleep(0.02) diff --git a/scripts/09-test-random-steps.py b/scripts/09-test-random-steps.py new file mode 100644 index 00000000..9a1b1b2d --- /dev/null +++ b/scripts/09-test-random-steps.py @@ -0,0 +1,15 @@ +from stanford_quad.sim.simulator2 import PupperSim2 + +STEP_WIDTH = 2 # if you're standing in front of a staircase and looking up, what's the left-right distance? +STEP_DEPTH = 0.3 # if you're stepping onto one step, how much of your foot can fit on the step? +STEP_HEIGHT = 0.05 # how far for you have to go up between each step? +OFFSET = (0.3, 0, 0) + +sim = PupperSim2(debug=True) +sim.reset() +sim.add_stairs(step_width=STEP_WIDTH, step_depth=STEP_DEPTH, step_height=STEP_HEIGHT, offset=OFFSET, random_color=True) + +for step in range(100000): + # sim.action(motorPo) + sim.step() + print(sim.get_pos_orn_vel()[0][0]) diff --git a/scripts/10-test-camera.py b/scripts/10-test-camera.py new file mode 100644 index 00000000..4696c92d --- /dev/null +++ b/scripts/10-test-camera.py @@ -0,0 +1,20 @@ +import cv2 +import numpy as np +from stanford_quad.sim.simulator2 import PupperSim2 + +sim = PupperSim2(debug=True) +sim.reset() +sim.add_stairs(step_width=2, step_depth=0.3, step_height=0.05, offset=(0.3, 0, 0), random_color=True) + +for step in range(100000): + # sim.action(motorPo) + action = np.random.uniform(-0.3, 0.3, 12) + sim.action(action) + sim.step() + + # render an image + img = sim.take_photo() + + # display the image (converting from RGB to BGR because OpenCV is weird) + cv2.imshow("rendered img", img[:, :, ::-1]) + cv2.waitKey(10) diff --git a/scripts/11-test-glen-playback.py b/scripts/11-test-glen-playback.py new file mode 100644 index 00000000..f0cfd0ac --- /dev/null +++ b/scripts/11-test-glen-playback.py @@ -0,0 +1,120 @@ +import time +import numpy as np +import h5py +from scipy.spatial.transform import Rotation +from stanford_quad.sim.HardwareInterface import HardwareInterface +from stanford_quad.sim.simulator2 import PupperSim2, REST_POS +import cv2 +from stanford_quad.sim.utils import segmap2color_fixed + +RECORDING_FPS = 60 # limited by camera +SIM_FPS = 240 # arbitrary but should be a multiple of the RECORDING_FPS + +TRICK = "walk-forward" +FRAME_START = 90 +FRAME_END = 400 + +# TRICK = "walk-backward" +# FRAME_START = 150 +# FRAME_END = 450 + +# TRICK = "jump" +# FRAME_START = 120 +# FRAME_END = 250 + +# TRICK = "turn-right" +# FRAME_START = 110 +# FRAME_END = 570 + +substeps = int(round(SIM_FPS / RECORDING_FPS)) + +f = h5py.File(f"/Users/florian/dev/pyvicon/scripts/pupper-{TRICK}.hdf5", "r") + +vicon_positions = f["vicon_positions"] +vicon_rotations = f["vicon_rotations"] +joints = f["joints"] +feet = f["foot_positions"] + +######## +# KINEMATIC SIM +######## + + +sim = PupperSim2(debug=False, img_size=(256, 256), frequency=SIM_FPS) + +# move the robot down to the ground +sim.reset(rest=True) +for _ in range(10): + sim.step() + +# make the robot limp for demonstration +sim.make_kinematic() + +base_pos = vicon_positions[FRAME_START] +base_rot = vicon_rotations[FRAME_START] + +for frame_idx in range(FRAME_START, FRAME_END): + + # pick the next joint recording from the HDF5 + joint_angles = np.reshape(joints[frame_idx], (3, 4)) + joint_angles_robot = HardwareInterface.parallel_to_serial_joint_angles(joint_angles) + + sim.set(joint_angles_robot.T.flatten()) + + diff_pos = vicon_positions[frame_idx] - base_pos + diff_pos /= 500 # for some weird reason it's not /100 + z = diff_pos[2] + 0.21 # (compensating for the model) + x = diff_pos[1] + y = diff_pos[0] + + # fixed manual rotation for now + rot = Rotation.from_quat([0, 0, 0, 1]) + + # rot = vicon_rotations[frame_idx] + # # rot = Rotation.from_quat((rot[1], rot[2], rot[0], rot[3])) # ish + # rot = Rotation.from_quat((-rot[1], -rot[2], rot[0], rot[3])) # ish + # rot_before = np.copy(rot.as_euler("xyz", degrees=True)) + # + # rot_e = rot.as_euler("xyz", degrees=True) + # # if rot_e[0] > -160: + # # rot = rot.inv() + + sim.move_kinectic_body((x, y, z), rot.as_quat()) + + # Here we only do a single step because we _set_ the joint positions directly. + # In the real simulation, we need to step multiple times, corresponding to SIM_FPS/RECORDING_FPS + sim.step() + + img, segmap = sim.take_photo(with_segmap=True) + cv2.imshow("frame", img[:, :, ::-1]) # RGB to BGR + cv2.imshow("segmap", segmap2color_fixed(segmap)) + cv2.waitKey(1) + + +######## +# REAL SIM - to learn the imitation policy +######## + + +sim2 = PupperSim2(debug=False, img_size=(256, 256), frequency=SIM_FPS) +# note: technically there's no reason for sim1 to stay open and open a second sim here. This is just for show. + +sim2.reset(rest=True) +for _ in range(10): + sim2.step() + +for idx in range(FRAME_END - FRAME_START): + # start with the stable resting position and learn a policy that applies small values to this + joints = np.copy(sim2.get_rest_pos()) + + # joints = policy(last_observation) + joints + + sim2.action(joints) + + for _ in range(substeps): + sim2.step() + + img, segmap = sim2.take_photo(with_segmap=True) + cv2.imshow("frame", img[:, :, ::-1]) # RGB to BGR + cv2.imshow("segmap", segmap2color_fixed(segmap)) + cv2.waitKey(1) diff --git a/scripts/12-synced-playback.py b/scripts/12-synced-playback.py new file mode 100644 index 00000000..530c2989 --- /dev/null +++ b/scripts/12-synced-playback.py @@ -0,0 +1,97 @@ +import time +import numpy as np +import h5py +from scipy.spatial.transform import Rotation + +from stanford_quad.envs.imitation_env import get_recording_joints, get_recording_pose +from stanford_quad.sim.HardwareInterface import HardwareInterface +from stanford_quad.sim.simulator2 import PupperSim2, REST_POS +import cv2 +from stanford_quad.sim.utils import segmap2color_fixed + +RECORDING_FPS = 60 # limited by camera +SIM_FPS = 240 # arbitrary but should be a multiple of the RECORDING_FPS + +TRICK = "walk-forward" +FRAME_START = 90 +FRAME_END = 400 + +RESOLUTION = 256 + +substeps = int(round(SIM_FPS / RECORDING_FPS)) + +f = h5py.File(f"/Users/florian/dev/pyvicon/scripts/pupper-{TRICK}.hdf5", "r") + +vicon_positions = f["vicon_positions"] +vicon_rotations = f["vicon_rotations"] +joints = f["joints"] +feet = f["foot_positions"] + +######## +# KINEMATIC SIM +######## + + +sim_ref = PupperSim2(debug=False, img_size=(RESOLUTION, RESOLUTION), frequency=SIM_FPS) +sim_ref.reset() +# make the robot limp for demonstration +sim_ref.make_kinematic() + +sim_learner = PupperSim2(debug=False, img_size=(RESOLUTION, RESOLUTION), frequency=SIM_FPS) +sim_learner.reset(rest=True) +sim_learner.change_color((0, 1, 1, 1)) +for _ in range(10): + sim_learner.step() + +base_pos = vicon_positions[FRAME_START] +base_rot = vicon_rotations[FRAME_START] + + +def policy(_): + # make sure to initialize the policy with zeroed output + return np.array([0] * 12) + + +def merge_images(img_ref, img_learner, segmap_ref, segmap_learner): + # the "+2" is only there to create some space between the frames + img_merged = np.zeros((RESOLUTION * 2 + 2, RESOLUTION * 2 + 2, 3), np.uint8) + img_merged[:RESOLUTION, :RESOLUTION, :] = img_ref[:, :, ::-1] + img_merged[:RESOLUTION, RESOLUTION + 2 :, :] = img_learner[:, :, ::-1] + img_merged[RESOLUTION + 2 :, :RESOLUTION, :] = segmap2color_fixed(segmap_ref) + img_merged[RESOLUTION + 2 :, RESOLUTION + 2 :, :] = segmap2color_fixed(segmap_learner) + return img_merged + + +for frame_idx in range(FRAME_START, FRAME_END): + + # reference sim + + joints_reference = get_recording_joints(joints, frame_idx) + sim_ref.set(joints_reference) + + pos, rot = get_recording_pose(vicon_positions, base_pos, frame_idx) + sim_ref.move_kinectic_body(pos, rot.as_quat()) + + sim_ref.step() + img_ref, segmap_ref = sim_ref.take_photo(with_segmap=True) + + # learner sim + + joints_learner_base = np.copy(sim_learner.get_rest_pos()) + + last_observation = None + joints_learner = policy(last_observation) + joints_learner_base + + sim_learner.action(joints_learner) + + for _ in range(substeps): + sim_learner.step() + + img_learner, segmap_learner = sim_learner.take_photo(with_segmap=True) + + # temporary, just for demonstration: + + img_merged = merge_images(img_ref, img_learner, segmap_ref, segmap_learner) + + cv2.imshow("Merged Frames", img_merged) + cv2.waitKey(1) diff --git a/scripts/13-run-walker-incremental-gym-env.py b/scripts/13-run-walker-incremental-gym-env.py new file mode 100644 index 00000000..2df71771 --- /dev/null +++ b/scripts/13-run-walker-incremental-gym-env.py @@ -0,0 +1,21 @@ +import time + +import gym +import stanford_quad # need this unused import to get our custom environments +import numpy as np + +# env = gym.make("Pupper-Walk-Relative-ScaledDown-RandomZRot-Graphical-v0") +env = gym.make("Pupper-Walk-Incremental-aScale_0.1-aSmooth_1-RandomZRot_1-Graphical-v0") + +for run in range(5): + env.reset() + time.sleep(2) + for step in range(121): + action = env.action_space.sample() * 0.99 + # action = np.array([0] * 12) + obs, rew, done, misc = env.step(action) + print( + f"action: {np.around(action,2)}, " f"obs: {np.around(obs,2)}, " f"rew: {np.around(rew,2)}, " f"done: {done}" + ) + + time.sleep(0.02) diff --git a/scripts/14-test-gait-parameters.py b/scripts/14-test-gait-parameters.py new file mode 100644 index 00000000..792b2182 --- /dev/null +++ b/scripts/14-test-gait-parameters.py @@ -0,0 +1,69 @@ +import time +import numpy as np +import matplotlib.pyplot as plt +from stanford_quad.common.Utilities import controller_to_sim +from stanford_quad.pupper.policy import HardPolicy +from stanford_quad.sim.simulator2 import PupperSim2 + + +FPS = 60 # fraction of 240 +substeps = int(round(240 / FPS)) + +sim = PupperSim2(debug=True, frequency=240) + + +def reset_sim(): + sim.reset(rest=True) + sim.change_color((0, 1, 1, 1)) + for _ in range(10): + sim.step() + + +def plot_joints(joints): + joints = np.array(joints) + for i in range(12): + plt.plot(np.arange(len(joints)), joints[:, i], label=f"joint {i + 1}") + plt.legend() + plt.tight_layout() + plt.title("sim direct joints") + plt.show() + + +reset_sim() + +debug_params = [] + +for param in ["velocity fwd", "velocity left"]: + debug_params.append(sim.p.addUserDebugParameter(param, -1, 1, 0)) +debug_params.append(sim.p.addUserDebugParameter("reset", 1, 0, 1)) +debug_params.append(sim.p.addUserDebugParameter("plot joints", 1, 0, 1)) + +policy = HardPolicy() +last_reset_val = -1 +last_plot_val = -1 +joints = [] +counter = 0 + +while True: + vel = [0, 0] + for param_idx, param in enumerate(debug_params): + val = sim.p.readUserDebugParameter(param) + if param_idx < 2: + vel[param_idx] = val + if param_idx == 2 and val != last_reset_val: + reset_sim() + last_reset_val = val + if param_idx == 3 and val != last_plot_val: + if len(joints) > 0: + plot_joints(joints) + last_plot_val = val + joints.clear() + + action = controller_to_sim(policy.act(velocity_horizontal=vel)) + joints.append(np.copy(action)) + sim.action(action) + + for _ in range(substeps): + sim.step() + + time.sleep(1 / FPS) diff --git a/scripts/15-record-pupper-for-imitation.py b/scripts/15-record-pupper-for-imitation.py new file mode 100644 index 00000000..35c272ee --- /dev/null +++ b/scripts/15-record-pupper-for-imitation.py @@ -0,0 +1,19 @@ +import time + +import gym +import stanford_quad +from stanford_quad.common.Utilities import controller_to_sim +from stanford_quad.pupper.policy import HardPolicy + +ROLLOUTS = 5 +env = gym.make("Pupper-Walk-Absolute-aScale_1.0-aSmooth_1-RandomZRot_1-Graphical-v0") + +for run in range(ROLLOUTS): + state = env.reset() + policy = HardPolicy() + while True: + action = controller_to_sim(policy.act(velocity_horizontal=(0.2, 0), normalized=True)) + next_state, rew, done, misc = env.step(action) + if done: + break + time.sleep(1 / 60) diff --git a/scripts/RunPyBulletSim.py b/scripts/RunPyBulletSim.py new file mode 100644 index 00000000..352bc6e1 --- /dev/null +++ b/scripts/RunPyBulletSim.py @@ -0,0 +1,101 @@ +import os +import pickle +import time +import numpy as np +import copy + +from stanford_quad.assets import ASSET_DIR +from stanford_quad.common.Command import Command +from stanford_quad.common.Controller import Controller +from stanford_quad.common.State import State +from stanford_quad.pupper.Config import Configuration +from stanford_quad.pupper.Kinematics import four_legs_inverse_kinematics +from stanford_quad.sim.IMU import IMU +from stanford_quad.sim.HardwareInterface import HardwareInterface +from stanford_quad.sim.simulator import PySim + +USE_IMU = False +DEFAULT_VEL = np.array([0.15, 0]) +DEFAULT_YAW_RATE = 0.0 + +# Create config +config = Configuration() +config.z_clearance = 0.02 + +sim = PySim(xml_path=os.path.join(ASSET_DIR, "pupper_pybullet_out.xml"), headless=False) +hardware_interface = HardwareInterface(sim.model, sim.joint_indices) + +# Create imu handle +if USE_IMU: + imu = IMU() + +# Create controller and user input handles +controller = Controller(config, four_legs_inverse_kinematics) +state = State() +command = Command() + +# Emulate the joystick inputs required to activate the robot +command.activate_event = 1 +controller.run(state, command) +command.activate_event = 0 +command.trot_event = 1 +controller.run(state, command) +command = Command() # zero it out + +# Apply a constant command. # TODO Add support for user input or an external commander +command.horizontal_velocity = DEFAULT_VEL +command.yaw_rate = DEFAULT_YAW_RATE + +# The joystick service is linux-only, so commenting out for mac +# print("Creating joystick listener...") +# joystick_interface = JoystickInterface(config) +# print("Done.") + +print("Summary of gait parameters:") +print("overlap time: ", config.overlap_time) +print("swing time: ", config.swing_time) +print("z clearance: ", config.z_clearance) +print("x shift: ", config.x_shift) + +SIM_FPS = 240 + +# Run the simulation +timesteps = 120 * 60 * 10 # simulate for a max of 10 minutes + +# Sim seconds per sim step +sim_steps_per_sim_second = 240 +sim_dt = 1.0 / sim_steps_per_sim_second +last_control_update = 0 +start = time.time() + +saved_states = [] + +for steps in range(timesteps): + sim_time_elapsed = sim_dt * steps + if sim_time_elapsed - last_control_update > config.dt: + last_control_update = sim_time_elapsed + + # Get IMU measurement if enabled + state.quat_orientation = IMU.read_orientation() if USE_IMU else np.array([1, 0, 0, 0]) + + # Step the controller forward by dt + controller.run(state, command) + + saved_states.append(copy.copy(state)) + + # Update the pwm widths going to the servos + hardware_interface.set_actuator_postions(state.joint_angles) + + # Simulate physics for 1/240 seconds (the default timestep) + sim.step() + + # Performance testing + + if ((steps+1) % 240) == 0: + elapsed = (time.time() - start) / 100 + # print("Sim seconds elapsed: {}, Real seconds elapsed: {}".format(round(sim_time_elapsed, 3), round(elapsed, 3))) + print (f"Sim running at {1/elapsed} Hz") + pickle.dump(saved_states, open("saved-states.pkl", "wb")) + quit() + start = time.time() + diff --git a/calibrate_servos.py b/scripts/calibrate_servos.py similarity index 72% rename from calibrate_servos.py rename to scripts/calibrate_servos.py index 6013f95b..0e48abe8 100644 --- a/calibrate_servos.py +++ b/scripts/calibrate_servos.py @@ -1,6 +1,4 @@ -import pigpio -from pupper.HardwareInterface import HardwareInterface -from pupper.Config import PWMParams, ServoParams +from stanford_quad.pupper import HardwareInterface import numpy as np @@ -45,22 +43,21 @@ def step_until(hardware_interface, axis, leg, set_point): offset = 0 while not found_position: move_input = str( - input("Enter 'a' or 'b' to move the link until it is **" + set_names[axis] + "**. Enter 'd' when done. Input: " + input( + "Enter 'a' or 'b' to move the link until it is **" + + set_names[axis] + + "**. Enter 'd' when done. Input: " ) ) if move_input == "a": offset += 1.0 hardware_interface.set_actuator_position( - degrees_to_radians(set_point + offset), - axis, - leg, + degrees_to_radians(set_point + offset), axis, leg, ) elif move_input == "b": offset -= 1.0 hardware_interface.set_actuator_position( - degrees_to_radians(set_point + offset), - axis, - leg, + degrees_to_radians(set_point + offset), axis, leg, ) elif move_input == "d": found_position = True @@ -83,7 +80,11 @@ def calibrate_angle_offset(hardware_interface): # Found K value of (11.4) k = float( - input("Enter the scaling constant for your servo. This constant is how much you have to increase the pwm pulse width (in microseconds) to rotate the servo output 1 degree. (It is 11.333 for the newer CLS6336 and CLS6327 servos). Input: ") + input( + "Enter the scaling constant for your servo. This constant is how much you have to increase the pwm pulse " + "width (in microseconds) to rotate the servo output 1 degree. (It is 11.333 for the newer CLS6336 and " + "CLS6327 servos). Input: " + ) ) hardware_interface.servo_params.micros_per_rad = k * 180 / np.pi @@ -103,15 +104,11 @@ def calibrate_angle_offset(hardware_interface): # Move servo to set_point angle hardware_interface.set_actuator_position( - degrees_to_radians(set_point), - axis, - leg_index, + degrees_to_radians(set_point), axis, leg_index, ) # Adjust the angle using keyboard input until it matches the reference angle - offset = step_until( - hardware_interface, axis, leg_index, set_point - ) + offset = step_until(hardware_interface, axis, leg_index, set_point) print("Final offset: ", offset) # The upper leg link has a different equation because we're calibrating to make it horizontal, not vertical @@ -119,20 +116,22 @@ def calibrate_angle_offset(hardware_interface): hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] = set_point - offset else: hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] = -(set_point + offset) - print("Calibrated neutral angle: ", hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index]) + print( + "Calibrated neutral angle: ", hardware_interface.servo_params.neutral_angle_degrees[axis, leg_index] + ) # Send the servo command using the new beta value and check that it's ok hardware_interface.set_actuator_position( - degrees_to_radians([0, 45, -45][axis]), - axis, - leg_index, + degrees_to_radians([0, 45, -45][axis]), axis, leg_index, ) okay = "" - prompt = "The leg should be at exactly **" + ["horizontal", "45 degrees", "45 degrees"][axis] + "**. Are you satisfied? Enter 'yes' or 'no': " + prompt = ( + "The leg should be at exactly **" + + ["horizontal", "45 degrees", "45 degrees"][axis] + + "**. Are you satisfied? Enter 'yes' or 'no': " + ) while okay not in ["yes", "no"]: - okay = str( - input(prompt) - ) + okay = str(input(prompt)) completed = okay == "yes" @@ -146,7 +145,10 @@ def main(): print("Calibrated neutral angles:") print(hardware_interface.servo_params.neutral_angle_degrees) print("Copy these values into the NEUTRAL_ANGLE_DEGREES matrix defined pupper/HardwareConfig.py") - print("Set the MICROS_PER_RAD value in pupper/HardwareConfig.py to whatever you defined in the beginning of this program as well.") + print( + "Set the MICROS_PER_RAD value in pupper/HardwareConfig.py to whatever you defined in the beginning of " + "this program as well." + ) main() diff --git a/install.sh b/scripts/install.sh similarity index 96% rename from install.sh rename to scripts/install.sh index 2aeff727..e9428e0c 100644 --- a/install.sh +++ b/scripts/install.sh @@ -22,7 +22,7 @@ make sudo make install cd .. -cd StanfordQuadruped +cd StanfordQuadruped/scripts sudo ln -s $(realpath .)/robot.service /etc/systemd/system/ sudo systemctl daemon-reload sudo systemctl enable robot diff --git a/scripts/run_robot.py b/scripts/run_robot.py new file mode 100644 index 00000000..1a64b1a1 --- /dev/null +++ b/scripts/run_robot.py @@ -0,0 +1,70 @@ +import numpy as np +import time + +from stanford_quad.common.Controller import Controller +from stanford_quad.common.IMU import IMU +from stanford_quad.common.JoystickInterface import JoystickInterface +from stanford_quad.common.State import State +from stanford_quad.pupper.HardwareInterface import HardwareInterface +from stanford_quad.pupper.Config import Configuration +from stanford_quad.pupper.Kinematics import four_legs_inverse_kinematics + +USE_IMU = False + +# Create config +config = Configuration() +hardware_interface = HardwareInterface() + +# Create imu handle +if USE_IMU: + imu = IMU(port="/dev/ttyACM0") + imu.flush_buffer() + +# Create controller and user input handles +controller = Controller(config, four_legs_inverse_kinematics,) +state = State() +print("Creating joystick listener...") +joystick_interface = JoystickInterface(config) +print("Done.") + +last_loop = time.time() + +print("Summary of gait parameters:") +print("overlap time: ", config.overlap_time) +print("swing time: ", config.swing_time) +print("z clearance: ", config.z_clearance) +print("x shift: ", config.x_shift) + +# Wait until the activate button has been pressed +while True: + print("Waiting for L1 to activate robot.") + while True: + command = joystick_interface.get_command(state) + joystick_interface.set_color(config.ps4_deactivated_color) + if command.activate_event == 1: + break + time.sleep(0.1) + print("Robot activated.") + joystick_interface.set_color(config.ps4_color) + + while True: + now = time.time() + if now - last_loop < config.dt: + continue + last_loop = time.time() + + # Parse the udp joystick commands and then update the robot controller's parameters + command = joystick_interface.get_command(state) + if command.activate_event == 1: + print("Deactivating Robot") + break + + # Read imu data. Orientation will be None if no data was available + quat_orientation = np.array([1, 0, 0, 0]) if not USE_IMU else imu.read_orientation() + state.quat_orientation = quat_orientation + + # Step the controller forward by dt + controller.run(state, command) + + # Update the pwm widths going to the servos + hardware_interface.set_actuator_postions(state.joint_angles) diff --git a/setup.py b/setup.py new file mode 100644 index 00000000..462081f8 --- /dev/null +++ b/setup.py @@ -0,0 +1,14 @@ +from setuptools import setup + +setup( + name="stanford_quad", + version="1.0", + install_requires=["tqdm", "numpy", "gym"], + extras_require={ + "sim": ["matplotlib", "pybullet", "gym", "opencv-python", "torchvision"], + "robot": [ + "transforms3d", + "UDPComms @ git+https://github.com/stanfordroboticsclub/UDPComms@master#egg=UDPComms", + ], + }, +) diff --git a/src/Controller.py b/src/Controller.py deleted file mode 100644 index d7a5fba5..00000000 --- a/src/Controller.py +++ /dev/null @@ -1,170 +0,0 @@ -from src.Gaits import GaitController -from src.StanceController import StanceController -from src.SwingLegController import SwingController -from src.Utilities import clipped_first_order_filter -from src.State import BehaviorState, State - -import numpy as np -from transforms3d.euler import euler2mat, quat2euler -from transforms3d.quaternions import qconjugate, quat2axangle -from transforms3d.axangles import axangle2mat - - -class Controller: - """Controller and planner object - """ - - def __init__( - self, - config, - inverse_kinematics, - ): - self.config = config - - self.smoothed_yaw = 0.0 # for REST mode only - self.inverse_kinematics = inverse_kinematics - - self.contact_modes = np.zeros(4) - self.gait_controller = GaitController(self.config) - self.swing_controller = SwingController(self.config) - self.stance_controller = StanceController(self.config) - - self.hop_transition_mapping = {BehaviorState.REST: BehaviorState.HOP, BehaviorState.HOP: BehaviorState.FINISHHOP, BehaviorState.FINISHHOP: BehaviorState.REST, BehaviorState.TROT: BehaviorState.HOP} - self.trot_transition_mapping = {BehaviorState.REST: BehaviorState.TROT, BehaviorState.TROT: BehaviorState.REST, BehaviorState.HOP: BehaviorState.TROT, BehaviorState.FINISHHOP: BehaviorState.TROT} - self.activate_transition_mapping = {BehaviorState.DEACTIVATED: BehaviorState.REST, BehaviorState.REST: BehaviorState.DEACTIVATED} - - - def step_gait(self, state, command): - """Calculate the desired foot locations for the next timestep - - Returns - ------- - Numpy array (3, 4) - Matrix of new foot locations. - """ - contact_modes = self.gait_controller.contacts(state.ticks) - new_foot_locations = np.zeros((3, 4)) - for leg_index in range(4): - contact_mode = contact_modes[leg_index] - foot_location = state.foot_locations[:, leg_index] - if contact_mode == 1: - new_location = self.stance_controller.next_foot_location(leg_index, state, command) - else: - swing_proportion = ( - self.gait_controller.subphase_ticks(state.ticks) / self.config.swing_ticks - ) - new_location = self.swing_controller.next_foot_location( - swing_proportion, - leg_index, - state, - command - ) - new_foot_locations[:, leg_index] = new_location - return new_foot_locations, contact_modes - - - def run(self, state, command): - """Steps the controller forward one timestep - - Parameters - ---------- - controller : Controller - Robot controller object. - """ - - ########## Update operating state based on command ###### - if command.activate_event: - state.behavior_state = self.activate_transition_mapping[state.behavior_state] - elif command.trot_event: - state.behavior_state = self.trot_transition_mapping[state.behavior_state] - elif command.hop_event: - state.behavior_state = self.hop_transition_mapping[state.behavior_state] - - if state.behavior_state == BehaviorState.TROT: - state.foot_locations, contact_modes = self.step_gait( - state, - command, - ) - - # Apply the desired body rotation - rotated_foot_locations = ( - euler2mat( - command.roll, command.pitch, 0.0 - ) - @ state.foot_locations - ) - - # Construct foot rotation matrix to compensate for body tilt - (roll, pitch, yaw) = quat2euler(state.quat_orientation) - correction_factor = 0.8 - max_tilt = 0.4 - roll_compensation = correction_factor * np.clip(roll, -max_tilt, max_tilt) - pitch_compensation = correction_factor * np.clip(pitch, -max_tilt, max_tilt) - rmat = euler2mat(roll_compensation, pitch_compensation, 0) - - rotated_foot_locations = rmat.T @ rotated_foot_locations - - state.joint_angles = self.inverse_kinematics( - rotated_foot_locations, self.config - ) - - elif state.behavior_state == BehaviorState.HOP: - state.foot_locations = ( - self.config.default_stance - + np.array([0, 0, -0.09])[:, np.newaxis] - ) - state.joint_angles = self.inverse_kinematics( - state.foot_locations, self.config - ) - - elif state.behavior_state == BehaviorState.FINISHHOP: - state.foot_locations = ( - self.config.default_stance - + np.array([0, 0, -0.22])[:, np.newaxis] - ) - state.joint_angles = self.inverse_kinematics( - state.foot_locations, self.config - ) - - elif state.behavior_state == BehaviorState.REST: - yaw_proportion = command.yaw_rate / self.config.max_yaw_rate - self.smoothed_yaw += ( - self.config.dt - * clipped_first_order_filter( - self.smoothed_yaw, - yaw_proportion * -self.config.max_stance_yaw, - self.config.max_stance_yaw_rate, - self.config.yaw_time_constant, - ) - ) - # Set the foot locations to the default stance plus the standard height - state.foot_locations = ( - self.config.default_stance - + np.array([0, 0, command.height])[:, np.newaxis] - ) - # Apply the desired body rotation - rotated_foot_locations = ( - euler2mat( - command.roll, - command.pitch, - self.smoothed_yaw, - ) - @ state.foot_locations - ) - state.joint_angles = self.inverse_kinematics( - rotated_foot_locations, self.config - ) - - state.ticks += 1 - state.pitch = command.pitch - state.roll = command.roll - state.height = command.height - - def set_pose_to_default(self): - state.foot_locations = ( - self.config.default_stance - + np.array([0, 0, self.config.default_z_ref])[:, np.newaxis] - ) - state.joint_angles = controller.inverse_kinematics( - state.foot_locations, self.config - ) diff --git a/src/Tests.py b/src/Tests.py deleted file mode 100644 index 83a5afb1..00000000 --- a/src/Tests.py +++ /dev/null @@ -1,311 +0,0 @@ -# using LinearAlgebra -# using Profile -# using StaticArrays -# using Plots -# using BenchmarkTools - -# include("Kinematics.jl") -# include("PupperConfig.jl") -# include("Gait.jl") -# include("StanceController.jl") -# include("SwingLegController.jl") -# include("Types.jl") -# include("Controller.jl") - -import numpy as np -import matplotlib.pyplot as plt - -from Kinematics import leg_explicit_inverse_kinematics -from PupperConfig import * -from Gaits import * -from StanceController import position_delta, stance_foot_location -from SwingLegController import * -from Types import MovementReference, GaitParams, StanceParams, SwingParams -from Controller import * - -# function round_(a, dec) -# return map(x -> round(x, digits=dec), a) -# end - -# function testInverseKinematicsExplicit!() -# println("\n-------------- Testing Inverse Kinematics -----------") -# config = PupperConfig() -# println("\nTesting Inverse Kinematics") -# function testHelper(r, alpha_true, i; do_assert=true) -# eps = 1e-6 -# @time α = leg_explicitinversekinematics_prismatic(r, i, config) -# println("Leg ", i, ": r: ", r, " -> α: ", α) -# if do_assert -# @assert norm(α - alpha_true) < eps -# end -# end - -# c = config.LEG_L/sqrt(2) -# offset = config.ABDUCTION_OFFSET -# testHelper(SVector(0, offset, -0.125), SVector(0, 0, 0), 2) -# testHelper(SVector(c, offset, -c), SVector(0, -pi/4, 0), 2) -# testHelper(SVector(-c, offset, -c), SVector(0, pi/4, 0), 2) -# testHelper(SVector(0, c, -c), missing, 2, do_assert=false) - -# testHelper(SVector(-c, -offset, -c), [0, pi/4, 0], 1) -# testHelper(SVector(config.LEG_L * sqrt(3)/2, offset, -config.LEG_L / 2), SVector(0, -pi/3, 0), 2) -# end - - -def test_inverse_kinematics_linkage(): - print("\n-------------- Testing Five-bar Linkage Inverse Kinematics -----------") - config = PupperConfig() - print("\nTesting Inverse Kinematics") - - def testHelper(r, alpha_true, i, do_assert=True): - eps = 1e-6 - alpha = leg_explicit_inverse_kinematics(r, i, config) - print("Leg ", i, ": r: ", r, " -> α: ", alpha) - if do_assert: - assert np.linalg.norm(alpha - alpha_true) < eps - - c = config.LEG_L / (2 ** 0.5) - offset = config.ABDUCTION_OFFSET - testHelper(np.array([0, offset, -0.125]), None, 1, do_assert=False) - testHelper(np.array([c, offset, -c]), None, 1, do_assert=False) - testHelper(np.array([-c, offset, -c]), None, 1, do_assert=False) - testHelper(np.array([0, c, -c]), None, 1, do_assert=False) - - testHelper(np.array([-c, -offset, -c]), None, 0, do_assert=False) - testHelper( - np.array([config.LEG_L * (3 ** 0.5) / 2, offset, -config.LEG_L / 2]), - None, - 1, - do_assert=False, - ) - - -# function testForwardKinematics!() -# println("\n-------------- Testing Forward Kinematics -----------") -# config = PupperConfig() -# println("\nTesting Forward Kinematics") -# function testHelper(alpha, r_true, i; do_assert=true) -# eps = 1e-6 -# r = zeros(3) -# println("Vectors") -# a = [alpha.data...] -# @time legForwardKinematics!(r, a, i, config) -# println("SVectors") -# @time r = legForwardKinematics(alpha, i, config) -# println("Leg ", i, ": α: ", alpha, " -> r: ", r) -# if do_assert -# @assert norm(r_true - r) < eps -# end -# end - -# l = config.LEG_L -# offset = config.ABDUCTION_OFFSET -# testHelper(SVector{3}([0.0, 0.0, 0.0]), SVector{3}([0, offset, -l]), 2) -# testHelper(SVector{3}([0.0, pi/4, 0.0]), missing, 2, do_assert=false) -# # testHelper([0.0, 0.0, 0.0], [0, offset, -l], 2) -# # testHelper([0.0, pi/4, 0.0], missing, 2, do_assert=false) -# end - -# function testForwardInverseAgreeance() -# println("\n-------------- Testing Forward/Inverse Consistency -----------") -# config = PupperConfig() -# println("\nTest forward/inverse consistency") -# eps = 1e-6 -# for i in 1:10 -# alpha = SVector(rand()-0.5, rand()-0.5, (rand()-0.5)*0.05) -# leg = rand(1:4) -# @time r = legForwardKinematics(alpha, leg, config) -# # @code_warntype legForwardKinematics!(r, alpha, leg, config) -# @time alpha_prime = leg_explicitinversekinematics_prismatic(r, leg, config) -# # @code_warntype inverseKinematicsExplicit!(alpha_prime, r, leg, config) -# println("Leg ", leg, ": α: ", round_(alpha, 3), " -> r_body_foot: ", round_(r, 3), " -> α': ", round_(alpha_prime, 3)) -# @assert norm(alpha_prime - alpha) < eps -# end -# end - -# function testAllInverseKinematics() -# println("\n-------------- Testing Four Leg Inverse Kinematics -----------") -# function helper(r_body, alpha_true; do_assert=true) -# println("Timing for fourlegs_inversekinematics") -# config = PupperConfig() -# @time alpha = fourlegs_inversekinematics(SMatrix(r_body), config) -# @code_warntype fourlegs_inversekinematics(SMatrix(r_body), config) -# println("r: ", r_body, " -> α: ", alpha) - -# if do_assert -# @assert norm(alpha - alpha_true) < 1e-10 -# end -# end -# config = PupperConfig() -# f = config.LEG_FB -# l = config.LEG_LR -# s = -0.125 -# o = config.ABDUCTION_OFFSET -# r_body = MMatrix{3,4}(zeros(3,4)) -# r_body[:,1] = [f, -l-o, s] -# r_body[:,2] = [f, l+o, s] -# r_body[:,3] = [-f, -l-o, s] -# r_body[:,4] = [-f, l+o, s] - -# helper(r_body, zeros(3,4)) -# helper(SMatrix{3,4}(zeros(3,4)), missing, do_assert=false) -# end - -# function testKinematics() -# testInverseKinematicsExplicit!() -# testForwardKinematics!() -# testForwardInverseAgreeance() -# testAllInverseKinematics() -# end - -# function testGait() -# println("\n-------------- Testing Gait -----------") -# p = GaitParams() -# # println("Gait params=",p) -# t = 680 -# println("Timing for phaseindex") -# @time ph = phaseindex(t, p) -# # @code_warntype phaseindex(t, p) -# println("t=",t," phase=",ph) -# @assert ph == 4 -# @assert phaseindex(0, p) == 1 - -# println("Timing for contacts") -# @time c = contacts(t, p) -# # @code_warntype contacts(t, p) -# @assert typeof(c) == SArray{Tuple{4},Int64,1,4} -# println("t=", t, " contacts=", c) -# end - - -def test_stance_controller(): - print("\n-------------- Testing Stance Controller -----------") - stanceparams = StanceParams() - gaitparams = GaitParams() - - zmeas = -0.20 - mvref = MovementReference() - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - assert np.linalg.norm(dR - np.eye(3)) < 1e-10 - assert np.linalg.norm(dp - np.array([0, 0, gaitparams.dt * 0.04])) < 1e-10 - - zmeas = -0.18 - mvref = MovementReference() - mvref.v_xy_ref = np.array([1.0, 0.0]) - mvref.z_ref = -0.18 - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - - zmeas = -0.20 - mvref = MovementReference() - mvref.wz_ref = 1.0 - mvref.z_ref = -0.20 - dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) - assert np.linalg.norm(dp - np.array([0, 0, 0])) < 1e-10 - assert np.linalg.norm(dR[0, 1] - (gaitparams.dt)) < 1e-6 - - stancefootloc = np.zeros(3) - sloc = stance_foot_location(stancefootloc, stanceparams, gaitparams, mvref) - - -# function typeswinglegcontroller() -# println("\n--------------- Code warn type for raibert_tdlocation[s] ----------") -# swp = SwingParams() -# stp = StanceParams() -# gp = GaitParams() -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# raibert_tdlocations(swp, stp, gp, mvref) - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# raibert_tdlocation(1, swp, stp, gp, mvref) -# end - -# function TestSwingLegController() -# println("\n-------------- Testing Swing Leg Controller -----------") -# swp = SwingParams() -# stp = StanceParams() -# gp = GaitParams() -# p = ControllerParams() -# println("Timing for swingheight:") -# @time z = swingheight(0.5, swp) -# println("z clearance at t=1/2swingtime =>",z) -# @assert abs(z - swp.zclearance) < 1e-10 - -# println("Timing for swingheight:") -# @time z = swingheight(0, swp) -# println("Z clearance at t=0 =>",z) -# @assert abs(z) < 1e-10 - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# println("Timing for raibert tdlocation*s*:") -# @time l = raibert_tdlocations(swp, stp, gp, mvref) -# target = stp.defaultstance .+ [gp.stanceticks*gp.dt*0.5*1, 0, 0] -# println("Touchdown locations =>", l, " ", target) -# @assert norm(l - target) <= 1e-10 - -# mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) -# println("Timing for raibert tdlocation:") -# @time l = raibert_tdlocation(1, swp, stp, gp, mvref) - -# fcurrent = SMatrix{3, 4, Float64}(stp.defaultstance) -# mvref = MovementReference() -# tswing = 0.125 -# println("Timing for swingfootlocation*s* increment") -# @time l = swingfootlocations(tswing, fcurrent, swp, stp, gp, mvref) -# println(l) - -# fcurrent = SVector{3, Float64}(0.0, 0.0, 0.0) -# println("Timing for swingfootlocation") -# @time swingfootlocation(tswing, fcurrent, 1, swp, stp, gp, mvref) - -# typeswinglegcontroller() -# return nothing -# end - - -def test_run(): - print("Run timing") - foot_loc_history, joint_angle_history = run() - plt.subplot(211) - x = plt.plot(foot_loc_history[0, :, :].T, label="x") - y = plt.plot(foot_loc_history[1, :, :].T, label="y") - z = plt.plot(foot_loc_history[2, :, :].T, label="z") - - plt.subplot(212) - alpha = plt.plot(joint_angle_history[0, :, :].T, label="alpha") - beta = plt.plot(joint_angle_history[1, :, :].T, label="beta") - gamma = plt.plot(joint_angle_history[2, :, :].T, label="gamma") - plt.show() - - # plot(x, β, y, α, z, γ, layout=(3,2), legend=false)) - - -# function teststep() -# swingparams = SwingParams() -# stanceparams = StanceParams() -# gaitparams = GaitParams() -# mvref = MovementReference(vxyref=SVector{2}(0.2, 0.0), wzref=0.0) -# conparams = ControllerParams() -# robotconfig = PupperConfig() - - -# footlocations::SMatrix{3, 4, Float64, 12} = stanceparams.defaultstance .+ SVector{3, Float64}(0, 0, mvref.zref) - -# ticks = 1 -# println("Timing for step!") -# @btime step($ticks, $footlocations, $swingparams, $stanceparams, $gaitparams, $mvref, $conparams) -# @code_warntype step(ticks, footlocations, swingparams, stanceparams, gaitparams, mvref, conparams) -# end - -# # testGait() -# # testKinematics() -# # TestStanceController() -# # testStaticArrays() -# # TestSwingLegController() -# test_inversekinematics_linkage() - -# # teststep() -# # testrun() - -test_inverse_kinematics_linkage() -test_stance_controller() -test_run() diff --git a/src/Utilities.py b/src/Utilities.py deleted file mode 100644 index 4ca4ec6d..00000000 --- a/src/Utilities.py +++ /dev/null @@ -1,10 +0,0 @@ -import numpy as np - - -def deadband(value, band_radius): - return max(value - band_radius, 0) + min(value + band_radius, 0) - - -def clipped_first_order_filter(input, target, max_rate, tau): - rate = (target - input) / tau - return np.clip(rate, -max_rate, max_rate) diff --git a/stanford_quad/__init__.py b/stanford_quad/__init__.py new file mode 100644 index 00000000..c3cf7f9d --- /dev/null +++ b/stanford_quad/__init__.py @@ -0,0 +1,202 @@ +from gym import register +import numpy as np + +from stanford_quad.envs.imitation_recordings import IMITATION_LIB + +HEADLESSNESS = ["Headless", "Graphical"] + + +def make_imitation_env(trick): + steps = IMITATION_LIB[trick]["end"] - IMITATION_LIB[trick]["start"] + register( + id=f"Pupper-Recording-{IMITATION_LIB[trick]['env_name']}-v0", + entry_point="stanford_quad.envs:ImitationEnv", + kwargs={ + # "debug": (False if headlessness == "Headless" else True), + "trick": "walk-forward", + }, + max_episode_steps=steps, + ) + + +make_imitation_env("walk-forward") + + +ACTION_TYPE = ["Relative", "Absolute", "Incremental"] +ACTION_SMOOTHING = [1, 2, 3, 4] +RANDOM_ROT = [0, 1, 10, 100] +ACTION_SCALING = [1.0, 2.0, 4.0] + list(np.arange(0.05, 0.5, 0.05)) +STEPS = [120, 240, 360] + + +for headlessness in HEADLESSNESS: + for action_type in ACTION_TYPE: + for action_smoothing in ACTION_SMOOTHING: + for action_scaling in ACTION_SCALING: + for random_rot in RANDOM_ROT: + for steps in STEPS: + name = ( + f"Pupper-Walk-{action_type}-" + f"steps_{steps}-" + f"aScale_{action_scaling:.2}-" + f"aSmooth_{action_smoothing}-" + f"RandomZRot_{random_rot}-{headlessness}-v0" + ) + print(name) + register( + id=name, + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": steps, + "relative_action": True if action_type == "Relative" else False, + "incremental_action": True if action_type == "Incremental" else False, + "action_scaling": action_scaling, + "action_smoothing": action_smoothing, + "random_rot": (0, 0, random_rot), + }, + max_episode_steps=steps, + ) + + +###### LEGACY - WILL REMOVE SOON + + +for headlessness in HEADLESSNESS: + register( + id=f"Pupper-Walk-Absolute-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": False, + "action_scaling": 1, + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 1, + }, + max_episode_steps=120, + ) + for scale_down in list(np.arange(0.05, 0.5, 0.05)): + register( + id=f"Pupper-Walk-Relative-ScaledDown_{scale_down:.2}-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": scale_down, + }, + max_episode_steps=120, + ) + # print(f"Pupper-Walk-Relative-ScaledDown_{scale_down:.2}-{headlessness}-v0") + register( + id=f"Pupper-Walk-Relative-ScaledDown-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "random_rot": (0, 0, 15), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-ScaledNSmoothed3-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "action_smoothing": 3, + "random_rot": (0, 0, 15), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-ScaledNSmoothed5-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "action_smoothing": 5, + "random_rot": (0, 0, 15), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-Smoothed5-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 1, + "action_smoothing": 5, + "random_rot": (0, 0, 15), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-RewardStable0.5-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 1, + "reward_coefficients": (0.1, 1, 0.5), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-RewardStable0.5-ScaledDown3-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "reward_coefficients": (0.1, 1, 0.5), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-RewardStable0.5-ScaledDown-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "random_rot": (0, 0, 15), + "reward_coefficients": (0.1, 1, 0.5), + }, + max_episode_steps=120, + ) + register( + id=f"Pupper-Walk-Relative-RewardStable0.5-ScaledNSmoothed-RandomZRot-{headlessness}-v0", + entry_point="stanford_quad.envs:WalkingEnv", + kwargs={ + "debug": (False if headlessness == "Headless" else True), + "steps": 120, + "relative_action": True, + "action_scaling": 0.3, + "action_smoothing": 3, + "random_rot": (0, 0, 15), + "reward_coefficients": (0.1, 1, 0.5), + }, + max_episode_steps=120, + ) diff --git a/stanford_quad/assets/__init__.py b/stanford_quad/assets/__init__.py new file mode 100644 index 00000000..2e377da4 --- /dev/null +++ b/stanford_quad/assets/__init__.py @@ -0,0 +1,4 @@ +import os + +ASSET_DIR = os.path.dirname(os.path.realpath(__file__)) + diff --git a/stanford_quad/assets/pupper-v1.urdf.xml b/stanford_quad/assets/pupper-v1.urdf.xml new file mode 100644 index 00000000..09de202a --- /dev/null +++ b/stanford_quad/assets/pupper-v1.urdf.xml @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/stanford_quad/assets/pupper_pybullet_out.xml b/stanford_quad/assets/pupper_pybullet_out.xml new file mode 100644 index 00000000..b5754f96 --- /dev/null +++ b/stanford_quad/assets/pupper_pybullet_out.xml @@ -0,0 +1,129 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Command.py b/stanford_quad/common/Command.py similarity index 100% rename from src/Command.py rename to stanford_quad/common/Command.py diff --git a/stanford_quad/common/Controller.py b/stanford_quad/common/Controller.py new file mode 100644 index 00000000..9aaab64c --- /dev/null +++ b/stanford_quad/common/Controller.py @@ -0,0 +1,130 @@ +from transforms3d.euler import euler2mat, quat2euler + +from stanford_quad.common.Gaits import GaitController +from stanford_quad.common.StanceController import StanceController + +import numpy as np +from collections import defaultdict + +from stanford_quad.common.State import BehaviorState +from stanford_quad.common.SwingLegController import SwingController +from stanford_quad.common.Utilities import clipped_first_order_filter + + +class Controller: + """Controller and planner object + """ + + def __init__(self, config, inverse_kinematics): + self.config = config + + self.smoothed_yaw = 0.0 # for REST mode only + self.inverse_kinematics = inverse_kinematics + + self.contact_modes = np.zeros(4) + self.gait_controller = GaitController(self.config) + self.swing_controller = SwingController(self.config) + self.stance_controller = StanceController(self.config) + + self.hop_transition_mapping = { + BehaviorState.REST: BehaviorState.HOP, + BehaviorState.HOP: BehaviorState.FINISHHOP, + BehaviorState.FINISHHOP: BehaviorState.REST, + BehaviorState.TROT: BehaviorState.HOP, + } + self.trot_transition_mapping = { + BehaviorState.REST: BehaviorState.TROT, + BehaviorState.TROT: BehaviorState.REST, + BehaviorState.HOP: BehaviorState.TROT, + BehaviorState.FINISHHOP: BehaviorState.TROT, + } + self.activate_transition_mapping = defaultdict( + lambda: BehaviorState.DEACTIVATED, {BehaviorState.DEACTIVATED: BehaviorState.REST} + ) + + def step_gait(self, state, command): + """Calculate the desired foot locations for the next timestep + + Returns + ------- + Numpy array (3, 4) + Matrix of new foot locations. + """ + contact_modes = self.gait_controller.contacts(state.ticks) + new_foot_locations = np.zeros((3, 4)) + for leg_index in range(4): + contact_mode = contact_modes[leg_index] + foot_location = state.foot_locations[:, leg_index] + if contact_mode == 1: + new_location = self.stance_controller.next_foot_location(leg_index, state, command) + else: + swing_proportion = self.gait_controller.subphase_ticks(state.ticks) / self.config.swing_ticks + new_location = self.swing_controller.next_foot_location(swing_proportion, leg_index, state, command) + new_foot_locations[:, leg_index] = new_location + return new_foot_locations, contact_modes + + def run(self, state, command): + """Steps the controller forward one timestep + + Parameters + ---------- + controller : Controller + Robot controller object. + """ + + ########## Update operating state based on command ###### + if command.activate_event: + state.behavior_state = self.activate_transition_mapping[state.behavior_state] + elif command.trot_event: + state.behavior_state = self.trot_transition_mapping[state.behavior_state] + elif command.hop_event: + state.behavior_state = self.hop_transition_mapping[state.behavior_state] + + if state.behavior_state == BehaviorState.TROT: + state.foot_locations, contact_modes = self.step_gait(state, command,) + + # Apply the desired body rotation + rotated_foot_locations = euler2mat(command.roll, command.pitch, 0.0) @ state.foot_locations + + # Construct foot rotation matrix to compensate for body tilt + (roll, pitch, yaw) = quat2euler(state.quat_orientation) + correction_factor = 0.8 + max_tilt = 0.4 + roll_compensation = correction_factor * np.clip(roll, -max_tilt, max_tilt) + pitch_compensation = correction_factor * np.clip(pitch, -max_tilt, max_tilt) + rmat = euler2mat(roll_compensation, pitch_compensation, 0) + + rotated_foot_locations = rmat.T @ rotated_foot_locations + + state.joint_angles = self.inverse_kinematics(rotated_foot_locations, self.config) + + elif state.behavior_state == BehaviorState.HOP: + state.foot_locations = self.config.default_stance + np.array([0, 0, -0.09])[:, np.newaxis] + state.joint_angles = self.inverse_kinematics(state.foot_locations, self.config) + + elif state.behavior_state == BehaviorState.FINISHHOP: + state.foot_locations = self.config.default_stance + np.array([0, 0, -0.22])[:, np.newaxis] + state.joint_angles = self.inverse_kinematics(state.foot_locations, self.config) + + elif state.behavior_state == BehaviorState.REST: + yaw_proportion = command.yaw_rate / self.config.max_yaw_rate + self.smoothed_yaw += self.config.dt * clipped_first_order_filter( + self.smoothed_yaw, + yaw_proportion * -self.config.max_stance_yaw, + self.config.max_stance_yaw_rate, + self.config.yaw_time_constant, + ) + # Set the foot locations to the default stance plus the standard height + state.foot_locations = self.config.default_stance + np.array([0, 0, command.height])[:, np.newaxis] + # Apply the desired body rotation + rotated_foot_locations = euler2mat(command.roll, command.pitch, self.smoothed_yaw,) @ state.foot_locations + state.joint_angles = self.inverse_kinematics(rotated_foot_locations, self.config) + + state.ticks += 1 + state.pitch = command.pitch + state.roll = command.roll + state.height = command.height + + def set_pose_to_default(self, state, controller): + state.foot_locations = self.config.default_stance + np.array([0, 0, self.config.default_z_ref])[:, np.newaxis] + state.joint_angles = controller.inverse_kinematics(state.foot_locations, self.config) diff --git a/src/Gaits.py b/stanford_quad/common/Gaits.py similarity index 100% rename from src/Gaits.py rename to stanford_quad/common/Gaits.py diff --git a/src/IMU.py b/stanford_quad/common/IMU.py similarity index 100% rename from src/IMU.py rename to stanford_quad/common/IMU.py diff --git a/src/JoystickInterface.py b/stanford_quad/common/JoystickInterface.py similarity index 94% rename from src/JoystickInterface.py rename to stanford_quad/common/JoystickInterface.py index e3ade947..5211515b 100644 --- a/src/JoystickInterface.py +++ b/stanford_quad/common/JoystickInterface.py @@ -1,9 +1,9 @@ import UDPComms import numpy as np -import time -from src.State import BehaviorState, State -from src.Command import Command -from src.Utilities import deadband, clipped_first_order_filter + +from stanford_quad.common.Command import Command +from stanford_quad.common.State import BehaviorState +from stanford_quad.common.Utilities import deadband, clipped_first_order_filter class JoystickInterface: diff --git a/src/StanceController.py b/stanford_quad/common/StanceController.py similarity index 99% rename from src/StanceController.py rename to stanford_quad/common/StanceController.py index dfd925d3..63f2a55b 100644 --- a/src/StanceController.py +++ b/stanford_quad/common/StanceController.py @@ -1,6 +1,7 @@ import numpy as np from transforms3d.euler import euler2mat + class StanceController: def __init__(self, config): self.config = config diff --git a/src/State.py b/stanford_quad/common/State.py similarity index 78% rename from src/State.py rename to stanford_quad/common/State.py index abb7c98b..acb148f8 100644 --- a/src/State.py +++ b/stanford_quad/common/State.py @@ -10,13 +10,13 @@ def __init__(self): self.pitch = 0.0 self.roll = 0.0 self.activation = 0 - self.behavior_state = BehaviorState.REST + self.behavior_state = BehaviorState.DEACTIVATED self.ticks = 0 self.foot_locations = np.zeros((3, 4)) self.joint_angles = np.zeros((3, 4)) - self.behavior_state = BehaviorState.REST + self.quat_orientation = np.array([1, 0, 0, 0]) class BehaviorState(Enum): @@ -24,4 +24,4 @@ class BehaviorState(Enum): REST = 0 TROT = 1 HOP = 2 - FINISHHOP = 3 \ No newline at end of file + FINISHHOP = 3 diff --git a/src/SwingLegController.py b/stanford_quad/common/SwingLegController.py similarity index 72% rename from src/SwingLegController.py rename to stanford_quad/common/SwingLegController.py index ac3b670b..063b44b1 100644 --- a/src/SwingLegController.py +++ b/stanford_quad/common/SwingLegController.py @@ -1,30 +1,18 @@ import numpy as np from transforms3d.euler import euler2mat + class SwingController: def __init__(self, config): self.config = config - def raibert_touchdown_location( - self, leg_index, command - ): - delta_p_2d = ( - self.config.alpha - * self.config.stance_ticks - * self.config.dt - * command.horizontal_velocity - ) + def raibert_touchdown_location(self, leg_index, command): + delta_p_2d = self.config.alpha * self.config.stance_ticks * self.config.dt * command.horizontal_velocity delta_p = np.array([delta_p_2d[0], delta_p_2d[1], 0]) - theta = ( - self.config.beta - * self.config.stance_ticks - * self.config.dt - * command.yaw_rate - ) + theta = self.config.beta * self.config.stance_ticks * self.config.dt * command.yaw_rate R = euler2mat(0, 0, theta) return R @ self.config.default_stance[:, leg_index] + delta_p - def swing_height(self, swing_phase, triangular=True): if triangular: if swing_phase < 0.5: @@ -33,13 +21,8 @@ def swing_height(self, swing_phase, triangular=True): swing_height_ = self.config.z_clearance * (1 - (swing_phase - 0.5) / 0.5) return swing_height_ - def next_foot_location( - self, - swing_prop, - leg_index, - state, - command, + self, swing_prop, leg_index, state, command, ): assert swing_prop >= 0 and swing_prop <= 1 foot_location = state.foot_locations[:, leg_index] diff --git a/stanford_quad/common/Tests.py b/stanford_quad/common/Tests.py new file mode 100644 index 00000000..ffa55b22 --- /dev/null +++ b/stanford_quad/common/Tests.py @@ -0,0 +1,304 @@ +# # using LinearAlgebra +# # using Profile +# # using StaticArrays +# # using Plots +# # using BenchmarkTools +# +# # include("Kinematics.jl") +# # include("PupperConfig.jl") +# # include("Gait.jl") +# # include("StanceController.jl") +# # include("SwingLegController.jl") +# # include("Types.jl") +# # include("Controller.jl") +# +# import numpy as np +# import matplotlib.pyplot as plt +# +# # function round_(a, dec) +# # return map(x -> round(x, digits=dec), a) +# # end +# +# # function testInverseKinematicsExplicit!() +# # println("\n-------------- Testing Inverse Kinematics -----------") +# # config = PupperConfig() +# # println("\nTesting Inverse Kinematics") +# # function testHelper(r, alpha_true, i; do_assert=true) +# # eps = 1e-6 +# # @time α = leg_explicitinversekinematics_prismatic(r, i, config) +# # println("Leg ", i, ": r: ", r, " -> α: ", α) +# # if do_assert +# # @assert norm(α - alpha_true) < eps +# # end +# # end +# +# # c = config.LEG_L/sqrt(2) +# # offset = config.ABDUCTION_OFFSET +# # testHelper(SVector(0, offset, -0.125), SVector(0, 0, 0), 2) +# # testHelper(SVector(c, offset, -c), SVector(0, -pi/4, 0), 2) +# # testHelper(SVector(-c, offset, -c), SVector(0, pi/4, 0), 2) +# # testHelper(SVector(0, c, -c), missing, 2, do_assert=false) +# +# # testHelper(SVector(-c, -offset, -c), [0, pi/4, 0], 1) +# # testHelper(SVector(config.LEG_L * sqrt(3)/2, offset, -config.LEG_L / 2), SVector(0, -pi/3, 0), 2) +# # end +# from stanford_quad.pupper.Kinematics import leg_explicit_inverse_kinematics +# +# +# def test_inverse_kinematics_linkage(): +# print("\n-------------- Testing Five-bar Linkage Inverse Kinematics -----------") +# config = PupperConfig() +# print("\nTesting Inverse Kinematics") +# +# def testHelper(r, alpha_true, i, do_assert=True): +# eps = 1e-6 +# alpha = leg_explicit_inverse_kinematics(r, i, config) +# print("Leg ", i, ": r: ", r, " -> α: ", alpha) +# if do_assert: +# assert np.linalg.norm(alpha - alpha_true) < eps +# +# c = config.LEG_L / (2 ** 0.5) +# offset = config.ABDUCTION_OFFSET +# testHelper(np.array([0, offset, -0.125]), None, 1, do_assert=False) +# testHelper(np.array([c, offset, -c]), None, 1, do_assert=False) +# testHelper(np.array([-c, offset, -c]), None, 1, do_assert=False) +# testHelper(np.array([0, c, -c]), None, 1, do_assert=False) +# +# testHelper(np.array([-c, -offset, -c]), None, 0, do_assert=False) +# testHelper( +# np.array([config.LEG_L * (3 ** 0.5) / 2, offset, -config.LEG_L / 2]), +# None, +# 1, +# do_assert=False, +# ) +# +# +# # function testForwardKinematics!() +# # println("\n-------------- Testing Forward Kinematics -----------") +# # config = PupperConfig() +# # println("\nTesting Forward Kinematics") +# # function testHelper(alpha, r_true, i; do_assert=true) +# # eps = 1e-6 +# # r = zeros(3) +# # println("Vectors") +# # a = [alpha.data...] +# # @time legForwardKinematics!(r, a, i, config) +# # println("SVectors") +# # @time r = legForwardKinematics(alpha, i, config) +# # println("Leg ", i, ": α: ", alpha, " -> r: ", r) +# # if do_assert +# # @assert norm(r_true - r) < eps +# # end +# # end +# +# # l = config.LEG_L +# # offset = config.ABDUCTION_OFFSET +# # testHelper(SVector{3}([0.0, 0.0, 0.0]), SVector{3}([0, offset, -l]), 2) +# # testHelper(SVector{3}([0.0, pi/4, 0.0]), missing, 2, do_assert=false) +# # # testHelper([0.0, 0.0, 0.0], [0, offset, -l], 2) +# # # testHelper([0.0, pi/4, 0.0], missing, 2, do_assert=false) +# # end +# +# # function testForwardInverseAgreeance() +# # println("\n-------------- Testing Forward/Inverse Consistency -----------") +# # config = PupperConfig() +# # println("\nTest forward/inverse consistency") +# # eps = 1e-6 +# # for i in 1:10 +# # alpha = SVector(rand()-0.5, rand()-0.5, (rand()-0.5)*0.05) +# # leg = rand(1:4) +# # @time r = legForwardKinematics(alpha, leg, config) +# # # @code_warntype legForwardKinematics!(r, alpha, leg, config) +# # @time alpha_prime = leg_explicitinversekinematics_prismatic(r, leg, config) +# # # @code_warntype inverseKinematicsExplicit!(alpha_prime, r, leg, config) +# # println("Leg ", leg, ": α: ", round_(alpha, 3), " -> r_body_foot: ", round_(r, 3), " -> α': ", round_(alpha_prime, 3)) +# # @assert norm(alpha_prime - alpha) < eps +# # end +# # end +# +# # function testAllInverseKinematics() +# # println("\n-------------- Testing Four Leg Inverse Kinematics -----------") +# # function helper(r_body, alpha_true; do_assert=true) +# # println("Timing for fourlegs_inversekinematics") +# # config = PupperConfig() +# # @time alpha = fourlegs_inversekinematics(SMatrix(r_body), config) +# # @code_warntype fourlegs_inversekinematics(SMatrix(r_body), config) +# # println("r: ", r_body, " -> α: ", alpha) +# +# # if do_assert +# # @assert norm(alpha - alpha_true) < 1e-10 +# # end +# # end +# # config = PupperConfig() +# # f = config.LEG_FB +# # l = config.LEG_LR +# # s = -0.125 +# # o = config.ABDUCTION_OFFSET +# # r_body = MMatrix{3,4}(zeros(3,4)) +# # r_body[:,1] = [f, -l-o, s] +# # r_body[:,2] = [f, l+o, s] +# # r_body[:,3] = [-f, -l-o, s] +# # r_body[:,4] = [-f, l+o, s] +# +# # helper(r_body, zeros(3,4)) +# # helper(SMatrix{3,4}(zeros(3,4)), missing, do_assert=false) +# # end +# +# # function testKinematics() +# # testInverseKinematicsExplicit!() +# # testForwardKinematics!() +# # testForwardInverseAgreeance() +# # testAllInverseKinematics() +# # end +# +# # function testGait() +# # println("\n-------------- Testing Gait -----------") +# # p = GaitParams() +# # # println("Gait params=",p) +# # t = 680 +# # println("Timing for phaseindex") +# # @time ph = phaseindex(t, p) +# # # @code_warntype phaseindex(t, p) +# # println("t=",t," phase=",ph) +# # @assert ph == 4 +# # @assert phaseindex(0, p) == 1 +# +# # println("Timing for contacts") +# # @time c = contacts(t, p) +# # # @code_warntype contacts(t, p) +# # @assert typeof(c) == SArray{Tuple{4},Int64,1,4} +# # println("t=", t, " contacts=", c) +# # end +# +# +# def test_stance_controller(): +# print("\n-------------- Testing Stance Controller -----------") +# stanceparams = StanceParams() +# gaitparams = GaitParams() +# +# zmeas = -0.20 +# mvref = MovementReference() +# dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) +# assert np.linalg.norm(dR - np.eye(3)) < 1e-10 +# assert np.linalg.norm(dp - np.array([0, 0, gaitparams.dt * 0.04])) < 1e-10 +# +# zmeas = -0.18 +# mvref = MovementReference() +# mvref.v_xy_ref = np.array([1.0, 0.0]) +# mvref.z_ref = -0.18 +# dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) +# +# zmeas = -0.20 +# mvref = MovementReference() +# mvref.wz_ref = 1.0 +# mvref.z_ref = -0.20 +# dp, dR = position_delta(zmeas, stanceparams, mvref, gaitparams) +# assert np.linalg.norm(dp - np.array([0, 0, 0])) < 1e-10 +# assert np.linalg.norm(dR[0, 1] - (gaitparams.dt)) < 1e-6 +# +# stancefootloc = np.zeros(3) +# sloc = stance_foot_location(stancefootloc, stanceparams, gaitparams, mvref) +# +# +# # function typeswinglegcontroller() +# # println("\n--------------- Code warn type for raibert_tdlocation[s] ----------") +# # swp = SwingParams() +# # stp = StanceParams() +# # gp = GaitParams() +# # mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) +# # raibert_tdlocations(swp, stp, gp, mvref) +# +# # mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) +# # raibert_tdlocation(1, swp, stp, gp, mvref) +# # end +# +# # function TestSwingLegController() +# # println("\n-------------- Testing Swing Leg Controller -----------") +# # swp = SwingParams() +# # stp = StanceParams() +# # gp = GaitParams() +# # p = ControllerParams() +# # println("Timing for swingheight:") +# # @time z = swingheight(0.5, swp) +# # println("z clearance at t=1/2swingtime =>",z) +# # @assert abs(z - swp.zclearance) < 1e-10 +# +# # println("Timing for swingheight:") +# # @time z = swingheight(0, swp) +# # println("Z clearance at t=0 =>",z) +# # @assert abs(z) < 1e-10 +# +# # mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) +# # println("Timing for raibert tdlocation*s*:") +# # @time l = raibert_tdlocations(swp, stp, gp, mvref) +# # target = stp.defaultstance .+ [gp.stanceticks*gp.dt*0.5*1, 0, 0] +# # println("Touchdown locations =>", l, " ", target) +# # @assert norm(l - target) <= 1e-10 +# +# # mvref = MovementReference(SVector(1.0, 0.0), 0, -0.18) +# # println("Timing for raibert tdlocation:") +# # @time l = raibert_tdlocation(1, swp, stp, gp, mvref) +# +# # fcurrent = SMatrix{3, 4, Float64}(stp.defaultstance) +# # mvref = MovementReference() +# # tswing = 0.125 +# # println("Timing for swingfootlocation*s* increment") +# # @time l = swingfootlocations(tswing, fcurrent, swp, stp, gp, mvref) +# # println(l) +# +# # fcurrent = SVector{3, Float64}(0.0, 0.0, 0.0) +# # println("Timing for swingfootlocation") +# # @time swingfootlocation(tswing, fcurrent, 1, swp, stp, gp, mvref) +# +# # typeswinglegcontroller() +# # return nothing +# # end +# +# +# def test_run(): +# print("Run timing") +# foot_loc_history, joint_angle_history = run() +# plt.subplot(211) +# x = plt.plot(foot_loc_history[0, :, :].T, label="x") +# y = plt.plot(foot_loc_history[1, :, :].T, label="y") +# z = plt.plot(foot_loc_history[2, :, :].T, label="z") +# +# plt.subplot(212) +# alpha = plt.plot(joint_angle_history[0, :, :].T, label="alpha") +# beta = plt.plot(joint_angle_history[1, :, :].T, label="beta") +# gamma = plt.plot(joint_angle_history[2, :, :].T, label="gamma") +# plt.show() +# +# # plot(x, β, y, α, z, γ, layout=(3,2), legend=false)) +# +# +# # function teststep() +# # swingparams = SwingParams() +# # stanceparams = StanceParams() +# # gaitparams = GaitParams() +# # mvref = MovementReference(vxyref=SVector{2}(0.2, 0.0), wzref=0.0) +# # conparams = ControllerParams() +# # robotconfig = PupperConfig() +# +# +# # footlocations::SMatrix{3, 4, Float64, 12} = stanceparams.defaultstance .+ SVector{3, Float64}(0, 0, mvref.zref) +# +# # ticks = 1 +# # println("Timing for step!") +# # @btime step($ticks, $footlocations, $swingparams, $stanceparams, $gaitparams, $mvref, $conparams) +# # @code_warntype step(ticks, footlocations, swingparams, stanceparams, gaitparams, mvref, conparams) +# # end +# +# # # testGait() +# # # testKinematics() +# # # TestStanceController() +# # # testStaticArrays() +# # # TestSwingLegController() +# # test_inversekinematics_linkage() +# +# # # teststep() +# # # testrun() +# +# test_inverse_kinematics_linkage() +# test_stance_controller() +# test_run() diff --git a/stanford_quad/common/Utilities.py b/stanford_quad/common/Utilities.py new file mode 100644 index 00000000..68d30419 --- /dev/null +++ b/stanford_quad/common/Utilities.py @@ -0,0 +1,36 @@ +import numpy as np + + +def deadband(value, band_radius): + return max(value - band_radius, 0) + min(value + band_radius, 0) + + +def clipped_first_order_filter(input, target, max_rate, tau): + rate = (target - input) / tau + return np.clip(rate, -max_rate, max_rate) + + +def parallel_to_serial_joint_angles(joint_matrix): + """Convert from joint angles meant for the parallel linkage in + Pupper to the joint angles in the serial linkage approximation implemented in the simulation + + Parameters + ---------- + joint_matrix : Numpy array (3, 4) + Joint angles for parallel linkage + + Returns + ------- + Numpy array (3, 4) + Joint angles for equivalent serial linkage + """ + temp = joint_matrix + temp[2, :] -= joint_matrix[1, :] + return temp + + +def controller_to_sim(joints): + joint_angles = np.reshape(joints, (3, 4)) + joint_angles_robot = parallel_to_serial_joint_angles(joint_angles) + + return joint_angles_robot.T.flatten() diff --git a/pupper/__init__.py b/stanford_quad/common/__init__.py similarity index 100% rename from pupper/__init__.py rename to stanford_quad/common/__init__.py diff --git a/stanford_quad/envs/__init__.py b/stanford_quad/envs/__init__.py new file mode 100644 index 00000000..46c4f10b --- /dev/null +++ b/stanford_quad/envs/__init__.py @@ -0,0 +1,2 @@ +from stanford_quad.envs.walking_env import WalkingEnv +from stanford_quad.envs.imitation_env import ImitationEnv diff --git a/stanford_quad/envs/imitation_env.py b/stanford_quad/envs/imitation_env.py new file mode 100644 index 00000000..e58a75b8 --- /dev/null +++ b/stanford_quad/envs/imitation_env.py @@ -0,0 +1,223 @@ +import gym +import h5py +import numpy as np +from scipy.spatial.transform import Rotation +import cv2 + +from stanford_quad.envs.utils import rendermode_from_string, RenderMode +from stanford_quad.sim.HardwareInterface import HardwareInterface +from stanford_quad.sim.simulator2 import PupperSim2, FREQ_SIM +from stanford_quad.envs.imitation_recordings import IMITATION_LIB + +CONTROL_FREQUENCY = 60 +RECORDINGS_PATH = "/Users/florian/dev/pyvicon/scripts/pupper-{}.hdf5" +RESOLUTION = 48 +SIM_AGENT_COLOR = (0, 1, 1, 1) +SIM_REF_COLOR = (1, 0, 1, 1) +JOINT_ERROR_SCALE = 5.0 # copied over from https://github.com/google-research/motion_imitation/blob/master/motion_imitation/envs/env_wrappers/imitation_task.py#L59 +CAMERA_OFFSET = (-0.03, -0.275, -0.05) +LOOKAT_OFFSET = (-0.03, 0, -0.05) +MASKED = True # do you want to cut out the robot from the background + + +def get_recording_joints(joints, frame_idx): + joint_angles = np.reshape(joints[frame_idx], (3, 4)) + joint_angles_robot = HardwareInterface.parallel_to_serial_joint_angles(joint_angles) + + return joint_angles_robot.T.flatten() + + +def get_recording_pose(vicon_positions, base_pos, frame_idx): + diff_pos = vicon_positions[frame_idx] - base_pos + diff_pos /= 500 # for some weird reason it's not /100 + z = diff_pos[2] + 0.21 # (compensating for the model) + x = diff_pos[1] + y = diff_pos[0] + + # fixed manual rotation for now + rot = Rotation.from_quat([0, 0, 0, 1]) + return (x, y, z), rot + + +def merge_images(img_ref, img_agent): + # the "+2" is only there to create some space between the frames + img_merged = np.zeros((RESOLUTION, RESOLUTION * 2 + 2, 3), np.uint8) + img_merged[:, :RESOLUTION, :] = img_ref[:, :, ::-1] + img_merged[:, RESOLUTION + 2 :, :] = img_agent[:, :, ::-1] + return img_merged + + +class ImitationEnv(gym.Env): + def __init__(self, trick, control_freq=CONTROL_FREQUENCY, action_scaling=0.5): + super().__init__() + + self.control_freq = control_freq + self.action_scaling = action_scaling + + f = h5py.File(RECORDINGS_PATH.format(trick), "r") + + self.vicon_positions = f["vicon_positions"] + self.vicon_rotations = f["vicon_rotations"] + + self.idx_start = IMITATION_LIB[trick]["start"] + self.idx_end = IMITATION_LIB[trick]["end"] + + self.base_pos = self.vicon_positions[self.idx_start] + self.base_rot = self.vicon_rotations[self.idx_end] + + self.joints = f["joints"] + self.feet = f["foot_positions"] + + # observation space: + # - 12 lef joints in the order + # - front right hip + # - front right upper leg + # - front right lower leg + # - front left hip/upper/lower leg + # - back right hip/upper/lower leg + # - back left hip/upper/lower leg + # - 3 body orientation in euler angles + # - 2 linear velocity (only along the plane, we don't care about z velocity + + self.observation_space = gym.spaces.Box(low=-1, high=1, shape=(12 + 3 + 2,), dtype=np.float32) + + # action = 12 joint angles in the same order as above (fr/fl/br/bl and each with hip/upper/lower) + self.action_space = gym.spaces.Box(low=-1, high=1, shape=(12,), dtype=np.float32) + + self.sim_agent = PupperSim2( + debug=False, + start_standing=False, + gain_pos=1 / 16, + gain_vel=1 / 8, + max_torque=1 / 2, + img_size=(RESOLUTION, RESOLUTION), + ) + self.sim_agent.change_color((SIM_AGENT_COLOR)) + self.dt = 1 / self.control_freq + self.sim_steps = int(round(FREQ_SIM / control_freq)) + + self.sim_ref = PupperSim2(debug=False, img_size=(RESOLUTION, RESOLUTION)) + self.sim_ref.reset() + # make the robot limp for demonstration + self.sim_ref.make_kinematic(SIM_REF_COLOR) + + self.episode_steps = 0 + self.frame_idx = self.idx_start + + def _get_obs(self): + pos, orn, vel = self.sim_agent.get_pos_orn_vel() + + # to normalize to [-1,1] + joint_states = np.array(self.sim_agent.get_joint_states()) / np.pi + obs = list(joint_states) + list(orn) + list(vel)[:2] + return obs + + def reset(self): + self.frame_idx = self.idx_start + + # reset the learning agent + self.sim_agent.reset(rest=True) + + for _ in range(10): + self.sim_agent.step() + + return self._get_obs() + + def _sanitize_actions(self, actions): + assert len(actions) == 12 + scaled = actions * np.pi * self.action_scaling # because 1/-1 corresponds to pi/-pi radians rotation + scaled += self.sim_agent.get_rest_pos() + # this enforces an action range of -1/1, except if it's relative action - then the action space is asymmetric + clipped = np.clip(scaled, -np.pi + 0.001, np.pi - 0.001) + return clipped + + def _calc_imitation_error(self, joints_agent, joints_ref): + diff = np.array(joints_ref - joints_agent) + pose_err = diff.dot(diff) + pose_reward = np.exp(-JOINT_ERROR_SCALE * pose_err) + return pose_reward + + def step(self, action): + action_clean = self._sanitize_actions(action) + + ## reference sim + self.frame_idx += 1 # retrieving the next recording frame + + joints_reference = get_recording_joints(self.joints, self.frame_idx) + self.sim_ref.set(joints_reference) + + pos, rot = get_recording_pose(self.vicon_positions, self.base_pos, self.frame_idx) + self.sim_ref.move_kinectic_body(pos, rot.as_quat()) + + self.sim_ref.step() + + ## learner sim + self.sim_agent.action(action_clean) + + for _ in range(self.sim_steps): + self.sim_agent.step() + + joints_agent = self.sim_agent.get_joint_states() + + obs = self._get_obs() + reward = self._calc_imitation_error(joints_agent, joints_reference) + done = False + misc = {} + + if self.frame_idx == self.idx_end: + done = True + + return obs, reward, done, misc + + def _render(self, target_sim): + with_segmap = False + if MASKED: + with_segmap = True + + img, segmap = getattr(self, target_sim).take_photo( + with_segmap=with_segmap, camera_offset=CAMERA_OFFSET, lookat_offset=LOOKAT_OFFSET + ) + + if MASKED: + img[segmap != 1] = 0 + return img + + def _render_agent(self): + return self._render("sim_agent") + + def _render_ref(self): + return self._render("sim_ref") + + def render(self, mode="human"): + mode_i = rendermode_from_string(mode) + + if mode_i is RenderMode.HUMAN: + img_agent = self._render_agent() + img_ref = self._render_ref() + cv2.imshow("Left: Ref, Right: Agent", merge_images(img_ref, img_agent)) + cv2.waitKey(1) + + elif mode_i is RenderMode.RGB_ARRAY: + return self._render_agent() + elif mode_i is RenderMode.RGB_ARRAY_REF: + return self._render_ref() + else: + raise NotImplementedError(f"I don't have a render function for mode {mode_i}.") + + +if __name__ == "__main__": + + # necessary for gym env to register (not here but when you copy this snippet over) + import stanford_quad + + env = gym.make("Pupper-Recording-WalkForward-v0") + + for _ in range(3): + obs = env.reset() + print(obs) + while True: + obs, rew, done, misc = env.step(np.random.uniform(-0.1, 0.1, 12)) + print(rew, obs) + env.render("human") + if done: + break diff --git a/stanford_quad/envs/imitation_recordings.py b/stanford_quad/envs/imitation_recordings.py new file mode 100644 index 00000000..fce76d02 --- /dev/null +++ b/stanford_quad/envs/imitation_recordings.py @@ -0,0 +1 @@ +IMITATION_LIB = {"walk-forward": {"start": 90, "end": 400, "env_name": "WalkForward"}} diff --git a/stanford_quad/envs/utils.py b/stanford_quad/envs/utils.py new file mode 100644 index 00000000..a03f4c3f --- /dev/null +++ b/stanford_quad/envs/utils.py @@ -0,0 +1,14 @@ +from enum import IntEnum + + +class RenderMode(IntEnum): + HUMAN = 0 + RGB_ARRAY = 1 + RGB_ARRAY_REF = 2 + + +def rendermode_from_string(mode): + assert mode in ["human", "rgb_array", "rgb_array_ref"] + return {"human": RenderMode.HUMAN, "rgb_array": RenderMode.RGB_ARRAY, "rgb_array_ref": RenderMode.RGB_ARRAY_REF}[ + mode + ] diff --git a/stanford_quad/envs/walking_env.py b/stanford_quad/envs/walking_env.py new file mode 100644 index 00000000..9c9c1085 --- /dev/null +++ b/stanford_quad/envs/walking_env.py @@ -0,0 +1,164 @@ +from collections import deque + +import gym +from gym import spaces +import numpy as np +import matplotlib.pyplot as plt +from stanford_quad.sim.simulator2 import PupperSim2, FREQ_SIM + +CONTROL_FREQUENCY = 60 # Hz, the simulation runs at 240Hz by default and doing a multiple of that is easier +MAX_ANGLE_PER_SEC = 90 + + +class WalkingEnv(gym.Env): + def __init__( + self, + debug=False, + steps=120, + control_freq=CONTROL_FREQUENCY, + relative_action=True, + incremental_action=False, + action_scaling=1.0, + action_smoothing=1, + random_rot=(0, 0, 0), + reward_coefficients=(0.1, 1, 0), + stop_on_flip=False, + ): + """ Gym-compatible environment to teach the pupper how to walk + + :param bool debug: If True, shows PyBullet in GUI mode. Set to False when training. + :param int steps: How long is the episode? Each step = one call to WalkingEnv.step() + :param int control_freq: How many simulation steps are there in a second of Pybullet sim. Pybullet always runs + at 240Hz but that's not optimal for RL control. + :param bool relative_action: If set to True, then all actions are added to the resting position. + This give the robot a more stable starting position. + """ + + super().__init__() + + # observation space: + # - 12 lef joints in the order + # - front right hip + # - front right upper leg + # - front right lower leg + # - front left hip/upper/lower leg + # - back right hip/upper/lower leg + # - back left hip/upper/lower leg + # - 3 body orientation in euler angles + # - 2 linear velocity (only along the plane, we don't care about z velocity + + self.observation_space = spaces.Box(low=-1, high=1, shape=(12 + 3 + 2,), dtype=np.float32) + + # action = 12 joint angles in the same order as above (fr/fl/br/bl and each with hip/upper/lower) + self.action_space = spaces.Box(low=-1, high=1, shape=(12,), dtype=np.float32) + + # turning off start_standing because the that's done in self.reset() + kwargs = {} + # if relative_action: # if this is turned on, the robot is underpowered for the hardcoded gait + # kwargs = {"gain_pos": 1 / 16, "gain_vel": 1 / 8, "max_torque": 1 / 2} + + self.sim = PupperSim2(debug=debug, start_standing=False, **kwargs) + self.episode_steps = 0 + self.episode_steps_max = steps + self.control_freq = control_freq + self.dt = 1 / self.control_freq + self.incremental_angle = np.deg2rad(MAX_ANGLE_PER_SEC) / self.control_freq + self.sim_steps = int(round(FREQ_SIM / control_freq)) + self.relative_action = relative_action + self.incremental_action = incremental_action + self.action_scaling = action_scaling + self.action_smoothing = deque(maxlen=action_smoothing) + self.random_rot = random_rot + self.stop_on_flip = stop_on_flip + self.current_action = np.array([0] * 12) + + # new reward coefficients + self.rcoeff_ctrl, self.rcoeff_run, self.rcoeff_stable = reward_coefficients + + def reset(self): + self.episode_steps = 0 + + # both when the action formulation is incremental and when it's relative, we need to start standing + self.sim.reset(rest=True) # also stand up the robot + # for _ in range(10): + # self.sim.step() + + # this is used when self.incremental_action == True + self.current_action = self.sim.get_rest_pos() + + return self.get_obs() + + def seed(self, seed=None): + np.random.seed(seed) + return super().seed(seed) + + def close(self): + self.sim.p.disconnect() + super().close() + + def sanitize_actions(self, actions): + assert len(actions) == 12 + + if not self.incremental_action: + scaled = actions * np.pi * self.action_scaling # because 1/-1 corresponds to pi/-pi radians rotation + if self.relative_action: + scaled += self.sim.get_rest_pos() + # this enforces an action range of -1/1, except if it's relative action - then the action space is asymmetric + else: + scaled = actions * self.incremental_angle + self.current_action + + clipped = np.clip(scaled, -np.pi + 0.001, np.pi - 0.001) + self.current_action = np.copy(clipped) + return clipped + + def get_obs(self): + pos, orn, vel = self.sim.get_pos_orn_vel() + + joint_states = np.array(self.sim.get_joint_states()) / np.pi # to normalize to [-1,1] + obs = list(joint_states) + list(orn) + list(vel)[:2] + return obs + + def step(self, action): + action_clean = self.sanitize_actions(action) + + pos_before, _, _ = self.sim.get_pos_orn_vel() + + # The action command only sets the goals of the motors. It doesn't actually step the simulation forward in + # time. Instead of feeding the simulator the action directly, we take the mean of the last N actions, + # where N comes from the action_smoothing hyper-parameter + self.action_smoothing.append(action_clean) + self.sim.action(np.mean(self.action_smoothing, axis=0)) + + for _ in range(self.sim_steps): + self.sim.step() + + pos_after, orn_after, _ = self.sim.get_pos_orn_vel() + + obs = self.get_obs() + + # this reward calculation is taken verbatim from halfcheetah-v2, save + reward_ctrl = -np.square(action).sum() + reward_run = (pos_after[0] - pos_before[0]) / self.dt + # technically we should divide the next line by (3*pi^2) but that's really hard to reach + reward_stable = -np.square(orn_after).sum() / np.square(np.pi) + reward = self.rcoeff_ctrl * reward_ctrl + self.rcoeff_run * reward_run + self.rcoeff_stable * reward_stable + + done = False + self.episode_steps += 1 + if self.episode_steps == self.episode_steps_max: + done = True + + if self.stop_on_flip: + flipped = self.sim.check_collision() + if flipped: + done = True + reward -= 1000 + + return obs, reward, done, dict(reward_run=reward_run, reward_ctrl=reward_ctrl, reward_stable=reward_stable) + + def render(self, mode="human"): + # todo: if the mode=="human" then this should open and display a + # window a la "cv2.imshow('xxx',img), cv2.waitkey(10)" + + img, _ = self.sim.take_photo() + return img diff --git a/pupper/Config.py b/stanford_quad/pupper/Config.py similarity index 80% rename from pupper/Config.py rename to stanford_quad/pupper/Config.py index df1c98ca..33fbdc37 100644 --- a/pupper/Config.py +++ b/stanford_quad/pupper/Config.py @@ -1,8 +1,9 @@ import numpy as np -from pupper.HardwareConfig import MICROS_PER_RAD, NEUTRAL_ANGLE_DEGREES, PS4_COLOR, PS4_DEACTIVATED_COLOR -from enum import Enum # TODO: put these somewhere else +from stanford_quad.pupper.HardwareConfig import MICROS_PER_RAD, NEUTRAL_ANGLE_DEGREES, PS4_COLOR, PS4_DEACTIVATED_COLOR + + class PWMParams: def __init__(self): self.pins = np.array([[2, 14, 18, 23], [3, 15, 27, 24], [4, 17, 22, 25]]) @@ -18,9 +19,7 @@ def __init__(self): # The neutral angle of the joint relative to the modeled zero-angle in degrees, for each joint self.neutral_angle_degrees = NEUTRAL_ANGLE_DEGREES - self.servo_multipliers = np.array( - [[1, 1, 1, 1], [-1, 1, -1, 1], [1, -1, 1, -1]] - ) + self.servo_multipliers = np.array([[1, 1, 1, 1], [-1, 1, -1, 1], [1, -1, 1, -1]]) @property def neutral_angles(self): @@ -30,15 +29,15 @@ def neutral_angles(self): class Configuration: def __init__(self): ################# CONTROLLER BASE COLOR ############## - self.ps4_color = PS4_COLOR - self.ps4_deactivated_color = PS4_DEACTIVATED_COLOR + self.ps4_color = PS4_COLOR + self.ps4_deactivated_color = PS4_DEACTIVATED_COLOR #################### COMMANDS #################### self.max_x_velocity = 0.4 self.max_y_velocity = 0.3 self.max_yaw_rate = 2.0 self.max_pitch = 30.0 * np.pi / 180.0 - + #################### MOVEMENT PARAMS #################### self.z_time_constant = 0.02 self.z_speed = 0.03 # maximum speed [m/s] @@ -59,25 +58,15 @@ def __init__(self): #################### SWING ###################### self.z_coeffs = None self.z_clearance = 0.07 - self.alpha = ( - 0.5 # Ratio between touchdown distance and total horizontal stance movement - ) - self.beta = ( - 0.5 # Ratio between touchdown distance and total horizontal stance movement - ) + self.alpha = 0.5 # Ratio between touchdown distance and total horizontal stance movement + self.beta = 0.5 # Ratio between touchdown distance and total horizontal stance movement #################### GAIT ####################### self.dt = 0.01 self.num_phases = 4 - self.contact_phases = np.array( - [[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]] - ) - self.overlap_time = ( - 0.10 # duration of the phase where all four feet are on the ground - ) - self.swing_time = ( - 0.15 # duration of the phase when only two feet are on the ground - ) + self.contact_phases = np.array([[1, 1, 1, 0], [1, 0, 1, 1], [1, 0, 1, 1], [1, 1, 1, 0]]) + self.overlap_time = 0.10 # duration of the phase where all four feet are on the ground + self.swing_time = 0.15 # duration of the phase when only two feet are on the ground ######################## GEOMETRY ###################### self.LEG_FB = 0.10 # front-back distance from center line to leg axis @@ -105,12 +94,7 @@ def __init__(self): ) self.ABDUCTION_OFFSETS = np.array( - [ - -self.ABDUCTION_OFFSET, - self.ABDUCTION_OFFSET, - -self.ABDUCTION_OFFSET, - self.ABDUCTION_OFFSET, - ] + [-self.ABDUCTION_OFFSET, self.ABDUCTION_OFFSET, -self.ABDUCTION_OFFSET, self.ABDUCTION_OFFSET,] ) ################### INERTIAL #################### @@ -122,9 +106,7 @@ def __init__(self): # Compensation factor of 3 because the inertia measurement was just # of the carbon fiber and plastic parts of the frame and did not # include the hip servos and electronics - self.FRAME_INERTIA = tuple( - map(lambda x: 3.0 * x, (1.844e-4, 1.254e-3, 1.337e-3)) - ) + self.FRAME_INERTIA = tuple(map(lambda x: 3.0 * x, (1.844e-4, 1.254e-3, 1.337e-3))) self.MODULE_INERTIA = (3.698e-5, 7.127e-6, 4.075e-5) leg_z = 1e-6 @@ -183,15 +165,13 @@ def stance_ticks(self): @property def phase_ticks(self): - return np.array( - [self.overlap_ticks, self.swing_ticks, self.overlap_ticks, self.swing_ticks] - ) + return np.array([self.overlap_ticks, self.swing_ticks, self.overlap_ticks, self.swing_ticks]) @property def phase_length(self): return 2 * self.overlap_ticks + 2 * self.swing_ticks - + class SimulationConfig: def __init__(self): self.XML_IN = "pupper.xml" @@ -204,7 +184,7 @@ def __init__(self): self.JOINT_SOLIMP = "0.9 0.95 0.001" # joint constraint parameters self.GEOM_SOLREF = "0.01 1" # time constant and damping ratio for geom contacts self.GEOM_SOLIMP = "0.9 0.95 0.001" # geometry contact parameters - + # Joint params G = 220 # Servo gear ratio m_rotor = 0.016 # Servo rotor mass @@ -215,9 +195,7 @@ def __init__(self): NATURAL_DAMPING = 1.0 # Damping resulting from friction ELECTRICAL_DAMPING = 0.049 # Damping resulting from back-EMF - self.REV_DAMPING = ( - NATURAL_DAMPING + ELECTRICAL_DAMPING - ) # Damping torque on the revolute joints + self.REV_DAMPING = NATURAL_DAMPING + ELECTRICAL_DAMPING # Damping torque on the revolute joints # Servo params self.SERVO_REV_KP = 300 # Position gain [Nm/rad] diff --git a/stanford_quad/pupper/HardwareConfig.py b/stanford_quad/pupper/HardwareConfig.py new file mode 100644 index 00000000..3ac1094c --- /dev/null +++ b/stanford_quad/pupper/HardwareConfig.py @@ -0,0 +1,11 @@ +""" +Per-robot configuration file that is particular to each individual robot, not just the type of robot. +""" +import numpy as np + + +MICROS_PER_RAD = 11.333 * 180.0 / np.pi # Must be calibrated +NEUTRAL_ANGLE_DEGREES = np.array([[-1.0, -4.0, -14.0, -10.0], [45.0, 47.0, 43.0, 40.0], [-41.0, -43.0, -28.0, -22.0]]) + +PS4_COLOR = {"red": 0, "blue": 255, "green": 0} +PS4_DEACTIVATED_COLOR = {"red": 0, "blue": 50, "green": 0} diff --git a/pupper/HardwareInterface.py b/stanford_quad/pupper/HardwareInterface.py similarity index 98% rename from pupper/HardwareInterface.py rename to stanford_quad/pupper/HardwareInterface.py index 0c2bbfc7..0a3a4978 100644 --- a/pupper/HardwareInterface.py +++ b/stanford_quad/pupper/HardwareInterface.py @@ -1,5 +1,5 @@ import pigpio -from pupper.Config import ServoParams, PWMParams +from stanford_quad.pupper.Config import ServoParams, PWMParams class HardwareInterface: diff --git a/pupper/Kinematics.py b/stanford_quad/pupper/Kinematics.py similarity index 100% rename from pupper/Kinematics.py rename to stanford_quad/pupper/Kinematics.py diff --git a/src/__init__.py b/stanford_quad/pupper/__init__.py similarity index 100% rename from src/__init__.py rename to stanford_quad/pupper/__init__.py diff --git a/stanford_quad/pupper/policy.py b/stanford_quad/pupper/policy.py new file mode 100644 index 00000000..a8644bad --- /dev/null +++ b/stanford_quad/pupper/policy.py @@ -0,0 +1,56 @@ +import numpy as np + +from stanford_quad.common.Command import Command +from stanford_quad.common.Controller import Controller +from stanford_quad.common.State import State +from stanford_quad.pupper.Config import Configuration +from stanford_quad.pupper.Kinematics import four_legs_inverse_kinematics + + +class HardPolicy: + def __init__(self, fps=60, warmup=0) -> None: + super().__init__() + + self.fps = fps + self.warmup = warmup + + self.config = Configuration() + self.config.dt = 1 / fps + # config.z_clearance = 0.02 + self.controller = Controller(self.config, four_legs_inverse_kinematics) + self.state = State() + command = Command() + + # initializing the controller + command.activate_event = 1 + self.controller.run(self.state, command) + command.activate_event = 0 + command.trot_event = 1 + self.controller.run(self.state, command) + self.command = Command() # zero it out + + print("Summary of gait parameters:") + print("overlap time: ", self.config.overlap_time) + print("swing time: ", self.config.swing_time) + print("z clearance: ", self.config.z_clearance) + print("x shift: ", self.config.x_shift) + self.step = 0 + + def act(self, velocity_horizontal=(0, 0), yaw_rate=0, normalized=False): + if self.step > self.warmup: + self.command.horizontal_velocity = np.array(velocity_horizontal) + else: + self.command.horizontal_velocity = np.array([0, 0]) + + self.command.yaw_rate = yaw_rate + self.state.quat_orientation = np.array([1, 0, 0, 0]) + + # Step the controller forward by dt + self.controller.run(self.state, self.command) + + if normalized: + self.state.joint_angles /= np.pi + + self.step += 1 + + return self.state.joint_angles diff --git a/stanford_quad/sim/HardwareInterface.py b/stanford_quad/sim/HardwareInterface.py new file mode 100644 index 00000000..e8cc4220 --- /dev/null +++ b/stanford_quad/sim/HardwareInterface.py @@ -0,0 +1,42 @@ +import pybullet +import pybullet_data + + +class HardwareInterface: + def __init__(self, model, joint_indices, kp=0.25, kv=0.5, max_torque=10): + self.model = model + self.joint_indices = joint_indices + self.kp = kp + self.kv = kv + self.max_torque = max_torque + + def set_actuator_postions(self, joint_angles): + serial_joint_angles = self.parallel_to_serial_joint_angles(joint_angles) + pybullet.setJointMotorControlArray( + bodyUniqueId=self.model[1], + jointIndices=self.joint_indices, + controlMode=pybullet.POSITION_CONTROL, + targetPositions=list(serial_joint_angles.T.reshape(12)), + positionGains=[self.kp] * 12, + velocityGains=[self.kv] * 12, + forces=[self.max_torque] * 12, + ) + + @staticmethod + def parallel_to_serial_joint_angles(joint_matrix): + """Convert from joint angles meant for the parallel linkage in + Pupper to the joint angles in the serial linkage approximation implemented in the simulation + + Parameters + ---------- + joint_matrix : Numpy array (3, 4) + Joint angles for parallel linkage + + Returns + ------- + Numpy array (3, 4) + Joint angles for equivalent serial linkage + """ + temp = joint_matrix + temp[2, :] -= joint_matrix[1, :] + return temp diff --git a/stanford_quad/sim/IMU.py b/stanford_quad/sim/IMU.py new file mode 100644 index 00000000..397179ee --- /dev/null +++ b/stanford_quad/sim/IMU.py @@ -0,0 +1,13 @@ +import numpy as np +import pybullet + +class IMU: + def __init__(self): + pass + + def read_orientation(self): + (_, q_scalar_last) = pybullet.getBasePositionAndOrientation(1) + return np.array( + [q_scalar_last[3], q_scalar_last[0], q_scalar_last[1], q_scalar_last[2]] + ) + diff --git a/woofer/__init__.py b/stanford_quad/sim/__init__.py similarity index 100% rename from woofer/__init__.py rename to stanford_quad/sim/__init__.py diff --git a/stanford_quad/sim/simulator.py b/stanford_quad/sim/simulator.py new file mode 100644 index 00000000..b31bcfd5 --- /dev/null +++ b/stanford_quad/sim/simulator.py @@ -0,0 +1,29 @@ +import pybullet +import pybullet_data + + +class PySim: + def __init__(self, xml_path, freq=240, headless=False, kp=0.25, kv=0.5, max_torque=10, g=-9.81): + # Set up PyBullet Simulator + if not headless: + pybullet.connect(pybullet.GUI) # or p.DIRECT for non-graphical version + pybullet.resetDebugVisualizerCamera( + cameraDistance=1, cameraYaw=45, cameraPitch=-45, cameraTargetPosition=[0, 0.0, 0] + ) + else: + pybullet.connect(pybullet.DIRECT) # or p.DIRECT for non-graphical version + pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally + pybullet.setGravity(0, 0, g) + pybullet.setTimeStep(1 / freq) + self.model = pybullet.loadMJCF(xml_path) + print("") + print("Pupper body IDs:", self.model) + numjoints = pybullet.getNumJoints(self.model[1]) + print("Number of joints in converted MJCF: ", numjoints) + print("Joint Info: ") + for i in range(numjoints): + print(pybullet.getJointInfo(self.model[1], i)) + self.joint_indices = list(range(0, 24, 2)) + + def step(self): + pybullet.stepSimulation() diff --git a/stanford_quad/sim/simulator2.py b/stanford_quad/sim/simulator2.py new file mode 100644 index 00000000..1cfb2882 --- /dev/null +++ b/stanford_quad/sim/simulator2.py @@ -0,0 +1,373 @@ +import os +import time + +import pybullet +import pybullet_data +import pybullet_utils.bullet_client as bc +import numpy as np + +from stanford_quad.assets import ASSET_DIR +from stanford_quad.sim.HardwareInterface import HardwareInterface +from stanford_quad.sim.utils import random_bright_color, pybulletimage2numpy, pybulletsegmap2numpy + +FREQ_SIM = 240 + +REST_POS = HardwareInterface.parallel_to_serial_joint_angles( + np.array( + [ + [-0.12295051, 0.12295051, -0.12295051, 0.12295051], + [0.77062617, 0.77062617, 0.77062617, 0.77062617], + [-0.845151, -0.845151, -0.845151, -0.845151], + ] + ) +) + + +class PupperSim2: + def __init__( + self, + xml_path=None, + debug=False, + gain_pos=0.125, + gain_vel=0.25, + max_torque=1, + g=-9.81, + start_standing=True, + img_size=(84, 84), + enable_new_floor=False, + frequency=FREQ_SIM, + ): + """ + + :param str xml_path: Path to Mujoco robot XML definition file + :param bool debug: Set to True to enable visual inspection + :param float gain_pos: Gain position value for low-level controller + :param float gain_vel: Gain velocity value for low-level controller + :param float max_torque: Max torque for PID controller + :param float g: Gravity value + """ + if xml_path is None: + xml_path = os.path.join(ASSET_DIR, "pupper_pybullet_out.xml") + + self.gain_pos = gain_pos + self.gain_vel = gain_vel + self.max_torque = max_torque + self.img_size = img_size + + if debug: + startup_flag = pybullet.GUI + else: + startup_flag = pybullet.DIRECT + self.p = bc.BulletClient(connection_mode=startup_flag) + self.p.setTimeStep(1 / frequency) + self.p.setAdditionalSearchPath(pybullet_data.getDataPath()) + self.p.setGravity(0, 0, g) + + if debug: + self.p.resetDebugVisualizerCamera( + cameraDistance=1.5, cameraYaw=180, cameraPitch=-45, cameraTargetPosition=[0, 0.0, 0] + ) + + # self.p.loadURDF("plane.urdf") + + self.floor, self.model = self.p.loadMJCF(xml_path) + + if debug: + numjoints = self.p.getNumJoints(self.model) + for i in range(numjoints): + print(self.p.getJointInfo(self.model, i)) + self.joint_indices = list(range(0, 24, 2)) + + self.cam_proj = self.p.computeProjectionMatrixFOV( + fov=90, aspect=self.img_size[0] / self.img_size[1], nearVal=0.001, farVal=10 + ) + + self.collision_floor = None + if enable_new_floor: + self.collision_floor = self.create_box((0, 0, 0.001), (0, 0, 0), (1, 1, 0.0005), visible=False) + self.p.createConstraint( + parentBodyUniqueId=self.collision_floor, + parentLinkIndex=-1, + childBodyUniqueId=-1, + childLinkIndex=-1, + jointType=self.p.JOINT_FIXED, + jointAxis=(1, 1, 1), + parentFramePosition=(0, 0, 0.0), + childFramePosition=(0, 0, 0.0005), + ) + + if start_standing: + self.reset(True) + for _ in range(10): + self.step() + + # time.sleep(1) + + def step(self): + self.p.stepSimulation() + + def reset(self, rest=True, random_rot=(0, 0, 0), random_pos=(0, 0, 0)): + height = 0.301 + if rest: + height = 0.183 + # self.p.resetBasePositionAndOrientation(self.model, [0, 0, height], self.p.getQuaternionFromEuler([0, 0, 0])) + rand_pos = ( + np.random.uniform(-random_pos[0], random_pos[0], 1)[0], + np.random.uniform(-random_pos[1], random_pos[1], 1)[0], + np.random.uniform(-random_pos[2], random_pos[2], 1)[0] + height, + ) + rand_rot = ( + np.random.normal(0, random_rot[0], 1)[0], + np.random.normal(0, random_rot[1], 1)[0], + np.random.normal(0, random_rot[2], 1)[0], + ) + rand_rot = [np.deg2rad(x) for x in rand_rot] + + self.p.resetBasePositionAndOrientation( + self.model, rand_pos, self.p.getQuaternionFromEuler(rand_rot), + ) + if rest: + action = self.get_rest_pos() + else: + action = [0] * 12 + self.action(action) + self.set(action) + # self.step() # one step to move joints into place + + for _ in range(10): + self.step() + + def make_kinematic(self, color=(1, 0, 1, 1)): + sleepy_state = ( + self.p.ACTIVATION_STATE_SLEEP + + self.p.ACTIVATION_STATE_ENABLE_SLEEPING + + self.p.ACTIVATION_STATE_DISABLE_WAKEUP + ) + + self.p.changeDynamics(self.model, -1, linearDamping=0, angularDamping=0) + self.p.setCollisionFilterGroupMask(self.model, -1, collisionFilterGroup=0, collisionFilterMask=0) + + self.p.changeDynamics( + self.model, -1, activationState=sleepy_state, + ) + for j in range(24): # not 12 because there's some fixed joints in there + self.p.setCollisionFilterGroupMask(self.model, j, collisionFilterGroup=0, collisionFilterMask=0) + + self.p.changeDynamics( + self.model, j, activationState=sleepy_state, + ) + + self.change_color(color) + + def change_color(self, color=(1, 0, 1, 1)): + self.p.changeVisualShape(self.model, -1, rgbaColor=color) + for j in range(24): # not 12 because there's some fixed joints in there + self.p.changeVisualShape(self.model, j, rgbaColor=color) + + def move_kinectic_body(self, pos, rot): + self.p.resetBasePositionAndOrientation(self.model, pos, rot) + + @staticmethod + def get_rest_pos(): + return REST_POS.T.flatten() + + def get_pos_orn_vel(self): + posorn = self.p.getBasePositionAndOrientation(self.model) + pos = np.array(posorn[0]) + orn = self.p.getEulerFromQuaternion(posorn[1]) + vel = self.p.getBaseVelocity(self.model)[0] + + return pos, orn, vel + + def get_joint_states(self): + joint_states = self.p.getJointStates(self.model, self.joint_indices) + joint_states = [joint[0] for joint in joint_states] + return joint_states + + @staticmethod + def joint_sanity_check(joint_angles): + assert len(joint_angles) == 12 + assert np.min(np.rad2deg(joint_angles)) >= -180 + assert np.max(np.rad2deg(joint_angles)) <= 180 + + def action(self, joint_angles): + self.joint_sanity_check(joint_angles) + + self.p.setJointMotorControlArray( + bodyUniqueId=self.model, + jointIndices=self.joint_indices, + controlMode=self.p.POSITION_CONTROL, + targetPositions=list(joint_angles), + positionGains=[self.gain_pos] * 12, + velocityGains=[self.gain_vel] * 12, + forces=[self.max_torque] * 12, + ) + + def set(self, joint_angles): + self.joint_sanity_check(joint_angles) + + for joint_idx in range(12): + self.p.resetJointState( + bodyUniqueId=self.model, + jointIndex=self.joint_indices[joint_idx], + targetValue=joint_angles[joint_idx], + targetVelocity=0, + ) + + def create_box(self, pos, orn, size, color=(1, 0, 0), random_color=False, visible=True): + # we need to round or small float errors will explode the simulation + pos = np.around(pos, 4) + size = np.around(size, 4) + orn = np.around(self.p.getQuaternionFromEuler(orn), 4) + + if random_color: + color = random_bright_color(uint=False) + + obj_visual = -1 + if visible: + obj_visual = self.p.createVisualShape( + shapeType=self.p.GEOM_BOX, rgbaColor=list(color) + [1], halfExtents=size + ) + + obj_collision = self.p.createCollisionShape(shapeType=self.p.GEOM_BOX, halfExtents=size) + + obj = self.p.createMultiBody( + baseMass=0.1, # doesn't matter + baseCollisionShapeIndex=obj_collision, + baseVisualShapeIndex=obj_visual, + basePosition=pos, + baseOrientation=orn, + useMaximalCoordinates=False, + ) + return obj + + def add_stairs( + self, + no_steps=8, + step_width=1, + step_height=0.1, + step_depth=0.1, + offset=None, + color=(0.5, 0, 0.5), + random_color=False, + ) -> list: + pos_x = 0 + pos_y = 0 + pos_z = 0 + if offset is not None: + assert len(offset) == 2 or len(offset) == 3 + pos_x += offset[0] + pos_y += offset[1] + if len(offset) == 3: + pos_z += offset[2] + + steps = [] + positions = [] + + orientation = [0, 0, 0] + size = (step_depth / 2, step_width / 2, step_height / 2) + + # create steps boxes + for step_idx in range(no_steps): + pos = (pos_x + step_depth / 2, pos_y, pos_z + size[2]) + + positions.append(np.array(pos)) + + step = self.create_box(pos, orientation, size, color, random_color) + steps.append(step) + + pos_x += step_depth + size = (size[0], size[1], size[2] + step_height / 2) + + # connect the steps to each other + for step_idx in range(no_steps - 1): + step_1 = steps[step_idx] + step_2 = steps[step_idx + 1] + + pos_1 = positions[step_idx] + pos_2 = positions[step_idx + 1] + + # in order to create a fixing constraint between parent and child step, + # you need to find a point where they touch. + center = (pos_1 + pos_2) / 2 + parent_frame = center - pos_1 + child_frame = center - pos_2 + + self.p.createConstraint( + parentBodyUniqueId=step_1, + parentLinkIndex=-1, + childBodyUniqueId=step_2, + childLinkIndex=-1, + jointType=self.p.JOINT_FIXED, + jointAxis=(1, 1, 1), + parentFramePosition=parent_frame, + childFramePosition=child_frame, + ) + + # and fix the first step to the ground/world + self.p.createConstraint( + parentBodyUniqueId=steps[0], + parentLinkIndex=-1, + childBodyUniqueId=-1, + childLinkIndex=-1, + jointType=self.p.JOINT_FIXED, + jointAxis=(1, 1, 1), + parentFramePosition=-positions[0], + childFramePosition=(0, 0, 0), + ) + + return steps + + def take_photo(self, camera_offset=(0, -0.3, 0.3), lookat_offset=(0, 0, 0), with_segmap=False): + pos, _, _ = self.get_pos_orn_vel() + cam_pos = pos + camera_offset + cam_lookat = pos + lookat_offset + cam_view = self.p.computeViewMatrix( + cameraEyePosition=cam_pos, cameraTargetPosition=cam_lookat, cameraUpVector=[0, 0, 1] + ) + flags = 0 + if not with_segmap: + flags |= pybullet.ER_NO_SEGMENTATION_MASK + img = self.p.getCameraImage( + self.img_size[0], + self.img_size[1], + cam_view, + self.cam_proj, + renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, + flags=flags + # flags=pybullet.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX + # lightDirection=[-.5, -1, .5], lightDistance=1, + # renderer=self.p0.ER_TINY_RENDERER + ) + output_img = pybulletimage2numpy(img) + + if with_segmap: + output_segmap = pybulletsegmap2numpy(img) + return output_img, output_segmap + else: + return output_img, None + + def check_collision(self): + if self.collision_floor is None: + raise Exception( + "You need to call the PupperSim2 class with the parameter enable_new_floor=True for floor collisions to work." + ) + + contacts = self.p.getClosestPoints( + bodyA=self.model, bodyB=self.collision_floor, distance=0.1, linkIndexA=-1, linkIndexB=-1 + ) + # if len(contacts) > 0: + # contacts = contacts[0] + # self.p.removeAllUserDebugItems() + # self.p.addUserDebugLine(lineFromXYZ=contacts[5], lineToXYZ=contacts[6], lineColorRGB=(1, 0, 1), lineWidth=2) + # self.p.addUserDebugText(text=str(contacts[8]), textPosition=(0, 0, 0), textColorRGB=(1, 1, 0), textSize=5) + if len(contacts) > 0 and contacts[0][8] < 0.02: + return True + + return False + + +# depth = 0.2, height = 0.2 +# pos 0.1, 0.1, size 0.1, 0.1, total height 0.2 +# pos 0.2, 0.2, size 0.1, 0.2, total height 0.4 +# diff --git a/stanford_quad/sim/utils.py b/stanford_quad/sim/utils.py new file mode 100644 index 00000000..56df3706 --- /dev/null +++ b/stanford_quad/sim/utils.py @@ -0,0 +1,47 @@ +import colorsys + +import numpy as np + + +def random_bright_color(uint=False): + base = np.random.rand(3) + h, s, l = base[0], 0.5 + base[1] / 2.0, 0.4 + base[2] / 5.0 + r, g, b = [int(256 * i) for i in colorsys.hls_to_rgb(h, l, s)] + + if uint: + return r, g, b + else: + return [x / 255 for x in (r, g, b)] + + +def pybulletimage2numpy(img): + width, height = img[0:2] + rgb = img[2] + rgb = np.reshape(rgb, (height, width, 4)) + rgb = rgb[:, :, :3] + return rgb + + +def pybulletsegmap2numpy(img): + width, height = img[0:2] + segmap = img[4] + segmap = np.reshape(segmap, (height, width)) + segmap = segmap[:, :] + return segmap + + +def segmap2color(segmap): + output = np.zeros((segmap.shape[0], segmap.shape[1], 3), np.uint8) + + for inst in range(segmap.max() + 1): + output[segmap == inst, :] = np.random.uniform(200, 255, 3) + + return output + + +def segmap2color_fixed(segmap): + output = np.zeros((segmap.shape[0], segmap.shape[1], 3), np.uint8) + output[segmap == 0, :] = (255, 0, 0) + output[segmap == 1, :] = (0, 255, 0) + + return output diff --git a/woofer/Config.py b/woofer/Config.py deleted file mode 100644 index 11225c5f..00000000 --- a/woofer/Config.py +++ /dev/null @@ -1,240 +0,0 @@ -import numpy as np -from scipy.linalg import solve - - -class BehaviorState(Enum): - REST = 0 - TROT = 1 - HOP = 2 - FINISHHOP = 3 - - -class UserInputParams: - def __init__(self): - self.max_x_velocity = 0.5 - self.max_y_velocity = 0.24 - self.max_yaw_rate = 0.2 - self.max_pitch = 30.0 * np.pi / 180.0 - - -class MovementReference: - """Stores movement reference - """ - - def __init__(self): - self.v_xy_ref = np.array([0, 0]) - self.wz_ref = 0.0 - self.z_ref = -0.265 - self.pitch = 0.0 - self.roll = 0.0 - - -class SwingParams: - """Swing Parameters - """ - - def __init__(self): - self.z_coeffs = None - self.z_clearance = 0.05 - self.alpha = ( - 0.5 - ) # Ratio between touchdown distance and total horizontal stance movement - self.beta = ( - 0.5 - ) # Ratio between touchdown distance and total horizontal stance movement - - @property - def z_clearance(self): - return self.__z_clearance - - @z_clearance.setter - def z_clearance(self, z): - self.__z_clearance = z - b_z = np.array([0, 0, 0, 0, self.__z_clearance]) - A_z = np.array( - [ - [0, 0, 0, 0, 1], - [1, 1, 1, 1, 1], - [0, 0, 0, 1, 0], - [4, 3, 2, 1, 0], - [0.5 ** 4, 0.5 ** 3, 0.5 ** 2, 0.5 ** 1, 0.5 ** 0], - ] - ) - self.z_coeffs = solve(A_z, b_z) - - -class StanceParams: - """Stance parameters - """ - - def __init__(self): - self.z_time_constant = 0.02 - self.z_speed = 0.03 # maximum speed [m/s] - self.pitch_deadband = 0.02 - self.pitch_time_constant = 0.25 - self.max_pitch_rate = 0.15 - self.roll_speed = 0.16 # maximum roll rate [rad/s] - self.delta_x = 0.23 - self.delta_y = 0.173 - self.x_shift = -0.01 - - @property - def default_stance(self): - return np.array( - [ - [ - self.delta_x + self.x_shift, - self.delta_x + self.x_shift, - -self.delta_x + self.x_shift, - -self.delta_x + self.x_shift, - ], - [-self.delta_y, self.delta_y, -self.delta_y, self.delta_y], - [0, 0, 0, 0], - ] - ) - - -class GaitParams: - """Gait Parameters - """ - - def __init__(self): - self.dt = 0.01 - self.num_phases = 4 - self.contact_phases = np.array( - [[1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1], [1, 1, 1, 1]] - ) - self.overlap_time = ( - 0.5 # duration of the phase where all four feet are on the ground - ) - self.swing_time = ( - 0.5 # duration of the phase when only two feet are on the ground - ) - - @property - def overlap_ticks(self): - return int(self.overlap_time / self.dt) - - @property - def swing_ticks(self): - return int(self.swing_time / self.dt) - - @property - def stance_ticks(self): - return 2 * self.overlap_ticks + self.swing_ticks - - @property - def phase_times(self): - return np.array( - [self.overlap_ticks, self.swing_ticks, self.overlap_ticks, self.swing_ticks] - ) - - @property - def phase_length(self): - return 2 * self.overlap_ticks + 2 * self.swing_ticks - - -class RobotConfig: - """Woofer hardware parameters - """ - - def __init__(self): - - # Robot geometry - self.LEG_FB = 0.23 # front-back distance from center line to leg axis - self.LEG_LR = 0.109 # left-right distance from center line to leg plane - self.ABDUCTION_OFFSET = 0.064 # distance from abduction axis to leg - self.FOOT_RADIUS = 0.02 - - self.UPPER_LEG = 0.18 - self.LOWER_LEG = 0.32 - - # Update hip geometry - self.HIP_L = 0.0394 - self.HIP_W = 0.0744 - self.HIP_T = 0.0214 - self.HIP_OFFSET = 0.0132 - - self.L = 0.66 - self.W = 0.176 - self.T = 0.092 - - self.LEG_ORIGINS = np.array( - [ - [self.LEG_FB, self.LEG_FB, -self.LEG_FB, -self.LEG_FB], - [-self.LEG_LR, self.LEG_LR, -self.LEG_LR, self.LEG_LR], - [0, 0, 0, 0], - ] - ) - - self.ABDUCTION_OFFSETS = np.array( - [ - -self.ABDUCTION_OFFSET, - self.ABDUCTION_OFFSET, - -self.ABDUCTION_OFFSET, - self.ABDUCTION_OFFSET, - ] - ) - - self.START_HEIGHT = 0.3 - - # Robot inertia params - self.FRAME_MASS = 3.0 # kg - self.MODULE_MASS = 1.033 # kg - self.LEG_MASS = 0.15 # kg - self.MASS = self.FRAME_MASS + (self.MODULE_MASS + self.LEG_MASS) * 4 - - # Compensation factor of 3 because the inertia measurement was just - # of the carbon fiber and plastic parts of the frame and did not - # include the hip servos and electronics - self.FRAME_INERTIA = tuple( - map(lambda x: 3.0 * x, (1.844e-4, 1.254e-3, 1.337e-3)) - ) - self.MODULE_INERTIA = (3.698e-5, 7.127e-6, 4.075e-5) - - leg_z = 1e-6 - leg_mass = 0.010 - leg_x = 1 / 12 * self.LOWER_LEG ** 2 * leg_mass - leg_y = leg_x - self.LEG_INERTIA = (leg_x, leg_y, leg_z) - - # Joint params - G = 220 # Servo gear ratio - m_rotor = 0.016 # Servo rotor mass - r_rotor = 0.005 # Rotor radius - self.ARMATURE = G ** 2 * m_rotor * r_rotor ** 2 # Inertia of rotational joints - # print("Servo armature", self.ARMATURE) - - NATURAL_DAMPING = 1.0 # Damping resulting from friction - ELECTRICAL_DAMPING = 0.049 # Damping resulting from back-EMF - - self.REV_DAMPING = ( - NATURAL_DAMPING + ELECTRICAL_DAMPING - ) # Damping torque on the revolute joints - - # Force limits - self.MAX_JOINT_TORQUE = 12.0 - self.REVOLUTE_RANGE = 3 - self.NUM_ODRIVES = 6 - self.ENCODER_CPR = 2000 - self.MOTOR_REDUCTION = 4 - - -class EnvironmentConfig: - """Environmental parameters - """ - - def __init__(self): - self.MU = 1.5 # coeff friction - self.DT = 0.001 # seconds between simulation steps - - -class SolverConfig: - """MuJoCo solver parameters - """ - - def __init__(self): - self.JOINT_SOLREF = "0.001 1" # time constant and damping ratio for joints - self.JOINT_SOLIMP = "0.9 0.95 0.001" # joint constraint parameters - self.GEOM_SOLREF = "0.01 1" # time constant and damping ratio for geom contacts - self.GEOM_SOLIMP = "0.9 0.95 0.001" # geometry contact parameters diff --git a/woofer/HardwareConfig.py b/woofer/HardwareConfig.py deleted file mode 100644 index 02f4c8e8..00000000 --- a/woofer/HardwareConfig.py +++ /dev/null @@ -1,44 +0,0 @@ -""" -Per-robot configuration file that is particular to each individual robot, not just the type of robot. -""" - -import numpy as np - -ODRIVE_SERIAL_NUMBERS = [ - "2065339F304B", - "208F3384304B", - "365833753037", - "207E35753748", - "208F3385304B", - "208E3387304B", -] - -ACTUATOR_DIRECTIONS = np.array([[1, 1, -1, -1], [-1, -1, -1, -1], [1, 1, 1, 1]]) - -ANGLE_OFFSETS = np.array( - [ - [0, 0, 0, 0], - [np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2], - [-np.pi / 2, -np.pi / 2, -np.pi / 2, -np.pi / 2], - ] -) - - -def map_actuators_to_axes(odrives): - axes = [[None for _ in range(4)] for _ in range(3)] - axes[0][0] = odrives[1].axis1 - axes[1][0] = odrives[0].axis0 - axes[2][0] = odrives[0].axis1 - - axes[0][1] = odrives[1].axis0 - axes[1][1] = odrives[2].axis1 - axes[2][1] = odrives[2].axis0 - - axes[0][2] = odrives[4].axis1 - axes[1][2] = odrives[5].axis0 - axes[2][2] = odrives[5].axis1 - - axes[0][3] = odrives[4].axis0 - axes[1][3] = odrives[3].axis1 - axes[2][3] = odrives[3].axis0 - return axes diff --git a/woofer/HardwareInterface.py b/woofer/HardwareInterface.py deleted file mode 100644 index 3f4be8be..00000000 --- a/woofer/HardwareInterface.py +++ /dev/null @@ -1,97 +0,0 @@ -import odrive -from odrive.enums import * -from woofer.Config import RobotConfig -from woofer.HardwareConfig import ( - ODRIVE_SERIAL_NUMBERS, - ACTUATOR_DIRECTIONS, - ANGLE_OFFSETS, - map_actuators_to_axes, -) -import time -import threading -import numpy as np - - -class HardwareInterface: - def __init__(self): - self.config = RobotConfig() - assert len(ODRIVE_SERIAL_NUMBERS) == self.config.NUM_ODRIVES - self.odrives = [None for _ in range(self.config.NUM_ODRIVES)] - threads = [] - for i in range(self.config.NUM_ODRIVES): - t = threading.Thread(target=find_odrive, args=(i, self.odrives)) - threads.append(t) - t.start() - for t in threads: - t.join() - input("Press enter to calibrate odrives...") - calibrate_odrives(self.odrives) - set_position_control(self.odrives) - - self.axes = assign_axes(self.odrives) - - def set_actuator_postions(self, joint_angles): - set_all_odrive_positions(self.axes, joint_angles, self.config) - - def deactivate_actuators(self): - set_odrives_idle(self.odrives) - - -def find_odrive(i, odrives): - o = odrive.find_any(serial_number=ODRIVE_SERIAL_NUMBERS[i]) - print("Found odrive: ", i) - odrives[i] = o - - -def calibrate_odrives(odrives): - for odrv in odrives: - odrv.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE - odrv.axis1.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE - for odrv in odrives: - while ( - odrv.axis0.current_state != AXIS_STATE_IDLE - or odrv.axis1.current_state != AXIS_STATE_IDLE - ): - time.sleep(0.1) # busy waiting - not ideal - - -def set_position_control(odrives): - for odrv in odrives: - for axis in [odrv.axis0, odrv.axis1]: - axis.controller.config.pos_gain = 60 - axis.controller.config.vel_gain = 0.002 - axis.controller.config.vel_limit_tolerance = 0 - axis.controller.config.vel_integrator_gain = 0 - axis.motor.config.current_lim = 15 - print("Updated gains") - - odrv.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL - odrv.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL - - -def set_odrives_idle(odrives): - for odrv in odrives: - odrv.axis0.requested_state = AXIS_STATE_IDLE - odrv.axis1.requested_state = AXIS_STATE_IDLE - - -def assign_axes(odrives): - return map_actuators_to_axes(odrives) - - -def set_all_odrive_positions(axes, joint_angles, config): - for i in range(joint_angles.shape[0]): - for j in range(joint_angles.shape[1]): - axes[i][j].controller.pos_setpoint = actuator_angle_to_odrive( - joint_angles, i, j, config - ) - - -def radians_to_encoder_count(angle, config): - return (angle / (2 * np.pi)) * config.ENCODER_CPR * config.MOTOR_REDUCTION - - -def actuator_angle_to_odrive(joint_angles, i, j, config): - offset_angle = joint_angles[i][j] + ANGLE_OFFSETS[i][j] - odrive_radians = offset_angle * ACTUATOR_DIRECTIONS[i][j] - return radians_to_encoder_count(odrive_radians, config) diff --git a/woofer/Kinematics.py b/woofer/Kinematics.py deleted file mode 100644 index 57e176b6..00000000 --- a/woofer/Kinematics.py +++ /dev/null @@ -1,98 +0,0 @@ -import numpy as np - - -def leg_forward_kinematics(alpha, leg_index, config): - """Find the body-centric coordinates of a given foot given the joint angles. - Parameters - ---------- - alpha : Numpy array (3) - Joint angles ordered as (abduction, hip, knee) - leg_index : int - Leg index. - config : Config object - Robot parameters object - Returns - ------- - Numpy array (3) - Body-centric coordinates of the specified foot - """ - pass - - -def leg_explicit_inverse_kinematics(r_body_foot, leg_index, config): - """Find the joint angles corresponding to the given body-relative foot position for a given leg and configuration - Parameters - ---------- - r_body_foot : [type] - [description] - leg_index : [type] - [description] - config : [type] - [description] - Returns - ------- - numpy array (3) - Array of corresponding joint angles. - """ - (x, y, z) = r_body_foot - - # Distance from the leg origin to the foot, projected into the y-z plane - R_body_foot_yz = (y ** 2 + z ** 2) ** 0.5 - - # Distance from the leg's forward/back point of rotation to the foot - R_hip_foot_yz = (R_body_foot_yz ** 2 - config.ABDUCTION_OFFSET ** 2) ** 0.5 - - # Interior angle of the right triangle formed in the y-z plane by the leg that is coincident to the ab/adduction axis - # For feet 2 (front left) and 4 (back left), the abduction offset is positive, for the right feet, the abduction offset is negative. - cos_param = config.ABDUCTION_OFFSETS[leg_index] / R_body_foot_yz - if abs(cos_param) > 0.9: - print("Clipping 1st cos param") - cos_param = np.clip(cos_param, -0.9, 0.9) - phi = np.arccos(cos_param) - - # Angle of the y-z projection of the hip-to-foot vector, relative to the positive y-axis - hip_foot_angle = np.arctan2(z, y) - - # Ab/adduction angle, relative to the positive y-axis - abduction_angle = phi + hip_foot_angle - - # theta: Angle between the tilted negative z-axis and the hip-to-foot vector - theta = np.arctan2(-x, R_hip_foot_yz) - - # Distance between the hip and foot - R_hip_foot = (R_hip_foot_yz ** 2 + x ** 2) ** 0.5 - - # Using law of cosines to determine the angle between upper leg links - cos_param = (config.UPPER_LEG ** 2 + R_hip_foot ** 2 - config.LOWER_LEG ** 2) / (2.0*config.UPPER_LEG*R_hip_foot) - - # Ensure that the leg isn't over or under extending - cos_param = np.clip(cos_param, -0.9, 0.9) - if abs(cos_param) > 0.9: - print("Clipping 2nd cos param") - - # gamma: Angle between upper leg links and the center of the leg - gamma = np.arccos(cos_param) - - return np.array([abduction_angle, theta - gamma, theta + gamma]) - - -def four_legs_inverse_kinematics(r_body_foot, config): - """Find the joint angles for all twelve DOF correspoinding to the given matrix of body-relative foot positions. - Parameters - ---------- - r_body_foot : numpy array (3,4) - Matrix of the body-frame foot positions. Each column corresponds to a separate foot. - config : Config object - Object of robot configuration parameters. - Returns - ------- - numpy array (3,4) - Matrix of corresponding joint angles. - """ - alpha = np.zeros((3, 4)) - for i in range(4): - body_offset = config.LEG_ORIGINS[:, i] - alpha[:, i] = leg_explicit_inverse_kinematics( - r_body_foot[:, i] - body_offset, i, config - ) - return alpha \ No newline at end of file