diff --git a/unitree_legged_msgs/CMakeLists.txt b/unitree_legged_msgs/CMakeLists.txt index 4ae8a82..31f9bca 100644 --- a/unitree_legged_msgs/CMakeLists.txt +++ b/unitree_legged_msgs/CMakeLists.txt @@ -12,6 +12,8 @@ add_message_files( FILES MotorCmd.msg MotorState.msg + BmsCmd.msg + BmsState.msg Cartesian.msg IMU.msg LED.msg diff --git a/unitree_legged_msgs/msg/BmsCmd.msg b/unitree_legged_msgs/msg/BmsCmd.msg new file mode 100644 index 0000000..35e992f --- /dev/null +++ b/unitree_legged_msgs/msg/BmsCmd.msg @@ -0,0 +1,2 @@ +uint8 off # off 0xA5 +uint8[3] reserve \ No newline at end of file diff --git a/unitree_legged_msgs/msg/BmsState.msg b/unitree_legged_msgs/msg/BmsState.msg new file mode 100644 index 0000000..7d54c56 --- /dev/null +++ b/unitree_legged_msgs/msg/BmsState.msg @@ -0,0 +1,9 @@ +uint8 version_h +uint8 version_l +uint8 bms_status +uint8 SOC # SOC 0-100% +int32 current # mA +uint16 cycle +int8[2] BQ_NTC # x1 degrees centigrade +int8[2] MCU_NTC # x1 degrees centigrade +uint16[10] cell_vol # cell voltage mV \ No newline at end of file diff --git a/unitree_legged_msgs/msg/HighCmd.msg b/unitree_legged_msgs/msg/HighCmd.msg index 65aa796..e61b29e 100644 --- a/unitree_legged_msgs/msg/HighCmd.msg +++ b/unitree_legged_msgs/msg/HighCmd.msg @@ -3,17 +3,31 @@ uint16 commVersion # Old version Aliengo does not have uint16 robotID # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have -uint8 mode -float32 forwardSpeed -float32 sideSpeed -float32 rotateSpeed -float32 bodyHeight -float32 footRaiseHeight -float32 yaw -float32 pitch -float32 roll +uint8 mode # 0. idle, default stand + # 1. force stand (controlled by dBodyHeight + ypr) + # 2. target velocity walking (controlled by velocity + yawSpeed) + # 3. target position walking (controlled by position + ypr[0]) + # 4. path mode walking (reserve for future release) + # 5. position stand down. + # 6. position stand up + # 7. damping mode + # 8. recovery stand + # 9. backflip + # 10. jumpYaw + # 11. straightHand + # 12. dance1 + # 13. dance2 + # 14. two leg stand +uint8 gaitType # 0.idle 1.trot 2.trot running 3.climb stair +uint8 speedLevel # 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3 +float32 footRaiseHeight # (unit: m, default: 0.08m), foot up height while walking +float32 bodyHeight # (unit: m, default: 0.28m), +float32[2] postion # (unit: m), desired position in inertial frame +float32[3] euler # (unit: rad), roll pitch yaw in stand mode +float32[2] velocity # (unit: m/s), forwardSpeed, sideSpeed in body frame +float32 yawSpeed # (unit: rad/s), rotateSpeed in body frame +BmsCmd bms LED[4] led uint8[40] wirelessRemote -uint8[40] AppRemote # Old version Aliengo does not have uint32 reserve # Old version Aliengo does not have int32 crc \ No newline at end of file diff --git a/unitree_legged_msgs/msg/HighState.msg b/unitree_legged_msgs/msg/HighState.msg index 08fa238..f2cf8e1 100644 --- a/unitree_legged_msgs/msg/HighState.msg +++ b/unitree_legged_msgs/msg/HighState.msg @@ -4,23 +4,20 @@ uint16 robotID # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have uint8 mode +float32 progress # new on Go1, reserve IMU imu -float32 forwardSpeed -float32 sideSpeed -float32 rotateSpeed -float32 bodyHeight -float32 updownSpeed -float32 forwardPosition # (will be float type next version) # Old version Aliengo is different -float32 sidePosition # (will be float type next version) # Old version Aliengo is different -Cartesian[4] footPosition2Body -Cartesian[4] footSpeed2Body +uint8 gaitType # new on Go1, 0.idle 1.trot 2.trot running 3.climb stair +float32 footRaiseHeight # (unit: m, default: 0.08m), foot up height while walking +float32[3] position # (unit: m), from own odometry in inertial frame, usually drift +float32 bodyHeight # (unit: m, default: 0.28m) +float32[3] velocity # (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame +float32 yawSpeed # (unit: rad/s), rotateSpeed in body frame +Cartesian[4] footPosition2Body # foot position relative to body +Cartesian[4] footSpeed2Body # foot speed relative to body +int8[20] temperature +BmsState bms int16[4] footForce # Old version Aliengo is different int16[4] footForceEst # Old version Aliengo does not have -uint32 tick uint8[40] wirelessRemote uint32 reserve # Old version Aliengo does not have -uint32 crc - -# Under are not defined in SDK yet. # Old version Aliengo does not have -Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization. -float32[12] jointP # for visualization \ No newline at end of file +uint32 crc \ No newline at end of file diff --git a/unitree_legged_msgs/msg/LowCmd.msg b/unitree_legged_msgs/msg/LowCmd.msg index 62f28ba..d0b1ac7 100644 --- a/unitree_legged_msgs/msg/LowCmd.msg +++ b/unitree_legged_msgs/msg/LowCmd.msg @@ -4,7 +4,8 @@ uint16 robotID # Old version Aliengo does not have uint32 SN # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have MotorCmd[20] motorCmd -LED[4] led +BmsCmd bms # new on Go1, battery command +# LED[4] led uint8[40] wirelessRemote uint32 reserve # Old version Aliengo does not have uint32 crc diff --git a/unitree_legged_msgs/msg/LowState.msg b/unitree_legged_msgs/msg/LowState.msg index 2614fa6..9c3f95b 100644 --- a/unitree_legged_msgs/msg/LowState.msg +++ b/unitree_legged_msgs/msg/LowState.msg @@ -5,6 +5,7 @@ uint32 SN # Old version Aliengo does not have uint8 bandWidth # Old version Aliengo does not have IMU imu MotorState[20] motorState +BmsState bms # new on Go1, battery state int16[4] footForce # force sensors # Old version Aliengo is different int16[4] footForceEst # force sensors # Old version Aliengo does not have uint32 tick # reference real-time from motion controller (unit: us) diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt index 9c91639..64dd880 100755 --- a/unitree_legged_real/CMakeLists.txt +++ b/unitree_legged_real/CMakeLists.txt @@ -19,30 +19,14 @@ include_directories( set(CMAKE_CXX_FLAGS "-O3") -if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1") - include_directories($ENV{ALIENGO_SDK_PATH}/include) - link_directories($ENV{ALIENGO_SDK_PATH}/lib) - string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so) - set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) +include_directories(/home/bian/Robot_SDK/unitree_legged_sdk/include) +link_directories(/home/bian/Robot_SDK/unitree_legged_sdk/lib) +string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk.so) +set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) - add_definitions(-DSDK3_1) - - add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp) - target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES}) - add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -elseif( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_2") - include_directories($ENV{UNITREE_LEGGED_SDK_PATH}/include) - link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib) - string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so) - set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) - - add_definitions(-DSDK3_2) - - add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp) - target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES}) - add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -endif() +add_executable(lcm_server /home/bian/Robot_SDK/unitree_legged_sdk/examples/lcm_server.cpp) +target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(lcm_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_executable(position_lcm src/exe/position_mode.cpp) target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h index 833b29b..9abd480 100755 --- a/unitree_legged_real/include/convert.h +++ b/unitree_legged_real/include/convert.h @@ -12,160 +12,46 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include +#include +#include #include - -#ifdef SDK3_1 -#include "aliengo_sdk/aliengo_sdk.hpp" - -unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){ - unitree_legged_msgs::IMU ros; - ros.quaternion[0] = lcm.quaternion[0]; - ros.quaternion[1] = lcm.quaternion[1]; - ros.quaternion[2] = lcm.quaternion[2]; - ros.quaternion[3] = lcm.quaternion[3]; - ros.gyroscope[0] = lcm.gyroscope[0]; - ros.gyroscope[1] = lcm.gyroscope[1]; - ros.gyroscope[2] = lcm.gyroscope[2]; - ros.accelerometer[0] = lcm.acceleration[0]; - ros.accelerometer[1] = lcm.acceleration[1]; - ros.accelerometer[2] = lcm.acceleration[2]; - // ros.rpy[0] = lcm.rpy[0]; - // ros.rpy[1] = lcm.rpy[1]; - // ros.rpy[2] = lcm.rpy[2]; - ros.temperature = lcm.temp; - return ros; -} - -unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){ - unitree_legged_msgs::MotorState ros; - ros.mode = lcm.mode; - ros.q = lcm.position; - ros.dq = lcm.velocity; - ros.ddq = 0; - ros.tauEst = lcm.torque; - ros.q_raw = 0; - ros.dq_raw = 0; - ros.ddq_raw = 0; - ros.temperature = lcm.temperature; - return ros; -} - -aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){ - aliengo::MotorCmd lcm; - lcm.mode = ros.mode; - lcm.position = ros.q; - lcm.velocity = ros.dq; - lcm.positionStiffness = ros.Kp; - lcm.velocityStiffness = ros.Kd; - lcm.torque = ros.tau; - return lcm; -} - -unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){ - unitree_legged_msgs::LowState ros; - ros.levelFlag = lcm.levelFlag; - ros.commVersion = 0; - ros.robotID = 0; - ros.SN = 0; - ros.bandWidth = 0; - ros.imu = ToRos(lcm.imu); - for(int i = 0; i < 20; i++){ - ros.motorState[i] = ToRos(lcm.motorState[i]); - } - for(int i = 0; i < 4; i++){ - ros.footForce[i] = lcm.footForce[i]; - ros.footForceEst[i] = 0; - } - ros.tick = lcm.tick; - for(int i = 0; i < 40; i++){ - ros.wirelessRemote[i] = lcm.wirelessRemote[i]; - } - ros.crc = lcm.crc; - - return ros; -} - -aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){ - aliengo::LowCmd lcm; - lcm.levelFlag = ros.levelFlag; - for(int i = 0; i < 20; i++){ - lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); - } - for(int i = 0; i < 4; i++){ - lcm.led[i].r = ros.led[i].r; - lcm.led[i].g = ros.led[i].g; - lcm.led[i].b = ros.led[i].b; - } - for(int i = 0; i < 40; i++){ - lcm.wirelessRemote[i] = ros.wirelessRemote[i]; - } - lcm.crc = ros.crc; - return lcm; -} - -unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){ - unitree_legged_msgs::HighState ros; - ros.levelFlag = lcm.levelFlag; - ros.commVersion = 0; - ros.robotID = 0; - ros.SN = 0; - ros.bandWidth = 0; - ros.mode = lcm.mode; - ros.imu = ToRos(lcm.imu); - ros.forwardSpeed = lcm.forwardSpeed; - ros.sideSpeed = lcm.sideSpeed; - ros.rotateSpeed = lcm.rotateSpeed; - ros.bodyHeight = lcm.bodyHeight; - ros.updownSpeed = lcm.updownSpeed; - ros.forwardPosition = lcm.forwardPosition.x; - ros.sidePosition = lcm.sidePosition.y; - for(int i = 0; i < 4; i++){ - ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x; - ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y; - ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z; - ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x; - ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y; - ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z; - ros.footForce[i] = lcm.footForce[i]; - ros.footForceEst[i] = 0; - } - ros.tick = lcm.tick; - for(int i = 0; i<40; i++){ - ros.wirelessRemote[i] = lcm.wirelessRemote[i]; - } - ros.reserve = 0; - ros.crc = lcm.crc; - return ros; -} - -aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){ - aliengo::HighCmd lcm; - lcm.levelFlag = ros.levelFlag; - lcm.mode = ros.mode; - lcm.forwardSpeed = ros.forwardSpeed; - lcm.sideSpeed = ros.sideSpeed; - lcm.rotateSpeed = ros.rotateSpeed; - lcm.bodyHeight = ros.bodyHeight; - lcm.footRaiseHeight = ros.footRaiseHeight; - lcm.yaw = ros.yaw; - lcm.pitch = ros.pitch; - lcm.roll = ros.roll; - for(int i = 0; i < 4; i++){ - lcm.led[i].r = ros.led[i].r; - lcm.led[i].g = ros.led[i].g; - lcm.led[i].b = ros.led[i].b; - } - for(int i = 0; i < 40; i++){ - lcm.wirelessRemote[i] = ros.wirelessRemote[i]; - } - lcm.crc = ros.crc; - return lcm; -} -#endif - -#ifdef SDK3_2 #include "unitree_legged_sdk/unitree_legged_sdk.h" +unitree_legged_msgs::Cartesian ToRos(UNITREE_LEGGED_SDK::Cartesian& lcm){ + unitree_legged_msgs::Cartesian ros; + ros.x = lcm.x; + ros.y = lcm.y; + ros.z = lcm.z; + return ros; +} + +UNITREE_LEGGED_SDK::BmsCmd ToLcm(unitree_legged_msgs::BmsCmd& ros){ + UNITREE_LEGGED_SDK::BmsCmd lcm; + lcm.off = ros.off; + for(int i(0); i<3; ++i){ + lcm.reserve[i] = ros.reserve[i]; + } + return lcm; +} + +unitree_legged_msgs::BmsState ToRos(UNITREE_LEGGED_SDK::BmsState& lcm){ + unitree_legged_msgs::BmsState ros; + ros.version_h = lcm.version_h; + ros.version_l = lcm.version_l; + ros.bms_status = lcm.bms_status; + ros.SOC = lcm.SOC; + ros.current = lcm.current; + ros.cycle = lcm.cycle; + for(int i(0); i<2; ++i){ + ros.BQ_NTC[i] = lcm.BQ_NTC[i]; + ros.MCU_NTC[i] = lcm.MCU_NTC[i]; + } + for(int i(0); i<10; ++i){ + ros.cell_vol[i] = lcm.cell_vol[i]; + } + return ros; +} + unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm) { unitree_legged_msgs::IMU ros; @@ -179,9 +65,6 @@ unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm) ros.accelerometer[0] = lcm.accelerometer[0]; ros.accelerometer[1] = lcm.accelerometer[1]; ros.accelerometer[2] = lcm.accelerometer[2]; - // ros.rpy[0] = lcm.rpy[0]; - // ros.rpy[1] = lcm.rpy[1]; - // ros.rpy[2] = lcm.rpy[2]; ros.temperature = lcm.temperature; return ros; } @@ -230,6 +113,7 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm) for(int i = 0; i<20; i++){ ros.motorState[i] = ToRos(lcm.motorState[i]); } + ros.bms = ToRos(lcm.bms); for(int i = 0; i<4; i++){ ros.footForce[i] = lcm.footForce[i]; ros.footForceEst[i] = lcm.footForceEst[i]; @@ -254,11 +138,7 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGE for(int i = 0; i<20; i++){ lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); } - for(int i = 0; i<4; i++){ - lcm.led[i].r = ros.led[i].r; - lcm.led[i].g = ros.led[i].g; - lcm.led[i].b = ros.led[i].b; - } + lcm.bms = ToLcm(ros.bms); for(int i = 0; i<40; i++){ lcm.wirelessRemote[i] = ros.wirelessRemote[i]; } @@ -276,30 +156,36 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm) ros.SN = lcm.SN; ros.bandWidth = lcm.bandWidth; ros.mode = lcm.mode; + ros.progress = lcm.progress; ros.imu = ToRos(lcm.imu); - ros.forwardSpeed = lcm.forwardSpeed; - ros.sideSpeed = lcm.sideSpeed; - ros.rotateSpeed = lcm.rotateSpeed; + ros.gaitType = lcm.gaitType; + ros.footRaiseHeight = lcm.footRaiseHeight; ros.bodyHeight = lcm.bodyHeight; - ros.updownSpeed = lcm.updownSpeed; - ros.forwardPosition = lcm.forwardPosition; - ros.sidePosition = lcm.sidePosition; - for(int i = 0; i<4; i++){ - ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x; - ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y; - ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z; - ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x; - ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y; - ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z; + ros.yawSpeed = lcm.yawSpeed; + ros.bms = ToRos(lcm.bms); + ros.reserve = lcm.reserve; + ros.crc = lcm.crc; + + for(int i(0); i<3; ++i){ + ros.position[i] = lcm.position[i]; + ros.velocity[i] = lcm.velocity[i]; + } + + for(int i(0); i<4; ++i){ + ros.footPosition2Body[i] = ToRos(lcm.footPosition2Body[i]); + ros.footSpeed2Body[i] = ToRos(lcm.footSpeed2Body[i]); ros.footForce[i] = lcm.footForce[i]; ros.footForceEst[i] = lcm.footForceEst[i]; } - ros.tick = lcm.tick; - for(int i = 0; i<40; i++){ + + for(int i(0); i<20; ++i){ + ros.temperature[i] = lcm.temperature[i]; + } + + for(int i(0); i<40; ++i){ ros.wirelessRemote[i] = lcm.wirelessRemote[i]; } - ros.reserve = lcm.reserve; - ros.crc = lcm.crc; + return ros; } @@ -312,27 +198,35 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG lcm.SN = ros.SN; lcm.bandWidth = ros.bandWidth; lcm.mode = ros.mode; - lcm.forwardSpeed = ros.forwardSpeed; - lcm.sideSpeed = ros.sideSpeed; - lcm.rotateSpeed = ros.rotateSpeed; - lcm.bodyHeight = ros.bodyHeight; + lcm.gaitType = ros.gaitType; + lcm.speedLevel = ros.speedLevel; lcm.footRaiseHeight = ros.footRaiseHeight; - lcm.yaw = ros.yaw; - lcm.pitch = ros.pitch; - lcm.roll = ros.roll; + lcm.bodyHeight = ros.bodyHeight; + lcm.yawSpeed = ros.yawSpeed; + lcm.bms = ToLcm(ros.bms); + lcm.reserve = ros.reserve; + lcm.crc = ros.crc; + + for(int i(0); i<2; ++i){ + lcm.postion[i] = ros.postion[i]; + lcm.velocity[i] = ros.velocity[i]; + } + + for(int i(0); i<3; ++i){ + lcm.euler[i] = ros.euler[i]; + } + for(int i = 0; i<4; i++){ lcm.led[i].r = ros.led[i].r; lcm.led[i].g = ros.led[i].g; lcm.led[i].b = ros.led[i].b; } + for(int i = 0; i<40; i++){ lcm.wirelessRemote[i] = ros.wirelessRemote[i]; - lcm.AppRemote[i] = ros.AppRemote[i]; } - lcm.reserve = ros.reserve; - lcm.crc = ros.crc; + return lcm; } -#endif #endif // _CONVERT_H_ \ No newline at end of file diff --git a/unitree_legged_real/launch/real.launch b/unitree_legged_real/launch/real.launch index a73bb85..0e557c3 100644 --- a/unitree_legged_real/launch/real.launch +++ b/unitree_legged_real/launch/real.launch @@ -1,12 +1,8 @@ - - - + - - \ No newline at end of file diff --git a/unitree_legged_real/src/exe/position_mode.cpp b/unitree_legged_real/src/exe/position_mode.cpp index 55f7683..66316c7 100755 --- a/unitree_legged_real/src/exe/position_mode.cpp +++ b/unitree_legged_real/src/exe/position_mode.cpp @@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include "convert.h" -#ifdef SDK3_1 -using namespace aliengo; -#endif -#ifdef SDK3_2 using namespace UNITREE_LEGGED_SDK; -#endif template void* update_loop(void* param) @@ -116,7 +111,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) } double sin_joint1, sin_joint2; // last, do sine wave - if( motiontime >= 1700){ + if( motiontime >= 400){ sin_count++; sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0); sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0); @@ -164,27 +159,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) int main(int argc, char *argv[]){ ros::init(argc, argv, "position_ros_mode"); - std::string firmwork; - ros::param::get("/firmwork", firmwork); - #ifdef SDK3_1 - aliengo::Control control(aliengo::LOWLEVEL); - aliengo::LCM roslcm; - mainHelper(argc, argv, roslcm); - #endif - - #ifdef SDK3_2 - std::string robot_name; - UNITREE_LEGGED_SDK::LeggedType rname; - ros::param::get("/robot_name", robot_name); - if(strcasecmp(robot_name.c_str(), "A1") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::A1; - else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - - // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - // UNITREE_LEGGED_SDK::InitEnvironment(); - UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); - mainHelper(argc, argv, roslcm); - #endif + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/torque_mode.cpp b/unitree_legged_real/src/exe/torque_mode.cpp index b88a217..d80919a 100755 --- a/unitree_legged_real/src/exe/torque_mode.cpp +++ b/unitree_legged_real/src/exe/torque_mode.cpp @@ -11,12 +11,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include "convert.h" -#ifdef SDK3_1 -using namespace aliengo; -#endif -#ifdef SDK3_2 using namespace UNITREE_LEGGED_SDK; -#endif template void* update_loop(void* param) @@ -89,26 +84,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) int main(int argc, char *argv[]){ ros::init(argc, argv, "torque_ros_mode"); - std::string firmwork; - ros::param::get("/firmwork", firmwork); - #ifdef SDK3_1 - aliengo::Control control(aliengo::LOWLEVEL); - aliengo::LCM roslcm; - mainHelper(argc, argv, roslcm); - #endif - - #ifdef SDK3_2 - std::string robot_name; - UNITREE_LEGGED_SDK::LeggedType rname; - ros::param::get("/robot_name", robot_name); - if(strcasecmp(robot_name.c_str(), "A1") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::A1; - else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - - // UNITREE_LEGGED_SDK::InitEnvironment(); - UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); - mainHelper(argc, argv, roslcm); - #endif + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/velocity_mode.cpp b/unitree_legged_real/src/exe/velocity_mode.cpp index fcddb9a..888c330 100755 --- a/unitree_legged_real/src/exe/velocity_mode.cpp +++ b/unitree_legged_real/src/exe/velocity_mode.cpp @@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include "convert.h" -#ifdef SDK3_1 -using namespace aliengo; -#endif -#ifdef SDK3_2 using namespace UNITREE_LEGGED_SDK; -#endif template void* update_loop(void* param) @@ -92,27 +87,7 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) int main(int argc, char *argv[]){ ros::init(argc, argv, "velocity_ros_mode"); - std::string firmwork; - ros::param::get("/firmwork", firmwork); - #ifdef SDK3_1 - aliengo::Control control(aliengo::LOWLEVEL); - aliengo::LCM roslcm; - mainHelper(argc, argv, roslcm); - #endif - - #ifdef SDK3_2 - std::string robot_name; - UNITREE_LEGGED_SDK::LeggedType rname; - ros::param::get("/robot_name", robot_name); - if(strcasecmp(robot_name.c_str(), "A1") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::A1; - else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - - // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - // UNITREE_LEGGED_SDK::InitEnvironment(); - UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); - mainHelper(argc, argv, roslcm); - #endif + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/walk_mode.cpp b/unitree_legged_real/src/exe/walk_mode.cpp index ee81f05..7f12d0d 100755 --- a/unitree_legged_real/src/exe/walk_mode.cpp +++ b/unitree_legged_real/src/exe/walk_mode.cpp @@ -12,12 +12,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include "convert.h" -#ifdef SDK3_1 -using namespace aliengo; -#endif -#ifdef SDK3_2 using namespace UNITREE_LEGGED_SDK; -#endif template void* update_loop(void* param) @@ -56,73 +51,85 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) motiontime = motiontime+2; roslcm.Get(RecvHighLCM); RecvHighROS = ToRos(RecvHighLCM); - // printf("%f\n", RecvHighROS.forwardSpeed); - SendHighROS.forwardSpeed = 0.0f; - SendHighROS.sideSpeed = 0.0f; - SendHighROS.rotateSpeed = 0.0f; - SendHighROS.bodyHeight = 0.0f; + SendHighROS.mode = 0; + SendHighROS.gaitType = 0; + SendHighROS.speedLevel = 0; + SendHighROS.footRaiseHeight = 0; + SendHighROS.bodyHeight = 0; + SendHighROS.euler[0] = 0; + SendHighROS.euler[1] = 0; + SendHighROS.euler[2] = 0; + SendHighROS.velocity[0] = 0.0f; + SendHighROS.velocity[1] = 0.0f; + SendHighROS.yawSpeed = 0.0f; + SendHighROS.reserve = 0; - SendHighROS.mode = 0; - SendHighROS.roll = 0; - SendHighROS.pitch = 0; - SendHighROS.yaw = 0; - - if(motiontime>1000 && motiontime<1500){ + if(motiontime > 0 && motiontime < 1000){ SendHighROS.mode = 1; - // SendHighROS.roll = 0.3f; + SendHighROS.euler[0] = -0.3; } - - if(motiontime>1500 && motiontime<2000){ + if(motiontime > 1000 && motiontime < 2000){ SendHighROS.mode = 1; - SendHighROS.pitch = 0.3f; + SendHighROS.euler[0] = 0.3; } - - if(motiontime>2000 && motiontime<2500){ + if(motiontime > 2000 && motiontime < 3000){ SendHighROS.mode = 1; - SendHighROS.yaw = 0.2f; + SendHighROS.euler[1] = -0.2; } - - if(motiontime>2500 && motiontime<3000){ + if(motiontime > 3000 && motiontime < 4000){ SendHighROS.mode = 1; - SendHighROS.bodyHeight = -0.3f; + SendHighROS.euler[1] = 0.2; } - - if(motiontime>3000 && motiontime<3500){ + if(motiontime > 4000 && motiontime < 5000){ SendHighROS.mode = 1; - SendHighROS.bodyHeight = 0.3f; + SendHighROS.euler[2] = -0.2; } - - if(motiontime>3500 && motiontime<4000){ + if(motiontime > 5000 && motiontime < 6000){ SendHighROS.mode = 1; - SendHighROS.bodyHeight = 0.0f; + SendHighROS.euler[2] = 0.2; } - - if(motiontime>4000 && motiontime<5000){ + if(motiontime > 6000 && motiontime < 7000){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = -0.2; + } + if(motiontime > 7000 && motiontime < 8000){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = 0.1; + } + if(motiontime > 8000 && motiontime < 9000){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = 0.0; + } + if(motiontime > 9000 && motiontime < 11000){ + SendHighROS.mode = 5; + } + if(motiontime > 11000 && motiontime < 13000){ + SendHighROS.mode = 6; + } + if(motiontime > 13000 && motiontime < 14000){ + SendHighROS.mode = 0; + } + if(motiontime > 14000 && motiontime < 18000){ SendHighROS.mode = 2; + SendHighROS.gaitType = 2; + SendHighROS.velocity[0] = 0.4f; // -1 ~ +1 + SendHighROS.yawSpeed = 2; + SendHighROS.footRaiseHeight = 0.1; + // printf("walk\n"); } - - if(motiontime>5000 && motiontime<8500){ + if(motiontime > 18000 && motiontime < 20000){ + SendHighROS.mode = 0; + SendHighROS.velocity[0] = 0; + } + if(motiontime > 20000 && motiontime < 24000){ SendHighROS.mode = 2; - SendHighROS.forwardSpeed = 0.1f; // -1 ~ +1 + SendHighROS.gaitType = 1; + SendHighROS.velocity[0] = 0.2f; // -1 ~ +1 + SendHighROS.bodyHeight = 0.1; + // printf("walk\n"); } - - if(motiontime>8500 && motiontime<12000){ - SendHighROS.mode = 2; - SendHighROS.forwardSpeed = -0.2f; // -1 ~ +1 - } - - if(motiontime>12000 && motiontime<16000){ - SendHighROS.mode = 2; - SendHighROS.rotateSpeed = 0.1f; // turn - } - - if(motiontime>16000 && motiontime<20000){ - SendHighROS.mode = 2; - SendHighROS.rotateSpeed = -0.1f; // turn - } - - if(motiontime>20000 && motiontime<21000){ + if(motiontime>24000 ){ SendHighROS.mode = 1; } @@ -136,27 +143,8 @@ int mainHelper(int argc, char *argv[], TLCM &roslcm) int main(int argc, char *argv[]){ ros::init(argc, argv, "walk_ros_mode"); - std::string firmwork; - ros::param::get("/firmwork", firmwork); - #ifdef SDK3_1 - aliengo::Control control(aliengo::HIGHLEVEL); - aliengo::LCM roslcm; - mainHelper(argc, argv, roslcm); - #endif - - #ifdef SDK3_2 - std::string robot_name; - UNITREE_LEGGED_SDK::LeggedType rname; - ros::param::get("/robot_name", robot_name); - if(strcasecmp(robot_name.c_str(), "A1") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::A1; - else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) - rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - - // UNITREE_LEGGED_SDK::InitEnvironment(); - UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); - mainHelper(argc, argv, roslcm); - #endif + UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); + mainHelper(argc, argv, roslcm); } \ No newline at end of file