├── .gitignore ├── cassie ├── meshes │ ├── foot.stl │ ├── knee.stl │ ├── shin.stl │ ├── hip-roll.stl │ ├── hip-yaw.stl │ ├── pelvis.stl │ ├── tarsus.stl │ ├── foot-crank.stl │ ├── hip-pitch.stl │ ├── achilles-rod.stl │ ├── heel-spring.stl │ ├── knee-spring.stl │ └── plantar-rod.stl ├── model.config ├── launch_sim.sh ├── cassie.world └── cassie.sdf ├── plugin ├── libagilitycassie.a ├── CMakeLists.txt ├── include │ ├── udp_lcm_translator.h │ ├── cassie_user_in_t.h │ ├── cassie_core_sim.h │ ├── cassie_in_t.h │ ├── udp.h │ ├── cassie_out_t.h │ └── CassiePlugin.hpp ├── example │ └── cassiectrl.c └── src │ ├── udp.c │ ├── udp_lcm_translator.cc │ └── CassiePlugin.cpp ├── cassie_fixed_base ├── meshes │ ├── foot.stl │ ├── knee.stl │ ├── shin.stl │ ├── hip-yaw.stl │ ├── pelvis.stl │ ├── tarsus.stl │ ├── hip-pitch.stl │ ├── hip-roll.stl │ ├── achilles-rod.stl │ ├── foot-crank.stl │ ├── heel-spring.stl │ ├── knee-spring.stl │ └── plantar-rod.stl ├── model.config ├── launch_sim.sh ├── cassie.world └── cassie.sdf └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | /build/ 2 | -------------------------------------------------------------------------------- /cassie/meshes/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/foot.stl -------------------------------------------------------------------------------- /cassie/meshes/knee.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/knee.stl -------------------------------------------------------------------------------- /cassie/meshes/shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/shin.stl -------------------------------------------------------------------------------- /cassie/meshes/hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/hip-roll.stl -------------------------------------------------------------------------------- /cassie/meshes/hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/hip-yaw.stl -------------------------------------------------------------------------------- /cassie/meshes/pelvis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/pelvis.stl -------------------------------------------------------------------------------- /cassie/meshes/tarsus.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/tarsus.stl -------------------------------------------------------------------------------- /plugin/libagilitycassie.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/plugin/libagilitycassie.a -------------------------------------------------------------------------------- /cassie/meshes/foot-crank.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/foot-crank.stl -------------------------------------------------------------------------------- /cassie/meshes/hip-pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/hip-pitch.stl -------------------------------------------------------------------------------- /cassie/meshes/achilles-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/achilles-rod.stl -------------------------------------------------------------------------------- /cassie/meshes/heel-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/heel-spring.stl -------------------------------------------------------------------------------- /cassie/meshes/knee-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/knee-spring.stl -------------------------------------------------------------------------------- /cassie/meshes/plantar-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie/meshes/plantar-rod.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/foot.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/knee.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/knee.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/shin.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/hip-yaw.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/pelvis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/pelvis.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/tarsus.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/tarsus.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/hip-pitch.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/hip-pitch.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/hip-roll.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/achilles-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/achilles-rod.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/foot-crank.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/foot-crank.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/heel-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/heel-spring.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/knee-spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/knee-spring.stl -------------------------------------------------------------------------------- /cassie_fixed_base/meshes/plantar-rod.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DAIRLab/cassie-gazebo-sim/HEAD/cassie_fixed_base/meshes/plantar-rod.stl -------------------------------------------------------------------------------- /cassie/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cassie 5 | 1.0 6 | cassie.sdf 7 | 8 | 9 | -------------------------------------------------------------------------------- /cassie_fixed_base/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cassie 5 | 1.0 6 | cassie.sdf 7 | 8 | 9 | -------------------------------------------------------------------------------- /cassie/launch_sim.sh: -------------------------------------------------------------------------------- 1 | source /usr/share/gazebo/setup.sh 2 | cd "${0%/*}" 3 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$PWD/.. 4 | export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:$PWD/../build 5 | export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:$PWD/../build 6 | gazebo cassie.world -------------------------------------------------------------------------------- /cassie_fixed_base/launch_sim.sh: -------------------------------------------------------------------------------- 1 | source /usr/share/gazebo/setup.sh 2 | cd "${0%/*}" 3 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$PWD/.. 4 | export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:$PWD/../build 5 | export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:$PWD/../build 6 | gazebo cassie.world -------------------------------------------------------------------------------- /plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | find_package(gazebo REQUIRED) 4 | find_package(lcm REQUIRED) 5 | include_directories(${GAZEBO_INCLUDE_DIRS}) 6 | link_directories(${GAZEBO_LIBRARY_DIRS}) 7 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 8 | 9 | add_library(cassie_plugin SHARED src/CassiePlugin.cpp src/udp.c src/udp_lcm_translator.cc) 10 | include_directories(include) 11 | include_directories(../../dairlib/bazel-bin/lcmtypes) 12 | target_link_libraries(cassie_plugin ${GAZEBO_LIBRARIES} ${CMAKE_SOURCE_DIR}/libagilitycassie.a) 13 | target_link_libraries(cassie_plugin ${LCM_NAMESPACE}lcm) 14 | 15 | add_executable(cassiectrl example/cassiectrl.c) 16 | target_link_libraries(cassiectrl cassie_plugin) 17 | -------------------------------------------------------------------------------- /plugin/include/udp_lcm_translator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "cassie_out_t.h" 4 | #include "cassie_user_in_t.h" 5 | #include "dairlib/lcmt_cassie_out.hpp" 6 | #include "dairlib/lcmt_cassie_in.hpp" 7 | 8 | namespace dairlib { 9 | 10 | // Convert from an LCM message, dairlib::lcmt_cassie_out, to the Agility 11 | // cassie_out_t struct 12 | void cassieOutFromLcm(const lcmt_cassie_out& message, 13 | cassie_out_t* cassie_out); 14 | 15 | // Convert from Agility cassie_out_t struct to LCM message, 16 | // dairlib::lcmt_cassie_out. Since the struct does not include time, time (s) 17 | // is a required additional input 18 | void cassieOutToLcm(const cassie_out_t& cassie_out, double time_seconds, 19 | lcmt_cassie_out* message); 20 | 21 | // Convert from Agility cassie_user_in_t struct to LCM message, 22 | // dairlib::lcmt_cassie_in. Since the struct does not include time, time (s) 23 | // is a required additional input 24 | void cassieInToLcm(const cassie_user_in_t& cassie_in, double time_seconds, 25 | lcmt_cassie_in* message); 26 | 27 | // Convert from an LCM message, dairlib::lcmt_cassie_in to 28 | // Agility cassie_user_in_t struct 29 | void cassieInFromLcm(const lcmt_cassie_in& message, 30 | cassie_user_in_t* cassie_in); 31 | 32 | } // namespace dairlib 33 | -------------------------------------------------------------------------------- /cassie/cassie.world: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 0.0002 22 | 2000 23 | 24 | 25 | model://sun 26 | 27 | 28 | model://ground_plane 29 | 30 | 31 | model://cassie 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /cassie_fixed_base/cassie.world: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 0.0002 22 | 2000 23 | 24 | 25 | model://sun 26 | 27 | 28 | model://ground_plane 29 | 30 | 31 | model://cassie_fixed_base 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /plugin/include/cassie_user_in_t.h: -------------------------------------------------------------------------------- 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 | #ifndef CASSIE_USER_IN_T_H 18 | #define CASSIE_USER_IN_T_H 19 | 20 | #define CASSIE_USER_IN_T_PACKED_LEN 58 21 | 22 | #include 23 | 24 | typedef struct { 25 | double torque[10]; 26 | short telemetry[9]; 27 | } cassie_user_in_t; 28 | 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | void pack_cassie_user_in_t(const cassie_user_in_t *bus, unsigned char *bytes); 35 | void unpack_cassie_user_in_t(const unsigned char *bytes, cassie_user_in_t *bus); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | #endif // CASSIE_USER_IN_T_H 41 | -------------------------------------------------------------------------------- /plugin/include/cassie_core_sim.h: -------------------------------------------------------------------------------- 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 | #ifndef CASSIE_CORE_SIM_H 18 | #define CASSIE_CORE_SIM_H 19 | 20 | #include "cassie_user_in_t.h" 21 | #include "cassie_out_t.h" 22 | #include "cassie_in_t.h" 23 | 24 | typedef struct CassieCoreSim cassie_core_sim_t; 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | cassie_core_sim_t* cassie_core_sim_alloc(void); 31 | void cassie_core_sim_copy(cassie_core_sim_t *dst, const cassie_core_sim_t *src); 32 | void cassie_core_sim_free(cassie_core_sim_t *sys); 33 | void cassie_core_sim_setup(cassie_core_sim_t *sys); 34 | void cassie_core_sim_step(cassie_core_sim_t *sys, const cassie_user_in_t *in1, 35 | const cassie_out_t *in2, cassie_in_t *out1); 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | #endif // CASSIE_CORE_SIM_H 41 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # agility-cassie-gazebo-simulator 2 | 3 | A simulation library for Agility Robotics' Cassie robot using Gazebo. 4 | Developed and tested on Ubuntu 18.04 with Gazebo 9.9 5 | 6 | This has been modified from the original Agility Robotics library to be used alongside **dairlib**. 7 | ## Instructions: 8 | 9 | ### Building the code: 10 | * Download and build **dairlib** (https://github.com/DAIRLab/dairlib). NOTE: this is needed for the LCM types only. 11 | * Download and install Gazebo 9 - http://gazebosim.org/tutorials?tut=install_ubuntu 12 | * Clone this repository into the same root directory as **dairlib**. 13 | * Make a build directory 14 | * `cd build` 15 | * `cmake ../plugin` 16 | * `make` 17 | * `source /usr/share/gazebo/setup.sh` 18 | * `export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/cassie-gazebo-sim/` 19 | * `export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/cassie-gazebo-sim/build` 20 | * `export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/cassie-gazebo-sim/build` 21 | * From the `cassie-gazebo-sim/cassie` directory, run `gazebo cassie.world` to test if the installation is successful. 22 | 23 | Note: The Gazebo plugin and model path needs to be updated to load the meshes and plugins - http://gazebosim.org/tutorials?tut=components 24 | 25 | ### Running the sim: 26 | * `cd ~/path-to-repo/cassie` 27 | * ` gazebo cassie.world` 28 | 29 | The robot should be spawned in Gazebo 30 | 31 | ### Connecting to the sim with a demo UDP based controller: 32 | * `cd ~/path-to-repo/your-build-folder` 33 | * `./cassiectrl` 34 | 35 | The controller should be connected to the simulator. 36 | 37 | The examples include cassiectrl, a null controller operating over UDP. 38 | -------------------------------------------------------------------------------- /plugin/include/cassie_in_t.h: -------------------------------------------------------------------------------- 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 | #ifndef CASSIE_IN_T_H 18 | #define CASSIE_IN_T_H 19 | 20 | #define CASSIE_IN_T_PACKED_LEN 91 21 | 22 | #include 23 | 24 | typedef struct { 25 | unsigned short controlWord; 26 | double torque; 27 | } elmo_in_t; 28 | 29 | typedef struct { 30 | elmo_in_t hipRollDrive; 31 | elmo_in_t hipYawDrive; 32 | elmo_in_t hipPitchDrive; 33 | elmo_in_t kneeDrive; 34 | elmo_in_t footDrive; 35 | } cassie_leg_in_t; 36 | 37 | typedef struct { 38 | short channel[14]; 39 | } radio_in_t; 40 | 41 | typedef struct { 42 | radio_in_t radio; 43 | bool sto; 44 | bool piezoState; 45 | unsigned char piezoTone; 46 | } cassie_pelvis_in_t; 47 | 48 | typedef struct { 49 | cassie_pelvis_in_t pelvis; 50 | cassie_leg_in_t leftLeg; 51 | cassie_leg_in_t rightLeg; 52 | } cassie_in_t; 53 | 54 | 55 | #ifdef __cplusplus 56 | extern "C" { 57 | #endif 58 | 59 | void pack_cassie_in_t(const cassie_in_t *bus, unsigned char *bytes); 60 | void unpack_cassie_in_t(const unsigned char *bytes, cassie_in_t *bus); 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | #endif // CASSIE_IN_T_H 66 | -------------------------------------------------------------------------------- /plugin/include/udp.h: -------------------------------------------------------------------------------- 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 | #ifndef UDP_H 18 | #define UDP_H 19 | 20 | #define PACKET_HEADER_LEN 2 21 | 22 | // Data and results for processing packet header 23 | typedef struct { 24 | char seq_num_out; 25 | char seq_num_in_last; 26 | char delay; 27 | char seq_num_in_diff; 28 | } packet_header_info_t; 29 | 30 | #ifdef __cplusplus 31 | extern "C" { 32 | #endif 33 | 34 | // Process packet header used to measure delay and skipped packets 35 | void process_packet_header(packet_header_info_t *info, 36 | const unsigned char *header_in, 37 | unsigned char *header_out); 38 | 39 | #ifdef _WIN32 40 | #include 41 | #else 42 | #include 43 | #endif 44 | 45 | // Create a UDP socket listening at a specific address/port 46 | int udp_init_host(const char *addr_str, const char *port_str); 47 | 48 | // Create a UDP socket connected and listening to specific addresses/ports 49 | int udp_init_client(const char *remote_addr_str, const char *remote_port_str, 50 | const char *local_addr_str, const char *local_port_str); 51 | 52 | // Close a UDP socket 53 | void udp_close(int sock); 54 | 55 | // Get newest valid packet in RX buffer 56 | ssize_t get_newest_packet(int sock, void *recvbuf, size_t recvlen, 57 | struct sockaddr *src_addr, socklen_t *addrlen); 58 | 59 | // Wait for a new valid packet 60 | ssize_t wait_for_packet(int sock, void *recvbuf, size_t recvlen, 61 | struct sockaddr *src_addr, socklen_t *addrlen); 62 | 63 | // Send a packet 64 | ssize_t send_packet(int sock, void *sendbuf, size_t sendlen, 65 | struct sockaddr *dst_addr, socklen_t addrlen); 66 | 67 | #ifdef __cplusplus 68 | } 69 | #endif 70 | 71 | #endif // UDP_H 72 | -------------------------------------------------------------------------------- /plugin/example/cassiectrl.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 | #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 "udp.h" 26 | 27 | 28 | int main(int argc, char *argv[]) 29 | { 30 | // Option variables and flags 31 | char *remote_addr_str = "127.0.0.1"; 32 | char *remote_port_str = "25000"; 33 | char *iface_addr_str = "0.0.0.0"; 34 | char *iface_port_str = "25001"; 35 | 36 | // Parse arguments 37 | int c; 38 | while ((c = getopt(argc, argv, "a:p:b:c:")) != -1) { 39 | switch (c) { 40 | case 'a': 41 | // Remote address to connect to 42 | remote_addr_str = optarg; 43 | break; 44 | case 'p': 45 | // Port to connect to 46 | remote_port_str = optarg; 47 | break; 48 | case 'b': 49 | // Remote address to connect to 50 | iface_addr_str = optarg; 51 | break; 52 | case 'c': 53 | // Port to connect to 54 | iface_port_str = optarg; 55 | break; 56 | default: 57 | // Print usage 58 | printf( 59 | "Usage: cassiectrl [OPTION]...\n" 60 | "Controls the Cassie robot over UDP.\n" 61 | "\n" 62 | " -a [ADDRESS] Specify the local interface to bind to.\n" 63 | " -p [PORT] Specify the port to listen on.\n" 64 | " -b [ADDRESS] Specify the remote address to connect to.\n" 65 | " -c [PORT] Specify the remote port to connect to.\n" 66 | "\n" 67 | "By default, the controller connects to localhost over IPv4 to port %s" 68 | "from port %s.\n\n", 69 | remote_port_str, iface_port_str); 70 | exit(EXIT_SUCCESS); 71 | } 72 | } 73 | 74 | // Bind to network interface 75 | int sock = udp_init_client(remote_addr_str, remote_port_str, 76 | iface_addr_str, iface_port_str); 77 | if (-1 == sock) 78 | exit(EXIT_FAILURE); 79 | 80 | // Create packet input/output buffers 81 | const int dinlen = CASSIE_OUT_T_PACKED_LEN; 82 | const int doutlen = CASSIE_USER_IN_T_PACKED_LEN; 83 | const int recvlen = PACKET_HEADER_LEN + dinlen; 84 | const int sendlen = PACKET_HEADER_LEN + doutlen; 85 | unsigned char *recvbuf = malloc(recvlen); 86 | unsigned char *sendbuf = malloc(sendlen); 87 | 88 | // Separate input/output buffers into header and payload 89 | const unsigned char *header_in = recvbuf; 90 | const unsigned char *data_in = &recvbuf[PACKET_HEADER_LEN]; 91 | unsigned char *header_out = sendbuf; 92 | unsigned char *data_out = &sendbuf[PACKET_HEADER_LEN]; 93 | 94 | // Create standard input/output structs 95 | cassie_user_in_t cassie_user_in = {0}; 96 | cassie_out_t cassie_out; 97 | 98 | // Create header information struct 99 | packet_header_info_t header_info = {0}; 100 | 101 | // Prepare initial null command packet to start communication 102 | printf("Connecting to cassie...\n"); 103 | memset(sendbuf, 0, sendlen); 104 | bool received_data = false; 105 | 106 | // Listen/respond loop 107 | while (true) { 108 | if (!received_data) { 109 | // Send null commands until the simulator responds 110 | ssize_t nbytes; 111 | do { 112 | send_packet(sock, sendbuf, sendlen, NULL, 0); 113 | usleep(1000); 114 | nbytes = get_newest_packet(sock, recvbuf, recvlen, NULL, NULL); 115 | } while (recvlen != nbytes); 116 | received_data = true; 117 | printf("Connected!\n\n"); 118 | } else { 119 | // Wait for a new packet 120 | wait_for_packet(sock, recvbuf, recvlen, NULL, NULL); 121 | } 122 | 123 | // Process incoming header and write outgoing header 124 | process_packet_header(&header_info, header_in, header_out); 125 | printf("\033[F\033[Jdelay: %d, diff: %d\n", 126 | header_info.delay, header_info.seq_num_in_diff); 127 | 128 | // Unpack received data into cassie user input struct 129 | unpack_cassie_out_t(data_in, &cassie_out); 130 | 131 | // Run controller 132 | // Do nothing in this example 133 | 134 | // Pack cassie out struct into outgoing packet 135 | pack_cassie_user_in_t(&cassie_user_in, data_out); 136 | 137 | // Send response 138 | send_packet(sock, sendbuf, sendlen, NULL, 0); 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /plugin/include/cassie_out_t.h: -------------------------------------------------------------------------------- 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 | #ifndef CASSIE_OUT_T_H 18 | #define CASSIE_OUT_T_H 19 | 20 | #define CASSIE_OUT_T_PACKED_LEN 697 21 | 22 | #include 23 | 24 | typedef short DiagnosticCodes; 25 | 26 | 27 | typedef struct { 28 | bool dataGood; 29 | double stateOfCharge; 30 | double voltage[12]; 31 | double current; 32 | double temperature[4]; 33 | } battery_out_t; 34 | 35 | typedef struct { 36 | double position; 37 | double velocity; 38 | } cassie_joint_out_t; 39 | 40 | typedef struct { 41 | unsigned short statusWord; 42 | double position; 43 | double velocity; 44 | double torque; 45 | double driveTemperature; 46 | double dcLinkVoltage; 47 | double torqueLimit; 48 | double gearRatio; 49 | } elmo_out_t; 50 | 51 | typedef struct { 52 | elmo_out_t hipRollDrive; 53 | elmo_out_t hipYawDrive; 54 | elmo_out_t hipPitchDrive; 55 | elmo_out_t kneeDrive; 56 | elmo_out_t footDrive; 57 | cassie_joint_out_t shinJoint; 58 | cassie_joint_out_t tarsusJoint; 59 | cassie_joint_out_t footJoint; 60 | unsigned char medullaCounter; 61 | unsigned short medullaCpuLoad; 62 | bool reedSwitchState; 63 | } cassie_leg_out_t; 64 | 65 | typedef struct { 66 | bool radioReceiverSignalGood; 67 | bool receiverMedullaSignalGood; 68 | double channel[16]; 69 | } radio_out_t; 70 | 71 | typedef struct { 72 | int etherCatStatus[6]; 73 | int etherCatNotifications[21]; 74 | double taskExecutionTime; 75 | unsigned int overloadCounter; 76 | double cpuTemperature; 77 | } target_pc_out_t; 78 | 79 | typedef struct { 80 | bool dataGood; 81 | unsigned short vpeStatus; 82 | double pressure; 83 | double temperature; 84 | double magneticField[3]; 85 | double angularVelocity[3]; 86 | double linearAcceleration[3]; 87 | double orientation[4]; 88 | } vectornav_out_t; 89 | 90 | typedef struct { 91 | target_pc_out_t targetPc; 92 | battery_out_t battery; 93 | radio_out_t radio; 94 | vectornav_out_t vectorNav; 95 | unsigned char medullaCounter; 96 | unsigned short medullaCpuLoad; 97 | bool bleederState; 98 | bool leftReedSwitchState; 99 | bool rightReedSwitchState; 100 | double vtmTemperature; 101 | } cassie_pelvis_out_t; 102 | 103 | typedef struct { 104 | cassie_pelvis_out_t pelvis; 105 | cassie_leg_out_t leftLeg; 106 | cassie_leg_out_t rightLeg; 107 | bool isCalibrated; 108 | DiagnosticCodes messages[4]; 109 | } cassie_out_t; 110 | 111 | #define EMPTY ((DiagnosticCodes)0) 112 | #define LEFT_HIP_NOT_CALIB ((DiagnosticCodes)5) 113 | #define LEFT_KNEE_NOT_CALIB ((DiagnosticCodes)6) 114 | #define RIGHT_HIP_NOT_CALIB ((DiagnosticCodes)7) 115 | #define RIGHT_KNEE_NOT_CALIB ((DiagnosticCodes)8) 116 | #define ETHERCAT_GENERIC ((DiagnosticCodes)10) 117 | #define ETHERCAT_HOTCONNECT ((DiagnosticCodes)11) 118 | #define LOW_BATTERY_CHARGE ((DiagnosticCodes)200) 119 | #define HIGH_CPU_TEMP ((DiagnosticCodes)205) 120 | #define HIGH_VTM_TEMP ((DiagnosticCodes)210) 121 | #define HIGH_ELMO_DRIVE_TEMP ((DiagnosticCodes)215) 122 | #define HIGH_STATOR_TEMP ((DiagnosticCodes)220) 123 | #define LOW_ELMO_LINK_VOLTAGE ((DiagnosticCodes)221) 124 | #define HIGH_BATTERY_TEMP ((DiagnosticCodes)225) 125 | #define RADIO_DATA_BAD ((DiagnosticCodes)230) 126 | #define RADIO_SIGNAL_BAD ((DiagnosticCodes)231) 127 | #define BMS_DATA_BAD ((DiagnosticCodes)235) 128 | #define VECTORNAV_DATA_BAD ((DiagnosticCodes)236) 129 | #define VPE_GYRO_SATURATION ((DiagnosticCodes)240) 130 | #define VPE_MAG_SATURATION ((DiagnosticCodes)241) 131 | #define VPE_ACC_SATURATION ((DiagnosticCodes)242) 132 | #define VPE_ATTITUDE_BAD ((DiagnosticCodes)245) 133 | #define VPE_ATTITUDE_NOT_TRACKING ((DiagnosticCodes)246) 134 | #define ETHERCAT_DC_ERROR ((DiagnosticCodes)400) 135 | #define ETHERCAT_ERROR ((DiagnosticCodes)410) 136 | #define ETHERCAT_SCANBUS ((DiagnosticCodes)411) 137 | #define LOAD_CALIB_DATA_ERROR ((DiagnosticCodes)590) 138 | #define CRITICAL_BATTERY_CHARGE ((DiagnosticCodes)600) 139 | #define CRITICAL_CPU_TEMP ((DiagnosticCodes)605) 140 | #define CRITICAL_VTM_TEMP ((DiagnosticCodes)610) 141 | #define CRITICAL_ELMO_DRIVE_TEMP ((DiagnosticCodes)615) 142 | #define CRITICAL_STATOR_TEMP ((DiagnosticCodes)620) 143 | #define CRITICAL_BATTERY_TEMP ((DiagnosticCodes)625) 144 | #define TORQUE_LIMIT_REACHED ((DiagnosticCodes)630) 145 | #define JOINT_LIMIT_REACHED ((DiagnosticCodes)635) 146 | #define ENCODER_FAILURE ((DiagnosticCodes)640) 147 | #define SPRING_FAILURE ((DiagnosticCodes)645) 148 | #define LEFT_LEG_MEDULLA_HANG ((DiagnosticCodes)700) 149 | #define RIGHT_LEG_MEDULLA_HANG ((DiagnosticCodes)701) 150 | #define PELVIS_MEDULLA_HANG ((DiagnosticCodes)703) 151 | #define CPU_OVERLOAD ((DiagnosticCodes)704) 152 | 153 | #ifdef __cplusplus 154 | extern "C" { 155 | #endif 156 | 157 | void pack_cassie_out_t(const cassie_out_t *bus, unsigned char *bytes); 158 | void unpack_cassie_out_t(const unsigned char *bytes, cassie_out_t *bus); 159 | 160 | #ifdef __cplusplus 161 | } 162 | #endif 163 | #endif // CASSIE_OUT_T_H 164 | -------------------------------------------------------------------------------- /plugin/include/CassiePlugin.hpp: -------------------------------------------------------------------------------- 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 | #ifndef CASSIEPLUGIN 18 | #define CASSIEPLUGIN 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "cassie_core_sim.h" 30 | #include "udp.h" 31 | 32 | #define RECVLEN (PACKET_HEADER_LEN + CASSIE_USER_IN_T_PACKED_LEN) 33 | #define SENDLEN (PACKET_HEADER_LEN + CASSIE_OUT_T_PACKED_LEN) 34 | 35 | #define MOTOR_FILTER_NB 9 36 | #define JOINT_FILTER_NB 4 37 | #define JOINT_FILTER_NA 3 38 | 39 | 40 | /** 41 | * @brief The CassiePlugin class contains vars and methods used by 42 | * Gazebo to load the plugin and communicate over UDP 43 | */ 44 | class CassiePlugin : public gazebo::ModelPlugin { 45 | 46 | // Event pointer 47 | gazebo::event::ConnectionPtr updateConnectionPtr_; 48 | 49 | // World Pointer 50 | gazebo::physics::WorldPtr worldPtr_; 51 | 52 | // Cassie core 53 | cassie_core_sim_t *corePtr_; 54 | 55 | // Motor Filter 56 | int motorFilterB_[MOTOR_FILTER_NB]; 57 | typedef struct motor_filter { 58 | int x[MOTOR_FILTER_NB]; 59 | } motor_filter_t; 60 | 61 | // Joint Filter 62 | double jointFilterB_[JOINT_FILTER_NB]; 63 | double jointFilterA_[JOINT_FILTER_NA]; 64 | typedef struct joint_filter { 65 | double x[JOINT_FILTER_NB]; 66 | double y[JOINT_FILTER_NA]; 67 | } joint_filter_t; 68 | 69 | // Motor properties 70 | std::array motor_; 71 | std::array driveOut_; 72 | std::array motorFilter_; 73 | const std::array kMotorBits_; 74 | const std::array kGearRatio_; 75 | const std::array kMaxSpeed_; 76 | const std::array kMaxTorque_; 77 | const std::array kMotorOffset_; 78 | 79 | // Joint properties 80 | std::array joint_; 81 | std::array jointOut_; 82 | std::array jointFilter_; 83 | const std::array kJointBits_; 84 | const std::array kJointOffset_; 85 | 86 | // Pelvis pointer 87 | gazebo::physics::LinkPtr pelvisPtr_; 88 | 89 | // Latency buffer for cassie out 90 | cassie_out_t cassieOut_; 91 | 92 | // Update Rate 93 | double updateRate_; 94 | double updatePeriod_; 95 | gazebo::common::Time lastUpdateTime_; 96 | 97 | // UDP socket 98 | int sock_; 99 | 100 | // Send/receive buffers 101 | unsigned char recvBuf_[RECVLEN]; 102 | unsigned char sendBuf_[SENDLEN]; 103 | 104 | // Pointers to parts of buffers 105 | unsigned char *headerInPtr_; 106 | unsigned char *dataInPtr_; 107 | unsigned char *headerOutPtr_; 108 | unsigned char *dataOutPtr_; 109 | 110 | // Other UDP structures 111 | packet_header_info_t headerInfo_; 112 | cassie_user_in_t cassieUserIn_; 113 | struct sockaddr_storage srcAddr_; 114 | socklen_t addrLen_; 115 | gazebo::common::Time lastPacketTime_; 116 | gazebo::common::Time firstPacketTime_; 117 | struct sockaddr * dispatch_addr_; 118 | 119 | lcm::LCM lcm; 120 | 121 | // Sim flag 122 | bool runSim_; 123 | 124 | /** 125 | * @brief setMotorEncoder reads the drive position and sets motor encoder values 126 | * @param drive is the data struct with drive information 127 | * @param position is the drive position 128 | * @param filter is the motor filter pointer 129 | * @param bits number of bits for the drive encoder 130 | * @param ratio gear ratio 131 | */ 132 | void setMotorEncoder(elmo_out_t *drive, double position, 133 | motor_filter_t *filter,const int bits); 134 | 135 | /** 136 | * @brief setJointEncoder reads the joint position and sets joint encoder value 137 | * @param joint is the data struct with joint information 138 | * @param position is the joint position 139 | * @param filter is the joint filter pointer 140 | * @param bits number of bits for the joint encoder 141 | */ 142 | void setJointEncoder(cassie_joint_out_t *joint, double position, 143 | joint_filter_t *filter,const int bits); 144 | 145 | /** 146 | * @brief setMotor commands motor torques 147 | * @param outjoint is the joint pointer 148 | * @param u is calculated torque 149 | * @param sto STO switch 150 | * @param ratio is the gear ratio 151 | * @param tmax is the maximum safe torque 152 | * @param wmax is the max motor speed 153 | * @return 154 | */ 155 | double setMotor(gazebo::physics::JointPtr outJoint, double u, 156 | const bool sto, const double ratio, 157 | const double tmax, const double wmax); 158 | 159 | /** 160 | * @brief initElmoOut sets the elmo drive params 161 | * @param elmoOut elmo pointer 162 | * @param torqueLimit 163 | * @param gearRatio 164 | */ 165 | void initElmoOut(elmo_out_t *elmoOut, const double torqueLimit, 166 | const double gearRatio); 167 | 168 | /** 169 | * @brief initCassieLegOut sets the Cassie Leg Out params 170 | * @param legOut cassie leg out pointer 171 | */ 172 | void initCassieLegOut(cassie_leg_out_t *legOut); 173 | 174 | /** 175 | * @brief initCassieOut sets Cassie Out struct params 176 | * @param cassieOut pointer to Cassie Out 177 | */ 178 | void initCassieOut(cassie_out_t *cassieOut); 179 | 180 | /** 181 | * @brief Load is called when the plugin is loaded in the sdf 182 | * @param _model pointer to the model 183 | * @param _sdf pointer to the SDF 184 | */ 185 | virtual void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf); 186 | 187 | /** 188 | * @brief onUpdate is called after every simulation update 189 | */ 190 | void onUpdate(); 191 | 192 | /** 193 | * @brief applyTorques is used to apply Torques to all the drives 194 | * @param cassieIn is the data struct which is fed to the robot 195 | */ 196 | void applyTorques(const cassie_in_t* cassieIn); 197 | 198 | /** 199 | * @brief updateCassieOut updates the CassieOut data struct 200 | */ 201 | void updateCassieOut(); 202 | 203 | /** 204 | * @brief detachPelvis detaches the pelvis from the world 205 | */ 206 | void detachPelvis(); 207 | 208 | public: 209 | /** 210 | * @brief CassiePlugin constructor for the plugin class 211 | */ 212 | CassiePlugin(); 213 | 214 | /** 215 | * @brief CassiePlugin destructor for the plugin class 216 | */ 217 | ~CassiePlugin(); 218 | 219 | 220 | }; 221 | 222 | #endif // CASSIEPLUGIN 223 | 224 | -------------------------------------------------------------------------------- /plugin/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 | -------------------------------------------------------------------------------- /plugin/src/udp_lcm_translator.cc: -------------------------------------------------------------------------------- 1 | #include "udp_lcm_translator.h" 2 | 3 | namespace dairlib { 4 | 5 | void copy_elmo(const elmo_out_t& input, lcmt_elmo_out* output); 6 | void copy_leg(const cassie_leg_out_t& input, lcmt_cassie_leg_out* output); 7 | template void copy_vector(const T* input, T* output, int size); 8 | 9 | // Utility methods for copying data 10 | void copy_elmo(const lcmt_elmo_out& input, elmo_out_t* output) { 11 | output->statusWord = input.statusWord; 12 | output->position = input.position; 13 | output->velocity = input.velocity; 14 | output->torque = input.torque; 15 | output->driveTemperature = input.driveTemperature; 16 | output->dcLinkVoltage = input.dcLinkVoltage; 17 | output->torqueLimit = input.torqueLimit; 18 | output->gearRatio = input.gearRatio; 19 | } 20 | 21 | void copy_elmo(const elmo_out_t& input, lcmt_elmo_out* output) { 22 | output->statusWord = input.statusWord; 23 | output->position = input.position; 24 | output->velocity = input.velocity; 25 | output->torque = input.torque; 26 | output->driveTemperature = input.driveTemperature; 27 | output->dcLinkVoltage = input.dcLinkVoltage; 28 | output->torqueLimit = input.torqueLimit; 29 | output->gearRatio = input.gearRatio; 30 | } 31 | 32 | 33 | void copy_leg(const lcmt_cassie_leg_out& input, cassie_leg_out_t* output) { 34 | copy_elmo(input.hipRollDrive, &output->hipRollDrive); 35 | copy_elmo(input.hipYawDrive, &output->hipYawDrive); 36 | copy_elmo(input.hipPitchDrive, &output->hipPitchDrive); 37 | copy_elmo(input.kneeDrive, &output->kneeDrive); 38 | copy_elmo(input.footDrive, &output->footDrive); 39 | output->shinJoint.position = input.shinJoint.position; 40 | output->shinJoint.velocity = input.shinJoint.velocity; 41 | output->tarsusJoint.position = input.tarsusJoint.position; 42 | output->tarsusJoint.velocity = input.tarsusJoint.velocity; 43 | output->footJoint.position = input.footJoint.position; 44 | output->footJoint.velocity = input.footJoint.velocity; 45 | output->medullaCounter = input.medullaCounter; 46 | output->medullaCpuLoad = input.medullaCpuLoad; 47 | output->reedSwitchState = input.reedSwitchState; 48 | } 49 | 50 | void copy_leg(const cassie_leg_out_t& input, lcmt_cassie_leg_out* output) { 51 | copy_elmo(input.hipRollDrive, &output->hipRollDrive); 52 | copy_elmo(input.hipYawDrive, &output->hipYawDrive); 53 | copy_elmo(input.hipPitchDrive, &output->hipPitchDrive); 54 | copy_elmo(input.kneeDrive, &output->kneeDrive); 55 | copy_elmo(input.footDrive, &output->footDrive); 56 | output->shinJoint.position = input.shinJoint.position; 57 | output->shinJoint.velocity = input.shinJoint.velocity; 58 | output->tarsusJoint.position = input.tarsusJoint.position; 59 | output->tarsusJoint.velocity = input.tarsusJoint.velocity; 60 | output->footJoint.position = input.footJoint.position; 61 | output->footJoint.velocity = input.footJoint.velocity; 62 | output->medullaCounter = input.medullaCounter; 63 | output->medullaCpuLoad = input.medullaCpuLoad; 64 | output->reedSwitchState = input.reedSwitchState; 65 | } 66 | 67 | template 68 | void copy_vector(const T* input, T* output, int size) { 69 | for (int i = 0; i < size; i++) { 70 | output[i] = input[i]; 71 | } 72 | } 73 | 74 | void cassieOutFromLcm(const lcmt_cassie_out& message, 75 | cassie_out_t* cassie_out) { 76 | // copy pelvis 77 | copy_vector(message.pelvis.targetPc.etherCatStatus, 78 | cassie_out->pelvis.targetPc.etherCatStatus, 6); 79 | copy_vector(message.pelvis.targetPc.etherCatNotifications, 80 | cassie_out->pelvis.targetPc.etherCatNotifications, 21); 81 | 82 | cassie_out->pelvis.targetPc.taskExecutionTime = 83 | message.pelvis.targetPc.taskExecutionTime; 84 | cassie_out->pelvis.targetPc.overloadCounter = 85 | message.pelvis.targetPc.overloadCounter; 86 | 87 | cassie_out->pelvis.targetPc.cpuTemperature = 88 | message.pelvis.targetPc.cpuTemperature; 89 | 90 | cassie_out->pelvis.battery.dataGood = 91 | message.pelvis.battery.dataGood; 92 | cassie_out->pelvis.battery.stateOfCharge = 93 | message.pelvis.battery.stateOfCharge; 94 | cassie_out->pelvis.battery.current = 95 | message.pelvis.battery.current; 96 | copy_vector(message.pelvis.battery.voltage, 97 | cassie_out->pelvis.battery.voltage, 12); 98 | copy_vector(message.pelvis.battery.temperature, 99 | cassie_out->pelvis.battery.temperature, 4); 100 | 101 | cassie_out->pelvis.radio.radioReceiverSignalGood = 102 | message.pelvis.radio.radioReceiverSignalGood; 103 | cassie_out->pelvis.radio.receiverMedullaSignalGood = 104 | message.pelvis.radio.receiverMedullaSignalGood; 105 | copy_vector(message.pelvis.radio.channel, 106 | cassie_out->pelvis.radio.channel, 16); 107 | 108 | cassie_out->pelvis.vectorNav.dataGood = 109 | message.pelvis.vectorNav.dataGood; 110 | cassie_out->pelvis.vectorNav.vpeStatus = 111 | message.pelvis.vectorNav.vpeStatus; 112 | cassie_out->pelvis.vectorNav.pressure = 113 | message.pelvis.vectorNav.pressure; 114 | cassie_out->pelvis.vectorNav.temperature = 115 | message.pelvis.vectorNav.temperature; 116 | copy_vector(message.pelvis.vectorNav.magneticField, 117 | cassie_out->pelvis.vectorNav.magneticField, 3); 118 | copy_vector(message.pelvis.vectorNav.angularVelocity, 119 | cassie_out->pelvis.vectorNav.angularVelocity, 3); 120 | copy_vector(message.pelvis.vectorNav.linearAcceleration, 121 | cassie_out->pelvis.vectorNav.linearAcceleration, 3); 122 | copy_vector(message.pelvis.vectorNav.orientation, 123 | cassie_out->pelvis.vectorNav.orientation, 4); 124 | cassie_out->pelvis.medullaCounter = message.pelvis.medullaCounter; 125 | cassie_out->pelvis.medullaCpuLoad = message.pelvis.medullaCpuLoad; 126 | cassie_out->pelvis.bleederState = message.pelvis.bleederState; 127 | cassie_out->pelvis.leftReedSwitchState = message.pelvis.leftReedSwitchState; 128 | cassie_out->pelvis.rightReedSwitchState = message.pelvis.rightReedSwitchState; 129 | cassie_out->pelvis.vtmTemperature = message.pelvis.vtmTemperature; 130 | 131 | copy_leg(message.leftLeg, &cassie_out->leftLeg); 132 | copy_leg(message.rightLeg, &cassie_out->rightLeg); 133 | 134 | copy_vector(message.messages, 135 | cassie_out->messages, 4); 136 | cassie_out->isCalibrated = message.isCalibrated; 137 | } 138 | 139 | void cassieOutToLcm(const cassie_out_t& cassie_out, double time_seconds, 140 | lcmt_cassie_out* message) { 141 | message->utime = time_seconds * 1e6; 142 | 143 | // copy pelvis 144 | copy_vector(cassie_out.pelvis.targetPc.etherCatStatus, 145 | message->pelvis.targetPc.etherCatStatus, 6); 146 | copy_vector(cassie_out.pelvis.targetPc.etherCatNotifications, 147 | message->pelvis.targetPc.etherCatNotifications, 21); 148 | 149 | message->pelvis.targetPc.taskExecutionTime = 150 | cassie_out.pelvis.targetPc.taskExecutionTime; 151 | message->pelvis.targetPc.overloadCounter = 152 | cassie_out.pelvis.targetPc.overloadCounter; 153 | 154 | message->pelvis.targetPc.cpuTemperature = 155 | cassie_out.pelvis.targetPc.cpuTemperature; 156 | 157 | message->pelvis.battery.dataGood = 158 | cassie_out.pelvis.battery.dataGood; 159 | message->pelvis.battery.stateOfCharge = 160 | cassie_out.pelvis.battery.stateOfCharge; 161 | message->pelvis.battery.current = 162 | cassie_out.pelvis.battery.current; 163 | copy_vector(cassie_out.pelvis.battery.voltage, 164 | message->pelvis.battery.voltage, 12); 165 | copy_vector(cassie_out.pelvis.battery.temperature, 166 | message->pelvis.battery.temperature, 4); 167 | 168 | message->pelvis.radio.radioReceiverSignalGood = 169 | cassie_out.pelvis.radio.radioReceiverSignalGood; 170 | message->pelvis.radio.receiverMedullaSignalGood = 171 | cassie_out.pelvis.radio.receiverMedullaSignalGood; 172 | copy_vector(cassie_out.pelvis.radio.channel, 173 | message->pelvis.radio.channel, 16); 174 | 175 | message->pelvis.vectorNav.dataGood = 176 | cassie_out.pelvis.vectorNav.dataGood; 177 | message->pelvis.vectorNav.vpeStatus = 178 | cassie_out.pelvis.vectorNav.vpeStatus; 179 | message->pelvis.vectorNav.pressure = 180 | cassie_out.pelvis.vectorNav.pressure; 181 | message->pelvis.vectorNav.temperature = 182 | cassie_out.pelvis.vectorNav.temperature; 183 | copy_vector(cassie_out.pelvis.vectorNav.magneticField, 184 | message->pelvis.vectorNav.magneticField, 3); 185 | copy_vector(cassie_out.pelvis.vectorNav.angularVelocity, 186 | message->pelvis.vectorNav.angularVelocity, 3); 187 | copy_vector(cassie_out.pelvis.vectorNav.linearAcceleration, 188 | message->pelvis.vectorNav.linearAcceleration, 3); 189 | copy_vector(cassie_out.pelvis.vectorNav.orientation, 190 | message->pelvis.vectorNav.orientation, 4); 191 | message->pelvis.medullaCounter = cassie_out.pelvis.medullaCounter; 192 | message->pelvis.medullaCpuLoad = cassie_out.pelvis.medullaCpuLoad; 193 | message->pelvis.bleederState = cassie_out.pelvis.bleederState; 194 | message->pelvis.leftReedSwitchState = cassie_out.pelvis.leftReedSwitchState; 195 | message->pelvis.rightReedSwitchState = cassie_out.pelvis.rightReedSwitchState; 196 | message->pelvis.vtmTemperature = cassie_out.pelvis.vtmTemperature; 197 | 198 | copy_leg(cassie_out.leftLeg, &message->leftLeg); 199 | copy_leg(cassie_out.rightLeg, &message->rightLeg); 200 | 201 | copy_vector(cassie_out.messages, 202 | message->messages, 4); 203 | 204 | message->isCalibrated = cassie_out.isCalibrated; 205 | } 206 | 207 | 208 | void cassieInToLcm(const cassie_user_in_t& cassie_in, double time_seconds, 209 | lcmt_cassie_in* message) { 210 | message->utime = time_seconds * 1e6; 211 | 212 | copy_vector(cassie_in.torque, message->torque, 10); 213 | copy_vector(cassie_in.telemetry, message->telemetry, 9); 214 | } 215 | 216 | void cassieInFromLcm(const lcmt_cassie_in& message, 217 | cassie_user_in_t* cassie_in) { 218 | copy_vector(message.torque, cassie_in->torque, 10); 219 | copy_vector(message.telemetry, cassie_in->telemetry, 9); 220 | } 221 | 222 | 223 | } // namespace dairlib 224 | -------------------------------------------------------------------------------- /plugin/src/CassiePlugin.cpp: -------------------------------------------------------------------------------- 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 | #include "CassiePlugin.hpp" 18 | #include "udp_lcm_translator.h" 19 | 20 | CassiePlugin::CassiePlugin() : 21 | kMotorBits_{13, 13, 13, 13, 18, 13, 13, 13, 13, 18}, 22 | kGearRatio_{25, 25, 16, 16, 50, 25, 25, 16, 16, 50}, 23 | kMaxSpeed_{2900, 2900, 1300, 1300, 5500, 2900, 2900, 1300, 1300, 5500}, 24 | kMaxTorque_{4.5, 4.5, 12.2, 12.2, 0.9, 4.5, 4.5, 12.2, 12.2, 0.9}, 25 | kMotorOffset_{0, 0, 0, -0.785398, 0, 0, 0, 0, -0.785398, 0}, 26 | kJointBits_{18, 18, 13, 18, 18, 13}, 27 | kJointOffset_{0, 1.012291, 0, 0, 1.01229, 0}, 28 | motorFilterB_{2727, 534, -2658, -795, 72, 110, 19, -6, -3}, 29 | jointFilterB_{12.348, 12.348, -12.348, -12.348}, 30 | jointFilterA_{1.0, -1.7658, 0.79045}, 31 | cassieOut_{}, 32 | headerInfo_{}, 33 | cassieUserIn_{} 34 | { 35 | // Pointers to each drive struct in cassieOut 36 | driveOut_ = { 37 | &cassieOut_.leftLeg.hipRollDrive, 38 | &cassieOut_.leftLeg.hipYawDrive, 39 | &cassieOut_.leftLeg.hipPitchDrive, 40 | &cassieOut_.leftLeg.kneeDrive, 41 | &cassieOut_.leftLeg.footDrive, 42 | &cassieOut_.rightLeg.hipRollDrive, 43 | &cassieOut_.rightLeg.hipYawDrive, 44 | &cassieOut_.rightLeg.hipPitchDrive, 45 | &cassieOut_.rightLeg.kneeDrive, 46 | &cassieOut_.rightLeg.footDrive 47 | }; 48 | 49 | // Pointers to each joint struct in cassieOut 50 | jointOut_ = { 51 | &cassieOut_.leftLeg.shinJoint, 52 | &cassieOut_.leftLeg.tarsusJoint, 53 | &cassieOut_.leftLeg.footJoint, 54 | &cassieOut_.rightLeg.shinJoint, 55 | &cassieOut_.rightLeg.tarsusJoint, 56 | &cassieOut_.rightLeg.footJoint 57 | }; 58 | 59 | // Initialize cassieOut 60 | initCassieOut(&cassieOut_); 61 | 62 | // Create cassie core 63 | corePtr_ = cassie_core_sim_alloc(); 64 | cassie_core_sim_setup(corePtr_); 65 | 66 | // Separate UDP input/output buffers into header and payload 67 | headerInPtr_ = recvBuf_; 68 | dataInPtr_ = &recvBuf_[PACKET_HEADER_LEN]; 69 | headerOutPtr_ = sendBuf_; 70 | dataOutPtr_ = &sendBuf_[PACKET_HEADER_LEN]; 71 | 72 | //udp initialization 73 | int err; 74 | 75 | // Get address info 76 | struct addrinfo *local; 77 | struct addrinfo hints = {0}; 78 | hints.ai_family = AF_UNSPEC; 79 | hints.ai_socktype = SOCK_DGRAM; 80 | hints.ai_protocol = IPPROTO_UDP; 81 | err = getaddrinfo("127.0.0.1", "25001", &hints, &local); 82 | dispatch_addr_ = (struct sockaddr *) local->ai_addr; 83 | 84 | runSim_ = true; 85 | } 86 | 87 | 88 | CassiePlugin::~CassiePlugin() 89 | { 90 | } 91 | 92 | 93 | void CassiePlugin::setMotorEncoder(elmo_out_t *drive, double position, 94 | motor_filter_t *filter, const int bits) 95 | { 96 | // Position 97 | // Get digital encoder value 98 | int encoderValue = position / (2 * M_PI) * (1 << bits); 99 | double scale = (2 * M_PI) / (1 << bits); 100 | drive->position = encoderValue * scale; 101 | 102 | // Velocity 103 | // Initialize unfiltered signal array to prevent bad transients 104 | bool allzero = true; 105 | for (size_t i = 0; i < MOTOR_FILTER_NB; ++i) 106 | allzero &= filter->x[i] == 0; 107 | if (allzero) { 108 | // If all filter values are zero, initialize the signal array 109 | // with the current encoder value 110 | for (size_t i = 0; i < MOTOR_FILTER_NB; ++i) 111 | filter->x[i] = encoderValue; 112 | } 113 | 114 | // Shift and update unfiltered signal array 115 | for (size_t i = MOTOR_FILTER_NB - 1; i > 0; --i) 116 | filter->x[i] = filter->x[i - 1]; 117 | filter->x[0] = encoderValue; 118 | 119 | // Compute filter value 120 | int y = 0; 121 | for (size_t i = 0; i < MOTOR_FILTER_NB; ++i) 122 | y += filter->x[i] * motorFilterB_[i]; 123 | drive->velocity = y * scale / M_PI; 124 | } 125 | 126 | 127 | void CassiePlugin::setJointEncoder(cassie_joint_out_t *joint, double position, 128 | joint_filter_t *filter, const int bits) 129 | { 130 | // Position 131 | // Get digital encoder value 132 | int encoderValue = position / (2 * M_PI) * (1 << bits); 133 | double scale = (2 * M_PI) / (1 << bits); 134 | joint->position = encoderValue * scale; 135 | 136 | // Velocity 137 | // Initialize unfiltered signal array to prevent bad transients 138 | bool allzero = true; 139 | for (size_t i = 0; i < JOINT_FILTER_NB; ++i) 140 | allzero &= filter->x[i] == 0; 141 | if (allzero) { 142 | // If all filter values are zero, initialize the signal array 143 | // with the current encoder value 144 | for (size_t i = 0; i < JOINT_FILTER_NB; ++i) 145 | filter->x[i] = joint->position; 146 | } 147 | 148 | // Shift and update signal arrays 149 | for (size_t i = JOINT_FILTER_NB - 1; i > 0; --i) 150 | filter->x[i] = filter->x[i - 1]; 151 | filter->x[0] = joint->position; 152 | for (size_t i = JOINT_FILTER_NA - 1; i > 0; --i) 153 | filter->y[i] = filter->y[i - 1]; 154 | 155 | // Compute filter value 156 | filter->y[0] = 0; 157 | for (size_t i = 0; i < JOINT_FILTER_NB; ++i) 158 | filter->y[0] += filter->x[i] * jointFilterB_[i]; 159 | for (size_t i = 1; i < JOINT_FILTER_NA; ++i) 160 | filter->y[0] -= filter->y[i] * jointFilterA_[i]; 161 | joint->velocity = filter->y[0]; 162 | } 163 | 164 | 165 | double CassiePlugin::setMotor(gazebo::physics::JointPtr outjoint, double u, 166 | const bool sto, const double ratio, 167 | const double tmax, const double wmax) 168 | { 169 | // Get rotor velocity 170 | double w = outjoint->GetVelocity(0) * ratio; 171 | 172 | // Calculate torque limit based on motor speed 173 | double tlim = 2 * tmax * (1 - fabs(w) / wmax); 174 | tlim = fmax(fmin(tlim, tmax), 0); 175 | 176 | // Apply STO 177 | if (sto) 178 | u = 0; 179 | 180 | // Compute output-side torque 181 | double tau = ratio * copysign(fmin(fabs(u / ratio), tlim), u); 182 | outjoint->SetForce(0, tau); 183 | 184 | // Return limited output-side torque 185 | return tau; 186 | } 187 | 188 | 189 | void CassiePlugin::initElmoOut(elmo_out_t *elmoOut, double torqueLimit, 190 | const double gearRatio) 191 | { 192 | elmoOut->statusWord = 0x0637; 193 | elmoOut->dcLinkVoltage = 48; 194 | elmoOut->driveTemperature = 30; 195 | elmoOut->torqueLimit = torqueLimit; 196 | elmoOut->gearRatio = gearRatio; 197 | } 198 | 199 | void CassiePlugin::initCassieLegOut(cassie_leg_out_t *legOut) 200 | { 201 | legOut->medullaCounter = 1; 202 | legOut->medullaCpuLoad = 94; 203 | initElmoOut(&legOut->hipRollDrive, 140.63, 25); 204 | initElmoOut(&legOut->hipYawDrive, 140.63, 25); 205 | initElmoOut(&legOut->hipPitchDrive, 216.16, 16); 206 | initElmoOut(&legOut->kneeDrive, 216.16, 16); 207 | initElmoOut(&legOut->footDrive, 45.14, 50); 208 | } 209 | 210 | 211 | void CassiePlugin::initCassieOut(cassie_out_t *cassieOut) 212 | { 213 | // Zero-initialize the struct 214 | *cassieOut = {}; 215 | 216 | // Calibrated 217 | cassieOut->isCalibrated = true; 218 | 219 | // Pelvis 220 | cassieOut->pelvis.medullaCounter = 1; 221 | cassieOut->pelvis.medullaCpuLoad = 159; 222 | cassieOut->pelvis.vtmTemperature = 40; 223 | 224 | // Target PC 225 | cassieOut->pelvis.targetPc.etherCatStatus[1] = 8; 226 | cassieOut->pelvis.targetPc.etherCatStatus[4] = 1; 227 | cassieOut->pelvis.targetPc.taskExecutionTime = 2e-4; 228 | cassieOut->pelvis.targetPc.cpuTemperature = 60; 229 | 230 | // Battery 231 | cassieOut->pelvis.battery.dataGood = true; 232 | cassieOut->pelvis.battery.stateOfCharge = 1; 233 | for (size_t i = 0; i < 4; ++i) 234 | cassieOut->pelvis.battery.temperature[i] = 30; 235 | for (size_t i = 0; i < 12; ++i) 236 | cassieOut->pelvis.battery.voltage[i] = 4.2; 237 | 238 | // Radio 239 | cassieOut->pelvis.radio.radioReceiverSignalGood = true; 240 | cassieOut->pelvis.radio.receiverMedullaSignalGood = true; 241 | cassieOut->pelvis.radio.channel[8] = 1; 242 | 243 | // VectorNav 244 | cassieOut->pelvis.vectorNav.dataGood = true; 245 | cassieOut->pelvis.vectorNav.pressure = 101.325; 246 | cassieOut->pelvis.vectorNav.temperature = 25; 247 | 248 | // Legs 249 | initCassieLegOut(&cassieOut->leftLeg); 250 | initCassieLegOut(&cassieOut->rightLeg); 251 | } 252 | 253 | 254 | void CassiePlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) 255 | { 256 | // Store a pointer to the world 257 | this->worldPtr_ = model->GetWorld(); 258 | 259 | // Store pointers to each model joint corresponding to a motor output 260 | motor_ = { 261 | model->GetJoint("left-roll-op"), 262 | model->GetJoint("left-yaw-op"), 263 | model->GetJoint("left-pitch-op"), 264 | model->GetJoint("left-knee-op"), 265 | model->GetJoint("left-foot-op"), 266 | model->GetJoint("right-roll-op"), 267 | model->GetJoint("right-yaw-op"), 268 | model->GetJoint("right-pitch-op"), 269 | model->GetJoint("right-knee-op"), 270 | model->GetJoint("right-foot-op") 271 | }; 272 | 273 | // Store pointers to each model joint corresponding to a measured joint 274 | joint_ = { 275 | model->GetJoint("left-knee-shin-joint"), 276 | model->GetJoint("left-shin-tarsus-joint"), 277 | model->GetJoint("left-foot-op"), 278 | model->GetJoint("right-knee-shin-joint"), 279 | model->GetJoint("right-shin-tarsus-joint"), 280 | model->GetJoint("right-foot-op") 281 | }; 282 | 283 | // Store a pointer to the pelvis link 284 | pelvisPtr_ = model->GetLink("pelvis"); 285 | 286 | // Update Rate 287 | this->updateRate_ = 2000; 288 | this->updatePeriod_ = 1.0 / this->updateRate_; 289 | lastUpdateTime_ = this->worldPtr_->SimTime(); 290 | 291 | // Open UDP socket 292 | sock_ = udp_init_host("0.0.0.0", "25000"); 293 | 294 | // Listen to the update event which is broadcast every simulation iteration 295 | this->updateConnectionPtr_ = gazebo::event::Events::ConnectWorldUpdateBegin( 296 | std::bind(&CassiePlugin::onUpdate, this)); 297 | } 298 | 299 | 300 | void CassiePlugin::onUpdate() 301 | { 302 | // Get current time and calculate last update time 303 | auto currentTime = worldPtr_->SimTime(); 304 | auto secondsSinceLastUpdate = (currentTime - lastUpdateTime_).Double(); 305 | 306 | // Run at the specified period 307 | if (secondsSinceLastUpdate < updatePeriod_*0.95) 308 | return; 309 | 310 | // Zero the input if no packets have been received in a while 311 | if ((currentTime - lastPacketTime_).Double() > 0.01) 312 | cassieUserIn_ = {}; 313 | 314 | // Get newest packet, or return -1 if no new packets are available 315 | addrLen_ = sizeof srcAddr_; 316 | ssize_t nbytes = get_newest_packet(sock_, recvBuf_, RECVLEN, 317 | (struct sockaddr *) &srcAddr_, 318 | &addrLen_); 319 | 320 | // If a new packet was received, process and unpack it 321 | if (RECVLEN == nbytes) { 322 | // Process incoming header and write outgoing header 323 | process_packet_header(&headerInfo_, headerInPtr_, headerOutPtr_); 324 | 325 | // Unpack received data into cassie user input struct 326 | unpack_cassie_user_in_t(dataInPtr_, &cassieUserIn_); 327 | 328 | // Start running Cassie core after the first valid packet is received 329 | if (!runSim_) { 330 | runSim_ = true; 331 | firstPacketTime_ = currentTime; 332 | } 333 | 334 | // Record time 335 | lastPacketTime_ = currentTime; 336 | } 337 | 338 | // Detatch pelvis 5 seconds after receiving data 339 | // if ((currentTime - firstPacketTime_).Double() > 5.0) 340 | // detachPelvis(); 341 | 342 | if (runSim_) { 343 | // Run simulator and pack output struct into outgoing packet 344 | cassie_in_t cassieIn; 345 | cassie_out_t output = cassieOut_; 346 | cassie_core_sim_step(corePtr_, &cassieUserIn_, &output, &cassieIn); 347 | applyTorques(&cassieIn); 348 | updateCassieOut(); 349 | lastUpdateTime_ += gazebo::common::Time(updatePeriod_); 350 | 351 | pack_cassie_out_t(&output, dataOutPtr_); 352 | 353 | // Send response 354 | send_packet(sock_, sendBuf_, SENDLEN, 355 | dispatch_addr_, addrLen_); 356 | 357 | // Send LCM response 358 | dairlib::lcmt_cassie_out message; 359 | cassieOutToLcm(output, currentTime.Double(), &message); 360 | lcm.publish("CASSIE_OUTPUT", &message); 361 | } 362 | } 363 | 364 | 365 | void CassiePlugin::updateCassieOut() 366 | { 367 | // Motor encoders 368 | for (size_t i = 0; i < 10; ++i) { 369 | setMotorEncoder(driveOut_[i], motor_[i]->Position(0) + kMotorOffset_[i], 370 | &motorFilter_[i], kMotorBits_[i]); 371 | } 372 | 373 | // Joint encoders 374 | for (size_t i = 0; i < 6; ++i) { 375 | setJointEncoder(jointOut_[i], joint_[i]->Position(0) + kJointOffset_[i], 376 | &jointFilter_[i], kJointBits_[i]); 377 | } 378 | 379 | // IMU 380 | auto pose = pelvisPtr_->WorldPose(); 381 | 382 | auto worldAccel = pelvisPtr_->WorldLinearAccel() - worldPtr_->Gravity(); 383 | auto worldGyro = pelvisPtr_->WorldAngularVel(); 384 | auto worldMag = ignition::math::Vector3d(0, 1, 0); 385 | 386 | auto rot = pose.Rot().Inverse(); 387 | auto accel = rot.RotateVector(worldAccel); 388 | auto gyro = rot.RotateVector(worldGyro); 389 | auto mag = rot.RotateVector(worldMag); 390 | 391 | // Set computed orientation 392 | cassieOut_.pelvis.vectorNav.orientation[0] = pose.Rot().W(); 393 | cassieOut_.pelvis.vectorNav.orientation[1] = pose.Rot().X(); 394 | cassieOut_.pelvis.vectorNav.orientation[2] = pose.Rot().Y(); 395 | cassieOut_.pelvis.vectorNav.orientation[3] = pose.Rot().Z(); 396 | 397 | // Set accelerometer/gyro/magnetometer measurement 398 | for (size_t i = 0; i < 3; ++i) { 399 | cassieOut_.pelvis.vectorNav.linearAcceleration[i] = accel[i]; 400 | cassieOut_.pelvis.vectorNav.angularVelocity[i] = gyro[i]; 401 | cassieOut_.pelvis.vectorNav.magneticField[i] = mag[i]; 402 | } 403 | } 404 | 405 | 406 | void CassiePlugin::applyTorques(const cassie_in_t *cassieIn) 407 | { 408 | // STO 409 | bool sto = this->cassieOut_.pelvis.radio.channel[8] < 1; 410 | 411 | // Get torque commands 412 | const double torque[] = { 413 | cassieIn->leftLeg.hipRollDrive.torque, 414 | cassieIn->leftLeg.hipYawDrive.torque, 415 | cassieIn->leftLeg.hipPitchDrive.torque, 416 | cassieIn->leftLeg.kneeDrive.torque, 417 | cassieIn->leftLeg.footDrive.torque, 418 | cassieIn->rightLeg.hipRollDrive.torque, 419 | cassieIn->rightLeg.hipYawDrive.torque, 420 | cassieIn->rightLeg.hipPitchDrive.torque, 421 | cassieIn->rightLeg.kneeDrive.torque, 422 | cassieIn->rightLeg.footDrive.torque 423 | }; 424 | 425 | // Set and output limited torque commands 426 | for (size_t i = 0; i < 10; ++i) { 427 | driveOut_[i]->torque = setMotor(motor_[i], torque[i], sto, 428 | kGearRatio_[i], kMaxTorque_[i], 429 | kMaxSpeed_[i]); 430 | } 431 | } 432 | 433 | 434 | void CassiePlugin::detachPelvis() 435 | { 436 | this->worldPtr_->ModelByName("cassie")->RemoveJoint("static"); 437 | } 438 | 439 | 440 | // Register plugin with Gazebo 441 | GZ_REGISTER_MODEL_PLUGIN(CassiePlugin) 442 | -------------------------------------------------------------------------------- /cassie/cassie.sdf: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 1 20 | 21 | 22 | 0 0 1.01 0 0 0 23 | 24 | 0.05066 0.000346 0.02841 0 0 0 25 | 10.33 26 | 27 | 0.085821 28 | 0.049222 29 | 0.08626 30 | 1.276e-05 31 | -0.00016022 32 | -0.000414 33 | 34 | 35 | 36 | 37 | 38 | model://cassie/meshes/pelvis.stl 39 | 40 | 41 | 42 | 43 | 0.02 0 0.04 0 0 0 44 | 45 | 46 | 0.14 47 | 48 | 49 | 50 | 51 | 52 | 53 | 0.021 0.135 1.01 0 1.5708 0 54 | 55 | -0.01793 0.0001 -0.04428 0 0 0 56 | 1.82 57 | 58 | 0.003431 59 | 0.003793 60 | 0.002135 61 | -6.65e-07 62 | -0.00084 63 | 3.99e-06 64 | 65 | 66 | 67 | 68 | 69 | model://cassie/meshes/hip-roll.stl 70 | 71 | 72 | 73 | 74 | 75 | 76 | 0.021 0.135 1.01 0 1.5708 0 77 | 78 | 0.000001 0.000001 -0.027063 0 1.57 0 79 | 0.145608 80 | 81 | 0.000073735 82 | 1.340232e-073 83 | 7.241957e-08 84 | 7.357073e-05 85 | -7.096017e-08 86 | 8.262566e-05 87 | 88 | 89 | 90 | 0 0 0 0 -0 0 91 | 92 | 93 | 0.01 0.01 0.01 94 | 95 | 96 | 97 | 98 | 99 | 100 | left-roll-reference 101 | pelvis 102 | 103 | 1 0 0 104 | 1 105 | 106 | 107 | 108 | 109 | left-hip-roll 110 | pelvis 111 | 112 | 1 0 0 113 | 114 | -0.2618 115 | 0.3491 116 | 117 | 118 | 1 119 | 120 | 1 121 | 122 | 123 | 124 | 125 | left-hip-roll 126 | left-roll-reference 127 | pelvis 128 | 0.04 129 | 130 | 1 0 0 131 | 1 132 | 133 | 134 | 1 0 0 135 | 1 136 | 137 | 138 | 139 | 140 | -0.049 0.135 1.01 0 0 0 141 | 142 | 0 -1e-05 -0.034277 0 0 0 143 | 1.171 144 | 145 | 0.002443 146 | 0.002803 147 | 0.000842 148 | -4e-08 149 | 2.462e-07 150 | -2.71e-08 151 | 152 | 153 | 154 | 155 | 156 | model://cassie/meshes/hip-yaw.stl 157 | 158 | 159 | 160 | 161 | 162 | 163 | -0.049 0.135 1.01 0 0 0 164 | 165 | 0.000001 0.000001 -0.027063 0 1.57 0 166 | 0.145608 167 | 168 | 7.368810e-05 169 | 1.340232e-073 170 | 7.241957e-08 171 | 7.357073e-05 172 | -7.096017e-08 173 | 0.000082672 174 | 175 | 176 | 177 | 0 0 0 0 -0 0 178 | 179 | 180 | 0.01 0.01 0.01 181 | 182 | 183 | 184 | 185 | 186 | 187 | left-yaw-reference 188 | left-hip-roll 189 | 190 | 0 0 1 191 | 1 192 | 193 | 194 | 195 | 196 | left-hip-yaw 197 | left-hip-roll 198 | 199 | 0 0 1 200 | 201 | -0.3840 202 | 0.3840 203 | 204 | 205 | 1 206 | 207 | 1 208 | 209 | 210 | 211 | 212 | left-hip-yaw 213 | left-yaw-reference 214 | left-hip-roll 215 | 0.04 216 | 217 | 0 0 1 218 | 1 219 | 220 | 221 | 0 0 1 222 | 1 223 | 224 | 225 | 226 | 227 | -0.049 0.135 0.92 0 1.5708 -1.57 228 | 229 | 0.05946 5e-05 -0.03581 0 0 0 230 | 5.52 231 | 232 | 0.010898 233 | 0.029714 234 | 0.030257 235 | -0.0002669 236 | -5.721e-05 237 | 9.17e-06 238 | 239 | 240 | 241 | 242 | 243 | model://cassie/meshes/hip-pitch.stl 244 | 245 | 246 | 247 | 248 | 0.053 0.0 -0.0379 0 1.57 0 249 | 250 | 251 | 0.18 252 | 0.06 253 | 254 | 255 | 256 | 257 | 258 | 259 | -0.049 0.135 0.92 0 1.5708 -1.57 260 | 261 | -0.000001 -0.000011 0 1.57 1.57 0 262 | 0.272280 263 | 264 | 2.288457e-04 265 | 6.854960e-07 266 | -4.163281e-08 267 | 0.00023115 268 | -9.248118e-07 269 | 0.00020476 270 | 271 | 272 | 273 | 0 0 0 0 -0 0 274 | 275 | 276 | 0.01 0.01 0.01 277 | 278 | 279 | 280 | 281 | 282 | 283 | left-pitch-reference 284 | left-hip-yaw 285 | 286 | 0 -1 0 287 | 1 288 | 289 | 290 | 291 | 292 | left-hip-pitch 293 | left-hip-yaw 294 | 295 | 0 -1 0 296 | 297 | -0.8727 298 | 1.3963 299 | 300 | 301 | 1 302 | 303 | 1 304 | 305 | 306 | 307 | 308 | left-hip-pitch 309 | left-pitch-reference 310 | left-hip-yaw 311 | 0.0625 312 | 313 | 0 -1 0 314 | 1 315 | 316 | 317 | 0 -1 0 318 | 1 319 | 320 | 321 | 322 | 323 | -0.049 0.09 0.92 -1.7199 0.91441 3.0232 324 | 325 | 0.24719 0 0 0 0 0 326 | 0.1567 327 | 328 | 3.754e-06 329 | 0.004487 330 | 0.004488 331 | -3.74e-08 332 | -1.61e-08 333 | 0 334 | 335 | 336 | 337 | 338 | 339 | model://cassie/meshes/achilles-rod.stl 340 | 341 | 342 | 343 | 344 | 345 | 346 | left-achilles-rod 347 | left-hip-pitch 348 | 349 | 0 -1 0 350 | 351 | 0.01 352 | 353 | 1 354 | 355 | 356 | 357 | 358 | -0.049 0.1305 0.8 -1.5708 0.7854 3.1416 359 | 360 | 0.023 0.03207 -0.002181 0 0 0 361 | 0.7578 362 | 363 | 0.001376 364 | 0.0010335 365 | 0.0021637 366 | -0.00039744 367 | -4.085e-05 368 | -5.374e-05 369 | 370 | 371 | 372 | 373 | 374 | model://cassie/meshes/knee.stl 375 | 376 | 377 | 378 | 379 | 380 | 381 | -0.049 0.1305 0.8 -1.5708 0.7854 3.1416 382 | 383 | -0.000001 -0.000011 0.024425 1.57 1.57 0 384 | 0.272280 385 | 386 | 2.288457e-04 387 | 6.854960e-07 388 | -4.163281e-08 389 | 0.00023115 390 | -9.248118e-07 391 | 0.00020476 392 | 393 | 394 | 395 | 0 0 0 0 -0 0 396 | 397 | 398 | 0.01 0.01 0.01 399 | 400 | 401 | 402 | 403 | 404 | 405 | left-hip-pitch 406 | left-knee-reference 407 | 408 | 0 -1 0 409 | 1 410 | 411 | 412 | 413 | 414 | left-knee 415 | left-hip-pitch 416 | 417 | 0 -1 0 418 | 419 | -2.07694 420 | 0.13963 421 | 422 | 423 | 1 424 | 425 | 1 426 | 427 | 428 | 429 | 430 | left-knee 431 | left-knee-reference 432 | left-hip-pitch 433 | 0.0625 434 | 435 | 0 -1 0 436 | 1 437 | 438 | 439 | 0 -1 0 440 | 1 441 | 442 | 443 | 444 | 445 | -0.033635 0.1305 0.69882 -1.5708 0.7854 3.1416 446 | 447 | 0.0836 0.0034 0 0 0 0 448 | 0.186 449 | 450 | 5.215e-05 451 | 0.00041205 452 | 0.0003669 453 | 6.87e-06 454 | 0 455 | 0 456 | 457 | 458 | 459 | 460 | 461 | model://cassie/meshes/knee-spring.stl 462 | 463 | 464 | 465 | 466 | 467 | 468 | left-knee-spring 469 | left-knee 470 | 471 | 0.000796 -1 1e-06 472 | 1 473 | 474 | 475 | 476 | 477 | -0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416 478 | 479 | 0.18338 0.001169 0.0002123 0 0 0 480 | 0.577 481 | 482 | 0.00035939 483 | 0.014728 484 | 0.014707 485 | -0.00020981 486 | 2.266e-05 487 | -1.2e-07 488 | 489 | 490 | 491 | 492 | 493 | model://cassie/meshes/shin.stl 494 | 495 | 496 | 497 | 498 | 0.22 0.01 0.0 0 1.57 0 499 | 500 | 501 | 0.44 502 | 0.04 503 | 504 | 505 | 506 | 507 | 508 | 509 | left-shin 510 | left-knee 511 | 512 | 0.000796 -1 1e-06 513 | 514 | -0.34907 515 | 0.34907 516 | 517 | 518 | 0.1 519 | 0 520 | 1500 521 | 522 | 1 523 | 524 | 525 | 526 | 527 | -0.35166 0.1305 0.402 1.5708 1.3439 0 528 | 529 | 0.11046 -0.03058 -0.00131 0 0 0 530 | 0.782 531 | 532 | 0.00039238 533 | 0.013595 534 | 0.013674 535 | 0.00023651 536 | -4.987e-05 537 | -4.82e-06 538 | 539 | 540 | 541 | 542 | 543 | model://cassie/meshes/tarsus.stl 544 | 545 | 546 | 547 | 548 | 0.21 -0.03 0.0 0 1.57 0 549 | 550 | 551 | 0.38 552 | 0.02 553 | 554 | 555 | 556 | 557 | 558 | 559 | left-tarsus 560 | left-shin 561 | 562 | 0 -1 0 563 | 564 | -0.13963 565 | 1.95477 566 | 567 | 568 | 0.1 569 | 570 | 1 571 | 572 | 573 | 574 | 575 | -0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949 576 | 577 | 0.081 0.0022 0 0 0 0 578 | 0.126 579 | 580 | 2.959e-05 581 | 0.00022231 582 | 0.0002007 583 | 7.15e-06 584 | -6e-07 585 | 1e-07 586 | 587 | 588 | 589 | 590 | 591 | model://cassie/meshes/heel-spring.stl 592 | 593 | 594 | 595 | 596 | 597 | 598 | left-heel-spring 599 | left-tarsus 600 | 601 | 0 -1 0 602 | 603 | 0 604 | 0 605 | 1250 606 | 607 | 1 608 | 609 | 610 | 611 | 612 | -0.37174 0.10775 0.33784 1.5708 1.3439 0 613 | 614 | 0.00493 2e-05 -0.00215 0 0 0 615 | 0.1261 616 | 617 | 2.6941e-05 618 | 4.9621e-05 619 | 6.3362e-05 620 | -2.1e-09 621 | -3.9623e-06 622 | -1.09e-08 623 | 624 | 625 | 626 | 627 | 628 | model://cassie/meshes/foot-crank.stl 629 | 630 | 631 | 632 | 633 | 634 | 635 | left-foot-crank 636 | left-tarsus 637 | 638 | 0 -1 0 639 | 640 | -2.4435 641 | -0.6109 642 | 643 | 644 | 1 645 | 646 | 1 647 | 648 | 649 | 650 | 651 | -0.35937 0.11566 0.28425 1.5708 1.3639 0.1 652 | 653 | 0.17792 0 0 0 0 0 654 | 0.1186 655 | 656 | 2.779e-06 657 | 0.001774 658 | 0.001775 659 | -2.34e-08 660 | -8.1e-09 661 | 0 662 | 663 | 664 | 665 | 666 | 667 | model://cassie/meshes/plantar-rod.stl 668 | 669 | 670 | 671 | 672 | 673 | 674 | left-plantar-rod 675 | left-foot-crank 676 | 677 | 0 -1 0 678 | 1 679 | 680 | 681 | 682 | 683 | -0.29886 0.1305 -0.0045361 1.5708 1.3439 0 684 | 685 | 0.00474 0.02748 -0.00014 0 0 0 686 | 0.1498 687 | 688 | 0.00017388 689 | 0.00016793 690 | 0.00033261 691 | 0.00011814 692 | 1.36e-06 693 | -4e-07 694 | 695 | 696 | 697 | 698 | 699 | model://cassie/meshes/foot.stl 700 | 701 | 702 | 703 | 704 | 0.01 0.045 0 0 1.57 -0.695 705 | 706 | 707 | 0.18 708 | 0.0168 709 | 710 | 711 | 712 | 713 | 714 | 0.005 715 | 716 | 717 | 718 | 719 | 720 | 721 | 722 | -0.29886 0.1305 -0.0045361 1.5708 1.3439 0 723 | 724 | 0.00474 0.02748 -0.00014 0 0 0 725 | 0.001 726 | 727 | 9.4e-7 728 | 9.4e-9 729 | 9.4e-9 730 | 9.4e-7 731 | 9.4e-9 732 | 9.4e-6 733 | 734 | 735 | 736 | 0 0 0 0 -0 0 737 | 738 | 739 | 0.01 0.01 0.01 740 | 741 | 742 | 743 | 744 | 745 | 746 | left-tarsus 747 | left-foot-reference 748 | 749 | 0 -1 0 750 | 1 751 | 752 | 753 | 754 | 755 | left-foot 756 | left-tarsus 757 | 758 | 0 -1 0 759 | 760 | -2.4435 761 | -0.6109 762 | 763 | 764 | 1 765 | 766 | 1 767 | 768 | 769 | 770 | 771 | left-foot 772 | left-foot-reference 773 | left-tarsus 774 | 0.02 775 | 776 | 0 -1 0 777 | 1 778 | 779 | 780 | 0 -1 0 781 | 1 782 | 783 | 784 | 785 | 786 | 0.021 -0.135 1.01 0 1.5708 0 787 | 788 | -0.01793 -junk0.0001 -0.04428 0 0 0 789 | 1.82 790 | 791 | 0.003431 792 | 0.003793 793 | 0.002135 794 | 6.65e-07 795 | -0.00084 796 | -3.99e-06 797 | 798 | 799 | 800 | 801 | 802 | 1 -1 1 803 | model://cassie/meshes/hip-roll.stl 804 | 805 | 806 | 807 | 808 | 809 | 810 | 0.021 -0.135 1.01 0 1.5708 0 811 | 812 | 0.000001 0.000001 -0.027063 0 1.57 0 813 | 0.145608 814 | 815 | 0.000073735 816 | 1.340232e-073 817 | 7.241957e-08 818 | 7.357073e-05 819 | -7.096017e-08 820 | 8.262566e-05 821 | 822 | 823 | 824 | 0 0 0 0 -0 0 825 | 826 | 827 | 0.01 0.01 0.01 828 | 829 | 830 | 831 | 832 | 833 | 834 | right-roll-reference 835 | pelvis 836 | 837 | 1 0 0 838 | 1 839 | 840 | 841 | 842 | 843 | right-hip-roll 844 | pelvis 845 | 846 | 1 0 0 847 | 848 | -0.3491 849 | 0.2618 850 | 851 | 852 | 1 853 | 854 | 1 855 | 856 | 857 | 858 | 859 | right-hip-roll 860 | right-roll-reference 861 | pelvis 862 | 0.04 863 | 864 | 1 0 0 865 | 1 866 | 867 | 868 | 1 0 0 869 | 1 870 | 871 | 872 | 873 | 874 | -0.049 -0.135 1.01 0 0 0 875 | 876 | 0 1e-05 -0.034277 0 0 0 877 | 1.171 878 | 879 | 0.002443 880 | 0.002803 881 | 0.000842 882 | 4e-08 883 | 2.462e-07 884 | 2.71e-08 885 | 886 | 887 | 888 | 889 | 890 | 1 1 1 891 | model://cassie/meshes/hip-yaw.stl 892 | 893 | 894 | 895 | 896 | 897 | 898 | -0.049 -0.135 1.01 0 0 0 899 | 900 | 0.000001 0.000001 -0.027063 0 1.57 0 901 | 0.145608 902 | 903 | 7.368810e-05 904 | 1.340232e-073 905 | 7.241957e-08 906 | 7.357073e-05 907 | -7.096017e-08 908 | 0.000082672 909 | 910 | 911 | 912 | 0 0 0 0 -0 0 913 | 914 | 915 | 0.01 0.01 0.01 916 | 917 | 918 | 919 | 920 | 921 | 922 | right-yaw-reference 923 | right-hip-roll 924 | 925 | 0 0 1 926 | 1 927 | 928 | 929 | 930 | 931 | right-hip-yaw 932 | right-hip-roll 933 | 934 | 0 0 1 935 | 936 | -0.3840 937 | 0.3840 938 | 939 | 940 | 1 941 | 942 | 1 943 | 944 | 945 | 946 | 947 | right-hip-yaw 948 | right-yaw-reference 949 | right-hip-roll 950 | 0.04 951 | 952 | 0 0 1 953 | 1 954 | 955 | 956 | 0 0 1 957 | 1 958 | 959 | 960 | 961 | 962 | -0.049 -0.135 0.92 0 1.5708 -1.57 963 | 964 | 0.05946 5e-05 0.03581 0 0 0 965 | 5.52 966 | 967 | 0.010898 968 | 0.029714 969 | 0.030257 970 | -0.0002669 971 | 5.721e-05 972 | -9.17e-06 973 | 974 | 975 | 976 | 977 | 978 | 1 1 -1 979 | model://cassie/meshes/hip-pitch.stl 980 | 981 | 982 | 983 | 984 | 0.053 0.0 0.0379 0 1.57 0 985 | 986 | 987 | 0.18 988 | 0.06 989 | 990 | 991 | 992 | 993 | 994 | 995 | -0.049 -0.135 0.92 0 1.5708 -1.57 996 | 997 | -0.000001 -0.000011 0 1.57 1.57 0 998 | 0.272280 999 | 1000 | 2.288457e-04 1001 | 6.854960e-07 1002 | -4.163281e-08 1003 | 0.00023115 1004 | -9.248118e-07 1005 | 0.00020476 1006 | 1007 | 1008 | 1009 | 0 0 0 0 -0 0 1010 | 1011 | 1012 | 0.01 0.01 0.01 1013 | 1014 | 1015 | 1016 | 1017 | 1018 | 1019 | right-pitch-reference 1020 | right-hip-yaw 1021 | 1022 | 0 -1 0 1023 | 1 1024 | 1025 | 1026 | 1027 | 1028 | right-hip-pitch 1029 | right-hip-yaw 1030 | 1031 | 0 -1 0 1032 | 1033 | -0.8727 1034 | 1.3963 1035 | 1036 | 1037 | 1 1038 | 1039 | 1 1040 | 1041 | 1042 | 1043 | 1044 | right-hip-pitch 1045 | right-pitch-reference 1046 | right-hip-yaw 1047 | 0.0625 1048 | 1049 | 0 -1 0 1050 | 1 1051 | 1052 | 1053 | 0 -1 0 1054 | 1 1055 | 1056 | 1057 | 1058 | 1059 | -0.049 -0.09 0.92 -1.4217 0.91441 -3.0232 1060 | 1061 | 0.24719 0 0 0 0 0 1062 | 0.1567 1063 | 1064 | 3.754e-06 1065 | 0.004487 1066 | 0.004488 1067 | -3.74e-08 1068 | 1.61e-08 1069 | 0 1070 | 1071 | 1072 | 1073 | 1074 | 1075 | 1 1 1 1076 | model://cassie/meshes/achilles-rod.stl 1077 | 1078 | 1079 | 1080 | 1081 | 1082 | 1083 | right-achilles-rod 1084 | right-hip-pitch 1085 | 1086 | 0 -1 0 1087 | 1088 | 0.01 1089 | 1090 | 1 1091 | 1092 | 1093 | 1094 | 1095 | -0.049 -0.1305 0.8 -1.5708 0.7854 3.1416 1096 | 1097 | 0.023 0.03207 0.002181 0 0 0 1098 | 0.7578 1099 | 1100 | 0.001376 1101 | 0.0010335 1102 | 0.0021637 1103 | -0.00039744 1104 | 4.085e-05 1105 | 5.374e-05 1106 | 1107 | 1108 | 1109 | 1110 | 1111 | 1 1 1 1112 | model://cassie/meshes/knee.stl 1113 | 1114 | 1115 | 1116 | 1117 | 1118 | 1119 | -0.049 -0.1305 0.8 -1.5708 0.7854 3.1416 1120 | 1121 | -0.000001 -0.000011 -0.024425 1.57 1.57 0 1122 | 0.272280 1123 | 1124 | 2.288457e-04 1125 | 6.854960e-07 1126 | -4.163281e-08 1127 | 0.00023115 1128 | -9.248118e-07 1129 | 0.00020476 1130 | 1131 | 1132 | 1133 | 0 0 0 0 -0 0 1134 | 1135 | 1136 | 0.01 0.01 0.01 1137 | 1138 | 1139 | 1140 | 1141 | 1142 | 1143 | right-hip-pitch 1144 | right-knee-reference 1145 | 1146 | 0 -1 0 1147 | 1 1148 | 1149 | 1150 | 1151 | 1152 | right-knee 1153 | right-hip-pitch 1154 | 1155 | 0 -1 0 1156 | 1157 | -2.07694 1158 | 0.13963 1159 | 1160 | 1161 | 1 1162 | 1163 | 1 1164 | 1165 | 1166 | 1167 | 1168 | right-knee 1169 | right-knee-reference 1170 | right-hip-pitch 1171 | 0.0625 1172 | 1173 | 0 -1 0 1174 | 1 1175 | 1176 | 1177 | 0 -1 0 1178 | 1 1179 | 1180 | 1181 | 1182 | 1183 | -0.033635 -0.1305 0.69882 -1.5708 0.7854 3.1416 1184 | 1185 | 0.0836 0.0034 0 0 0 0 1186 | 0.186 1187 | 1188 | 5.215e-05 1189 | 0.00041205 1190 | 0.0003669 1191 | 6.87e-06 1192 | 0 1193 | 0 1194 | 1195 | 1196 | 1197 | 1198 | 1199 | 1 1 1 1200 | model://cassie/meshes/knee-spring.stl 1201 | 1202 | 1203 | 1204 | 1205 | 1206 | 1207 | right-knee-spring 1208 | right-knee 1209 | 1210 | 0 -1 0 1211 | 1 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | -0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416 1218 | 1219 | 0.18338 0.001169 -0.0002123 0 0 0 1220 | 0.577 1221 | 1222 | 0.00035939 1223 | 0.014728 1224 | 0.014707 1225 | -0.00020981 1226 | -2.266e-05 1227 | 1.2e-07 1228 | 1229 | 1230 | 1231 | 1232 | 1233 | 1 1 1 1234 | model://cassie/meshes/shin.stl 1235 | 1236 | 1237 | 1238 | 1239 | 0.22 0.01 0.0 0 1.57 0 1240 | 1241 | 1242 | 0.44 1243 | 0.04 1244 | 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | right-shin 1251 | right-knee 1252 | 1253 | 0 -1 0 1254 | 1255 | -0.34907 1256 | 0.34907 1257 | 1258 | 1259 | 0.1 1260 | 0 1261 | 1500 1262 | 1263 | 1 1264 | 1265 | 1266 | 1267 | 1268 | -0.35166 -0.1305 0.402 1.5708 1.3439 0 1269 | 1270 | 0.11046 -0.03058 0.00131 0 0 0 1271 | 0.782 1272 | 1273 | 0.00039238 1274 | 0.013595 1275 | 0.013674 1276 | 0.00023651 1277 | 4.987e-05 1278 | 4.82e-06 1279 | 1280 | 1281 | 1282 | 1283 | 1284 | 1 1 1 1285 | model://cassie/meshes/tarsus.stl 1286 | 1287 | 1288 | 1289 | 1290 | 0.21 -0.03 0.0 0 1.57 0 1291 | 1292 | 1293 | 0.38 1294 | 0.02 1295 | 1296 | 1297 | 1298 | 1299 | 1300 | 1301 | right-tarsus 1302 | right-shin 1303 | 1304 | 0 -1 0 1305 | 1306 | -0.13963 1307 | 1.95477 1308 | 1309 | 1310 | 0.1 1311 | 1312 | 1 1313 | 1314 | 1315 | 1316 | 1317 | -0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949 1318 | 1319 | 0.081 0.0022 0 0 0 0 1320 | 0.126 1321 | 1322 | 2.959e-05 1323 | 0.00022231 1324 | 0.0002007 1325 | 7.15e-06 1326 | 6e-07 1327 | -1e-07 1328 | 1329 | 1330 | 1331 | 1332 | 1333 | 1 1 1 1334 | model://cassie/meshes/heel-spring.stl 1335 | 1336 | 1337 | 1338 | 1339 | 1340 | 1341 | right-heel-spring 1342 | right-tarsus 1343 | 1344 | 0 -1 0 1345 | 1346 | 0 1347 | 1250 1348 | 1349 | 1 1350 | 1351 | 1352 | 1353 | 1354 | -0.37174 -0.10775 0.33784 1.5708 1.3439 0 1355 | 1356 | 0.00493 2e-05 0.00215 0 0 0 1357 | 0.1261 1358 | 1359 | 2.6941e-05 1360 | 4.9621e-05 1361 | 6.3362e-05 1362 | -2.1e-09 1363 | 3.9623e-06 1364 | 1.09e-08 1365 | 1366 | 1367 | 1368 | 1369 | 1370 | 1 1 -1 1371 | model://cassie/meshes/foot-crank.stl 1372 | 1373 | 1374 | 1375 | 1376 | 1377 | 1378 | right-foot-crank 1379 | right-tarsus 1380 | 1381 | 0 -1 0 1382 | 1383 | -2.4435 1384 | -0.6109 1385 | 1386 | 1387 | 1 1388 | 1389 | 1 1390 | 1391 | 1392 | 1393 | 1394 | -0.35937 -0.11566 0.28425 1.5708 1.364 -0.1 1395 | 1396 | 0.17792 0 0 0 0 0 1397 | 0.1186 1398 | 1399 | 2.779e-06 1400 | 0.001774 1401 | 0.001775 1402 | -2.34e-08 1403 | 8.1e-09 1404 | 0 1405 | 1406 | 1407 | 1408 | 1409 | 1410 | 1 1 1 1411 | model://cassie/meshes/plantar-rod.stl 1412 | 1413 | 1414 | 1415 | 1416 | 1417 | 1418 | right-plantar-rod 1419 | right-foot-crank 1420 | 1421 | 0 -1 0 1422 | 1 1423 | 1424 | 1425 | 1426 | 1427 | -0.29886 -0.1305 -0.0045361 1.5708 1.364 0 1428 | 1429 | 0.00474 0.02748 0.00014 0 0 0 1430 | 0.1498 1431 | 1432 | 0.00017388 1433 | 0.00016793 1434 | 0.00033261 1435 | 0.00011814 1436 | -1.36e-06 1437 | 4e-07 1438 | 1439 | 1440 | 1441 | 1442 | 1443 | 1 1 -1 1444 | model://cassie/meshes/foot.stl 1445 | 1446 | 1447 | 1448 | 1449 | 0.01 0.045 0 0 1.57 -0.695 1450 | 1451 | 1452 | 0.18 1453 | 0.0168 1454 | 1455 | 1456 | 1457 | 1458 | 1459 | 0.005 1460 | 1461 | 1462 | 1463 | 1464 | 1465 | 1466 | 1467 | -0.29886 -0.1305 -0.0045361 1.5708 1.3439 0 1468 | 1469 | 0.00474 0.02748 0.00014 0 0 0 1470 | 0.001 1471 | 1472 | 9.4e-7 1473 | 9.4e-9 1474 | 9.4e-9 1475 | 9.4e-7 1476 | 9.4e-9 1477 | 9.4e-6 1478 | 1479 | 1480 | 1481 | 0 0 0 0 -0 0 1482 | 1483 | 1484 | 0.01 0.01 0.01 1485 | 1486 | 1487 | 1488 | 1489 | 1490 | 1491 | right-tarsus 1492 | right-foot-reference 1493 | 1494 | 0 -1 0 1495 | 1 1496 | 1497 | 1498 | 1499 | 1500 | right-foot 1501 | right-tarsus 1502 | 1503 | 0 -1 0 1504 | 1505 | -2.4435 1506 | -0.6109 1507 | 1508 | 1509 | 1 1510 | 1511 | 1 1512 | 1513 | 1514 | 1515 | 1516 | right-foot 1517 | right-foot-reference 1518 | right-tarsus 1519 | 0.02 1520 | 1521 | 0 -1 0 1522 | 1 1523 | 1524 | 1525 | 0 -1 0 1526 | 1 1527 | 1528 | 1529 | 1530 | 1531 | 0.13 0.005 0 0 0 0 1532 | right-shin 1533 | right-knee-spring 1534 | 1535 | 1 0 0 1536 | 1537 | 0 1538 | 1539 | 1540 | 1541 | 1542 | 1543 | 0.11877 -0.01 0 0 0 0 1544 | right-achilles-rod 1545 | right-heel-spring 1546 | 1547 | 0 0 1 1548 | 1549 | 1550 | 1551 | 1552 | 0.055 0 -0.008 0 0 0 1553 | right-plantar-rod 1554 | right-foot 1555 | 1556 | 0 0 1 1557 | 1558 | 1559 | 1560 | 1561 | 0.13 0.005 0 0 0 0 1562 | left-shin 1563 | left-knee-spring 1564 | 1565 | 1 0 0 1566 | 1567 | 0 1568 | 1569 | 1570 | 1571 | 1572 | 1573 | 0.11877 -0.01 0 0 0 0 1574 | left-achilles-rod 1575 | left-heel-spring 1576 | 1577 | 0 0 1 1578 | 1579 | 1580 | 1581 | 1582 | 0.055 0 0.008 0 0 0 1583 | left-plantar-rod 1584 | left-foot 1585 | 1586 | 0 0 1 1587 | 1588 | 1589 | 1590 | 1591 | 1592 | 1593 | 1594 | 1595 | 1596 | -------------------------------------------------------------------------------- /cassie_fixed_base/cassie.sdf: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 1 20 | 21 | 22 | 0 0 1.01 0 0 0 23 | 24 | 0.05066 0.000346 0.02841 0 0 0 25 | 10.33 26 | 27 | 0.085821 28 | 0.049222 29 | 0.08626 30 | 1.276e-05 31 | -0.00016022 32 | -0.000414 33 | 34 | 35 | 36 | 37 | 38 | model://cassie/meshes/pelvis.stl 39 | 40 | 41 | 42 | 43 | 0.02 0 0.04 0 0 0 44 | 45 | 46 | 0.14 47 | 48 | 49 | 50 | 51 | 52 | 53 | 0.021 0.135 1.01 0 1.5708 0 54 | 55 | -0.01793 0.0001 -0.04428 0 0 0 56 | 1.82 57 | 58 | 0.003431 59 | 0.003793 60 | 0.002135 61 | -6.65e-07 62 | -0.00084 63 | 3.99e-06 64 | 65 | 66 | 67 | 68 | 69 | model://cassie/meshes/hip-roll.stl 70 | 71 | 72 | 73 | 74 | 75 | 76 | 0.021 0.135 1.01 0 1.5708 0 77 | 78 | 0.000001 0.000001 -0.027063 0 1.57 0 79 | 0.145608 80 | 81 | 0.000073735 82 | 1.340232e-073 83 | 7.241957e-08 84 | 7.357073e-05 85 | -7.096017e-08 86 | 8.262566e-05 87 | 88 | 89 | 90 | 0 0 0 0 -0 0 91 | 92 | 93 | 0.01 0.01 0.01 94 | 95 | 96 | 97 | 98 | 99 | 100 | left-roll-reference 101 | pelvis 102 | 103 | 1 0 0 104 | 1 105 | 106 | 107 | 108 | 109 | left-hip-roll 110 | pelvis 111 | 112 | 1 0 0 113 | 114 | -0.2618 115 | 0.3491 116 | 117 | 118 | 1 119 | 120 | 1 121 | 122 | 123 | 124 | 125 | left-hip-roll 126 | left-roll-reference 127 | pelvis 128 | 0.04 129 | 130 | 1 0 0 131 | 1 132 | 133 | 134 | 1 0 0 135 | 1 136 | 137 | 138 | 139 | 140 | -0.049 0.135 1.01 0 0 0 141 | 142 | 0 -1e-05 -0.034277 0 0 0 143 | 1.171 144 | 145 | 0.002443 146 | 0.002803 147 | 0.000842 148 | -4e-08 149 | 2.462e-07 150 | -2.71e-08 151 | 152 | 153 | 154 | 155 | 156 | model://cassie/meshes/hip-yaw.stl 157 | 158 | 159 | 160 | 161 | 162 | 163 | -0.049 0.135 1.01 0 0 0 164 | 165 | 0.000001 0.000001 -0.027063 0 1.57 0 166 | 0.145608 167 | 168 | 7.368810e-05 169 | 1.340232e-073 170 | 7.241957e-08 171 | 7.357073e-05 172 | -7.096017e-08 173 | 0.000082672 174 | 175 | 176 | 177 | 0 0 0 0 -0 0 178 | 179 | 180 | 0.01 0.01 0.01 181 | 182 | 183 | 184 | 185 | 186 | 187 | left-yaw-reference 188 | left-hip-roll 189 | 190 | 0 0 1 191 | 1 192 | 193 | 194 | 195 | 196 | left-hip-yaw 197 | left-hip-roll 198 | 199 | 0 0 1 200 | 201 | -0.3840 202 | 0.3840 203 | 204 | 205 | 1 206 | 207 | 1 208 | 209 | 210 | 211 | 212 | left-hip-yaw 213 | left-yaw-reference 214 | left-hip-roll 215 | 0.04 216 | 217 | 0 0 1 218 | 1 219 | 220 | 221 | 0 0 1 222 | 1 223 | 224 | 225 | 226 | 227 | -0.049 0.135 0.92 0 1.5708 -1.57 228 | 229 | 0.05946 5e-05 -0.03581 0 0 0 230 | 5.52 231 | 232 | 0.010898 233 | 0.029714 234 | 0.030257 235 | -0.0002669 236 | -5.721e-05 237 | 9.17e-06 238 | 239 | 240 | 241 | 242 | 243 | model://cassie/meshes/hip-pitch.stl 244 | 245 | 246 | 247 | 248 | 0.053 0.0 -0.0379 0 1.57 0 249 | 250 | 251 | 0.18 252 | 0.06 253 | 254 | 255 | 256 | 257 | 258 | 259 | -0.049 0.135 0.92 0 1.5708 -1.57 260 | 261 | -0.000001 -0.000011 0 1.57 1.57 0 262 | 0.272280 263 | 264 | 2.288457e-04 265 | 6.854960e-07 266 | -4.163281e-08 267 | 0.00023115 268 | -9.248118e-07 269 | 0.00020476 270 | 271 | 272 | 273 | 0 0 0 0 -0 0 274 | 275 | 276 | 0.01 0.01 0.01 277 | 278 | 279 | 280 | 281 | 282 | 283 | left-pitch-reference 284 | left-hip-yaw 285 | 286 | 0 -1 0 287 | 1 288 | 289 | 290 | 291 | 292 | left-hip-pitch 293 | left-hip-yaw 294 | 295 | 0 -1 0 296 | 297 | -0.8727 298 | 1.3963 299 | 300 | 301 | 1 302 | 303 | 1 304 | 305 | 306 | 307 | 308 | left-hip-pitch 309 | left-pitch-reference 310 | left-hip-yaw 311 | 0.0625 312 | 313 | 0 -1 0 314 | 1 315 | 316 | 317 | 0 -1 0 318 | 1 319 | 320 | 321 | 322 | 323 | -0.049 0.09 0.92 -1.7199 0.91441 3.0232 324 | 325 | 0.24719 0 0 0 0 0 326 | 0.1567 327 | 328 | 3.754e-06 329 | 0.004487 330 | 0.004488 331 | -3.74e-08 332 | -1.61e-08 333 | 0 334 | 335 | 336 | 337 | 338 | 339 | model://cassie/meshes/achilles-rod.stl 340 | 341 | 342 | 343 | 344 | 345 | 346 | left-achilles-rod 347 | left-hip-pitch 348 | 349 | 0 -1 0 350 | 351 | 0.01 352 | 353 | 1 354 | 355 | 356 | 357 | 358 | -0.049 0.1305 0.8 -1.5708 0.7854 3.1416 359 | 360 | 0.023 0.03207 -0.002181 0 0 0 361 | 0.7578 362 | 363 | 0.001376 364 | 0.0010335 365 | 0.0021637 366 | -0.00039744 367 | -4.085e-05 368 | -5.374e-05 369 | 370 | 371 | 372 | 373 | 374 | model://cassie/meshes/knee.stl 375 | 376 | 377 | 378 | 379 | 380 | 381 | -0.049 0.1305 0.8 -1.5708 0.7854 3.1416 382 | 383 | -0.000001 -0.000011 0.024425 1.57 1.57 0 384 | 0.272280 385 | 386 | 2.288457e-04 387 | 6.854960e-07 388 | -4.163281e-08 389 | 0.00023115 390 | -9.248118e-07 391 | 0.00020476 392 | 393 | 394 | 395 | 0 0 0 0 -0 0 396 | 397 | 398 | 0.01 0.01 0.01 399 | 400 | 401 | 402 | 403 | 404 | 405 | left-hip-pitch 406 | left-knee-reference 407 | 408 | 0 -1 0 409 | 1 410 | 411 | 412 | 413 | 414 | left-knee 415 | left-hip-pitch 416 | 417 | 0 -1 0 418 | 419 | -2.07694 420 | 0.13963 421 | 422 | 423 | 1 424 | 425 | 1 426 | 427 | 428 | 429 | 430 | left-knee 431 | left-knee-reference 432 | left-hip-pitch 433 | 0.0625 434 | 435 | 0 -1 0 436 | 1 437 | 438 | 439 | 0 -1 0 440 | 1 441 | 442 | 443 | 444 | 445 | -0.033635 0.1305 0.69882 -1.5708 0.7854 3.1416 446 | 447 | 0.0836 0.0034 0 0 0 0 448 | 0.186 449 | 450 | 5.215e-05 451 | 0.00041205 452 | 0.0003669 453 | 6.87e-06 454 | 0 455 | 0 456 | 457 | 458 | 459 | 460 | 461 | model://cassie/meshes/knee-spring.stl 462 | 463 | 464 | 465 | 466 | 467 | 468 | left-knee-spring 469 | left-knee 470 | 471 | 0.000796 -1 1e-06 472 | 1 473 | 474 | 475 | 476 | 477 | -0.058383 0.1305 0.72357 -1.5708 0.7854 3.1416 478 | 479 | 0.18338 0.001169 0.0002123 0 0 0 480 | 0.577 481 | 482 | 0.00035939 483 | 0.014728 484 | 0.014707 485 | -0.00020981 486 | 2.266e-05 487 | -1.2e-07 488 | 489 | 490 | 491 | 492 | 493 | model://cassie/meshes/shin.stl 494 | 495 | 496 | 497 | 498 | 0.22 0.01 0.0 0 1.57 0 499 | 500 | 501 | 0.44 502 | 0.04 503 | 504 | 505 | 506 | 507 | 508 | 509 | left-shin 510 | left-knee 511 | 512 | 0.000796 -1 1e-06 513 | 514 | -0.34907 515 | 0.34907 516 | 517 | 518 | 0.1 519 | 0 520 | 1500 521 | 522 | 1 523 | 524 | 525 | 526 | 527 | -0.35166 0.1305 0.402 1.5708 1.3439 0 528 | 529 | 0.11046 -0.03058 -0.00131 0 0 0 530 | 0.782 531 | 532 | 0.00039238 533 | 0.013595 534 | 0.013674 535 | 0.00023651 536 | -4.987e-05 537 | -4.82e-06 538 | 539 | 540 | 541 | 542 | 543 | model://cassie/meshes/tarsus.stl 544 | 545 | 546 | 547 | 548 | 0.21 -0.03 0.0 0 1.57 0 549 | 550 | 551 | 0.38 552 | 0.02 553 | 554 | 555 | 556 | 557 | 558 | 559 | left-tarsus 560 | left-shin 561 | 562 | 0 -1 0 563 | 564 | -0.13963 565 | 1.95477 566 | 567 | 568 | 0.1 569 | 570 | 1 571 | 572 | 573 | 574 | 575 | -0.38432 0.12958 0.40749 1.6878 -1.3734 -0.18949 576 | 577 | 0.081 0.0022 0 0 0 0 578 | 0.126 579 | 580 | 2.959e-05 581 | 0.00022231 582 | 0.0002007 583 | 7.15e-06 584 | -6e-07 585 | 1e-07 586 | 587 | 588 | 589 | 590 | 591 | model://cassie/meshes/heel-spring.stl 592 | 593 | 594 | 595 | 596 | 597 | 598 | left-heel-spring 599 | left-tarsus 600 | 601 | 0 -1 0 602 | 603 | 0 604 | 0 605 | 1250 606 | 607 | 1 608 | 609 | 610 | 611 | 612 | -0.37174 0.10775 0.33784 1.5708 1.3439 0 613 | 614 | 0.00493 2e-05 -0.00215 0 0 0 615 | 0.1261 616 | 617 | 2.6941e-05 618 | 4.9621e-05 619 | 6.3362e-05 620 | -2.1e-09 621 | -3.9623e-06 622 | -1.09e-08 623 | 624 | 625 | 626 | 627 | 628 | model://cassie/meshes/foot-crank.stl 629 | 630 | 631 | 632 | 633 | 634 | 635 | left-foot-crank 636 | left-tarsus 637 | 638 | 0 -1 0 639 | 640 | -2.4435 641 | -0.6109 642 | 643 | 644 | 1 645 | 646 | 1 647 | 648 | 649 | 650 | 651 | -0.35937 0.11566 0.28425 1.5708 1.3639 0.1 652 | 653 | 0.17792 0 0 0 0 0 654 | 0.1186 655 | 656 | 2.779e-06 657 | 0.001774 658 | 0.001775 659 | -2.34e-08 660 | -8.1e-09 661 | 0 662 | 663 | 664 | 665 | 666 | 667 | model://cassie/meshes/plantar-rod.stl 668 | 669 | 670 | 671 | 672 | 673 | 674 | left-plantar-rod 675 | left-foot-crank 676 | 677 | 0 -1 0 678 | 1 679 | 680 | 681 | 682 | 683 | -0.29886 0.1305 -0.0045361 1.5708 1.3439 0 684 | 685 | 0.00474 0.02748 -0.00014 0 0 0 686 | 0.1498 687 | 688 | 0.00017388 689 | 0.00016793 690 | 0.00033261 691 | 0.00011814 692 | 1.36e-06 693 | -4e-07 694 | 695 | 696 | 697 | 698 | 699 | model://cassie/meshes/foot.stl 700 | 701 | 702 | 703 | 704 | 0.01 0.045 0 0 1.57 -0.695 705 | 706 | 707 | 0.18 708 | 0.0168 709 | 710 | 711 | 712 | 713 | 714 | 0.005 715 | 716 | 717 | 718 | 719 | 720 | 721 | 722 | -0.29886 0.1305 -0.0045361 1.5708 1.3439 0 723 | 724 | 0.00474 0.02748 -0.00014 0 0 0 725 | 0.001 726 | 727 | 9.4e-7 728 | 9.4e-9 729 | 9.4e-9 730 | 9.4e-7 731 | 9.4e-9 732 | 9.4e-6 733 | 734 | 735 | 736 | 0 0 0 0 -0 0 737 | 738 | 739 | 0.01 0.01 0.01 740 | 741 | 742 | 743 | 744 | 745 | 746 | left-tarsus 747 | left-foot-reference 748 | 749 | 0 -1 0 750 | 1 751 | 752 | 753 | 754 | 755 | left-foot 756 | left-tarsus 757 | 758 | 0 -1 0 759 | 760 | -2.4435 761 | -0.6109 762 | 763 | 764 | 1 765 | 766 | 1 767 | 768 | 769 | 770 | 771 | left-foot 772 | left-foot-reference 773 | left-tarsus 774 | 0.02 775 | 776 | 0 -1 0 777 | 1 778 | 779 | 780 | 0 -1 0 781 | 1 782 | 783 | 784 | 785 | 786 | 0.021 -0.135 1.01 0 1.5708 0 787 | 788 | -0.01793 -junk0.0001 -0.04428 0 0 0 789 | 1.82 790 | 791 | 0.003431 792 | 0.003793 793 | 0.002135 794 | 6.65e-07 795 | -0.00084 796 | -3.99e-06 797 | 798 | 799 | 800 | 801 | 802 | 1 -1 1 803 | model://cassie/meshes/hip-roll.stl 804 | 805 | 806 | 807 | 808 | 809 | 810 | 0.021 -0.135 1.01 0 1.5708 0 811 | 812 | 0.000001 0.000001 -0.027063 0 1.57 0 813 | 0.145608 814 | 815 | 0.000073735 816 | 1.340232e-073 817 | 7.241957e-08 818 | 7.357073e-05 819 | -7.096017e-08 820 | 8.262566e-05 821 | 822 | 823 | 824 | 0 0 0 0 -0 0 825 | 826 | 827 | 0.01 0.01 0.01 828 | 829 | 830 | 831 | 832 | 833 | 834 | right-roll-reference 835 | pelvis 836 | 837 | 1 0 0 838 | 1 839 | 840 | 841 | 842 | 843 | right-hip-roll 844 | pelvis 845 | 846 | 1 0 0 847 | 848 | -0.3491 849 | 0.2618 850 | 851 | 852 | 1 853 | 854 | 1 855 | 856 | 857 | 858 | 859 | right-hip-roll 860 | right-roll-reference 861 | pelvis 862 | 0.04 863 | 864 | 1 0 0 865 | 1 866 | 867 | 868 | 1 0 0 869 | 1 870 | 871 | 872 | 873 | 874 | -0.049 -0.135 1.01 0 0 0 875 | 876 | 0 1e-05 -0.034277 0 0 0 877 | 1.171 878 | 879 | 0.002443 880 | 0.002803 881 | 0.000842 882 | 4e-08 883 | 2.462e-07 884 | 2.71e-08 885 | 886 | 887 | 888 | 889 | 890 | 1 1 1 891 | model://cassie/meshes/hip-yaw.stl 892 | 893 | 894 | 895 | 896 | 897 | 898 | -0.049 -0.135 1.01 0 0 0 899 | 900 | 0.000001 0.000001 -0.027063 0 1.57 0 901 | 0.145608 902 | 903 | 7.368810e-05 904 | 1.340232e-073 905 | 7.241957e-08 906 | 7.357073e-05 907 | -7.096017e-08 908 | 0.000082672 909 | 910 | 911 | 912 | 0 0 0 0 -0 0 913 | 914 | 915 | 0.01 0.01 0.01 916 | 917 | 918 | 919 | 920 | 921 | 922 | right-yaw-reference 923 | right-hip-roll 924 | 925 | 0 0 1 926 | 1 927 | 928 | 929 | 930 | 931 | right-hip-yaw 932 | right-hip-roll 933 | 934 | 0 0 1 935 | 936 | -0.3840 937 | 0.3840 938 | 939 | 940 | 1 941 | 942 | 1 943 | 944 | 945 | 946 | 947 | right-hip-yaw 948 | right-yaw-reference 949 | right-hip-roll 950 | 0.04 951 | 952 | 0 0 1 953 | 1 954 | 955 | 956 | 0 0 1 957 | 1 958 | 959 | 960 | 961 | 962 | -0.049 -0.135 0.92 0 1.5708 -1.57 963 | 964 | 0.05946 5e-05 0.03581 0 0 0 965 | 5.52 966 | 967 | 0.010898 968 | 0.029714 969 | 0.030257 970 | -0.0002669 971 | 5.721e-05 972 | -9.17e-06 973 | 974 | 975 | 976 | 977 | 978 | 1 1 -1 979 | model://cassie/meshes/hip-pitch.stl 980 | 981 | 982 | 983 | 984 | 0.053 0.0 0.0379 0 1.57 0 985 | 986 | 987 | 0.18 988 | 0.06 989 | 990 | 991 | 992 | 993 | 994 | 995 | -0.049 -0.135 0.92 0 1.5708 -1.57 996 | 997 | -0.000001 -0.000011 0 1.57 1.57 0 998 | 0.272280 999 | 1000 | 2.288457e-04 1001 | 6.854960e-07 1002 | -4.163281e-08 1003 | 0.00023115 1004 | -9.248118e-07 1005 | 0.00020476 1006 | 1007 | 1008 | 1009 | 0 0 0 0 -0 0 1010 | 1011 | 1012 | 0.01 0.01 0.01 1013 | 1014 | 1015 | 1016 | 1017 | 1018 | 1019 | right-pitch-reference 1020 | right-hip-yaw 1021 | 1022 | 0 -1 0 1023 | 1 1024 | 1025 | 1026 | 1027 | 1028 | right-hip-pitch 1029 | right-hip-yaw 1030 | 1031 | 0 -1 0 1032 | 1033 | -0.8727 1034 | 1.3963 1035 | 1036 | 1037 | 1 1038 | 1039 | 1 1040 | 1041 | 1042 | 1043 | 1044 | right-hip-pitch 1045 | right-pitch-reference 1046 | right-hip-yaw 1047 | 0.0625 1048 | 1049 | 0 -1 0 1050 | 1 1051 | 1052 | 1053 | 0 -1 0 1054 | 1 1055 | 1056 | 1057 | 1058 | 1059 | -0.049 -0.09 0.92 -1.4217 0.91441 -3.0232 1060 | 1061 | 0.24719 0 0 0 0 0 1062 | 0.1567 1063 | 1064 | 3.754e-06 1065 | 0.004487 1066 | 0.004488 1067 | -3.74e-08 1068 | 1.61e-08 1069 | 0 1070 | 1071 | 1072 | 1073 | 1074 | 1075 | 1 1 1 1076 | model://cassie/meshes/achilles-rod.stl 1077 | 1078 | 1079 | 1080 | 1081 | 1082 | 1083 | right-achilles-rod 1084 | right-hip-pitch 1085 | 1086 | 0 -1 0 1087 | 1088 | 0.01 1089 | 1090 | 1 1091 | 1092 | 1093 | 1094 | 1095 | -0.049 -0.1305 0.8 -1.5708 0.7854 3.1416 1096 | 1097 | 0.023 0.03207 0.002181 0 0 0 1098 | 0.7578 1099 | 1100 | 0.001376 1101 | 0.0010335 1102 | 0.0021637 1103 | -0.00039744 1104 | 4.085e-05 1105 | 5.374e-05 1106 | 1107 | 1108 | 1109 | 1110 | 1111 | 1 1 1 1112 | model://cassie/meshes/knee.stl 1113 | 1114 | 1115 | 1116 | 1117 | 1118 | 1119 | -0.049 -0.1305 0.8 -1.5708 0.7854 3.1416 1120 | 1121 | -0.000001 -0.000011 -0.024425 1.57 1.57 0 1122 | 0.272280 1123 | 1124 | 2.288457e-04 1125 | 6.854960e-07 1126 | -4.163281e-08 1127 | 0.00023115 1128 | -9.248118e-07 1129 | 0.00020476 1130 | 1131 | 1132 | 1133 | 0 0 0 0 -0 0 1134 | 1135 | 1136 | 0.01 0.01 0.01 1137 | 1138 | 1139 | 1140 | 1141 | 1142 | 1143 | right-hip-pitch 1144 | right-knee-reference 1145 | 1146 | 0 -1 0 1147 | 1 1148 | 1149 | 1150 | 1151 | 1152 | right-knee 1153 | right-hip-pitch 1154 | 1155 | 0 -1 0 1156 | 1157 | -2.07694 1158 | 0.13963 1159 | 1160 | 1161 | 1 1162 | 1163 | 1 1164 | 1165 | 1166 | 1167 | 1168 | right-knee 1169 | right-knee-reference 1170 | right-hip-pitch 1171 | 0.0625 1172 | 1173 | 0 -1 0 1174 | 1 1175 | 1176 | 1177 | 0 -1 0 1178 | 1 1179 | 1180 | 1181 | 1182 | 1183 | -0.033635 -0.1305 0.69882 -1.5708 0.7854 3.1416 1184 | 1185 | 0.0836 0.0034 0 0 0 0 1186 | 0.186 1187 | 1188 | 5.215e-05 1189 | 0.00041205 1190 | 0.0003669 1191 | 6.87e-06 1192 | 0 1193 | 0 1194 | 1195 | 1196 | 1197 | 1198 | 1199 | 1 1 1 1200 | model://cassie/meshes/knee-spring.stl 1201 | 1202 | 1203 | 1204 | 1205 | 1206 | 1207 | right-knee-spring 1208 | right-knee 1209 | 1210 | 0 -1 0 1211 | 1 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | -0.058383 -0.1305 0.72357 -1.5708 0.7854 3.1416 1218 | 1219 | 0.18338 0.001169 -0.0002123 0 0 0 1220 | 0.577 1221 | 1222 | 0.00035939 1223 | 0.014728 1224 | 0.014707 1225 | -0.00020981 1226 | -2.266e-05 1227 | 1.2e-07 1228 | 1229 | 1230 | 1231 | 1232 | 1233 | 1 1 1 1234 | model://cassie/meshes/shin.stl 1235 | 1236 | 1237 | 1238 | 1239 | 0.22 0.01 0.0 0 1.57 0 1240 | 1241 | 1242 | 0.44 1243 | 0.04 1244 | 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | right-shin 1251 | right-knee 1252 | 1253 | 0 -1 0 1254 | 1255 | -0.34907 1256 | 0.34907 1257 | 1258 | 1259 | 0.1 1260 | 0 1261 | 1500 1262 | 1263 | 1 1264 | 1265 | 1266 | 1267 | 1268 | -0.35166 -0.1305 0.402 1.5708 1.3439 0 1269 | 1270 | 0.11046 -0.03058 0.00131 0 0 0 1271 | 0.782 1272 | 1273 | 0.00039238 1274 | 0.013595 1275 | 0.013674 1276 | 0.00023651 1277 | 4.987e-05 1278 | 4.82e-06 1279 | 1280 | 1281 | 1282 | 1283 | 1284 | 1 1 1 1285 | model://cassie/meshes/tarsus.stl 1286 | 1287 | 1288 | 1289 | 1290 | 0.21 -0.03 0.0 0 1.57 0 1291 | 1292 | 1293 | 0.38 1294 | 0.02 1295 | 1296 | 1297 | 1298 | 1299 | 1300 | 1301 | right-tarsus 1302 | right-shin 1303 | 1304 | 0 -1 0 1305 | 1306 | -0.13963 1307 | 1.95477 1308 | 1309 | 1310 | 0.1 1311 | 1312 | 1 1313 | 1314 | 1315 | 1316 | 1317 | -0.38432 -0.12958 0.40749 1.4538 -1.3734 0.18949 1318 | 1319 | 0.081 0.0022 0 0 0 0 1320 | 0.126 1321 | 1322 | 2.959e-05 1323 | 0.00022231 1324 | 0.0002007 1325 | 7.15e-06 1326 | 6e-07 1327 | -1e-07 1328 | 1329 | 1330 | 1331 | 1332 | 1333 | 1 1 1 1334 | model://cassie/meshes/heel-spring.stl 1335 | 1336 | 1337 | 1338 | 1339 | 1340 | 1341 | right-heel-spring 1342 | right-tarsus 1343 | 1344 | 0 -1 0 1345 | 1346 | 0 1347 | 1250 1348 | 1349 | 1 1350 | 1351 | 1352 | 1353 | 1354 | -0.37174 -0.10775 0.33784 1.5708 1.3439 0 1355 | 1356 | 0.00493 2e-05 0.00215 0 0 0 1357 | 0.1261 1358 | 1359 | 2.6941e-05 1360 | 4.9621e-05 1361 | 6.3362e-05 1362 | -2.1e-09 1363 | 3.9623e-06 1364 | 1.09e-08 1365 | 1366 | 1367 | 1368 | 1369 | 1370 | 1 1 -1 1371 | model://cassie/meshes/foot-crank.stl 1372 | 1373 | 1374 | 1375 | 1376 | 1377 | 1378 | right-foot-crank 1379 | right-tarsus 1380 | 1381 | 0 -1 0 1382 | 1383 | -2.4435 1384 | -0.6109 1385 | 1386 | 1387 | 1 1388 | 1389 | 1 1390 | 1391 | 1392 | 1393 | 1394 | -0.35937 -0.11566 0.28425 1.5708 1.364 -0.1 1395 | 1396 | 0.17792 0 0 0 0 0 1397 | 0.1186 1398 | 1399 | 2.779e-06 1400 | 0.001774 1401 | 0.001775 1402 | -2.34e-08 1403 | 8.1e-09 1404 | 0 1405 | 1406 | 1407 | 1408 | 1409 | 1410 | 1 1 1 1411 | model://cassie/meshes/plantar-rod.stl 1412 | 1413 | 1414 | 1415 | 1416 | 1417 | 1418 | right-plantar-rod 1419 | right-foot-crank 1420 | 1421 | 0 -1 0 1422 | 1 1423 | 1424 | 1425 | 1426 | 1427 | -0.29886 -0.1305 -0.0045361 1.5708 1.364 0 1428 | 1429 | 0.00474 0.02748 0.00014 0 0 0 1430 | 0.1498 1431 | 1432 | 0.00017388 1433 | 0.00016793 1434 | 0.00033261 1435 | 0.00011814 1436 | -1.36e-06 1437 | 4e-07 1438 | 1439 | 1440 | 1441 | 1442 | 1443 | 1 1 -1 1444 | model://cassie/meshes/foot.stl 1445 | 1446 | 1447 | 1448 | 1449 | 0.01 0.045 0 0 1.57 -0.695 1450 | 1451 | 1452 | 0.18 1453 | 0.0168 1454 | 1455 | 1456 | 1457 | 1458 | 1459 | 0.005 1460 | 1461 | 1462 | 1463 | 1464 | 1465 | 1466 | 1467 | -0.29886 -0.1305 -0.0045361 1.5708 1.3439 0 1468 | 1469 | 0.00474 0.02748 0.00014 0 0 0 1470 | 0.001 1471 | 1472 | 9.4e-7 1473 | 9.4e-9 1474 | 9.4e-9 1475 | 9.4e-7 1476 | 9.4e-9 1477 | 9.4e-6 1478 | 1479 | 1480 | 1481 | 0 0 0 0 -0 0 1482 | 1483 | 1484 | 0.01 0.01 0.01 1485 | 1486 | 1487 | 1488 | 1489 | 1490 | 1491 | right-tarsus 1492 | right-foot-reference 1493 | 1494 | 0 -1 0 1495 | 1 1496 | 1497 | 1498 | 1499 | 1500 | right-foot 1501 | right-tarsus 1502 | 1503 | 0 -1 0 1504 | 1505 | -2.4435 1506 | -0.6109 1507 | 1508 | 1509 | 1 1510 | 1511 | 1 1512 | 1513 | 1514 | 1515 | 1516 | right-foot 1517 | right-foot-reference 1518 | right-tarsus 1519 | 0.02 1520 | 1521 | 0 -1 0 1522 | 1 1523 | 1524 | 1525 | 0 -1 0 1526 | 1 1527 | 1528 | 1529 | 1530 | 1531 | 0.13 0.005 0 0 0 0 1532 | right-shin 1533 | right-knee-spring 1534 | 1535 | 1 0 0 1536 | 1537 | 0 1538 | 1539 | 1540 | 1541 | 1542 | 1543 | 0.11877 -0.01 0 0 0 0 1544 | right-achilles-rod 1545 | right-heel-spring 1546 | 1547 | 0 0 1 1548 | 1549 | 1550 | 1551 | 1552 | 0.055 0 -0.008 0 0 0 1553 | right-plantar-rod 1554 | right-foot 1555 | 1556 | 0 0 1 1557 | 1558 | 1559 | 1560 | 1561 | 0.13 0.005 0 0 0 0 1562 | left-shin 1563 | left-knee-spring 1564 | 1565 | 1 0 0 1566 | 1567 | 0 1568 | 1569 | 1570 | 1571 | 1572 | 1573 | 0.11877 -0.01 0 0 0 0 1574 | left-achilles-rod 1575 | left-heel-spring 1576 | 1577 | 0 0 1 1578 | 1579 | 1580 | 1581 | 1582 | 0.055 0 0.008 0 0 0 1583 | left-plantar-rod 1584 | left-foot 1585 | 1586 | 0 0 1 1587 | 1588 | 1589 | 1590 | 1591 | world 1592 | pelvis 1593 | 1594 | 0 0 1 1595 | 1596 | 0 1597 | 0.2 1598 | 1599 | 1600 | 1601 | 1602 | 1603 | 1604 | 1605 | 1606 | 1607 | 1608 | --------------------------------------------------------------------------------