Delete unitree_legged_real directory
This commit is contained in:
keith siilats
GitHub
parent
71c9649d13
commit
2122d87c08
|
|
@ -1,63 +0,0 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(unitree_legged_real)
|
||||
|
||||
add_compile_options(-std=c++11)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
geometry_msgs
|
||||
unitree_legged_msgs
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
|
||||
message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}")
|
||||
if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*")
|
||||
set(ARCH amd64)
|
||||
else()
|
||||
set(ARCH arm64)
|
||||
endif()
|
||||
|
||||
link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH})
|
||||
|
||||
set(EXTRA_LIBS -pthread libunitree_legged_sdk.so)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-O3 -fPIC")
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${CMAKE_SOURCE_DIR}/unitree_legged_sdk/include
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
add_executable(example_walk src/exe/example_walk.cpp)
|
||||
target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(example_position src/exe/example_position.cpp)
|
||||
target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(state_sub src/exe/state_sub.cpp)
|
||||
target_link_libraries(state_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(state_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(ros_udp src/exe/ros_udp.cpp)
|
||||
target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(ros_udp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(control_via_keyboard src/exe/control_via_keyboard.cpp)
|
||||
target_link_libraries(control_via_keyboard ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(control_via_keyboard ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(twist_sub src/exe/twist_sub.cpp)
|
||||
target_link_libraries(twist_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(twist_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
|
|
@ -1,334 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#ifndef _CONVERT_H_
|
||||
#define _CONVERT_H_
|
||||
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/MotorCmd.h>
|
||||
#include <unitree_legged_msgs/MotorState.h>
|
||||
#include <unitree_legged_msgs/BmsCmd.h>
|
||||
#include <unitree_legged_msgs/BmsState.h>
|
||||
#include <unitree_legged_msgs/IMU.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::BmsCmd cmd;
|
||||
|
||||
cmd.off = msg.off;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
cmd.head[i] = msg->head[i];
|
||||
cmd.SN[i] = msg->SN[i];
|
||||
cmd.version[i] = msg->version[i];
|
||||
cmd.position[i] = msg->position[i];
|
||||
cmd.velocity[i] = msg->velocity[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.euler[i] = msg->euler[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
cmd.led[i].r = msg->led[i].r;
|
||||
cmd.led[i].g = msg->led[i].g;
|
||||
cmd.led[i].b = msg->led[i].b;
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
|
||||
cmd.levelFlag = msg->levelFlag;
|
||||
cmd.frameReserve = msg->frameReserve;
|
||||
cmd.bandWidth = msg->bandWidth;
|
||||
cmd.mode = msg->mode;
|
||||
cmd.gaitType = msg->gaitType;
|
||||
cmd.speedLevel = msg->speedLevel;
|
||||
cmd.footRaiseHeight = msg->footRaiseHeight;
|
||||
cmd.bodyHeight = msg->bodyHeight;
|
||||
cmd.yawSpeed = msg->yawSpeed;
|
||||
cmd.reserve = msg->reserve;
|
||||
cmd.crc = msg->crc;
|
||||
|
||||
cmd.bms = rosMsg2Cmd(msg->bms);
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::MotorCmd cmd;
|
||||
|
||||
cmd.mode = msg.mode;
|
||||
cmd.q = msg.q;
|
||||
cmd.dq = msg.dq;
|
||||
cmd.tau = msg.tau;
|
||||
cmd.Kp = msg.Kp;
|
||||
cmd.Kd = msg.Kd;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::LowCmd cmd;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
cmd.head[i] = msg->head[i];
|
||||
cmd.SN[i] = msg->SN[i];
|
||||
cmd.version[i] = msg->version[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]);
|
||||
}
|
||||
|
||||
cmd.bms = rosMsg2Cmd(msg->bms);
|
||||
|
||||
cmd.levelFlag = msg->levelFlag;
|
||||
cmd.frameReserve = msg->frameReserve;
|
||||
cmd.bandWidth = msg->bandWidth;
|
||||
cmd.reserve = msg->reserve;
|
||||
cmd.crc = msg->crc;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state)
|
||||
{
|
||||
unitree_legged_msgs::MotorState ros_msg;
|
||||
|
||||
ros_msg.mode = state.mode;
|
||||
ros_msg.q = state.q;
|
||||
ros_msg.dq = state.dq;
|
||||
ros_msg.ddq = state.ddq;
|
||||
ros_msg.tauEst = state.tauEst;
|
||||
ros_msg.q_raw = state.q_raw;
|
||||
ros_msg.dq_raw = state.dq_raw;
|
||||
ros_msg.ddq_raw = state.ddq_raw;
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
ros_msg.reserve[0] = state.reserve[0];
|
||||
ros_msg.reserve[1] = state.reserve[1];
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
|
||||
{
|
||||
unitree_legged_msgs::IMU ros_msg;
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.quaternion[i] = state.quaternion[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
ros_msg.gyroscope[i] = state.gyroscope[i];
|
||||
ros_msg.accelerometer[i] = state.accelerometer[i];
|
||||
ros_msg.rpy[i] = state.rpy[i];
|
||||
}
|
||||
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
|
||||
{
|
||||
unitree_legged_msgs::BmsState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
|
||||
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 10; i++)
|
||||
{
|
||||
ros_msg.cell_vol[i] = state.cell_vol[i];
|
||||
}
|
||||
|
||||
ros_msg.version_h = state.version_h;
|
||||
ros_msg.version_l = state.version_l;
|
||||
ros_msg.bms_status = state.bms_status;
|
||||
ros_msg.SOC = state.SOC;
|
||||
ros_msg.current = state.current;
|
||||
ros_msg.cycle = state.cycle;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state)
|
||||
{
|
||||
unitree_legged_msgs::LowState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.head[i] = state.head[i];
|
||||
ros_msg.SN[i] = state.SN[i];
|
||||
ros_msg.version[i] = state.version[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
|
||||
}
|
||||
|
||||
ros_msg.imu = state2rosMsg(state.imu);
|
||||
|
||||
ros_msg.bms = state2rosMsg(state.bms);
|
||||
|
||||
ros_msg.tick = state.tick;
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
|
||||
{
|
||||
unitree_legged_msgs::Cartesian ros_msg;
|
||||
|
||||
ros_msg.x = state.x;
|
||||
ros_msg.y = state.y;
|
||||
ros_msg.z = state.z;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
|
||||
{
|
||||
unitree_legged_msgs::HighState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.head[i] = state.head[i];
|
||||
ros_msg.SN[i] = state.SN[i];
|
||||
ros_msg.version[i] = state.version[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
ros_msg.rangeObstacle[i] = state.rangeObstacle[i];
|
||||
ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
|
||||
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
|
||||
}
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
ros_msg.position[i] = state.position[i];
|
||||
ros_msg.velocity[i] = state.velocity[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
{
|
||||
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
|
||||
}
|
||||
|
||||
ros_msg.imu = state2rosMsg(state.imu);
|
||||
|
||||
ros_msg.bms = state2rosMsg(state.bms);
|
||||
|
||||
ros_msg.levelFlag = state.levelFlag;
|
||||
ros_msg.frameReserve = state.frameReserve;
|
||||
ros_msg.bandWidth = state.bandWidth;
|
||||
ros_msg.mode = state.mode;
|
||||
ros_msg.progress = state.progress;
|
||||
ros_msg.gaitType = state.gaitType;
|
||||
ros_msg.footRaiseHeight = state.footRaiseHeight;
|
||||
ros_msg.bodyHeight = state.bodyHeight;
|
||||
ros_msg.yawSpeed = state.yawSpeed;
|
||||
ros_msg.reserve = state.reserve;
|
||||
ros_msg.crc = state.crc;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd cmd;
|
||||
|
||||
cmd.head[0] = 0xFE;
|
||||
cmd.head[1] = 0xEF;
|
||||
cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL;
|
||||
cmd.mode = 0;
|
||||
cmd.gaitType = 0;
|
||||
cmd.speedLevel = 0;
|
||||
cmd.footRaiseHeight = 0;
|
||||
cmd.bodyHeight = 0;
|
||||
cmd.euler[0] = 0;
|
||||
cmd.euler[1] = 0;
|
||||
cmd.euler[2] = 0;
|
||||
cmd.velocity[0] = 0.0f;
|
||||
cmd.velocity[1] = 0.0f;
|
||||
cmd.yawSpeed = 0.0f;
|
||||
cmd.reserve = 0;
|
||||
|
||||
cmd.velocity[0] = msg->linear.x;
|
||||
cmd.velocity[1] = msg->linear.y;
|
||||
cmd.yawSpeed = msg->angular.z;
|
||||
|
||||
cmd.mode = 2;
|
||||
cmd.gaitType = 1;
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
<launch>
|
||||
|
||||
|
||||
<node pkg="unitree_legged_real" type="twist_sub" name="node_twist_sub" output="screen"/>
|
||||
<node pkg="unitree_legged_real" type="control_via_keyboard" name="node_control_via_keyboard" output="screen"/>
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
<launch>
|
||||
<arg name="ctrl_level" default="highlevel"/>
|
||||
|
||||
<node pkg="unitree_legged_real" type="ros_udp" name="node_ros_udp" output="screen" args="$(arg ctrl_level)"/>
|
||||
|
||||
<param name="control_level" value="$(arg ctrl_level)"/>
|
||||
</launch>
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>unitree_legged_real</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The unitree_legged_real package</description>
|
||||
|
||||
<maintainer email="laikago@unitree.cc">unitree</maintainer>
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<depend>unitree_legged_msgs</depend>
|
||||
<depend>eigen</depend>
|
||||
|
||||
</package>
|
||||
|
|
@ -1,111 +0,0 @@
|
|||
#include "ros/ros.h"
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <termios.h>
|
||||
|
||||
int getch()
|
||||
{
|
||||
int ch;
|
||||
struct termios oldt;
|
||||
struct termios newt;
|
||||
|
||||
// Store old settings, and copy to new settings
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
|
||||
// Make required changes and apply the settings
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
newt.c_iflag |= IGNBRK;
|
||||
newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
|
||||
newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
|
||||
newt.c_cc[VMIN] = 0;
|
||||
newt.c_cc[VTIME] = 1;
|
||||
tcsetattr(fileno(stdin), TCSANOW, &newt);
|
||||
|
||||
// Get the current character
|
||||
ch = getchar();
|
||||
|
||||
// Reapply old settings
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
|
||||
return ch;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "keyboard_input_node");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
||||
|
||||
geometry_msgs::Twist twist;
|
||||
|
||||
long count = 0;
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
twist.linear.x = 0.0;
|
||||
twist.linear.y = 0.0;
|
||||
twist.linear.z = 0.0;
|
||||
twist.angular.x = 0.0;
|
||||
twist.angular.y = 0.0;
|
||||
twist.angular.z = 0.0;
|
||||
|
||||
int ch = 0;
|
||||
|
||||
ch = getch();
|
||||
|
||||
printf("%ld\n", count++);
|
||||
printf("ch = %d\n\n", ch);
|
||||
|
||||
switch (ch)
|
||||
{
|
||||
case 'q':
|
||||
printf("already quit!\n");
|
||||
return 0;
|
||||
|
||||
case 'w':
|
||||
twist.linear.x = 0.5;
|
||||
printf("move forward!\n");
|
||||
break;
|
||||
|
||||
case 's':
|
||||
twist.linear.x = -0.5;
|
||||
printf("move backward!\n");
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
twist.linear.y = 0.5;
|
||||
printf("move left!\n");
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
twist.linear.y = -0.5;
|
||||
printf("move right!\n");
|
||||
break;
|
||||
|
||||
case 'j':
|
||||
twist.angular.z = 1.0;
|
||||
printf("turn left!\n");
|
||||
break;
|
||||
|
||||
case 'l':
|
||||
twist.angular.z = -1.0;
|
||||
printf("turn right!\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Stop!\n");
|
||||
break;
|
||||
}
|
||||
|
||||
pub.publish(twist);
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,93 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_postition_without_lcm");
|
||||
|
||||
std::cout << "Communication level is set to LOW-level." << std::endl
|
||||
<< "WARNING: Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle nh;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
int rate_count = 0;
|
||||
int sin_count = 0;
|
||||
float qInit[3] = {0};
|
||||
float qDes[3] = {0};
|
||||
float sin_mid_q[3] = {0.0, 1.2, -2.0};
|
||||
float Kp[3] = {0};
|
||||
float Kd[3] = {0};
|
||||
|
||||
unitree_legged_msgs::LowCmd low_cmd_ros;
|
||||
|
||||
bool initiated_flag = false; // initiate need time
|
||||
int count = 0;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::LowCmd>("low_cmd", 1);
|
||||
|
||||
low_cmd_ros.head[0] = 0xFE;
|
||||
low_cmd_ros.head[1] = 0xEF;
|
||||
low_cmd_ros.levelFlag = LOWLEVEL;
|
||||
|
||||
for (int i = 0; i < 12; i++)
|
||||
{
|
||||
low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环
|
||||
low_cmd_ros.motorCmd[i].Kp = 0;
|
||||
low_cmd_ros.motorCmd[i].dq = VelStopF; // 禁止速度环
|
||||
low_cmd_ros.motorCmd[i].Kd = 0;
|
||||
low_cmd_ros.motorCmd[i].tau = 0;
|
||||
}
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
|
||||
if (initiated_flag == true)
|
||||
{
|
||||
motiontime += 2;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_0].tau = -0.65f;
|
||||
low_cmd_ros.motorCmd[FL_0].tau = +0.65f;
|
||||
low_cmd_ros.motorCmd[RR_0].tau = -0.65f;
|
||||
low_cmd_ros.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3);
|
||||
low_cmd_ros.motorCmd[FR_2].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_2].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_2].Kd = 1.0;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_0].q = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_0].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_0].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_0].Kd = 1.0;
|
||||
|
||||
low_cmd_ros.motorCmd[FR_1].q = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_1].dq = 0.0;
|
||||
low_cmd_ros.motorCmd[FR_1].Kp = 5.0;
|
||||
low_cmd_ros.motorCmd[FR_1].Kd = 1.0;
|
||||
}
|
||||
|
||||
count++;
|
||||
if (count > 10)
|
||||
{
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
}
|
||||
|
||||
pub.publish(low_cmd_ros);
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,140 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "example_walk_without_lcm");
|
||||
|
||||
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
|
||||
<< "Make sure the robot is standing on the ground." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
|
||||
unitree_legged_msgs::HighCmd high_cmd_ros;
|
||||
|
||||
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
|
||||
motiontime += 2;
|
||||
|
||||
high_cmd_ros.head[0] = 0xFE;
|
||||
high_cmd_ros.head[1] = 0xEF;
|
||||
high_cmd_ros.levelFlag = HIGHLEVEL;
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.gaitType = 0;
|
||||
high_cmd_ros.speedLevel = 0;
|
||||
high_cmd_ros.footRaiseHeight = 0;
|
||||
high_cmd_ros.bodyHeight = 0;
|
||||
high_cmd_ros.euler[0] = 0;
|
||||
high_cmd_ros.euler[1] = 0;
|
||||
high_cmd_ros.euler[2] = 0;
|
||||
high_cmd_ros.velocity[0] = 0.0f;
|
||||
high_cmd_ros.velocity[1] = 0.0f;
|
||||
high_cmd_ros.yawSpeed = 0.0f;
|
||||
high_cmd_ros.reserve = 0;
|
||||
|
||||
if (motiontime > 0 && motiontime < 1000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = -0.3;
|
||||
}
|
||||
if (motiontime > 1000 && motiontime < 2000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[0] = 0.3;
|
||||
}
|
||||
if (motiontime > 2000 && motiontime < 3000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = -0.2;
|
||||
}
|
||||
if (motiontime > 3000 && motiontime < 4000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[1] = 0.2;
|
||||
}
|
||||
if (motiontime > 4000 && motiontime < 5000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = -0.2;
|
||||
}
|
||||
if (motiontime > 5000 && motiontime < 6000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.euler[2] = 0.2;
|
||||
}
|
||||
if (motiontime > 6000 && motiontime < 7000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = -0.2;
|
||||
}
|
||||
if (motiontime > 7000 && motiontime < 8000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.1;
|
||||
}
|
||||
if (motiontime > 8000 && motiontime < 9000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
high_cmd_ros.bodyHeight = 0.0;
|
||||
}
|
||||
if (motiontime > 9000 && motiontime < 11000)
|
||||
{
|
||||
high_cmd_ros.mode = 5;
|
||||
}
|
||||
if (motiontime > 11000 && motiontime < 13000)
|
||||
{
|
||||
high_cmd_ros.mode = 6;
|
||||
}
|
||||
if (motiontime > 13000 && motiontime < 14000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
}
|
||||
if (motiontime > 14000 && motiontime < 18000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 2;
|
||||
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1
|
||||
high_cmd_ros.yawSpeed = 2;
|
||||
high_cmd_ros.footRaiseHeight = 0.1;
|
||||
// printf("walk\n");
|
||||
}
|
||||
if (motiontime > 18000 && motiontime < 20000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.velocity[0] = 0;
|
||||
}
|
||||
if (motiontime > 20000 && motiontime < 24000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 1;
|
||||
high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1
|
||||
high_cmd_ros.bodyHeight = 0.1;
|
||||
// printf("walk\n");
|
||||
}
|
||||
if (motiontime > 24000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
}
|
||||
|
||||
pub.publish(high_cmd_ros);
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,152 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
#include <chrono>
|
||||
#include <pthread.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
class Custom
|
||||
{
|
||||
public:
|
||||
UDP low_udp;
|
||||
UDP high_udp;
|
||||
|
||||
HighCmd high_cmd = {0};
|
||||
HighState high_state = {0};
|
||||
|
||||
LowCmd low_cmd = {0};
|
||||
LowState low_state = {0};
|
||||
|
||||
public:
|
||||
Custom()
|
||||
:
|
||||
// low_udp(LOWLEVEL),
|
||||
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
|
||||
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
}
|
||||
|
||||
void highUdpSend()
|
||||
{
|
||||
// printf("high udp send is running\n");
|
||||
|
||||
high_udp.SetSend(high_cmd);
|
||||
high_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpSend()
|
||||
{
|
||||
|
||||
low_udp.SetSend(low_cmd);
|
||||
low_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpRecv()
|
||||
{
|
||||
|
||||
low_udp.Recv();
|
||||
low_udp.GetRecv(low_state);
|
||||
}
|
||||
|
||||
void highUdpRecv()
|
||||
{
|
||||
// printf("high udp recv is running\n");
|
||||
|
||||
high_udp.Recv();
|
||||
high_udp.GetRecv(high_state);
|
||||
}
|
||||
};
|
||||
|
||||
Custom custom;
|
||||
|
||||
ros::Subscriber sub_high;
|
||||
ros::Subscriber sub_low;
|
||||
|
||||
ros::Publisher pub_high;
|
||||
ros::Publisher pub_low;
|
||||
|
||||
long high_count = 0;
|
||||
long low_count = 0;
|
||||
|
||||
void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
||||
{
|
||||
printf("highCmdCallback is running !\t%ld\n", ::high_count);
|
||||
|
||||
custom.high_cmd = rosMsg2Cmd(msg);
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
high_state_ros = state2rosMsg(custom.high_state);
|
||||
|
||||
pub_high.publish(high_state_ros);
|
||||
|
||||
printf("highCmdCallback ending !\t%ld\n\n", ::high_count++);
|
||||
}
|
||||
|
||||
void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
|
||||
{
|
||||
|
||||
printf("lowCmdCallback is running !\t%ld\n", low_count);
|
||||
|
||||
custom.low_cmd = rosMsg2Cmd(msg);
|
||||
|
||||
unitree_legged_msgs::LowState low_state_ros;
|
||||
|
||||
low_state_ros = state2rosMsg(custom.low_state);
|
||||
|
||||
pub_low.publish(low_state_ros);
|
||||
|
||||
printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "ros_udp");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
if (strcasecmp(argv[1], "LOWLEVEL") == 0)
|
||||
{
|
||||
sub_low = nh.subscribe("low_cmd", 1, lowCmdCallback);
|
||||
pub_low = nh.advertise<unitree_legged_msgs::LowState>("low_state", 1);
|
||||
|
||||
LoopFunc loop_udpSend("low_udp_send", 0.002, 3, boost::bind(&Custom::lowUdpSend, &custom));
|
||||
LoopFunc loop_udpRecv("low_udp_recv", 0.002, 3, boost::bind(&Custom::lowUdpRecv, &custom));
|
||||
|
||||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("low level runing!\n");
|
||||
}
|
||||
else if (strcasecmp(argv[1], "HIGHLEVEL") == 0)
|
||||
{
|
||||
sub_high = nh.subscribe("high_cmd", 1, highCmdCallback);
|
||||
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
|
||||
|
||||
LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom));
|
||||
LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom));
|
||||
|
||||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("high level runing!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Control level name error! Can only be highlevel or lowlevel(not case sensitive)" << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/MotorCmd.h>
|
||||
#include <unitree_legged_msgs/MotorState.h>
|
||||
#include <unitree_legged_msgs/BmsCmd.h>
|
||||
#include <unitree_legged_msgs/BmsState.h>
|
||||
#include <unitree_legged_msgs/IMU.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.h>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &msg)
|
||||
{
|
||||
printf("yaw = %f\n", msg->imu.rpy[2]);
|
||||
}
|
||||
|
||||
void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &msg)
|
||||
{
|
||||
printf("FR_2_pos = %f\n", msg->motorState[FR_2].q);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "node_high_state_sub");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
ros::Subscriber high_sub = nh.subscribe("high_state", 1, highStateCallback);
|
||||
ros::Subscriber low_sub = nh.subscribe("low_state", 1, lowStateCallback);
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,112 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
#include "convert.h"
|
||||
#include <chrono>
|
||||
#include <pthread.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
class Custom
|
||||
{
|
||||
public:
|
||||
UDP low_udp;
|
||||
UDP high_udp;
|
||||
|
||||
HighCmd high_cmd = {0};
|
||||
HighState high_state = {0};
|
||||
|
||||
LowCmd low_cmd = {0};
|
||||
LowState low_state = {0};
|
||||
|
||||
public:
|
||||
Custom()
|
||||
:
|
||||
// low_udp(LOWLEVEL),
|
||||
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
|
||||
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
}
|
||||
|
||||
void highUdpSend()
|
||||
{
|
||||
// printf("high udp send is running\n");
|
||||
|
||||
high_udp.SetSend(high_cmd);
|
||||
high_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpSend()
|
||||
{
|
||||
|
||||
low_udp.SetSend(low_cmd);
|
||||
low_udp.Send();
|
||||
}
|
||||
|
||||
void lowUdpRecv()
|
||||
{
|
||||
|
||||
low_udp.Recv();
|
||||
low_udp.GetRecv(low_state);
|
||||
}
|
||||
|
||||
void highUdpRecv()
|
||||
{
|
||||
// printf("high udp recv is running\n");
|
||||
|
||||
high_udp.Recv();
|
||||
high_udp.GetRecv(high_state);
|
||||
}
|
||||
};
|
||||
|
||||
Custom custom;
|
||||
|
||||
ros::Subscriber sub_cmd_vel;
|
||||
ros::Publisher pub_high;
|
||||
|
||||
long cmd_vel_count = 0;
|
||||
|
||||
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg)
|
||||
{
|
||||
printf("cmdVelCallback is running!\t%ld\n", cmd_vel_count);
|
||||
|
||||
custom.high_cmd = rosMsg2Cmd(msg);
|
||||
|
||||
printf("cmd_x_vel = %f\n", custom.high_cmd.velocity[0]);
|
||||
printf("cmd_y_vel = %f\n", custom.high_cmd.velocity[1]);
|
||||
printf("cmd_yaw_vel = %f\n", custom.high_cmd.yawSpeed);
|
||||
|
||||
unitree_legged_msgs::HighState high_state_ros;
|
||||
|
||||
high_state_ros = state2rosMsg(custom.high_state);
|
||||
|
||||
pub_high.publish(high_state_ros);
|
||||
|
||||
printf("cmdVelCallback ending!\t%ld\n\n", cmd_vel_count++);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "twist_sub");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
|
||||
|
||||
sub_cmd_vel = nh.subscribe("cmd_vel", 1, cmdVelCallback);
|
||||
|
||||
LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom));
|
||||
LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom));
|
||||
|
||||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue