├── data ├── traj.pdf ├── footheight.pdf ├── footheight2.pdf ├── footheight3.pdf ├── dumpfootheight3.pdf └── ttttfootheight2.pdf ├── presentation.pdf ├── cassie_m ├── libcassiemujoco.so ├── src │ ├── agilitycassie.lib │ ├── libagilitycassie.a │ └── udp.c ├── model │ ├── terrains │ │ ├── noise.png │ │ └── noisy.png │ ├── cassie-stl-meshes │ │ ├── foot.stl │ │ ├── knee.stl │ │ ├── pelvis.stl │ │ ├── shin.stl │ │ ├── tarsus.stl │ │ ├── hip-roll.stl │ │ ├── hip-yaw.stl │ │ ├── foot-crank.stl │ │ ├── heel-spring.stl │ │ ├── hip-pitch.stl │ │ ├── knee-spring.stl │ │ ├── plantar-rod.stl │ │ └── achilles-rod.stl │ ├── cassie_mass.xml │ ├── cassie_noise_terrain.xml │ ├── cassie_hfield.xml │ ├── cassie_slosh_mass.xml │ ├── cassie_tray_box.xml │ └── cassiepole.xml ├── __pycache__ │ ├── cassiemujoco.cpython-38.pyc │ └── cassiemujoco_ctypes.cpython-38.pyc ├── cassietest.c ├── cassievideo.py ├── cassiectrl.py ├── cassietest.py ├── Makefile ├── test_hfield.py ├── cassievideo.c ├── test_heelforce.c ├── test_doublevis.c ├── cassietest_depth.py ├── test_hfield.c ├── cassietest_jac.py ├── cassiectrl.c ├── test_terrain.c ├── cassieLocalConnection.py ├── testOptCoords.py └── cassiesim.c ├── README.md ├── cassie_paral.py └── cassie.py /data/traj.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/traj.pdf -------------------------------------------------------------------------------- /presentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/presentation.pdf -------------------------------------------------------------------------------- /data/footheight.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/footheight.pdf -------------------------------------------------------------------------------- /data/footheight2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/footheight2.pdf -------------------------------------------------------------------------------- /data/footheight3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/footheight3.pdf -------------------------------------------------------------------------------- /data/dumpfootheight3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/dumpfootheight3.pdf -------------------------------------------------------------------------------- /data/ttttfootheight2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/data/ttttfootheight2.pdf -------------------------------------------------------------------------------- /cassie_m/libcassiemujoco.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/libcassiemujoco.so -------------------------------------------------------------------------------- /cassie_m/src/agilitycassie.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/src/agilitycassie.lib -------------------------------------------------------------------------------- /cassie_m/model/terrains/noise.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/terrains/noise.png -------------------------------------------------------------------------------- /cassie_m/model/terrains/noisy.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/terrains/noisy.png -------------------------------------------------------------------------------- /cassie_m/src/libagilitycassie.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/src/libagilitycassie.a -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/foot.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/knee.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/knee.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/pelvis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/pelvis.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/shin.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/tarsus.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/tarsus.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/hip-roll.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/hip-yaw.stl -------------------------------------------------------------------------------- /cassie_m/__pycache__/cassiemujoco.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/__pycache__/cassiemujoco.cpython-38.pyc -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/foot-crank.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/foot-crank.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/heel-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/heel-spring.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/hip-pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/hip-pitch.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/knee-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/knee-spring.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/plantar-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/plantar-rod.stl -------------------------------------------------------------------------------- /cassie_m/model/cassie-stl-meshes/achilles-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/model/cassie-stl-meshes/achilles-rod.stl -------------------------------------------------------------------------------- /cassie_m/__pycache__/cassiemujoco_ctypes.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/WooQi57/sin-cassie-rl/HEAD/cassie_m/__pycache__/cassiemujoco_ctypes.cpython-38.pyc -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sin-cassie-rl 2 | 3 | IEEE ICMA 2022 paper *Custom Sine Waves Are Enough for Imitation Learning of Bipedal Gaits with Different Styles* 4 | 5 | This work is based on https://github.com/osudrl/cassie-mujoco-sim and https://github.com/ZhaomingXie/cassie_rl_env 6 | 7 | mujoco210 is required and should be placed in ~/.mujoco 8 | 9 | If you are not using ubuntu18.04 you should compile libcassiemujoco.so as in https://github.com/osudrl/cassie-mujoco-sim and move it to cassie_m 10 | -------------------------------------------------------------------------------- /cassie_m/cassietest.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | int main(void) 24 | { 25 | const char* modelfile = "../model/cassie.xml"; 26 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 27 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 28 | 29 | state_out_t y; 30 | pd_in_t u = {0}; 31 | 32 | bool draw_state = cassie_vis_draw(v, c); 33 | while (draw_state) { 34 | if (!cassie_vis_paused(v)) { 35 | cassie_sim_step_pd(c, &y, &u); 36 | } 37 | draw_state = cassie_vis_draw(v, c); 38 | } 39 | 40 | cassie_sim_free(c); 41 | cassie_vis_free(v); 42 | cassie_cleanup(); 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /cassie_m/cassievideo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | 23 | # Initialize cassie simulation 24 | sim = CassieSim("../model/cassie.xml") 25 | vis = CassieVis(sim) 26 | 27 | # Set control parameters 28 | u = pd_in_t() 29 | 30 | # Record time 31 | t = time.monotonic() 32 | 33 | # Run until window is closed or vis is quit 34 | draw_state = vis.draw(sim) 35 | vis.init_recording("testVidpy.mp4") 36 | 37 | while draw_state: 38 | if not vis.ispaused(): 39 | for i in range(60): 40 | y = sim.step_pd(u) 41 | 42 | draw_state = vis.draw(sim) 43 | vis.record_frame() 44 | 45 | while time.monotonic() - t < 60*0.0005: 46 | time.sleep(0.0001) 47 | t = time.monotonic() 48 | 49 | vis.close_recording() 50 | 51 | -------------------------------------------------------------------------------- /cassie_m/cassiectrl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | import time 19 | 20 | # Null controller 21 | def controller(y): 22 | return pd_in_t() 23 | 24 | # Set up UDP connection 25 | cassie = CassieUdp(remote_addr='127.0.0.1') 26 | 27 | received_data = False 28 | 29 | # Listen/respond loop 30 | while True: 31 | if not received_data: 32 | # Send packets until the simulator responds 33 | print('Connecting...') 34 | y = None 35 | while y is None: 36 | cassie.send_pd(pd_in_t()) 37 | time.sleep(0.001) 38 | y = cassie.recv_newest_pd() 39 | received_data = True 40 | print('Connected!\n') 41 | else: 42 | # Wait for new data 43 | y = cassie.recv_wait_pd() 44 | 45 | # Print connection stats 46 | print('\033[F\033[Jdelay: {}, diff: {}'.format(cassie.delay(), 47 | cassie.seq_num_in_diff())) 48 | 49 | # Run controller 50 | u = controller(y) 51 | 52 | # Send response 53 | cassie.send_pd(u) 54 | -------------------------------------------------------------------------------- /cassie_m/cassietest.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | import time 19 | import numpy as np 20 | import math 21 | 22 | # Initialize cassie simulation 23 | sim = CassieSim("../model/cassie.xml") 24 | vis = CassieVis(sim) 25 | 26 | # Set control parameters 27 | u = pd_in_t() 28 | for i in range(5): 29 | u.leftLeg.motorPd.torque[i] = 0 30 | u.rightLeg.motorPd.torque[i] = 0 31 | u.leftLeg.motorPd.dTarget[i] = 0 32 | u.rightLeg.motorPd.dTarget[i] = 0 33 | u.leftLeg.motorPd.pGain[i] = 0 34 | u.rightLeg.motorPd.pGain[i] = 0 35 | u.leftLeg.motorPd.dGain[i] = 0 36 | u.rightLeg.motorPd.dGain[i] = 0 37 | 38 | u.leftLeg.motorPd.pTarget[i] = 0 39 | u.rightLeg.motorPd.pTarget[i] = 0 40 | 41 | # Record time 42 | t = time.monotonic() 43 | 44 | # Run until window is closed or vis is quit 45 | draw_state = vis.draw(sim) 46 | while draw_state:# and draw_state2: 47 | if not vis.ispaused(): 48 | for i in range(60): 49 | y = sim.step_pd(u) 50 | 51 | draw_state = vis.draw(sim) 52 | 53 | while time.monotonic() - t < 60*0.0005: 54 | time.sleep(0.0001) 55 | t = time.monotonic() 56 | 57 | -------------------------------------------------------------------------------- /cassie_m/Makefile: -------------------------------------------------------------------------------- 1 | # Build for linux by default 2 | PLATFORM := LINUX 3 | 4 | # Compilation settings 5 | INC := -Iinclude -I../include -I$(HOME)/.mujoco/mujoco210/include 6 | CFLAGS := -std=gnu11 -Wall -Wextra -O3 -march=sandybridge -flto -ggdb3 7 | LDFLAGS := -L. -L.. 8 | LIBS := -lcassiemujoco 9 | 10 | # Platform-specific settings 11 | ifeq ($(PLATFORM), WIN) 12 | CC := x86_64-w64-mingw32-gcc 13 | TESTOUT := cassietest.exe 14 | SIMOUT := cassiesim.exe 15 | CTRLOUT := cassiectrl.exe 16 | VIDOUT := cassievideo.exe 17 | else 18 | CC := gcc 19 | LDFLAGS += -Wl,-rpath,'$$ORIGIN' 20 | TESTOUT := cassietest 21 | SIMOUT := cassiesim 22 | CTRLOUT := cassiectrl 23 | VIDOUT := cassievideo 24 | DOUBLEVISOUT := test_doublevis 25 | HFIELDOUT := test_hfield 26 | TERRAINOUT := test_terrain 27 | FORCEOUT := test_heelforce 28 | endif 29 | 30 | # Default target 31 | all: $(TESTOUT) $(SIMOUT) $(CTRLOUT) $(VIDOUT) vel_check $(DOUBLEVISOUT) $(HFIELDOUT) $(TERRAINOUT) $(FORCEOUT) 32 | 33 | # Normal targets 34 | clean: 35 | rm -f $(TESTOUT) 36 | rm -f $(SIMOUT) 37 | rm -f $(CTRLOUT) 38 | rm -f $(VIDOUT) 39 | rm -f $(DOUBLEVISOUT) 40 | rm -f $(HFIELDOUT) 41 | rm -f vel_check 42 | 43 | $(TESTOUT): cassietest.c 44 | $(CC) cassietest.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(TESTOUT) 45 | 46 | $(SIMOUT): cassiesim.c 47 | $(CC) cassiesim.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(SIMOUT) 48 | 49 | $(CTRLOUT): cassiectrl.c 50 | $(CC) cassiectrl.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(CTRLOUT) 51 | 52 | $(VIDOUT): cassievideo.c 53 | $(CC) cassievideo.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(VIDOUT) 54 | 55 | $(DOUBLEVISOUT): test_doublevis.c 56 | $(CC) test_doublevis.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(DOUBLEVISOUT) 57 | 58 | $(HFIELDOUT): test_hfield.c 59 | $(CC) test_hfield.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(HFIELDOUT) 60 | 61 | $(TERRAINOUT): test_terrain.c 62 | $(CC) test_terrain.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(TERRAINOUT) 63 | 64 | $(FORCEOUT): test_heelforce.c 65 | $(CC) test_heelforce.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o $(FORCEOUT) 66 | 67 | vel_check: vel_check.c 68 | $(CC) vel_check.c $(INC) $(CFLAGS) $(LDFLAGS) $(LIBS) -o vel_check 69 | 70 | 71 | 72 | # Virtual targets 73 | .PHONY: all clean 74 | -------------------------------------------------------------------------------- /cassie_m/test_hfield.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | 23 | # Initialize cassie simulation 24 | sim = CassieSim("../model/cassie_hfield.xml") 25 | vis = CassieVis(sim) 26 | 27 | # Set control parameters 28 | u = pd_in_t() 29 | 30 | # Record time 31 | t = time.monotonic() 32 | count = 0 33 | 34 | # Generate random terrain. All hfield data must be scaled between 0-1, and max height from hfield_size will set the scaling. 35 | # Indexing starts at negative x negative y (back right corner of the hfield in relation to where Cassie faces foward.) 36 | # needs to be flattened before being passed to set_hfield_data 37 | nrows = sim.get_hfield_nrow() 38 | ncols = sim.get_hfield_ncol() 39 | rand_hdata = np.random.random((nrows, ncols)) 40 | # Set middle of hfield (where Cassie starts) to be flat 41 | rand_hdata[nrows//2-5:nrows//2 + 5, ncols//2-5:ncols//2+5] = 0 42 | sim.set_hfield_data(rand_hdata.flatten(), vis.v) 43 | 44 | # Run until window is closed or vis is quit 45 | draw_state = vis.draw(sim) 46 | 47 | while draw_state:# and draw_state2: 48 | if not vis.ispaused(): 49 | for i in range(60): 50 | y = sim.step_pd(u) 51 | 52 | draw_state = vis.draw(sim) 53 | 54 | while time.monotonic() - t < 60*0.0005: 55 | time.sleep(0.0001) 56 | t = time.monotonic() 57 | 58 | -------------------------------------------------------------------------------- /cassie_m/cassievideo.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | static long get_microseconds(void) { 24 | struct timespec now; 25 | clock_gettime(CLOCK_MONOTONIC, &now); 26 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 27 | } 28 | 29 | int main(void) 30 | { 31 | const char modelfile[] = "../model/cassie.xml"; 32 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 33 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 34 | 35 | state_out_t y; 36 | pd_in_t u = {0}; 37 | 38 | bool draw_state = cassie_vis_draw(v, c); 39 | cassie_vis_init_recording(v, "testFile", 1920, 1080); 40 | long start_t = get_microseconds(); 41 | long sleep_time = 0; 42 | while (draw_state) { 43 | start_t = get_microseconds(); 44 | if (!cassie_vis_paused(v)) { 45 | for (int i = 0; i < 50; i++) { 46 | cassie_sim_step_pd(c, &y, &u); 47 | } 48 | } 49 | draw_state = cassie_vis_draw(v, c); 50 | cassie_vis_record_frame(v); 51 | sleep_time = 25000 - (get_microseconds() - start_t); 52 | if (sleep_time < 0){ 53 | sleep_time = 0; 54 | } 55 | usleep(sleep_time); 56 | } 57 | 58 | cassie_vis_close_recording(v); 59 | cassie_vis_free(v); 60 | cassie_sim_free(c); 61 | cassie_cleanup(); 62 | 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /cassie_paral.py: -------------------------------------------------------------------------------- 1 | import gym 2 | import torch 3 | from stable_baselines3 import PPO 4 | from stable_baselines3.common.vec_env import SubprocVecEnv 5 | from stable_baselines3.common.callbacks import BaseCallback 6 | from cassie import CassieRefEnv 7 | import numpy as np 8 | 9 | def make_env(env_id): 10 | def _f(): 11 | if env_id == 0: 12 | env = CassieRefEnv(visual=False, dynamics_randomization=False) 13 | else: 14 | env = CassieRefEnv(visual=False, dynamics_randomization=False) 15 | return env 16 | return _f 17 | 18 | if __name__ == '__main__': 19 | envs =[make_env(seed) for seed in range(16)] 20 | envs = SubprocVecEnv(envs) 21 | 22 | class TensorboardCallback(BaseCallback): 23 | """ 24 | Custom callback for plotting additional values in tensorboard. 25 | """ 26 | def __init__(self, verbose=0): 27 | super(TensorboardCallback, self).__init__(verbose) 28 | 29 | def _on_step(self) -> bool: 30 | self.logger.record('reward/ref', np.mean(self.training_env.get_attr('rew_ref_buf'))) 31 | self.logger.record('reward/spring', np.mean(self.training_env.get_attr('rew_spring_buf'))) 32 | self.logger.record('reward/orientation', np.mean(self.training_env.get_attr('rew_ori_buf'))) 33 | self.logger.record('reward/velocity', np.mean(self.training_env.get_attr('rew_vel_buf'))) 34 | self.logger.record('reward/termination', np.mean(self.training_env.get_attr('rew_termin_buf'))) 35 | self.logger.record('reward/steps', np.mean(self.training_env.get_attr('time_buf'))) 36 | self.logger.record('reward/totalreward', np.mean(self.training_env.get_attr('reward_buf'))) 37 | self.logger.record('reward/omega', np.mean(self.training_env.get_attr('omega_buf'))) 38 | self.logger.record('reward/imit', 2*np.mean(self.training_env.get_attr('rew_ref_buf'))) 39 | self.logger.record('reward/perf', 2*np.mean(self.training_env.get_attr('rew_ori_buf')) 40 | + 2*np.mean(self.training_env.get_attr('rew_vel_buf'))) 41 | 42 | if self.n_calls % 51200 == 0: 43 | print("Saving new best model") 44 | self.model.save(f"./model_saved/ppo_cassie_{self.n_calls}") 45 | 46 | return True 47 | 48 | policy_kwargs = dict(activation_fn=torch.nn.ReLU, 49 | net_arch=[dict(pi=[512, 512], vf=[512, 512])]) 50 | model = PPO("MlpPolicy", envs, verbose=1, n_steps=256, policy_kwargs=policy_kwargs, 51 | batch_size=128,tensorboard_log="./log/") 52 | model.is_tb_set = False 53 | 54 | model.learn(total_timesteps=4e7,n_eval_episodes=10,callback=TensorboardCallback()) 55 | model.save("ppo_cassie") 56 | 57 | -------------------------------------------------------------------------------- /cassie_m/test_heelforce.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | static long get_microseconds(void) { 26 | struct timespec now; 27 | clock_gettime(CLOCK_MONOTONIC, &now); 28 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 29 | } 30 | 31 | int main(void) 32 | { 33 | const char* modelfile = "../model/cassie.xml"; 34 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 35 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 36 | 37 | state_out_t y; 38 | pd_in_t u = {0}; 39 | double heel_force[6]; 40 | double toe_force[6]; 41 | double foot_force[12]; 42 | long start_t = get_microseconds(); 43 | long sleep_time = 0; 44 | 45 | bool draw_state = cassie_vis_draw(v, c); 46 | while (draw_state) { 47 | start_t = get_microseconds(); 48 | if (!cassie_vis_paused(v)) { 49 | for (int i = 0; i < 50; i++) { 50 | cassie_sim_step_pd(c, &y, &u); 51 | } 52 | cassie_sim_heeltoe_forces(c, toe_force, heel_force); 53 | cassie_sim_foot_forces(c, foot_force); 54 | printf("Left Heel force %.2f toe force %.2f\n", heel_force[0]+heel_force[1]+heel_force[2], toe_force[0]+toe_force[1]+toe_force[2]); 55 | printf("Right Heel force %.2f toe force %.2f\n", heel_force[3]+heel_force[4]+heel_force[5], toe_force[3]+toe_force[4]+toe_force[5]); 56 | printf("Force check: left %f %f right %f %f\n", heel_force[0]+heel_force[1]+heel_force[2]+toe_force[0]+toe_force[1]+toe_force[2], 57 | foot_force[0]+foot_force[1]+foot_force[2], heel_force[3]+heel_force[4]+heel_force[5]+toe_force[3]+toe_force[4]+toe_force[5], foot_force[6]+foot_force[7]+foot_force[8]); 58 | } 59 | draw_state = cassie_vis_draw(v, c); 60 | sleep_time = 25000 - (get_microseconds() - start_t); 61 | if (sleep_time < 0){ 62 | sleep_time = 0; 63 | } 64 | usleep(sleep_time); 65 | } 66 | 67 | cassie_sim_free(c); 68 | cassie_vis_free(v); 69 | cassie_cleanup(); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /cassie_m/test_doublevis.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | /* DEVEL NOTES 24 | Note that when visualizing more than one window using the same sim model, interactive applied perturbations will be visualized 25 | different in the other window. This is due to the fact that each window (cassie_vis object) has its own mjvPerturb object, 26 | which is used to do the "correct" perturbation visualization (some arrow will still be shown in the other window automatically, 27 | but the scaling and color is different). Thus, second cassie_vis object doesn't get/update its own mjvPerturb when the first 28 | cassie_vis is being perturbed. Not sure how to fix this. Even if we make mjvPerturb part of cassie_sim instead of cassie_vis, 29 | mjvPerturb is changed in the GLFW callbacks, so then would have to have access to the cassie_sim object in the callbacks. So 30 | would have the have the cassie_vis object have the pointer to the whole cassie_sim object instead of mjData and mjModel. Seems 31 | bulky though. Can't make mjvPerturb a global var either, since it would break having two separate cassie_vis with their own 32 | cassie_sim each. Regardless, simulation is unaffected, the difference is purely visual. 33 | */ 34 | 35 | static long get_microseconds(void) { 36 | struct timespec now; 37 | clock_gettime(CLOCK_MONOTONIC, &now); 38 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 39 | } 40 | 41 | int main(void) 42 | { 43 | const char modelfile[] = "../model/cassie.xml"; 44 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 45 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 46 | cassie_vis_t *v2 = cassie_vis_init(c, modelfile, false); 47 | 48 | state_out_t y; 49 | pd_in_t u = {0}; 50 | 51 | bool draw_state = cassie_vis_draw(v, c); 52 | bool draw_state2 = cassie_vis_draw(v2, c); 53 | long start_t = get_microseconds(); 54 | long sleep_time = 0; 55 | while (draw_state || draw_state2) { 56 | start_t = get_microseconds(); 57 | if (!cassie_vis_paused(v) & !cassie_vis_paused(v2)) { 58 | // Base 40 fps 59 | for (int i = 0; i < 50; i++) { 60 | cassie_sim_step_pd(c, &y, &u); 61 | } 62 | } 63 | // Only draw if the window is open 64 | if (draw_state) { 65 | draw_state = cassie_vis_draw(v, c); 66 | } 67 | if (draw_state2) { 68 | draw_state2 = cassie_vis_draw(v2, c); 69 | } 70 | sleep_time = 25000 - (get_microseconds() - start_t); 71 | if (sleep_time < 0){ 72 | sleep_time = 0; 73 | } 74 | usleep(sleep_time); 75 | } 76 | 77 | // Should free vis before freeing sim. 78 | cassie_vis_free(v); 79 | cassie_vis_free(v2); 80 | cassie_sim_free(c); 81 | cassie_cleanup(); 82 | 83 | return 0; 84 | } 85 | -------------------------------------------------------------------------------- /cassie_m/cassietest_depth.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | import matplotlib.pyplot as plt 23 | 24 | def euler2quat(z=0, y=0, x=0): 25 | 26 | z = z/2.0 27 | y = y/2.0 28 | x = x/2.0 29 | cz = math.cos(z) 30 | sz = math.sin(z) 31 | cy = math.cos(y) 32 | sy = math.sin(y) 33 | cx = math.cos(x) 34 | sx = math.sin(x) 35 | result = np.array([ 36 | cx*cy*cz - sx*sy*sz, 37 | cx*sy*sz + cy*cz*sx, 38 | cx*cz*sy - sx*cy*sz, 39 | cx*cy*sz + sx*cz*sy]) 40 | if result[0] < 0: 41 | result = -result 42 | return result 43 | 44 | # Initialize cassie simulation 45 | sim = CassieSim("../model/cassie_depth.xml") 46 | vis = CassieVis(sim) 47 | visd = CassieVis(sim, offscreen=True) 48 | width = 300 49 | height = 300 50 | visd.window_resize(width, height) 51 | visd.attach_cam(cam_name='egocentric') 52 | visd.init_depth(width, height) 53 | 54 | # Set control parameters 55 | u = pd_in_t() 56 | # u.leftLeg.motorPd.torque[3] = 0 # Feedforward torque 57 | # u.leftLeg.motorPd.pTarget[3] = -2 58 | # u.leftLeg.motorPd.pGain[3] = 100 59 | # u.leftLeg.motorPd.dTarget[3] = -2 60 | # u.leftLeg.motorPd.dGain[3] = 10 61 | # u.rightLeg.motorPd = u.leftLeg.motorPd 62 | 63 | # Hold pelvis in place 64 | # sim.hold() 65 | 66 | # Record time 67 | t = time.monotonic() 68 | count = 0 69 | 70 | 71 | # Run until window is closed or vis is quit 72 | draw_state = vis.draw(sim) 73 | feet_vel = np.zeros(12) 74 | rfoot_quat = np.zeros(4) 75 | rfoot_body_quat = np.zeros(4) 76 | pel_vel = np.zeros(6) 77 | count = 0 78 | 79 | while draw_state:# and draw_state2: 80 | if not vis.ispaused(): 81 | for i in range(60): 82 | y = sim.step_pd(u) 83 | # sim.hold() 84 | # qpos = np.array(sim.qpos_full()) 85 | # qvel = np.array(sim.qvel_full()) 86 | # print("mass: ", qpos[0:2] - qpos[35:37]) 87 | # print(qpos[35:38]) 88 | # print("pel z:", qpos[2]) 89 | # print("left foot quat: ", sim.xquat("left-foot")) 90 | # qvel = sim.qvel() 91 | # sim.foot_vel(feet_vel) 92 | # sim.foot_quat(rfoot_quat) 93 | # rfoot_body_quat = sim.xquat("right-foot") 94 | # sim.body_vel(pel_vel, 'cassie-pelvis') 95 | # print("r foot quat: ", rfoot_quat) 96 | # print("rfoot_body_quat: ", rfoot_body_quat) 97 | # print("state est quat: ", y.rightFoot.orientation[:]) 98 | # print("left foot vel: ", feet_vel[3:6]) 99 | # print("right foot vel: ", feet_vel[9:12]) 100 | # print("state est vel: ", y.rightFoot.footTranslationalVelocity[:]) 101 | # print("diff: ", np.linalg.norm(feet_vel[9:12]-y.rightFoot.footTranslationalVelocity[:])) 102 | # print("pel com vel: ", pel_vel[3:]) 103 | # print("pel qvel: ", qvel[0:3]) 104 | # print("state est pel vel: ", y.pelvis.translationalVelocity[:]) 105 | # print("diff: ", np.linalg.norm(np.array(y.pelvis.translationalVelocity[:])-qvel[0:3])) 106 | # count += 1 107 | 108 | draw_state = vis.draw(sim) 109 | visd.draw(sim) 110 | depth_ptr = visd.draw_depth(sim, width=width, height=height) 111 | depth = np.ctypeslib.as_array(depth_ptr, shape=(width*height,)).reshape((1,1,width,height)) 112 | # print(min(depth), max(depth)) 113 | # plt.imshow(np.flip(depth[0,0,:,:],0), cmap='hot', interpolation='nearest') 114 | # plt.show() 115 | # plt.savefig('./figs/img_{}.png'.format(count)) 116 | count += 1 117 | 118 | while time.monotonic() - t < 60*0.0005: 119 | time.sleep(0.0001) 120 | t = time.monotonic() 121 | 122 | -------------------------------------------------------------------------------- /cassie_m/test_hfield.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | static long get_microseconds(void) { 25 | struct timespec now; 26 | clock_gettime(CLOCK_MONOTONIC, &now); 27 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 28 | } 29 | 30 | int main(void) 31 | { 32 | const char modelfile[] = "../model/cassie_hfield.xml"; 33 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 34 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 35 | 36 | state_out_t y; 37 | pd_in_t u = {0}; 38 | 39 | // Get size of height field data and set to random data 40 | // Can just grab hfield pointer and directly set data 41 | int nhfielddata = cassie_sim_get_nhfielddata(c); 42 | float* hfield_data = cassie_sim_hfielddata(c); 43 | srand(time(NULL)); 44 | for (int i = 0; i < nhfielddata; i++) { 45 | hfield_data[i] = (float)rand()/(float)(RAND_MAX); 46 | } 47 | 48 | // Set middle where Cassie is initialized to be flat. Indexing starts at negative x negative y 49 | // (back right corner of the hfield in relation to where Cassie faces foward.) 50 | int nrow = cassie_sim_get_hfield_nrow(c); 51 | int ncol = cassie_sim_get_hfield_ncol(c); 52 | int zero_rad = 5; // "Radius" around the center to make flat 53 | for (int i = -zero_rad; i < zero_rad; i++) { 54 | for (int j = -zero_rad; j < zero_rad; j++) { 55 | hfield_data[ncol*(nrow/2+i) + ncol/2+j] = 0; 56 | } 57 | } 58 | 59 | // Alternative way to set hfield data is to make array and pass to 60 | // set_hfielddata function 61 | #if 0 62 | int nhfielddata = cassie_sim_get_nhfielddata(c); 63 | float hfield_data[nhfielddata]; 64 | srand(time(NULL)); 65 | for (int i = 0; i < nhfielddata; i++) { 66 | hfield_data[i] = (float)rand()/(float)(RAND_MAX); 67 | } 68 | int nrow = cassie_sim_get_hfield_nrow(c); 69 | int ncol = cassie_sim_get_hfield_ncol(c); 70 | int zero_rad = 5; // "Radius" around the center to make flat 71 | for (int i = -zero_rad; i < zero_rad; i++) { 72 | for (int j = -zero_rad; j < zero_rad; j++) { 73 | hfield_data[ncol*(nrow/2+i) + ncol/2+j] = 0; 74 | } 75 | } 76 | cassie_sim_set_hfielddata(c, hfield_data); 77 | #endif 78 | 79 | /* 80 | Can also get and modify the hfield size to change max height or size of hfield 81 | All hfield data must be scaled between 0-1, and max height will set the scaling. 82 | Note that you can NOT change the number of columns and rows of hfield data at runtime 83 | */ 84 | #if 0 85 | double* hfield_size = cassie_sim_get_hfield_size(c); 86 | // hfield size is 4 long vector of (x, y, z_top, z_bottom) 87 | hfield_size[0] *= 0.5; // Shrink x 88 | hfield_size[1] *= 1.5; // Stretch y 89 | hfield_size[2] *= 2; // Increase maximum height 90 | hfield_size[3] *= 2; // Increase depth or "thickness" of box 91 | 92 | // Again can also use set_hfield_size function 93 | // double hfield_size[4] = {50, 25, .5, 0.5}; 94 | // cassie_sim_set_hfield_size(c, hfield_size); 95 | #endif 96 | 97 | // Need to remake mjvScene and mjrContext in order for visualization to reflect hfield changes 98 | // since cassie_vis was already made. This is not necessary if you modify hfield before cassie_vis is made 99 | cassie_vis_remakeSceneCon(v); 100 | 101 | bool draw_state = cassie_vis_draw(v, c);; 102 | long start_t = get_microseconds(); 103 | long sleep_time = 0; 104 | while (draw_state) { 105 | start_t = get_microseconds(); 106 | if (!cassie_vis_paused(v)) { 107 | // Base 40 fps 108 | for (int i = 0; i < 50; i++) { 109 | cassie_sim_step_pd(c, &y, &u); 110 | } 111 | } 112 | draw_state = cassie_vis_draw(v, c); 113 | sleep_time = 25000 - (get_microseconds() - start_t); 114 | if (sleep_time < 0){ 115 | sleep_time = 0; 116 | } 117 | usleep(sleep_time); 118 | } 119 | 120 | cassie_vis_free(v); 121 | cassie_sim_free(c); 122 | cassie_cleanup(); 123 | 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /cassie_m/cassietest_jac.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | import matplotlib.pyplot as plt 23 | 24 | """ 25 | Test the end-effector Jacobian, don't care 26 | 1. dynamically consistent (Mx*J*Mx^-1); 27 | 2. closed-loop constraints; 28 | 3. rotational components. 29 | Only to get the rough estimate of the Jacobian matrix on motors. 30 | """ 31 | 32 | # Initialize cassie simulation 33 | sim = CassieSim("../model/cassie.xml") 34 | vis = CassieVis(sim) 35 | # Set control parameters 36 | u = pd_in_t() 37 | test_jac_control = True 38 | test_random = True 39 | 40 | if test_jac_control: 41 | qpos = sim.qpos() 42 | qpos[2] = 1.5 43 | sim.set_qpos(qpos) 44 | sim.hold() 45 | 46 | # Record time 47 | t = time.monotonic() 48 | count = 0 49 | ltarget = np.array([0, 0.13, -0.8]) 50 | rtarget = np.array([0, -0.13, -0.5]) 51 | kp = np.array([70, 70, 100, 100, 50]) 52 | kd = np.array([7.0, 7.0, 8.0, 8.0, 5.0]) 53 | 54 | # Run until window is closed or vis is quit 55 | draw_state = vis.draw(sim) 56 | vel_idx = [6, 7, 8, 12, 18, 19, 20, 21, 25, 31] 57 | pos_idx = [7, 8, 9, 14, 20, 21, 22, 23, 28, 34] 58 | 59 | ts_noise_up = np.array([[0, 0, 0.13, 0.13, 0], 60 | [0.13, 0, 0.00, 0.00, 0], 61 | [0, 0, 0.13, 0.13, 0]]) 62 | 63 | ts_noise = np.block([ 64 | [ts_noise_up, np.zeros((3,5))], 65 | [np.zeros((3,5)), ts_noise_up] 66 | ]) 67 | 68 | offset = np.array([0.0045, 0.0, 0.4973, -1.1997, -1.5968, 0.0045, 0.0, 0.4973, -1.1997, -1.5968]) 69 | 70 | while draw_state: 71 | if not vis.ispaused(): 72 | for i in range(60): 73 | jacpl = sim.get_jacobian(name='left-foot').reshape(3, -1) 74 | jacpr = sim.get_jacobian(name='right-foot').reshape(3, -1) 75 | jacp = np.concatenate((jacpl, jacpr)) 76 | jacp_motor = jacp.take(vel_idx, axis=1) 77 | jdag = np.linalg.pinv(jacp_motor) 78 | 79 | if test_jac_control: 80 | lpos = np.array(sim.foot_pos()[0:3]) - np.array(sim.qpos()[0:3]) 81 | rpos = np.array(sim.foot_pos()[3:6]) - np.array(sim.qpos()[0:3]) 82 | dxl = ltarget - lpos 83 | dxr = rtarget - rpos 84 | # print(np.dot(jdag[:,0:3], dxl).shape) 85 | # print(np.dot(jdag[:,3:6], dxr).shape) 86 | dq = np.dot(jdag[:,0:3], dxl) + np.dot(jdag[:,3:6], dxr) 87 | # print(dq.shape) 88 | # print(dq) 89 | # print(lpos) 90 | qpos = sim.qpos() 91 | mpos = [qpos[i] for i in pos_idx] 92 | 93 | for i in range(5): 94 | u.leftLeg.motorPd.pGain[i] = kp[i] * 0.1 95 | u.rightLeg.motorPd.pGain[i] = kp[i] * 0.1 96 | u.leftLeg.motorPd.dGain[i] = kd[i] * 0.1 97 | u.rightLeg.motorPd.dGain[i] = kd[i] * 0.1 98 | u.leftLeg.motorPd.torque[i] = 0 # Feedforward torque 99 | u.rightLeg.motorPd.torque[i] = 0 100 | u.leftLeg.motorPd.pTarget[i] = dq[i] + mpos[i] 101 | u.rightLeg.motorPd.pTarget[i] = dq[i+5] + mpos[i+5] 102 | u.leftLeg.motorPd.dTarget[i] = 0 103 | u.rightLeg.motorPd.dTarget[i] = 0 104 | y = sim.step_pd(u) 105 | else: 106 | action = np.random.uniform(-10, 10, size=10) 107 | for i in range(5): 108 | u.leftLeg.motorPd.pGain[i] = kp[i] 109 | u.rightLeg.motorPd.pGain[i] = kp[i] 110 | u.leftLeg.motorPd.dGain[i] = kd[i] 111 | u.rightLeg.motorPd.dGain[i] = kd[i] 112 | u.leftLeg.motorPd.torque[i] = 0 # Feedforward torque 113 | u.rightLeg.motorPd.torque[i] = 0 114 | u.leftLeg.motorPd.pTarget[i] = action[i] + offset[i] 115 | u.rightLeg.motorPd.pTarget[i] = action[i+5] + offset[i+5] 116 | u.leftLeg.motorPd.dTarget[i] = 0 117 | u.rightLeg.motorPd.dTarget[i] = 0 118 | y = sim.step_pd(u) 119 | 120 | sd_final = np.matmul(jdag, ts_noise) 121 | # print(sd_final.shape) 122 | # print("new js noise matrix") 123 | # for i in range(10): 124 | # for j in range(10): 125 | # print("{: 3.2f}".format(sd_final[i][j]), end=" ") 126 | # print("\n") 127 | # input() 128 | 129 | draw_state = vis.draw(sim) 130 | count += 1 131 | 132 | while time.monotonic() - t < 60*0.0005: 133 | time.sleep(0.0001) 134 | t = time.monotonic() 135 | 136 | -------------------------------------------------------------------------------- /cassie_m/cassiectrl.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "cassie_out_t.h" 24 | #include "cassie_user_in_t.h" 25 | #include "state_out_t.h" 26 | #include "pd_in_t.h" 27 | #include "udp.h" 28 | 29 | 30 | enum mode { 31 | MODE_STANDARD, 32 | MODE_PD 33 | }; 34 | 35 | 36 | int main(int argc, char *argv[]) 37 | { 38 | // Option variables and flags 39 | char *remote_addr_str = "127.0.0.1"; 40 | char *remote_port_str = "25000"; 41 | char *iface_addr_str = "0.0.0.0"; 42 | char *iface_port_str = "25001"; 43 | int mode = MODE_STANDARD; 44 | 45 | // Parse arguments 46 | int c; 47 | while ((c = getopt(argc, argv, "a:p:b:c:x")) != -1) { 48 | switch (c) { 49 | case 'a': 50 | // Remote address to connect to 51 | remote_addr_str = optarg; 52 | break; 53 | case 'p': 54 | // Port to connect to 55 | remote_port_str = optarg; 56 | break; 57 | case 'b': 58 | // Remote address to connect to 59 | iface_addr_str = optarg; 60 | break; 61 | case 'c': 62 | // Port to connect to 63 | iface_port_str = optarg; 64 | break; 65 | case 'x': 66 | // Run in PD mode 67 | mode = MODE_PD; 68 | break; 69 | default: 70 | // Print usage 71 | printf( 72 | "Usage: cassiectrl [OPTION]...\n" 73 | "Controls the Cassie robot over UDP.\n" 74 | "\n" 75 | " -a [ADDRESS] Specify the local interface to bind to.\n" 76 | " -p [PORT] Specify the port to listen on.\n" 77 | " -b [ADDRESS] Specify the remote address to connect to.\n" 78 | " -c [PORT] Specify the remote port to connect to.\n" 79 | " -x Run in PD mode, taking state estimates and sending PD targets.\n" 80 | "\n" 81 | "By default, the controller connects to localhost over IPv4 to port %s" 82 | "from port %s.\n\n", 83 | remote_port_str, iface_port_str); 84 | exit(EXIT_SUCCESS); 85 | } 86 | } 87 | 88 | // Bind to network interface 89 | int sock = udp_init_client(remote_addr_str, remote_port_str, 90 | iface_addr_str, iface_port_str); 91 | if (-1 == sock) 92 | exit(EXIT_FAILURE); 93 | 94 | // Create packet input/output buffers 95 | int dinlen, doutlen; 96 | switch (mode) { 97 | case MODE_PD: 98 | dinlen = STATE_OUT_T_PACKED_LEN; 99 | doutlen = PD_IN_T_PACKED_LEN; 100 | break; 101 | default: 102 | dinlen = CASSIE_OUT_T_PACKED_LEN; 103 | doutlen = CASSIE_USER_IN_T_PACKED_LEN; 104 | } 105 | const int recvlen = PACKET_HEADER_LEN + dinlen; 106 | const int sendlen = PACKET_HEADER_LEN + doutlen; 107 | unsigned char *recvbuf = malloc(recvlen); 108 | unsigned char *sendbuf = malloc(sendlen); 109 | 110 | // Separate input/output buffers into header and payload 111 | const unsigned char *header_in = recvbuf; 112 | const unsigned char *data_in = &recvbuf[PACKET_HEADER_LEN]; 113 | unsigned char *header_out = sendbuf; 114 | unsigned char *data_out = &sendbuf[PACKET_HEADER_LEN]; 115 | 116 | // Create standard input/output structs 117 | cassie_user_in_t cassie_user_in = {0}; 118 | cassie_out_t cassie_out; 119 | 120 | // Create PD input/output structs 121 | pd_in_t pd_in = {0}; 122 | state_out_t state_out; 123 | 124 | // Create header information struct 125 | packet_header_info_t header_info = {0}; 126 | 127 | // Prepare initial null command packet to start communication 128 | printf("Connecting to cassie...\n"); 129 | memset(sendbuf, 0, sendlen); 130 | bool received_data = false; 131 | 132 | // Listen/respond loop 133 | while (true) { 134 | if (!received_data) { 135 | // Send null commands until the simulator responds 136 | ssize_t nbytes; 137 | do { 138 | send_packet(sock, sendbuf, sendlen, NULL, 0); 139 | usleep(1000); 140 | nbytes = get_newest_packet(sock, recvbuf, recvlen, NULL, NULL); 141 | } while (recvlen != nbytes); 142 | received_data = true; 143 | printf("Connected!\n\n"); 144 | } else { 145 | // Wait for a new packet 146 | wait_for_packet(sock, recvbuf, recvlen, NULL, NULL); 147 | } 148 | 149 | // Process incoming header and write outgoing header 150 | process_packet_header(&header_info, header_in, header_out); 151 | printf("\033[F\033[Jdelay: %d, diff: %d\n", 152 | header_info.delay, header_info.seq_num_in_diff); 153 | 154 | switch (mode) { 155 | case MODE_PD: 156 | // Unpack received data into cassie user input struct 157 | unpack_state_out_t(data_in, &state_out); 158 | 159 | // Run controller 160 | // Do nothing in this example 161 | 162 | // Pack cassie out struct into outgoing packet 163 | pack_pd_in_t(&pd_in, data_out); 164 | break; 165 | default: 166 | // Unpack received data into cassie user input struct 167 | unpack_cassie_out_t(data_in, &cassie_out); 168 | 169 | // Run controller 170 | // Do nothing in this example 171 | 172 | // Pack cassie out struct into outgoing packet 173 | pack_cassie_user_in_t(&cassie_user_in, data_out); 174 | } 175 | 176 | // Send response 177 | send_packet(sock, sendbuf, sendlen, NULL, 0); 178 | } 179 | } 180 | -------------------------------------------------------------------------------- /cassie_m/test_terrain.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include "cassiemujoco.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | static long get_microseconds(void) { 26 | struct timespec now; 27 | clock_gettime(CLOCK_MONOTONIC, &now); 28 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 29 | } 30 | 31 | void shift_terrain_x(float* hdata, int offset, int nrow, int ncol) { 32 | if (offset == 1) { // Shift forward 33 | for (int j = 1; j < ncol; j++) { 34 | for (int i = 0; i < nrow; i++) { 35 | hdata[(ncol-j)+ncol*i] = hdata[(ncol-j-1)+ncol*i]; 36 | } 37 | } 38 | } else if (offset == -1) { // Shift backwards 39 | for (int j = 0; j < ncol-1; j++) { 40 | for (int i = 0; i < nrow; i++) { 41 | hdata[(j)+ncol*i] = hdata[(j+1)+ncol*i]; 42 | } 43 | } 44 | } else { 45 | printf("wrong offset\n"); 46 | } 47 | } 48 | 49 | void shift_terrain_y(float* hdata, int offset, int nrow, int ncol) { 50 | if (offset == 1) { // Shift left 51 | for (int j = 1; j < nrow; j++) { 52 | for (int i = 0; i < ncol; i++) { 53 | hdata[ncol*(nrow-j)+i] = hdata[ncol*(nrow-j-1)+i]; 54 | } 55 | } 56 | } else if (offset == -1) { // Shift right 57 | for (int j = 0; j < nrow-1; j++) { 58 | for (int i = 0; i < ncol; i++) { 59 | hdata[ncol*j+i] = hdata[ncol*(j+1)+i]; 60 | } 61 | } 62 | } else { 63 | printf("wrong offset\n"); 64 | } 65 | } 66 | 67 | int main(void) 68 | { 69 | const char modelfile[] = "../model/cassie_hfield.xml"; 70 | cassie_sim_t *c = cassie_sim_init(modelfile, false); 71 | cassie_vis_t *v = cassie_vis_init(c, modelfile, false); 72 | time_t t; 73 | srand((unsigned) time(&t)); 74 | int nrow = cassie_sim_get_hfield_nrow(c); 75 | int ncol = cassie_sim_get_hfield_ncol(c); 76 | double* hfield_size = cassie_sim_get_hfield_size(c); 77 | double x_size = hfield_size[0]; 78 | double y_size = hfield_size[1]; 79 | double x_incr = x_size / ncol; // Due to mujoco data convention (column major) divide by ncol, not nrow 80 | double y_incr = y_size / nrow; 81 | double* hfield_pos = cassie_sim_get_body_name_pos(c, "floor"); 82 | int hdata_size = nrow*ncol; 83 | float hdata[hdata_size]; 84 | for (int i = 0; i < hdata_size; i++) { 85 | // hdata[i] = 0; 86 | hdata[i] = (float) rand() / RAND_MAX; 87 | } 88 | // Y left 89 | // for (int i = 0; i < ncol; i++) { 90 | // hdata[ncol*(nrow-10)+i] = 1; 91 | // } 92 | // X behind 93 | // for (int i = 0; i < nrow; i++) { 94 | // hdata[(ncol-6)+ncol*i] = 1; 95 | // } 96 | // X In front 97 | // for (int i = 0; i < nrow; i++) { 98 | // hdata[ncol*i] = 1; 99 | // } 100 | // shift_terrain_x(hdata, -1, nrow, ncol); 101 | // Y right 102 | // for (int i = 0; i < ncol; i++) { 103 | // hdata[i] = 1; 104 | // } 105 | // shift_terrain_y(hdata, -1, nrow, ncol); 106 | // hdata[ncol-1] = 1; 107 | cassie_sim_set_hfielddata(c, hdata); 108 | cassie_vis_remakeSceneCon(v); 109 | 110 | state_out_t y; 111 | pd_in_t u = {0}; 112 | 113 | bool draw_state = cassie_vis_draw(v, c); 114 | long start_t = get_microseconds(); 115 | long sleep_time = 0; 116 | double* qpos; 117 | double h_pos[3] = {hfield_pos[0], hfield_pos[1], -0.1}; 118 | while (draw_state) { 119 | start_t = get_microseconds(); 120 | if (!cassie_vis_paused(v)) { 121 | for (int i = 0; i < 50; i++) { 122 | cassie_sim_step_pd(c, &y, &u); 123 | } 124 | qpos = cassie_sim_qpos(c); 125 | double x_diff = qpos[0] - hfield_pos[0]; 126 | double y_diff = qpos[1] - hfield_pos[1]; 127 | if (x_diff > x_incr) { 128 | shift_terrain_x(hdata, -1, nrow, ncol); 129 | for (int i = 0; i < nrow; i++) { 130 | hdata[(ncol-1)+ncol*i] = (float) rand() / RAND_MAX; 131 | } 132 | h_pos[0] += x_incr; 133 | } else if (x_diff < -x_incr) { 134 | shift_terrain_x(hdata, 1, nrow, ncol); 135 | for (int i = 0; i < nrow; i++) { 136 | hdata[ncol*i] = (float) rand() / RAND_MAX; 137 | } 138 | h_pos[0] -= x_incr; 139 | } 140 | if (y_diff > y_incr) { 141 | shift_terrain_y(hdata, -1, nrow, ncol); 142 | for (int i = 0; i < nrow; i++) { 143 | hdata[ncol*(nrow-1)+i] = (float) rand() / RAND_MAX; 144 | } 145 | h_pos[1] += y_incr; 146 | } else if (y_diff < -y_incr) { 147 | shift_terrain_y(hdata, 1, nrow, ncol); 148 | for (int i = 0; i < nrow; i++) { 149 | hdata[i] = (float) rand() / RAND_MAX; 150 | } 151 | h_pos[1] -= y_incr; 152 | } 153 | if ((fabs(x_diff) > x_incr) || (fabs(y_diff) > y_incr)) { 154 | cassie_sim_set_body_name_pos(c, "floor", h_pos); 155 | cassie_sim_set_hfielddata(c, hdata); 156 | cassie_vis_remakeSceneCon(v); 157 | } 158 | // h_pos[0] = qpos[0]; 159 | // h_pos[1] = qpos[1]; 160 | // cassie_sim_set_geom_name_pos(c, "hfield1", h_pos); 161 | // cassie_sim_set_body_name_pos(c, "floor", h_pos); 162 | // printf("hfield pos %f %f\n", hfield_pos[0], hfield_pos[1]); 163 | } 164 | draw_state = cassie_vis_draw(v, c); 165 | sleep_time = 25000 - (get_microseconds() - start_t); 166 | if (sleep_time < 0){ 167 | sleep_time = 0; 168 | } 169 | usleep(sleep_time); 170 | 171 | } 172 | 173 | cassie_sim_free(c); 174 | 175 | // cassie_sim_free(c2); 176 | printf("done free\n"); 177 | cassie_vis_free(v); 178 | cassie_cleanup(); 179 | 180 | return 0; 181 | } 182 | -------------------------------------------------------------------------------- /cassie_m/cassieLocalConnection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | import matplotlib.pyplot as plt 23 | from matplotlib import cm 24 | from collections import namedtuple 25 | import json 26 | 27 | MOTOR_POS_IDX = [7,8,9,14,20,21,22,23,28,34] 28 | MOTOR_VEL_IDX = [7,8,9,14,20,21,22,23,28,34] 29 | PASSIVE_VEL_IDX = [9,10,11,14,22,23,24,27] 30 | 31 | DynamicInfo = namedtuple("DynamicInfo", "qpos motorPos M_min I_centroid cm_pos") 32 | 33 | 34 | def euler2quat(z=0, y=0, x=0): 35 | 36 | z = z/2.0 37 | y = y/2.0 38 | x = x/2.0 39 | cz = math.cos(z) 40 | sz = math.sin(z) 41 | cy = math.cos(y) 42 | sy = math.sin(y) 43 | cx = math.cos(x) 44 | sx = math.sin(x) 45 | result = np.array([ 46 | cx*cy*cz - sx*sy*sz, 47 | cx*sy*sz + cy*cz*sx, 48 | cx*cz*sy - sx*cy*sz, 49 | cx*cy*sz + sx*cz*sy]) 50 | if result[0] < 0: 51 | result = -result 52 | return result 53 | 54 | def forwardUpdateClosedLoop(sim, vis, motorPos): 55 | u = pd_in_t() 56 | # sim.full_reset() 57 | qpos = sim.qpos() 58 | qvel = sim.qvel() 59 | qpos[0] = 0 60 | qpos[1] = 0 61 | qpos[2] = 0.5 62 | qpos[3] = 1 63 | qpos[4] = 0 64 | qpos[5] = 0 65 | qpos[6] = 0 66 | 67 | for i in range(5): 68 | qpos[MOTOR_POS_IDX[i]] = motorPos[i] 69 | qpos[MOTOR_POS_IDX[i+5]] = motorPos[i+5] 70 | 71 | sim.set_qpos(qpos) 72 | sim.set_qvel(np.zeros((len(qvel), 1))) 73 | sim.hold() 74 | nStep = 0 75 | err_c = 10 76 | while nStep < 500 and np.linalg.norm(err_c) > 1e-5: 77 | J_c = sim.constraint_jacobian() 78 | err_c = sim.constraint_error() 79 | J_passive_c = np.zeros(J_c.shape) 80 | J_passive_c[:, PASSIVE_VEL_IDX] = J_c[:, PASSIVE_VEL_IDX] 81 | 82 | q_vel = np.linalg.lstsq(J_passive_c, -200*err_c, rcond=None) 83 | 84 | sim.set_qvel(q_vel[0]) 85 | sim.integrate_pos() 86 | 87 | nStep = nStep + 1 88 | # draw_state = vis.draw(sim) 89 | 90 | qpos_final = sim.qpos() 91 | print("Finished in " + str(nStep) + " steps with error norm of " + str(np.linalg.norm(err_c))) 92 | return qpos_final 93 | 94 | 95 | def getAllDynamicInfo(sim, vis, motor_pos): 96 | qpos_result = forwardUpdateClosedLoop(sim, vis, motor_pos) 97 | sim.set_qpos(qpos_result) 98 | M_minimal = sim.minimal_mass_matrix() 99 | centroid_interia = sim.centroid_inertia() 100 | centerofMass_pos = sim.center_of_mass_position() 101 | 102 | dynInf = {} 103 | dynInf['qpos']=qpos_result, 104 | dynInf['motorPos']=motor_pos, 105 | dynInf['M_min']= M_minimal.tolist() 106 | dynInf['I_centroid']=centroid_interia 107 | dynInf['cm_pos']=centerofMass_pos 108 | 109 | return dynInf 110 | 111 | def writeDynInfoJSON(dynInfo_list): 112 | # print(json.dumps({'q_pos':dynInfo_list[0].qpos, 'motorPos':dynInfo_list[0].motorPos, 113 | # 'M_minimal':dynInfo_list[0].M_min, 'I_centroid':dynInfo_list[0].I_centroid, 114 | # 'cm_pos':dynInfo_list[0].cm_pos})) 115 | with open('cassieInertia.txt', 'w') as outfile: 116 | json.dump(dynInfo_list, outfile) 117 | 118 | 119 | # Initialize cassie simulation 120 | sim = CassieSim("../model/cassie_no_grav.xml") 121 | print(sim.nq) 122 | vis = CassieVis(sim) 123 | 124 | # Set control parameters 125 | u = pd_in_t() 126 | 127 | # Record time 128 | t = time.monotonic() 129 | count = 0 130 | 131 | # Run until window is closed or vis is quit 132 | draw_state = vis.draw(sim) 133 | 134 | sim.full_reset() 135 | qpos = sim.qpos() 136 | motor_pos_nominal = [] 137 | for i in range(10): 138 | motor_pos_nominal.append(qpos[MOTOR_POS_IDX[i]]) 139 | 140 | 141 | 142 | N_grid_hip_roll = 3 143 | N_grid_knee = 3 144 | N_grid_hip = 3 145 | hip_list = np.linspace(-0.27, 1.25, num=N_grid_hip) 146 | knee_list = np.linspace(-1.90, -0.9, num=N_grid_knee) 147 | hip_roll_left_list = np.linspace(-0.18, 0.30, num=N_grid_hip_roll) 148 | hip_roll_right_list = np.linspace(-0.30, 0.18, num=N_grid_hip_roll) 149 | 150 | 151 | # for idx in range(N_grid): 152 | # motor_pos[0] = hip_roll[idx] 153 | # motor_pos[5] = hip_roll[idx] 154 | # sim.full_reset() 155 | 156 | # dynInfo = getAllDynamicInfo(sim, vis, motor_pos) 157 | # print('Hip Motor: ', hip_roll[idx]) 158 | # input('') 159 | 160 | 161 | header_info = {} 162 | header_info['grid_dimensions'] = [N_grid_hip_roll, N_grid_hip, N_grid_knee,N_grid_hip_roll, N_grid_hip, N_grid_knee] 163 | header_info['right_hip_roll_vals'] = hip_roll_right_list.tolist() 164 | header_info['right_hip_pitch_vals'] = hip_list.tolist() 165 | header_info['right_knee_vals'] = knee_list.tolist() 166 | header_info['left_hip_roll_vals'] = hip_roll_left_list.tolist() 167 | header_info['left_hip_pitch_vals'] = hip_list.tolist() 168 | header_info['left_knee_vals'] = knee_list.tolist() 169 | 170 | 171 | # writeDynInfoJSON(header_info) 172 | 173 | dynInfo_list = [] 174 | for right_hip_roll_idx in range(N_grid_hip_roll): 175 | for right_hip__pitch_idx in range(N_grid_hip): 176 | for right_knee_idx in range(N_grid_knee): 177 | for left_hip_roll_idx in range(N_grid_hip_roll): 178 | for left_hip__pitch_idx in range(N_grid_hip): 179 | for left_knee_idx in range(N_grid_knee): 180 | 181 | motor_pos = motor_pos_nominal 182 | motor_pos[0] = hip_roll_left_list[left_hip_roll_idx] 183 | motor_pos[2] = hip_list[left_hip__pitch_idx] 184 | motor_pos[3] = knee_list[left_knee_idx] 185 | motor_pos[5] = hip_roll_right_list[right_hip_roll_idx] 186 | motor_pos[7] = hip_list[right_hip__pitch_idx] 187 | motor_pos[8] = knee_list[right_knee_idx] 188 | 189 | sim.full_reset() 190 | idx_key = [right_hip_roll_idx, right_hip__pitch_idx, right_knee_idx, left_hip_roll_idx, left_hip__pitch_idx, left_knee_idx] 191 | dynInfo = getAllDynamicInfo(sim, vis, motor_pos) 192 | dynInfo['grid_indices'] = idx_key 193 | dynInfo_list.append(dynInfo) 194 | 195 | writeDynInfoJSON((header_info, dynInfo_list)) 196 | 197 | # Plot the surface. 198 | 199 | # fig, axs = plt.subplots(nrows=1, ncols=1) 200 | # temp_imshow = axs.imshow(error, extent = (hip_list[0], hip_list[-1], knee_list[0], knee_list[-1])) 201 | # fig.colorbar(temp_imshow, ax=axs) 202 | # plt.show() 203 | 204 | 205 | -------------------------------------------------------------------------------- /cassie_m/testOptCoords.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Dynamic Robotics Laboratory 4 | # 5 | # Permission to use, copy, modify, and distribute this software for any 6 | # purpose with or without fee is hereby granted, provided that the above 7 | # copyright notice and this permission notice appear in all copies. 8 | # 9 | # THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 | # WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 | # MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 | # ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 | # WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 | # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 | # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 | 17 | from cassiemujoco import * 18 | from cassiemujoco_ctypes import joint_filter_t, drive_filter_t 19 | import time 20 | import numpy as np 21 | import math 22 | import matplotlib.pyplot as plt 23 | from matplotlib import cm 24 | from collections import namedtuple 25 | import json 26 | from cassieXboxController import cassieXboxController 27 | from optimalCoords.optimal_cassie import load_data, interpolate_so3 28 | from scipy.spatial.transform import Rotation as R 29 | 30 | MOTOR_POS_IDX = [7,8,9,14,20,21,22,23,28,34] 31 | MOTOR_VEL_IDX = [7,8,9,14,20,21,22,23,28,34] 32 | PASSIVE_VEL_IDX = [9,10,11,14,22,23,24,27] 33 | 34 | DynamicInfo = namedtuple("DynamicInfo", "qpos motorPos M_min I_centroid cm_pos") 35 | 36 | 37 | def euler2quat(z=0, y=0, x=0): 38 | 39 | z = z/2.0 40 | y = y/2.0 41 | x = x/2.0 42 | cz = math.cos(z) 43 | sz = math.sin(z) 44 | cy = math.cos(y) 45 | sy = math.sin(y) 46 | cx = math.cos(x) 47 | sx = math.sin(x) 48 | result = np.array([ 49 | cx*cy*cz - sx*sy*sz, 50 | cx*sy*sz + cy*cz*sx, 51 | cx*cz*sy - sx*cy*sz, 52 | cx*cy*sz + sx*cz*sy]) 53 | if result[0] < 0: 54 | result = -result 55 | return result 56 | 57 | def forwardUpdateClosedLoop(sim, vis, motorPos): 58 | u = pd_in_t() 59 | # sim.full_reset() 60 | qpos = sim.qpos() 61 | qvel = sim.qvel() 62 | qpos[0] = 0 63 | qpos[1] = 0 64 | qpos[2] = 1.5 65 | qpos[3] = 1 66 | qpos[4] = 0 67 | qpos[5] = 0 68 | qpos[6] = 0 69 | 70 | for i in range(5): 71 | qpos[MOTOR_POS_IDX[i]] = motorPos[i] 72 | qpos[MOTOR_POS_IDX[i+5]] = motorPos[i+5] 73 | 74 | sim.set_qpos(qpos) 75 | sim.set_qvel(np.zeros((len(qvel), 1))) 76 | sim.hold() 77 | nStep = 0 78 | err_c = 10 79 | while nStep < 500 and np.linalg.norm(err_c) > 1e-5: 80 | J_c = sim.constraint_jacobian() 81 | err_c = sim.constraint_error() 82 | J_passive_c = np.zeros(J_c.shape) 83 | J_passive_c[:, PASSIVE_VEL_IDX] = J_c[:, PASSIVE_VEL_IDX] 84 | 85 | q_vel = np.linalg.lstsq(J_passive_c, -200*err_c, rcond=None) 86 | 87 | sim.set_qvel(q_vel[0]) 88 | sim.integrate_pos() 89 | 90 | nStep = nStep + 1 91 | # draw_state = vis.draw(sim) 92 | 93 | qpos_final = sim.qpos() 94 | # print("Finished in " + str(nStep) + " steps with error norm of " + str(np.linalg.norm(err_c))) 95 | return qpos_final 96 | 97 | 98 | def getAllDynamicInfo(sim, vis, motor_pos): 99 | qpos_result = forwardUpdateClosedLoop(sim, vis, motor_pos) 100 | sim.set_qpos(qpos_result) 101 | M_minimal = sim.minimal_mass_matrix() 102 | centroid_interia = sim.centroid_inertia() 103 | centerofMass_pos = sim.center_of_mass_position() 104 | 105 | dynInf = {} 106 | dynInf['qpos']=qpos_result, 107 | dynInf['motorPos']=motor_pos, 108 | dynInf['M_min']= M_minimal.tolist() 109 | dynInf['I_centroid']=centroid_interia 110 | dynInf['cm_pos']=centerofMass_pos 111 | 112 | return dynInf 113 | 114 | def writeDynInfoJSON(dynInfo_list): 115 | # print(json.dumps({'q_pos':dynInfo_list[0].qpos, 'motorPos':dynInfo_list[0].motorPos, 116 | # 'M_minimal':dynInfo_list[0].M_min, 'I_centroid':dynInfo_list[0].I_centroid, 117 | # 'cm_pos':dynInfo_list[0].cm_pos})) 118 | with open('cassieInertia.txt', 'w') as outfile: 119 | json.dump(dynInfo_list, outfile) 120 | 121 | def update_motor_pos(controller, motor_pos): 122 | gain = 0.01 123 | controller.process_events() 124 | flip, vals = controller.get_controller_state() 125 | if flip == 1: 126 | motor_pos[0] += vals[1]*gain 127 | motor_pos[2] += vals[2]*gain 128 | motor_pos[3] += vals[0]*gain 129 | else: 130 | motor_pos[5] += vals[1]*gain 131 | motor_pos[7] += vals[2]*gain 132 | motor_pos[8] += vals[0]*gain 133 | print(flip) 134 | return motor_pos 135 | 136 | 137 | 138 | trans_dict = load_data() 139 | alpha = np.array([0.1, 0.75, -1.2, 0, 0.75, -1.2]) 140 | controller = cassieXboxController() 141 | 142 | 143 | # Initialize cassie simulation 144 | sim = CassieSim("../model/cassie_no_grav.xml") 145 | print(sim.nq) 146 | vis = CassieVis(sim) 147 | 148 | # Set control parameters 149 | u = pd_in_t() 150 | 151 | # Record time 152 | t = time.monotonic() 153 | count = 0 154 | 155 | # Run until window is closed or vis is quit 156 | draw_state = vis.draw(sim) 157 | 158 | sim.full_reset() 159 | qpos = sim.qpos() 160 | 161 | 162 | motor_pos = [] 163 | for i in range(10): 164 | motor_pos.append(qpos[MOTOR_POS_IDX[i]]) 165 | 166 | 167 | vis.start_video_recording("pyVideoTest", 1280, 720) 168 | i = 0 169 | while controller.button_state['M'] == 0: 170 | 171 | motor_pos = update_motor_pos(controller, motor_pos) 172 | alpha[0] = motor_pos[0] 173 | alpha[1] = motor_pos[2] 174 | alpha[2] = motor_pos[3] 175 | alpha[3] = motor_pos[5] 176 | alpha[4] = motor_pos[7] 177 | alpha[5] = motor_pos[8] 178 | 179 | qpos_result = forwardUpdateClosedLoop(sim, vis, motor_pos) 180 | 181 | interp = interpolate_so3(trans_dict, alpha) 182 | r = R.from_matrix(interp['rot_mat']) 183 | body_orientation_offset = r.as_quat() 184 | qpos_result[3] = body_orientation_offset[0] 185 | qpos_result[4] = body_orientation_offset[1] 186 | qpos_result[5] = body_orientation_offset[2] 187 | qpos_result[6] = body_orientation_offset[3] 188 | 189 | sim.set_qpos(qpos_result) 190 | draw_state = vis.draw(sim) 191 | 192 | i = i+1 193 | if i%5 == 0: 194 | vis.record_frame() 195 | time.sleep(1.0/(60*5)) 196 | 197 | 198 | vis.close_video_recording() 199 | 200 | 201 | 202 | # hip_list = np.radians(np.linspace(-20, 60, num=N_grid)) 203 | # knee_list = np.radians(np.linspace(-100, -60, num=N_grid)) 204 | 205 | # error = np.zeros((N_grid,N_grid)) 206 | 207 | # dynInfo_list = [] 208 | # for left_hip_idx in range(N_grid): 209 | # for left_knee_idx in range(N_grid): 210 | 211 | # left_hip_pitch_angle = hip_list[left_hip_idx] 212 | # left_knee_angle = knee_list[left_knee_idx] 213 | 214 | # motor_pos[2] = left_hip_pitch_angle 215 | # motor_pos[3] = left_knee_angle 216 | # motor_pos[7] = left_hip_pitch_angle 217 | # motor_pos[8] = left_knee_angle 218 | 219 | # dynInfo = getAllDynamicInfo(sim, vis, motor_pos) 220 | # dynInfo_list.append(dynInfo) 221 | 222 | 223 | 224 | # writeDynInfoJSON(dynInfo_list) 225 | 226 | # Plot the surface. 227 | 228 | # fig, axs = plt.subplots(nrows=1, ncols=1) 229 | # temp_imshow = axs.imshow(error, extent = (hip_list[0], hip_list[-1], knee_list[0], knee_list[-1])) 230 | # fig.colorbar(temp_imshow, ax=axs) 231 | # plt.show() 232 | 233 | 234 | -------------------------------------------------------------------------------- /cassie_m/src/udp.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Agility Robotics 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #ifdef _WIN32 18 | 19 | // Winsock compatibility 20 | #define _WIN32_WINNT 0x0601 21 | #include 22 | 23 | #define poll WSAPoll 24 | #define close closesocket 25 | #define ioctl ioctlsocket 26 | #define perror(str) fprintf(stderr, str ": %d\n", WSAGetLastError()) 27 | #define SOCKETS_INIT \ 28 | do { \ 29 | WSADATA wsaData; \ 30 | int res = WSAStartup(MAKEWORD(2, 2), &wsaData); \ 31 | if (res) { \ 32 | printf("WSAStartup failed: %d\n", res); \ 33 | return -1; \ 34 | } \ 35 | } while (0) 36 | #define SOCKETS_CLEANUP WSACleanup() 37 | typedef u_long ioctl_arg_t; 38 | 39 | #else 40 | 41 | // Linux sockets 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #define SOCKETS_INIT 49 | #define SOCKETS_CLEANUP 50 | typedef int ioctl_arg_t; 51 | 52 | #endif // _WIN32 53 | 54 | #include "udp.h" 55 | #include 56 | 57 | 58 | void process_packet_header(packet_header_info_t *info, 59 | const unsigned char *header_in, 60 | unsigned char *header_out) 61 | { 62 | // Increment outgoing packet sequence number 63 | ++info->seq_num_out; 64 | 65 | // header_in[0]: sequence number of incoming packet 66 | // header_in[1]: sequence number of previous outgoing packet, looped back 67 | char seq_num_in = (char) header_in[0]; 68 | char loopback = (char) header_in[1]; 69 | 70 | // Compute round-trip delay and incoming sequence number diff 71 | info->delay = info->seq_num_out - loopback; 72 | info->seq_num_in_diff = seq_num_in - info->seq_num_in_last; 73 | info->seq_num_in_last = seq_num_in; 74 | 75 | // Write outgoing packet header 76 | header_out[0] = (unsigned char) info->seq_num_out; 77 | header_out[1] = (unsigned char) seq_num_in; 78 | } 79 | 80 | 81 | int udp_init_host(const char *local_addr_str, const char *local_port_str) 82 | { 83 | // Platform-specific socket library initialization 84 | SOCKETS_INIT; 85 | 86 | int err; 87 | 88 | // Get address info 89 | struct addrinfo *local; 90 | struct addrinfo hints = {0}; 91 | hints.ai_family = AF_UNSPEC; 92 | hints.ai_socktype = SOCK_DGRAM; 93 | hints.ai_protocol = IPPROTO_UDP; 94 | err = getaddrinfo(local_addr_str, local_port_str, &hints, &local); 95 | if (err) { 96 | printf("%s\n", gai_strerror(err)); 97 | SOCKETS_CLEANUP; 98 | return -1; 99 | } 100 | 101 | // Create socket 102 | int sock = socket(local->ai_family, local->ai_socktype, local->ai_protocol); 103 | if (-1 == sock) { 104 | perror("Error creating socket"); 105 | freeaddrinfo(local); 106 | SOCKETS_CLEANUP; 107 | return -1; 108 | } 109 | 110 | // Bind to interface address 111 | if (bind(sock, (struct sockaddr *) local->ai_addr, 112 | local->ai_addrlen)) { 113 | perror("Error binding to interface address"); 114 | close(sock); 115 | freeaddrinfo(local); 116 | SOCKETS_CLEANUP; 117 | return -1; 118 | } 119 | 120 | // Free addrinfo struct 121 | freeaddrinfo(local); 122 | 123 | // Make socket non-blocking 124 | ioctl_arg_t mode = 1; 125 | ioctl(sock, FIONBIO, &mode); 126 | 127 | return sock; 128 | } 129 | 130 | 131 | int udp_init_client(const char *remote_addr_str, const char *remote_port_str, 132 | const char *local_addr_str, const char *local_port_str) 133 | { 134 | // Platform-specific socket library initialization 135 | SOCKETS_INIT; 136 | 137 | int err; 138 | 139 | // Get remote address info 140 | struct addrinfo *remote; 141 | struct addrinfo hints = {0}; 142 | hints.ai_family = AF_UNSPEC; 143 | hints.ai_socktype = SOCK_DGRAM; 144 | hints.ai_protocol = IPPROTO_UDP; 145 | err = getaddrinfo(remote_addr_str, remote_port_str, &hints, &remote); 146 | if (err) { 147 | printf("%s\n", gai_strerror(err)); 148 | SOCKETS_CLEANUP; 149 | return -1; 150 | } 151 | 152 | // Get remote address info 153 | struct addrinfo *local; 154 | err = getaddrinfo(local_addr_str, local_port_str, &hints, &local); 155 | if (err) { 156 | printf("%s\n", gai_strerror(err)); 157 | SOCKETS_CLEANUP; 158 | return -1; 159 | } 160 | 161 | // Create socket 162 | int sock = socket(remote->ai_family, remote->ai_socktype, 163 | remote->ai_protocol); 164 | if (-1 == sock) { 165 | perror("Error creating socket"); 166 | freeaddrinfo(remote); 167 | freeaddrinfo(local); 168 | SOCKETS_CLEANUP; 169 | return -1; 170 | } 171 | 172 | // Bind to interface address 173 | if (bind(sock, (struct sockaddr *) local->ai_addr, local->ai_addrlen)) { 174 | perror("Error binding to interface address"); 175 | close(sock); 176 | freeaddrinfo(remote); 177 | freeaddrinfo(local); 178 | SOCKETS_CLEANUP; 179 | return -1; 180 | } 181 | 182 | // Connect to remote address 183 | if (connect(sock, (struct sockaddr *) remote->ai_addr, 184 | remote->ai_addrlen)) { 185 | perror("Error connecting to remote address"); 186 | close(sock); 187 | freeaddrinfo(remote); 188 | freeaddrinfo(local); 189 | SOCKETS_CLEANUP; 190 | return -1; 191 | } 192 | 193 | // Free addrinfo structs 194 | freeaddrinfo(remote); 195 | freeaddrinfo(local); 196 | 197 | // Make socket non-blocking 198 | ioctl_arg_t mode = 1; 199 | ioctl(sock, FIONBIO, &mode); 200 | 201 | return sock; 202 | } 203 | 204 | 205 | void udp_close(int sock) 206 | { 207 | close(sock); 208 | SOCKETS_CLEANUP; 209 | } 210 | 211 | 212 | ssize_t get_newest_packet(int sock, void *recvbuf, size_t recvlen, 213 | struct sockaddr *src_addr, socklen_t *addrlen) 214 | { 215 | // Does not use sequence number for determining newest packet 216 | ssize_t nbytes = -1; 217 | struct pollfd fd = {.fd = sock, .events = POLLIN, .revents = 0}; 218 | 219 | // Loop through RX buffer, copying data if packet is correct size 220 | while (poll(&fd, 1, 0)) { 221 | ioctl_arg_t nbytes_avail; 222 | ioctl(sock, FIONREAD, &nbytes_avail); 223 | if (recvlen == (size_t) nbytes_avail) 224 | nbytes = recvfrom(sock, recvbuf, recvlen, 0, src_addr, addrlen); 225 | else 226 | recv(sock, recvbuf, 0, 0); // Discard packet 227 | } 228 | 229 | // Return the copied packet size, or -1 if no data was copied 230 | return nbytes; 231 | } 232 | 233 | 234 | ssize_t wait_for_packet(int sock, void *recvbuf, size_t recvlen, 235 | struct sockaddr *src_addr, socklen_t *addrlen) 236 | { 237 | ssize_t nbytes; 238 | 239 | do { 240 | // Wait if no packets are available 241 | struct pollfd fd = {.fd = sock, .events = POLLIN, .revents = 0}; 242 | while (!poll(&fd, 1, 0)) {} 243 | 244 | // Get the newest available packet 245 | nbytes = get_newest_packet(sock, recvbuf, recvlen, src_addr, addrlen); 246 | } while ((ssize_t) recvlen != nbytes); 247 | 248 | // Return the copied packet size 249 | return nbytes; 250 | } 251 | 252 | 253 | ssize_t send_packet(int sock, void *sendbuf, size_t sendlen, 254 | struct sockaddr *dst_addr, socklen_t addrlen) 255 | { 256 | ssize_t nbytes; 257 | 258 | // Send packet, retrying if busy 259 | do { 260 | nbytes = sendto(sock, sendbuf, sendlen, 0, dst_addr, addrlen); 261 | } while (-1 == nbytes); 262 | 263 | // Return the sent packet size 264 | return nbytes; 265 | } 266 | -------------------------------------------------------------------------------- /cassie_m/cassiesim.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Dynamic Robotics Laboratory 3 | * 4 | * Permission to use, copy, modify, and distribute this software for any 5 | * purpose with or without fee is hereby granted, provided that the above 6 | * copyright notice and this permission notice appear in all copies. 7 | * 8 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 9 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 10 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 11 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 12 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "cassiemujoco.h" 25 | #include "udp.h" 26 | 27 | 28 | enum mode { 29 | MODE_STANDARD, 30 | MODE_PD 31 | }; 32 | 33 | 34 | #ifdef _WIN32 35 | #include 36 | 37 | static long long get_microseconds(void) 38 | { 39 | LARGE_INTEGER count, frequency; 40 | QueryPerformanceFrequency(&frequency); 41 | QueryPerformanceCounter(&count); 42 | return (count.QuadPart * 1000000) / frequency.QuadPart; 43 | } 44 | 45 | #else 46 | 47 | static long long get_microseconds(void) 48 | { 49 | struct timespec now; 50 | clock_gettime(CLOCK_MONOTONIC, &now); 51 | return now.tv_sec * 1000000 + now.tv_nsec / 1000; 52 | } 53 | 54 | #endif // _WIN32 55 | 56 | 57 | int main(int argc, char *argv[]) 58 | { 59 | // Option variables and flags 60 | char *iface_addr_str = "0.0.0.0"; 61 | char *iface_port_str = "25000"; 62 | bool realtime = false; 63 | bool visualize = false; 64 | bool hold = false; 65 | char *log_file_path = NULL; 66 | char *qlog_file_path = NULL; 67 | int mode = MODE_STANDARD; 68 | 69 | // Parse arguments 70 | int c; 71 | while ((c = getopt(argc, argv, "a:p:rvhl:q:x")) != -1) { 72 | switch (c) { 73 | case 'a': 74 | // Inteface address to bind 75 | iface_addr_str = optarg; 76 | break; 77 | case 'p': 78 | // Port to bind 79 | iface_port_str = optarg; 80 | break; 81 | case 'r': 82 | // Enable real-time mode 83 | realtime = true; 84 | break; 85 | case 'v': 86 | // Visualize simulation 87 | visualize = true; 88 | break; 89 | case 'h': 90 | // Hold pelvis 91 | hold = true; 92 | break; 93 | case 'l': 94 | // Log data 95 | log_file_path = optarg; 96 | break; 97 | case 'q': 98 | // Log simulator state data 99 | qlog_file_path = optarg; 100 | break; 101 | case 'x': 102 | // Run in PD mode 103 | mode = MODE_PD; 104 | break; 105 | default: 106 | // Print usage 107 | printf( 108 | "Usage: cassiesim [OPTION]...\n" 109 | "Simulates the Cassie robot communicating over UDP.\n" 110 | "\n" 111 | " -a [ADDRESS] Specify the local interface to bind to.\n" 112 | " -p [PORT] Specify the port to listen on.\n" 113 | " -r Run simulation continuously instead of in lockstep.\n" 114 | " -v Show a visualization of the running simulation.\n" 115 | " -h Hold the pelvis in place.\n" 116 | " -l [FILENAME] Log the input and output UDP data to a file.\n" 117 | " -q [FILENAME] Log simulation time, qpos, and qvel to a file.\n" 118 | " -x Run in PD mode, taking PD targets and sending state estimates.\n" 119 | "\n" 120 | "By default, the simulator listens on all IPv4 interfaces at port %s.\n" 121 | "If the option -r is not given, the simulator waits for input after sending\n" 122 | "each sensor data packet. With -r, the simulator runs in real-time, reusing\n" 123 | "old input if new data is not yet ready and waiting a minimum amount of time\n" 124 | "between sending sensor data packets.\n\n", iface_port_str); 125 | exit(EXIT_SUCCESS); 126 | } 127 | } 128 | 129 | // Bind to network interface 130 | int sock = udp_init_host(iface_addr_str, iface_port_str); 131 | if (-1 == sock) 132 | exit(EXIT_FAILURE); 133 | 134 | // Create packet input/output buffers 135 | int dinlen, doutlen; 136 | switch (mode) { 137 | case MODE_PD: 138 | dinlen = PD_IN_T_PACKED_LEN; 139 | doutlen = STATE_OUT_T_PACKED_LEN; 140 | break; 141 | default: 142 | dinlen = CASSIE_USER_IN_T_PACKED_LEN; 143 | doutlen = CASSIE_OUT_T_PACKED_LEN; 144 | } 145 | const int recvlen = PACKET_HEADER_LEN + dinlen; 146 | const int sendlen = PACKET_HEADER_LEN + doutlen; 147 | unsigned char *recvbuf = malloc(recvlen); 148 | unsigned char *sendbuf = malloc(sendlen); 149 | 150 | // Separate input/output buffers into header and payload 151 | const unsigned char *header_in = recvbuf; 152 | const unsigned char *data_in = &recvbuf[PACKET_HEADER_LEN]; 153 | unsigned char *header_out = sendbuf; 154 | unsigned char *data_out = &sendbuf[PACKET_HEADER_LEN]; 155 | 156 | // Create standard input/output structs 157 | cassie_user_in_t cassie_user_in = {0}; 158 | cassie_out_t cassie_out; 159 | 160 | // Create PD input/output structs 161 | pd_in_t pd_in = {0}; 162 | state_out_t state_out; 163 | 164 | // Create header information struct 165 | packet_header_info_t header_info = {0}; 166 | 167 | // Address to send sensor data packets to 168 | struct sockaddr_storage src_addr = {0}; 169 | socklen_t addrlen = sizeof src_addr; 170 | 171 | // Create cassie simulation 172 | const char modelfile[] = "../model/cassie.xml"; 173 | cassie_sim_t *sim = cassie_sim_init(modelfile, false); 174 | cassie_vis_t *vis; 175 | if (visualize) 176 | vis = cassie_vis_init(sim, modelfile, false); 177 | if (hold) 178 | cassie_sim_hold(sim); 179 | 180 | // Cassie input/output log file 181 | FILE *log_file = NULL; 182 | if (log_file_path) 183 | log_file = fopen(log_file_path, "wb"); 184 | 185 | // SImulator state log file 186 | FILE *qlog_file = NULL; 187 | if (qlog_file_path) 188 | qlog_file = fopen(qlog_file_path, "wb"); 189 | 190 | // Manage simulation loop 191 | unsigned long long loop_counter = 0; 192 | bool run_sim = false; 193 | const long long cycle_usec = 1000000 / 2000; 194 | const long long timeout_usec = mode == MODE_PD ? 100000 : 10000; 195 | long long send_time = get_microseconds(); 196 | long long recv_time = get_microseconds(); 197 | 198 | printf("Waiting for input...\n"); 199 | 200 | // Listen/respond loop 201 | bool render_state = true; 202 | while (render_state) { 203 | // Try to get a new packet 204 | ssize_t nbytes; 205 | if (realtime) { 206 | // Get newest packet, or return -1 if no new packets are available 207 | nbytes = get_newest_packet(sock, recvbuf, recvlen, 208 | (struct sockaddr *) &src_addr, &addrlen); 209 | } else { 210 | // If not in real-time mode, wait until a new packet comes in 211 | nbytes = wait_for_packet(sock, recvbuf, recvlen, 212 | (struct sockaddr *) &src_addr, &addrlen); 213 | } 214 | 215 | // If a new packet was received, process and unpack it 216 | if (recvlen == nbytes) { 217 | // Process incoming header and write outgoing header 218 | process_packet_header(&header_info, header_in, header_out); 219 | //printf("\033[F\033[Jdelay: %d, diff: %d\n", 220 | // header_info.delay, header_info.seq_num_in_diff); 221 | 222 | // Unpack received data into cassie user input struct 223 | switch (mode) { 224 | case MODE_PD: 225 | unpack_pd_in_t(data_in, &pd_in); 226 | break; 227 | default: 228 | unpack_cassie_user_in_t(data_in, &cassie_user_in); 229 | } 230 | 231 | // Update packet received timestamp 232 | recv_time = get_microseconds(); 233 | 234 | // Start the simulation after the first valid packet is received 235 | run_sim = true; 236 | } 237 | 238 | size_t compute_start = get_microseconds(); 239 | double sim_start = *cassie_sim_time(sim); 240 | if (run_sim) { 241 | // Run simulator and pack output struct into outgoing packet 242 | switch (mode) { 243 | case MODE_PD: 244 | cassie_sim_step_pd(sim, &state_out, &pd_in); 245 | pack_state_out_t(&state_out, data_out); 246 | break; 247 | default: 248 | cassie_sim_step(sim, &cassie_out, &cassie_user_in); 249 | pack_cassie_out_t(&cassie_out, data_out); 250 | } 251 | 252 | // Log Cassie input/output 253 | if (log_file) { 254 | fwrite(data_out, doutlen, 1, log_file); 255 | fwrite(data_in, dinlen, 1, log_file); 256 | } 257 | 258 | // Log simulation state 259 | if (qlog_file) { 260 | fwrite(cassie_sim_time(sim), sizeof (double), 1, qlog_file); 261 | fwrite(cassie_sim_qpos(sim), sizeof (double), 35, qlog_file); 262 | fwrite(cassie_sim_qvel(sim), sizeof (double), 32, qlog_file); 263 | } 264 | 265 | if (realtime) { 266 | // Wait at least cycle_usec between sending simulator updates 267 | while (get_microseconds() - send_time < cycle_usec) {} 268 | send_time = get_microseconds(); 269 | 270 | // Zero input data if no new packets have been 271 | // received in a while 272 | if (get_microseconds() - recv_time > timeout_usec) { 273 | memset(&cassie_user_in, 0, sizeof cassie_user_in); 274 | memset(&pd_in, 0, sizeof pd_in); 275 | } 276 | } 277 | 278 | // Send response 279 | send_packet(sock, sendbuf, sendlen, 280 | (struct sockaddr *) &src_addr, addrlen); 281 | } 282 | double cpu_deltat, sim_deltat; 283 | double sim_end = *cassie_sim_time(sim); 284 | do{ 285 | size_t compute_end = get_microseconds(); 286 | cpu_deltat = (double)(compute_end - compute_start) / 1e6; 287 | sim_deltat = sim_end - sim_start; 288 | } while(sim_deltat > cpu_deltat); 289 | 290 | if(cpu_deltat > sim_deltat + 1e-4){ 291 | printf("SLOWER THAN REAL TIME BY %6.5fs\n", cpu_deltat - sim_deltat); 292 | 293 | } 294 | 295 | 296 | //printf("%6.5f vs %6.5f\n", cpu_deltat, sim_deltat); 297 | 298 | 299 | // Draw no more then once every 33 simulation steps 300 | if (visualize && loop_counter % 33 == 0) 301 | render_state = cassie_vis_draw(vis, sim); 302 | 303 | // Increment loop counter 304 | ++loop_counter; 305 | } 306 | } 307 | -------------------------------------------------------------------------------- /cassie.py: -------------------------------------------------------------------------------- 1 | # isolated cassie env 2 | from cassie_m.cassiemujoco import pd_in_t, state_out_t, CassieSim, CassieVis 3 | 4 | from math import floor 5 | import gym 6 | from gym import spaces 7 | import numpy as np 8 | import os 9 | import random 10 | import copy 11 | import pickle 12 | 13 | class CassieRefEnv(gym.Env): 14 | def __init__(self, simrate=60, dynamics_randomization=True, 15 | visual=True, config="./cassie_m/model/cassie.xml", **kwargs): 16 | self.config = config 17 | self.visual = visual 18 | self.sim = CassieSim(self.config) 19 | if self.visual: 20 | self.vis = CassieVis(self.sim) 21 | 22 | self.dynamics_randomization = dynamics_randomization 23 | self.termination = False 24 | 25 | #state buffer 26 | self.state_buffer = [] 27 | self.buffer_size = 1 # 3 28 | 29 | # Observation space and State space 30 | self.observation_space = spaces.Box(low=-np.inf,high=np.inf,shape=(self.buffer_size*39+2+2,)) 31 | self.action_space = spaces.Box(low=np.array([-1]*10), high=np.array([1]*10)) 32 | 33 | self.P = np.array([100, 100, 88, 96, 50]) 34 | self.D = np.array([10.0, 10.0, 8.0, 9.6, 5.0]) 35 | self.u = pd_in_t() 36 | self.foot_pos = [0]*6 37 | 38 | self.cassie_state = state_out_t() 39 | self.simrate = simrate # simulate X mujoco steps with same pd target. 50 brings simulation from 2000Hz to exactly 40Hz 40 | self.time = 0 # number of time steps in current episode 41 | self.phase = 0 # portion of the phase the robot is in 42 | self.counter = 0 # number of phase cycles completed in episode 43 | self.time_limit = 600 44 | self.offset = np.array([0.0045, 0.0, 0.4973, -1.1997, -1.5968, 0.0045, 0.0, 0.4973, -1.1997, -1.5968]) 45 | self.time_buf = 0 46 | 47 | self.max_speed = 4.0 48 | self.min_speed = -0.3 49 | self.max_side_speed = 0.3 50 | self.min_side_speed = -0.3 51 | 52 | #### Dynamics Randomization #### 53 | 54 | self.max_pitch_incline = 0.03 55 | self.max_roll_incline = 0.03 56 | self.encoder_noise = 0.01 57 | self.damping_low = 0.3 58 | self.damping_high = 5.0 59 | self.mass_low = 0.5 60 | self.mass_high = 1.5 61 | self.fric_low = 0.4 62 | self.fric_high = 1.1 63 | self.speed = 0.6 64 | self.side_speed = 0 65 | self.orient_add = 0 66 | 67 | # Default dynamics parameters 68 | self.default_damping = self.sim.get_dof_damping() 69 | self.default_mass = self.sim.get_body_mass() 70 | self.default_ipos = self.sim.get_body_ipos() 71 | self.default_fric = self.sim.get_geom_friction() 72 | self.default_rgba = self.sim.get_geom_rgba() 73 | self.default_quat = self.sim.get_geom_quat() 74 | 75 | self.motor_encoder_noise = np.zeros(10) 76 | self.joint_encoder_noise = np.zeros(6) 77 | 78 | # rew_buf 79 | self.rew_ref = 0 80 | self.rew_spring = 0 81 | self.rew_ori = 0 82 | self.rew_vel = 0 83 | self.rew_termin = 0 84 | self.reward = 0 85 | self.rew_ref_buf = 0 86 | self.rew_spring_buf = 0 87 | self.rew_ori_buf = 0 88 | self.rew_vel_buf = 0 89 | self.rew_termin_buf = 0 90 | self.reward_buf = 0 91 | self.omega_buf = 0 92 | 93 | 94 | def custom_footheight(self): 95 | phase = self.phase 96 | h = 0.15 97 | h1 = max(0, h*np.sin(2*np.pi/28*phase)-0.2*h) 98 | h2 = max(0, h*np.sin(np.pi + 2*np.pi/28*phase)-0.2*h) 99 | return [h1,h2] 100 | 101 | def step_simulation(self,action): 102 | target = action + self.offset 103 | 104 | self.u = pd_in_t() 105 | 106 | for i in range(5): 107 | self.u.leftLeg.motorPd.pGain[i] = self.P[i] 108 | self.u.rightLeg.motorPd.pGain[i] = self.P[i] 109 | self.u.leftLeg.motorPd.dGain[i] = self.D[i] 110 | self.u.rightLeg.motorPd.dGain[i] = self.D[i] 111 | 112 | self.u.leftLeg.motorPd.torque[i] = 0 # Feedforward torque 113 | self.u.rightLeg.motorPd.torque[i] = 0 114 | 115 | self.u.leftLeg.motorPd.pTarget[i] = target[i] 116 | self.u.rightLeg.motorPd.pTarget[i] = target[i + 5] 117 | 118 | self.u.leftLeg.motorPd.dTarget[i] = 0 119 | self.u.rightLeg.motorPd.dTarget[i] = 0 120 | 121 | self.cassie_state = self.sim.step_pd(self.u) # cassie_state is different from qpos state??? 122 | 123 | def step(self, action): 124 | 125 | for _ in range(self.simrate): 126 | self.step_simulation(action) 127 | 128 | self.time += 1 129 | self.phase += 1 130 | if self.phase >= 28: 131 | self.phase = 0 132 | self.counter +=1 133 | 134 | obs = self.get_state() 135 | self.sim.foot_pos(self.foot_pos) 136 | 137 | xpos, ypos, height = self.qpos[0], self.qpos[1], self.qpos[2] 138 | xtarget, ytarget, ztarget = self.ref_pos[0], self.ref_pos[1], self.ref_pos[2] 139 | pos2target = (xpos-xtarget)**2 + (ypos-ytarget)**2 + (height-ztarget)**2 140 | die_radii = 0.6 + (self.speed**2 + self.side_speed**2)**0.5 141 | self.termination = height < 0.6 or height > 1.2 or pos2target > die_radii**2 142 | done = self.termination or self.time >= self.time_limit 143 | 144 | if self.visual: 145 | self.render() 146 | reward = self.compute_reward(action) 147 | 148 | return obs, reward, done, {} 149 | 150 | def reset(self): 151 | if self.time != 0 : 152 | self.rew_ref_buf = self.rew_ref / self.time 153 | self.rew_spring_buf = self.rew_spring / self.time 154 | self.rew_ori_buf = self.rew_ori / self.time 155 | self.rew_vel_buf = self.rew_vel / self.time 156 | self.rew_termin_buf = self.rew_termin / self.time 157 | self.reward_buf = self.reward # / self.time 158 | self.time_buf = self.time 159 | self.omega_buf = self.omega / self.time 160 | 161 | self.rew_ref = 0 162 | self.rew_spring = 0 163 | self.rew_ori = 0 164 | self.rew_vel = 0 165 | self.rew_termin = 0 166 | self.reward = 0 167 | self.omega = 0 168 | 169 | self.speed = 0.6 170 | self.side_speed = 0 171 | self.time = 0 172 | self.counter = 0 173 | self.termination = False 174 | self.phase = int((random.random()>0.5)*14) # random phase: 0 or 14 175 | 176 | # Randomize dynamics: 177 | if self.dynamics_randomization: 178 | damp = self.default_damping 179 | 180 | pelvis_damp_range = [[damp[0], damp[0]], 181 | [damp[1], damp[1]], 182 | [damp[2], damp[2]], 183 | [damp[3], damp[3]], 184 | [damp[4], damp[4]], 185 | [damp[5], damp[5]]] # 0->5 186 | 187 | hip_damp_range = [[damp[6]*self.damping_low, damp[6]*self.damping_high], 188 | [damp[7]*self.damping_low, damp[7]*self.damping_high], 189 | [damp[8]*self.damping_low, damp[8]*self.damping_high]] # 6->8 and 19->21 190 | 191 | achilles_damp_range = [[damp[9]*self.damping_low, damp[9]*self.damping_high], 192 | [damp[10]*self.damping_low, damp[10]*self.damping_high], 193 | [damp[11]*self.damping_low, damp[11]*self.damping_high]] # 9->11 and 22->24 194 | 195 | knee_damp_range = [[damp[12]*self.damping_low, damp[12]*self.damping_high]] # 12 and 25 196 | shin_damp_range = [[damp[13]*self.damping_low, damp[13]*self.damping_high]] # 13 and 26 197 | tarsus_damp_range = [[damp[14]*self.damping_low, damp[14]*self.damping_high]] # 14 and 27 198 | 199 | heel_damp_range = [[damp[15], damp[15]]] # 15 and 28 200 | fcrank_damp_range = [[damp[16]*self.damping_low, damp[16]*self.damping_high]] # 16 and 29 201 | prod_damp_range = [[damp[17], damp[17]]] # 17 and 30 202 | foot_damp_range = [[damp[18]*self.damping_low, damp[18]*self.damping_high]] # 18 and 31 203 | 204 | side_damp = hip_damp_range + achilles_damp_range + knee_damp_range + shin_damp_range + tarsus_damp_range + heel_damp_range + fcrank_damp_range + prod_damp_range + foot_damp_range 205 | damp_range = pelvis_damp_range + side_damp + side_damp 206 | damp_noise = [np.random.uniform(a, b) for a, b in damp_range] 207 | 208 | m = self.default_mass 209 | pelvis_mass_range = [[self.mass_low*m[1], self.mass_high*m[1]]] # 1 210 | hip_mass_range = [[self.mass_low*m[2], self.mass_high*m[2]], # 2->4 and 14->16 211 | [self.mass_low*m[3], self.mass_high*m[3]], 212 | [self.mass_low*m[4], self.mass_high*m[4]]] 213 | 214 | achilles_mass_range = [[self.mass_low*m[5], self.mass_high*m[5]]] # 5 and 17 215 | knee_mass_range = [[self.mass_low*m[6], self.mass_high*m[6]]] # 6 and 18 216 | knee_spring_mass_range = [[self.mass_low*m[7], self.mass_high*m[7]]] # 7 and 19 217 | shin_mass_range = [[self.mass_low*m[8], self.mass_high*m[8]]] # 8 and 20 218 | tarsus_mass_range = [[self.mass_low*m[9], self.mass_high*m[9]]] # 9 and 21 219 | heel_spring_mass_range = [[self.mass_low*m[10], self.mass_high*m[10]]] # 10 and 22 220 | fcrank_mass_range = [[self.mass_low*m[11], self.mass_high*m[11]]] # 11 and 23 221 | prod_mass_range = [[self.mass_low*m[12], self.mass_high*m[12]]] # 12 and 24 222 | foot_mass_range = [[self.mass_low*m[13], self.mass_high*m[13]]] # 13 and 25 223 | 224 | side_mass = hip_mass_range + achilles_mass_range \ 225 | + knee_mass_range + knee_spring_mass_range \ 226 | + shin_mass_range + tarsus_mass_range \ 227 | + heel_spring_mass_range + fcrank_mass_range \ 228 | + prod_mass_range + foot_mass_range 229 | 230 | mass_range = [[0, 0]] + pelvis_mass_range + side_mass + side_mass 231 | mass_noise = [np.random.uniform(a, b) for a, b in mass_range] 232 | 233 | delta = 0.0 234 | com_noise = [0, 0, 0] + [np.random.uniform(val - delta, val + delta) for val in self.default_ipos[3:]] 235 | 236 | fric_noise = [] 237 | translational = np.random.uniform(self.fric_low, self.fric_high) 238 | torsional = np.random.uniform(1e-4, 5e-4) 239 | rolling = np.random.uniform(1e-4, 2e-4) 240 | for _ in range(int(len(self.default_fric)/3)): 241 | fric_noise += [translational, torsional, rolling] 242 | 243 | self.sim.set_dof_damping(np.clip(damp_noise, 0, None)) 244 | self.sim.set_body_mass(np.clip(mass_noise, 0, None)) 245 | self.sim.set_body_ipos(com_noise) 246 | self.sim.set_geom_friction(np.clip(fric_noise, 0, None)) 247 | else: 248 | self.sim.set_body_mass(self.default_mass) 249 | self.sim.set_body_ipos(self.default_ipos) 250 | self.sim.set_dof_damping(self.default_damping) 251 | self.sim.set_geom_friction(self.default_fric) 252 | 253 | self.sim.set_geom_quat(self.default_quat) 254 | self.sim.set_const() 255 | return self.get_state() 256 | 257 | def get_state(self): 258 | self.qpos = np.copy(self.sim.qpos()) # dim=35 see cassiemujoco.h for details 259 | self.qvel = np.copy(self.sim.qvel()) # dim=32 260 | self.state_buffer.append((self.qpos, self.qvel)) 261 | 262 | if len(self.state_buffer) > self.buffer_size: 263 | self.state_buffer.pop(0) 264 | else: 265 | while len(self.state_buffer) < self.buffer_size: 266 | self.state_buffer.append((self.qpos, self.qvel)) 267 | 268 | pos = np.array([x[0] for x in self.state_buffer]) 269 | vel = np.array([x[1] for x in self.state_buffer]) 270 | 271 | self.ref_pos, self.ref_vel = self.get_kin_next_state() 272 | command = [self.speed, self.side_speed] 273 | ''' 274 | Position [1], [2] -> Pelvis y, z 275 | [3], [4], [5], [6] -> Pelvis Orientation qw, qx, qy, qz 276 | [7], [8], [9] -> Left Hip Roll (Motor[0]), Yaw (Motor[1]), Pitch (Motor[2]) 277 | [14] -> Left Knee (Motor[3]) 278 | [15] -> Left Shin (Joint[0]) 279 | [16] -> Left Tarsus (Joint[1]) 280 | [20] -> Left Foot (Motor[4], Joint[2]) 281 | [21], [22], [23] -> Right Hip Roll (Motor[5]), Yaw (Motor[6]), Pitch (Motor[7]) 282 | [28] -> Rigt Knee (Motor[8]) 283 | [29] -> Rigt Shin (Joint[3]) 284 | [30] -> Rigt Tarsus (Joint[4]) 285 | [34] -> Rigt Foot (Motor[9], Joint[5]) 286 | ''' 287 | pos_index = np.array([2,3,4,5,6,7,8,9,14,15,16,20,21,22,23,28,29,30,34]) 288 | ''' 289 | Velocity [0], [1], [2] -> Pelvis x, y, z 290 | [3], [4], [5] -> Pelvis Orientation wx, wy, wz 291 | [6], [7], [8] -> Left Hip Roll (Motor[0]), Yaw (Motor[1]), Pitch (Motor[2]) 292 | [12] -> Left Knee (Motor[3]) 293 | [13] -> Left Shin (Joint[0]) 294 | [14] -> Left Tarsus (Joint[1]) 295 | [18] -> Left Foot (Motor[4], Joint[2]) 296 | [19], [20], [21] -> Right Hip Roll (Motor[5]), Yaw (Motor[6]), Pitch (Motor[7]) 297 | [25] -> Rigt Knee (Motor[8]) 298 | [26] -> Rigt Shin (Joint[3]) 299 | [27] -> Rigt Tarsus (Joint[4]) 300 | [31] -> Rigt Foot (Motor[9], Joint[5]) 301 | ''' 302 | vel_index = np.array([0,1,2,3,4,5,6,7,8,12,13,14,18,19,20,21,25,26,27,31]) 303 | # next todo: x,y,z in state -> delta xyz to target + target velo 304 | return np.concatenate([pos[:,pos_index].reshape(-1), vel[:,vel_index].reshape(-1), 305 | [np.sin(self.phase/28*2*np.pi),np.cos(self.phase/28*2*np.pi)], 306 | # self.ref_pos[pos_index], self.ref_vel[vel_index] 307 | command 308 | ]) 309 | 310 | def compute_reward(self, action): 311 | ref_penalty = 0 312 | custom_footheight = np.array(self.custom_footheight()) 313 | real_footheight = np.array([self.foot_pos[2],self.foot_pos[5]]) 314 | ref_penalty = np.sum(np.square(custom_footheight - real_footheight)) 315 | ref_penalty = ref_penalty/0.0025 316 | 317 | orientation_penalty = (self.qpos[4])**2+(self.qpos[5])**2+(self.qpos[6])**2 318 | orientation_penalty = orientation_penalty/0.1 319 | 320 | vel_penalty = (self.speed - self.qvel[0])**2 + (self.side_speed - self.qvel[1])**2 + (self.qvel[2])**2 321 | vel_penalty = vel_penalty/max(0.5*(self.speed*self.speed+self.side_speed*self.side_speed),0.01) 322 | 323 | spring_penalty = (self.sim.qpos()[15])**2+(self.sim.qpos()[29])**2 324 | spring_penalty *= 1000 325 | 326 | rew_ref = 0.5*np.exp(-ref_penalty) 327 | rew_spring = 0.1*np.exp(-spring_penalty) 328 | rew_ori = 0.125*np.exp(-orientation_penalty) 329 | rew_vel = 0.375*np.exp(-vel_penalty) # 330 | rew_termin = -10 * self.termination 331 | 332 | R_star = 1 333 | Rp = (0.75 * np.exp(-vel_penalty) + 0.25 * np.exp(-orientation_penalty))/ R_star 334 | Ri = np.exp(-ref_penalty) / R_star 335 | Ri = (Ri-0.4)/(1.0-0.4) 336 | 337 | omega = 0.5 338 | 339 | reward = (1 - omega) * Ri + omega * Rp + rew_spring + rew_termin 340 | 341 | self.rew_ref += rew_ref 342 | self.rew_spring += rew_spring 343 | self.rew_ori += rew_ori 344 | self.rew_vel += rew_vel 345 | self.rew_termin += rew_termin 346 | self.reward += reward 347 | self.omega += omega 348 | 349 | return reward 350 | 351 | def render(self): 352 | return self.vis.draw(self.sim) 353 | 354 | def get_kin_state(self): 355 | pose = np.array([0]*3) 356 | vel = np.array([0]*3) 357 | pose[0] = self.speed * (self.counter * 28 + self.phase) * (self.simrate / 2000) 358 | pose[1] = self.side_speed * (self.counter * 28 + self.phase) * (self.simrate / 2000) 359 | pose[2] = 1.03 # 360 | vel[0] = self.speed 361 | return pose, vel 362 | 363 | def get_kin_next_state(self): 364 | phase = self.phase + 1 365 | counter = self.counter 366 | if phase >= 28: 367 | phase = 0 368 | counter += 1 369 | pose = np.array([0]*3) 370 | vel = np.array([0]*3) 371 | pose[0] = self.speed * (counter * 28 + phase) * (self.simrate / 2000) 372 | pose[1] = self.side_speed * (counter * 28 + phase) * (self.simrate / 2000) 373 | pose[2] = 1.03 # 374 | vel[0] = self.speed 375 | return pose, vel 376 | 377 | 378 | -------------------------------------------------------------------------------- /cassie_m/model/cassie_mass.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 274 | -------------------------------------------------------------------------------- /cassie_m/model/cassie_noise_terrain.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 273 | -------------------------------------------------------------------------------- /cassie_m/model/cassie_hfield.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 281 | -------------------------------------------------------------------------------- /cassie_m/model/cassie_slosh_mass.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 286 | -------------------------------------------------------------------------------- /cassie_m/model/cassie_tray_box.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 288 | -------------------------------------------------------------------------------- /cassie_m/model/cassiepole.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 290 | --------------------------------------------------------------------------------