-
Notifications
You must be signed in to change notification settings - Fork 35
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
20 changed files
with
1,236 additions
and
45 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,187 @@ | ||
| /************************************************************************ | ||
| 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/IMU.h> | ||
|
|
||
| #include "unitree_legged_sdk/unitree_legged_sdk.h" | ||
|
|
||
| unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::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.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; | ||
| } | ||
|
|
||
| unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm) | ||
| { | ||
| unitree_legged_msgs::MotorState ros; | ||
| ros.mode = lcm.mode; | ||
| ros.q = lcm.q; | ||
| ros.dq = lcm.dq; | ||
| ros.ddq = lcm.ddq; | ||
| ros.tauEst = lcm.tauEst; | ||
| ros.q_raw = lcm.q_raw; | ||
| ros.dq_raw = lcm.dq_raw; | ||
| ros.ddq_raw = lcm.ddq_raw; | ||
| ros.temperature = lcm.temperature; | ||
| ros.reserve[0] = lcm.reserve[0]; | ||
| ros.reserve[1] = lcm.reserve[1]; | ||
| return ros; | ||
| } | ||
|
|
||
| UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType) | ||
| { | ||
| UNITREE_LEGGED_SDK::MotorCmd lcm; | ||
| lcm.mode = ros.mode; | ||
| lcm.q = ros.q; | ||
| lcm.dq = ros.dq; | ||
| lcm.tau = ros.tau; | ||
| lcm.Kp = ros.Kp; | ||
| lcm.Kd = ros.Kd; | ||
| lcm.reserve[0] = ros.reserve[0]; | ||
| lcm.reserve[1] = ros.reserve[1]; | ||
| lcm.reserve[2] = ros.reserve[2]; | ||
| return lcm; | ||
| } | ||
|
|
||
| unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm) | ||
| { | ||
| unitree_legged_msgs::LowState ros; | ||
| ros.levelFlag = lcm.levelFlag; | ||
| ros.commVersion = lcm.commVersion; | ||
| ros.robotID = lcm.robotID; | ||
| ros.SN = lcm.SN; | ||
| ros.bandWidth = lcm.bandWidth; | ||
| 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] = lcm.footForceEst[i]; | ||
| } | ||
| ros.tick = lcm.tick; | ||
| for(int i = 0; i<40; i++){ | ||
| ros.wirelessRemote[i] = lcm.wirelessRemote[i]; | ||
| } | ||
| ros.reserve = lcm.reserve; | ||
| ros.crc = lcm.crc; | ||
| return ros; | ||
| } | ||
|
|
||
| UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType) | ||
| { | ||
| UNITREE_LEGGED_SDK::LowCmd lcm; | ||
| lcm.levelFlag = ros.levelFlag; | ||
| lcm.commVersion = ros.commVersion; | ||
| lcm.robotID = ros.robotID; | ||
| lcm.SN = ros.SN; | ||
| lcm.bandWidth = ros.bandWidth; | ||
| 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.reserve = ros.reserve; | ||
| lcm.crc = ros.crc; | ||
| return lcm; | ||
| } | ||
|
|
||
| unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm) | ||
| { | ||
| unitree_legged_msgs::HighState ros; | ||
| ros.levelFlag = lcm.levelFlag; | ||
| ros.commVersion = lcm.commVersion; | ||
| ros.robotID = lcm.robotID; | ||
| ros.SN = lcm.SN; | ||
| ros.bandWidth = lcm.bandWidth; | ||
| 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; | ||
| 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.footForce[i] = lcm.footForce[i]; | ||
| ros.footForceEst[i] = lcm.footForceEst[i]; | ||
| } | ||
| ros.tick = lcm.tick; | ||
| for(int i = 0; i<40; i++){ | ||
| ros.wirelessRemote[i] = lcm.wirelessRemote[i]; | ||
| } | ||
| ros.reserve = lcm.reserve; | ||
| ros.crc = lcm.crc; | ||
| return ros; | ||
| } | ||
|
|
||
| UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType) | ||
| { | ||
| UNITREE_LEGGED_SDK::HighCmd lcm; | ||
| lcm.levelFlag = ros.levelFlag; | ||
| lcm.commVersion = ros.commVersion; | ||
| lcm.robotID = ros.robotID; | ||
| 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.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.AppRemote[i] = ros.AppRemote[i]; | ||
| } | ||
| lcm.reserve = ros.reserve; | ||
| lcm.crc = ros.crc; | ||
| return lcm; | ||
| } | ||
|
|
||
| #endif // _CONVERT_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| #ifndef UNITREE_RL | ||
| #define UNITREE_RL | ||
|
|
||
| #include <ros/ros.h> | ||
| #include <gazebo_msgs/ModelStates.h> | ||
| #include <sensor_msgs/JointState.h> | ||
| #include <geometry_msgs/Twist.h> | ||
| #include "../lib/model.cpp" | ||
| #include "../lib/observation_buffer.hpp" | ||
| #include <unitree_legged_msgs/LowCmd.h> | ||
| #include "unitree_legged_msgs/LowState.h" | ||
| #include "convert.h" | ||
| #include <pthread.h> | ||
|
|
||
| using namespace UNITREE_LEGGED_SDK; | ||
|
|
||
| class Unitree_RL : public Model | ||
| { | ||
| public: | ||
| Unitree_RL(); | ||
| void modelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); | ||
| void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); | ||
| void cmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg); | ||
| void runModel(); | ||
| torch::Tensor forward() override; | ||
| torch::Tensor compute_observation() override; | ||
|
|
||
| ObservationBuffer history_obs_buf; | ||
| torch::Tensor history_obs; | ||
|
|
||
| torch::Tensor torques; | ||
|
|
||
| //udp | ||
| void UDPSend(); | ||
| void UDPRecv(); | ||
| void RobotControl(); | ||
| Safety safe; | ||
| UDP udp; | ||
| LowCmd cmd = {0}; | ||
| LowState state = {0}; | ||
| int motiontime = 0; | ||
|
|
||
| std::shared_ptr<LoopFunc> loop_control; | ||
| std::shared_ptr<LoopFunc> loop_udpSend; | ||
| std::shared_ptr<LoopFunc> loop_udpRecv; | ||
| std::shared_ptr<LoopFunc> loop_rl; | ||
|
|
||
|
|
||
| float _percent; | ||
| float _targetPos[12] = {0.0, 0.8, -1.6, 0.0, 0.8, -1.6, | ||
| 0.0, 0.8, -1.6, 0.0, 0.8, -1.6}; //0.0, 0.67, -1.3 | ||
| float _startPos[12]; | ||
|
|
||
| bool init_done = false; | ||
|
|
||
| private: | ||
| std::string ros_namespace; | ||
|
|
||
| std::vector<std::string> torque_command_topics; | ||
|
|
||
| ros::Subscriber model_state_subscriber_; | ||
| ros::Subscriber joint_state_subscriber_; | ||
| ros::Subscriber cmd_vel_subscriber_; | ||
|
|
||
| std::map<std::string, ros::Publisher> torque_publishers; | ||
| std::vector<unitree_legged_msgs::MotorCmd> torque_commands; | ||
|
|
||
| geometry_msgs::Twist vel; | ||
| geometry_msgs::Pose pose; | ||
| geometry_msgs::Twist cmd_vel; | ||
|
|
||
| std::vector<std::string> joint_names; | ||
| std::vector<double> joint_positions; | ||
| std::vector<double> joint_velocities; | ||
|
|
||
|
|
||
|
|
||
| ros::Timer timer; | ||
|
|
||
| std::chrono::high_resolution_clock::time_point start_time; | ||
|
|
||
| // other rl module | ||
| torch::jit::script::Module encoder; | ||
| torch::jit::script::Module vq; | ||
|
|
||
| UNITREE_LEGGED_SDK::LowCmd SendLowLCM = {0}; | ||
| UNITREE_LEGGED_SDK::LowState RecvLowLCM = {0}; | ||
| unitree_legged_msgs::LowCmd SendLowROS; | ||
| unitree_legged_msgs::LowState RecvLowROS; | ||
| }; | ||
|
|
||
| #endif |
19 changes: 19 additions & 0 deletions
19
src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| /************************************************************************ | ||
| Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. | ||
| Use of this source code is governed by the MPL-2.0 license, see LICENSE. | ||
| ************************************************************************/ | ||
|
|
||
| #ifndef _UNITREE_LEGGED_A1_H_ | ||
| #define _UNITREE_LEGGED_A1_H_ | ||
|
|
||
| namespace UNITREE_LEGGED_SDK | ||
| { | ||
| constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) | ||
| constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) | ||
| constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) | ||
| constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) | ||
| constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) | ||
| constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) | ||
| } | ||
|
|
||
| #endif |
19 changes: 19 additions & 0 deletions
19
src/unitree_rl/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| /************************************************************************ | ||
| Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. | ||
| Use of this source code is governed by the MPL-2.0 license, see LICENSE. | ||
| ************************************************************************/ | ||
|
|
||
| #ifndef _UNITREE_LEGGED_ALIENGO_H_ | ||
| #define _UNITREE_LEGGED_ALIENGO_H_ | ||
|
|
||
| namespace UNITREE_LEGGED_SDK | ||
| { | ||
| constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) | ||
| constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) | ||
| constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) | ||
| constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) | ||
| constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) | ||
| constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) | ||
| } | ||
|
|
||
| #endif |
Oops, something went wrong.