diff --git a/.gitignore b/.gitignore index 680e994..b45f491 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ build devel logs +install +log .catkin_tools .vscode *.csv diff --git a/README.md b/README.md index 05c9062..c2eacaa 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,13 @@ [中文文档](README_CN.md) +**Version Select: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)** + This repository provides a framework for simulation verification and physical deployment of robot reinforcement learning algorithms, suitable for quadruped robots, wheeled robots, and humanoid robots. "sar" stands for "simulation and real" feature: - Support legged_gym based on IaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish. +- The code has two versions: **ROS** and **ROS2** - The code supports both cpp and python, you can find python version in `src/rl_sar/scripts` [Click to discuss on Discord](https://discord.gg/vmVjkhVugU) @@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git ## Dependency -This project uses `ros-noetic` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages: +This project uses `ros2-foxy` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages: ```bash -sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller +sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro ``` Download and deploy `libtorch` at any location @@ -72,14 +75,17 @@ Compile in the root directory of the project ```bash cd .. -catkin build +colcon build --merge-install --symlink-install ``` -If catkin build report errors: `Unable to find either executable 'empy' or Python module 'em'`, run `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` before `catkin build` - ## Running -In the following text, **\_\** is used to represent different environments, which can be `a1_isaacgym`, `a1_isaacsim`, `go2_isaacgym`, `gr1t1_isaacgym`, or `gr1t2_isaacgym`. +In the following text, **\_\** is used to represent different environments. Currently supported list: + +| | isaacgym | isaacsim | +|-------|----------|----------| +| a1 | ✓ | ✓ | +| go2 | ✓ | | Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/_`, and configure the parameters in `config.yaml`. @@ -88,24 +94,25 @@ Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/_.launch +source install/setup.bash +ros2 launch rl_sar gazebo.launch.py rname:= framework:= +(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym) ``` Open a new terminal, launch the control program ```bash -source devel/setup.bash -(for cpp version) rosrun rl_sar rl_sim -(for python version) rosrun rl_sar rl_sim.py +source install/setup.bash +(for cpp version) ros2 run rl_sar rl_sim +(for python version) ros2 run rl_sar rl_sim.py ``` Control: -* Press **\** to toggle simulation start/stop. + * **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis. * Press **\** to sets all control commands to zero. -* If robot falls down, press **R** to reset Gazebo environment. + ### Real Robots @@ -119,8 +126,8 @@ Unitree A1 can be connected using both wireless and wired methods: Open a new terminal and start the control program ```bash -source devel/setup.bash -rosrun rl_sar rl_real_a1 +source install/local.bash +ros2 run rl_sar rl_real_a1 ``` Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right. @@ -133,8 +140,8 @@ Or press **0** on the keyboard to switch the robot to the default standing posit 2. Use the `ifconfig` command to find the name of the network interface for the 123 network segment, such as `enxf8e43b808e06`. In the following steps, replace `` with the actual network interface name. 3. Open a new terminal and start the control program: ```bash - source devel/setup.bash - rosrun rl_sar rl_real_go2 + source install/local.bash + ros2 run rl_sar rl_real_go2 ``` 4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1. @@ -146,11 +153,11 @@ Take A1 as an example below 2. Run the control program, and the program will log all data after execution. 3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths. ```bash - rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt + ros2 run rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt ``` 4. Verify the trained actuator network. ```bash - rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt + ros2 run rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt ``` ## Add Your Robot @@ -160,8 +167,8 @@ In the following text, **\_\** is used to represent your robo 1. Create a model package named `_description` in the `rl_sar/src/robots` directory. Place the robot's URDF file in the `rl_sar/src/robots/_description/urdf` directory and name it `.urdf`. Additionally, create a joint configuration file with the namespace `_gazebo` in the `rl_sar/src/robots/_description/config` directory. 2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/_` directory, and create a new `config.yaml` file in this path. Refer to the `rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml` file to modify the parameters. 3. Modify the `forward()` function in the code as needed to adapt to different models. -4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. -5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. + +4. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed. ## Contributing diff --git a/README_CN.md b/README_CN.md index 448b312..a09818a 100644 --- a/README_CN.md +++ b/README_CN.md @@ -2,10 +2,13 @@ [English document](README.md) +**版本选择: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)** + 本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real" 特性： - 支持基于IaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，用`framework`加以区分。 +- 代码有**ROS**和**ROS2**两个版本 - 代码有python和cpp两个版本，python版本可以在`src/rl_sar/scripts`中找到 [点击在Discord上讨论](https://discord.gg/MC9KguQHtt) @@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git ## 依赖 -本项目使用`ros-noetic`(Ubuntu20.04)，且需要安装以下的ros依赖包 +本项目使用`ros2-foxy`(Ubuntu20.04)，且需要安装以下的ros依赖包 ```bash -sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller +sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro ``` 在任意位置下载并部署`libtorch` @@ -72,14 +75,17 @@ sudo ldconfig ```bash cd .. -catkin build +colcon build --merge-install --symlink-install ``` -如果 catkin build 报错: `Unable to find either executable 'empy' or Python module 'em'`, 在`catkin build` 之前执行 `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` - ## 运行 -下文中使用 **\_\** 代替表示不同的环境，可以是 `a1_isaacgym` 、 `a1_isaacsim` 、 `go2_isaacgym` 、 `gr1t1_isaacgym` 、 `gr1t2_isaacgym` +下文中使用 **\** 和 **\** 代替表示不同的机器人和框架。目前支持列表： + +| | isaacgym | isaacsim | +|-------|----------|----------| +| a1 | ✓ | ✓ | +| go2 | ✓ | | 运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/_`中，并配置`config.yaml`中的参数。 @@ -88,24 +94,25 @@ catkin build 打开一个终端，启动gazebo仿真环境 ```bash -source devel/setup.bash -roslaunch rl_sar gazebo__.launch +source install/setup.bash +ros2 launch rl_sar gazebo.launch.py rname:= framework:= +(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym) ``` 打开一个新终端，启动控制程序 ```bash -source devel/setup.bash -(for cpp version) rosrun rl_sar rl_sim -(for python version) rosrun rl_sar rl_sim.py +source install/setup.bash +(for cpp version) ros2 run rl_sar rl_sim +(for python version) ros2 run rl_sar rl_sim.py ``` 控制： -* 按 **\** 切换仿真器运行/停止。 -* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。 + +* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴。 * 按 **\** 将所有控制指令设置为零。 -* 如果机器人摔倒，按 **R** 重置Gazebo环境。 + ### 真实机器人 @@ -119,8 +126,8 @@ source devel/setup.bash 新建终端，启动控制程序 ```bash -source devel/setup.bash -rosrun rl_sar rl_real_a1 +source install/local.bash +ros2 run rl_sar rl_real_a1 ``` 按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。 @@ -133,8 +140,8 @@ rosrun rl_sar rl_real_a1 2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \ 代替 3. 新建终端，启动控制程序 ```bash - source devel/setup.bash - rosrun rl_sar rl_real_go2 + source install/local.bash + ros2 run rl_sar rl_real_go2 ``` 4. Go2支持手柄与键盘控制，方法与上面a1相同 @@ -144,13 +151,13 @@ rosrun rl_sar rl_real_a1 1. 取消注释`rl_real_a1.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。 2. 运行控制程序，程序会在执行后记录所有数据。 -3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。 +3. 停止控制程序，开始训练执行器网络。 ```bash - rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt + ros2 run rl_sar actuator_net.py --mode train --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt ``` 4. 验证已经训练好的训练执行器网络。 ```bash - rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt + ros2 run rl_sar actuator_net.py --mode play --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt ``` ## 添加你的机器人 @@ -160,8 +167,8 @@ rosrun rl_sar rl_real_a1 1. 在`rl_sar/src/robots`路径下创建名为`_description`的模型包，将模型的urdf放到`rl_sar/src/robots/_description/urdf`路径下并命名为`.urdf`，并在`rl_sar/src/robots/_description/config`路径下创建命名空间为`_gazebo`的关节配置文件 2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/_`路径下，并在此路径中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml`文件修改其中参数 3. 按需修改代码中的`forward()`函数，以适配不同的模型 -4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 -5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 + +4. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改 ## 贡献 diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt index b2cc6c7..68b2eb9 100644 --- a/src/rl_sar/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(rl_sar) add_definitions(-DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") @@ -10,33 +10,27 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}") find_package(Torch REQUIRED) +set(CMAKE_INSTALL_RPATH "${Torch_DIR}/lib") +set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE) +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") find_package(gazebo REQUIRED) -find_package(catkin REQUIRED COMPONENTS - controller_manager - genmsg - joint_state_controller - robot_state_publisher - roscpp - gazebo_ros - std_msgs - tf - geometry_msgs - robot_msgs - robot_joint_controller - rospy -) +find_package(ament_cmake REQUIRED) +find_package(joint_state_controller REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(rclcpp REQUIRED) +find_package(gazebo_ros REQUIRED) +find_package(std_msgs REQUIRED) +find_package(robot_msgs REQUIRED) +find_package(robot_joint_controller REQUIRED) +find_package(rclpy REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(Python3 COMPONENTS Interpreter Development REQUIRED) -catkin_package( - CATKIN_DEPENDS - robot_joint_controller - rospy -) - link_directories(/usr/local/lib) include_directories(${YAML_CPP_INCLUDE_DIR}) @@ -44,6 +38,11 @@ include_directories(${YAML_CPP_INCLUDE_DIR}) include_directories(library/unitree_legged_sdk_3.2/include) link_directories(library/unitree_legged_sdk_3.2/lib) set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm) +file(GLOB GLOB_UNITREE_LEGGED_SDK "${PROJECT_SOURCE_DIR}/library/unitree_legged_sdk_3.2/lib/*.so") +install(FILES + ${GLOB_UNITREE_LEGGED_SDK} + DESTINATION lib/ +) # Unitree Go2 include_directories(library/unitree_sdk2/include) @@ -52,10 +51,16 @@ include_directories(library/unitree_sdk2/thirdparty/include) include_directories(library/unitree_sdk2/thirdparty/include/ddscxx) link_directories(library/unitree_sdk2/thirdparty/lib/x86_64) set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx) +file(GLOB GLOB_UNITREE_SDK2 "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/lib/x86_64/*.so") +file(GLOB GLOB_UNITREE_SDK2_THIRDPARTY "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/thirdparty/lib/x86_64/*.so") +install(FILES + ${GLOB_UNITREE_SDK2_THIRDPARTY} + ${GLOB_UNITREE_SDK2_THIRDPARTY} + DESTINATION lib/ +) include_directories( include - ${catkin_INCLUDE_DIRS} library/matplotlibcpp library/observation_buffer library/rl_sdk @@ -78,24 +83,53 @@ set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14) add_executable(rl_sim src/rl_sim.cpp ) target_link_libraries(rl_sim - ${catkin_LIBRARIES} -pthread + -pthread rl_sdk observation_buffer yaml-cpp ) add_executable(rl_real_a1 src/rl_real_a1.cpp ) target_link_libraries(rl_real_a1 - ${catkin_LIBRARIES} ${UNITREE_A1_LIBS} + ${UNITREE_A1_LIBS} rl_sdk observation_buffer yaml-cpp ) add_executable(rl_real_go2 src/rl_real_go2.cpp ) target_link_libraries(rl_real_go2 - ${catkin_LIBRARIES} ${UNITREE_GO2_LIBS} + ${UNITREE_GO2_LIBS} rl_sdk observation_buffer yaml-cpp ) -catkin_install_python(PROGRAMS +ament_target_dependencies(rl_sim + joint_state_controller + robot_state_publisher + rclcpp + gazebo_ros + std_msgs + robot_msgs + robot_joint_controller + rclpy + gazebo_msgs + std_srvs +) + +install(TARGETS + rl_sim + rl_sdk + observation_buffer + rl_real_a1 + rl_real_go2 + DESTINATION lib/${PROJECT_NAME} +) + +install(PROGRAMS scripts/rl_sim.py scripts/actuator_net.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) \ No newline at end of file + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY launch worlds models + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 9601024..a9e6807 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -4,19 +4,22 @@ #include "rl_sdk.hpp" #include "observation_buffer.hpp" #include "loop.hpp" -#include -#include -#include -#include "std_srvs/Empty.h" -#include -#include "robot_msgs/MotorCommand.h" +#include "robot_msgs/msg/robot_command.hpp" +#include "robot_msgs/msg/robot_state.hpp" +#include +#include +#include +#include +#include +#include +#include + #include -#include #include "matplotlibcpp.h" namespace plt = matplotlibcpp; -class RL_Sim : public RL +class RL_Sim : public RL, public rclcpp::Node { public: RL_Sim(); @@ -48,29 +51,27 @@ private: // ros interface std::string ros_namespace; - geometry_msgs::Twist vel; - geometry_msgs::Pose pose; - geometry_msgs::Twist cmd_vel; - ros::Subscriber model_state_subscriber; - ros::Subscriber joint_state_subscriber; - ros::Subscriber cmd_vel_subscriber; - ros::ServiceClient gazebo_set_model_state_client; - ros::ServiceClient gazebo_pause_physics_client; - ros::ServiceClient gazebo_unpause_physics_client; - std::map joint_publishers; - std::vector joint_publishers_commands; - void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); - void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg); + sensor_msgs::msg::Imu gazebo_imu; + geometry_msgs::msg::Twist cmd_vel; + robot_msgs::msg::RobotCommand robot_command_publisher_msg; + robot_msgs::msg::RobotState robot_state_subscriber_msg; + rclcpp::Subscription::SharedPtr gazebo_imu_subscriber; + rclcpp::Subscription::SharedPtr joint_state_subscriber; + rclcpp::Subscription::SharedPtr cmd_vel_subscriber; + rclcpp::Client::SharedPtr gazebo_set_model_state_client; + rclcpp::Client::SharedPtr gazebo_pause_physics_client; + rclcpp::Client::SharedPtr gazebo_unpause_physics_client; + rclcpp::Publisher::SharedPtr robot_command_publisher; + rclcpp::Subscription::SharedPtr robot_state_subscriber; + rclcpp::Client::SharedPtr param_client; + void GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + void CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg); // others std::string gazebo_model_name; int motiontime = 0; std::map sorted_to_original_index; - std::vector mapped_joint_positions; - std::vector mapped_joint_velocities; - std::vector mapped_joint_efforts; - void MapData(const std::vector &source_data, std::vector &target_data); }; #endif // RL_SIM_HPP diff --git a/src/rl_sar/launch/gazebo.launch.py b/src/rl_sar/launch/gazebo.launch.py new file mode 100644 index 0000000..e5cd5e7 --- /dev/null +++ b/src/rl_sar/launch/gazebo.launch.py @@ -0,0 +1,94 @@ +import os +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, TextSubstitution, Command +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + rname = LaunchConfiguration("rname") + framework = LaunchConfiguration("framework") + + wname = "stair" + robot_name = ParameterValue(Command(["echo -n ", rname, "_", framework]), value_type=str) + ros_namespace = ParameterValue(Command(["echo -n ", "/", rname, "_gazebo"]), value_type=str) + gazebo_model_name = ParameterValue(Command(["echo -n ", rname, "_gazebo"]), value_type=str) + + robot_description = ParameterValue( + Command([ + "xacro ", + Command(["echo -n ", Command(["ros2 pkg prefix ", rname, "_description"])]), + "/share/", rname, "_description/xacro/robot.xacro" + ]), + value_type=str + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py") + ), + launch_arguments={ + "verbose": "false", + "world": os.path.join(get_package_share_directory("rl_sar"), "worlds", wname + ".world"), + }.items(), + ) + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "/robot_description", "-entity", "robot_model"], + output="screen", + ) + + joint_state_broadcaster_node = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster"], + output="screen", + ) + + robot_joint_controller_node = Node( + package="controller_manager", + executable="spawner.py", + arguments=["robot_joint_controller"], + output="screen", + ) + + param_node = Node( + package="demo_nodes_cpp", + executable="parameter_blackboard", + name="param_node", + parameters=[{ + "robot_name": robot_name, + "gazebo_model_name": gazebo_model_name, + }], + ) + + return LaunchDescription([ + DeclareLaunchArgument( + "rname", + description="Robot name (e.g., a1, go2)", + default_value=TextSubstitution(text=""), + ), + DeclareLaunchArgument( + "framework", + description="Framework (isaacgym or isaacsim)", + default_value=TextSubstitution(text=""), + ), + robot_state_publisher_node, + gazebo, + spawn_entity, + joint_state_broadcaster_node, + robot_joint_controller_node, + param_node, + ]) diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp index 039c87d..3946e62 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp @@ -486,6 +486,7 @@ void RL::CSVInit(std::string robot_name) // csv_filename += "_" + timestamp; csv_filename += ".csv"; + std::cout << LOGGER::INFO << "Recording motor data in " << csv_filename << std::endl; std::ofstream file(csv_filename.c_str()); for(int i = 0; i < 12; ++i) { file << "tau_cal_" << i << ","; } diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp index 5f86f76..c55589f 100644 --- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp +++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp @@ -11,10 +11,10 @@ namespace LOGGER { - const char *const INFO = "\033[0;37m[INFO]\033[0m "; - const char *const WARNING = "\033[0;33m[WARNING]\033[0m "; - const char *const ERROR = "\033[0;31m[ERROR]\033[0m "; - const char *const DEBUG = "\033[0;32m[DEBUG]\033[0m "; + const std::string INFO = "\033[0;37m[INFO]\033[0m "; + const std::string WARNING = "\033[0;33m[WARNING]\033[0m "; + const std::string ERROR = "\033[0;31m[ERROR]\033[0m "; + const std::string DEBUG = "\033[0;32m[DEBUG]\033[0m "; } template @@ -45,7 +45,7 @@ struct RobotState std::vector q = std::vector(32, 0.0); std::vector dq = std::vector(32, 0.0); std::vector ddq = std::vector(32, 0.0); - std::vector tauEst = std::vector(32, 0.0); + std::vector tau_est = std::vector(32, 0.0); std::vector cur = std::vector(32, 0.0); } motor_state; }; @@ -156,7 +156,7 @@ public: // others std::string robot_name; STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING - bool simulation_running = false; + bool simulation_running = true; // protect func void TorqueProtect(torch::Tensor origin_output_torques); diff --git a/src/rl_sar/package.xml b/src/rl_sar/package.xml index ccb0b2a..f54b43c 100644 --- a/src/rl_sar/package.xml +++ b/src/rl_sar/package.xml @@ -1,5 +1,5 @@ - + rl_sar 2.0.0 The rl_sar package @@ -8,29 +8,19 @@ TODO - catkin - genmsg - controller_manager - joint_state_controller - robot_state_publisher - roscpp - std_msgs - controller_manager - joint_state_controller - robot_state_publisher - roscpp - std_msgs - controller_manager - joint_state_controller - robot_state_publisher - roscpp - std_msgs - rospy - rospy + ament_cmake + rclcpp + std_msgs + gazebo_ros + yaml-cpp robot_msgs robot_joint_controller + controller_manager + joint_state_controller + robot_state_publisher + rospy - + ament_cmake diff --git a/src/rl_sar/scripts/actuator_net.py b/src/rl_sar/scripts/actuator_net.py old mode 100644 new mode 100755 index b62b705..9d5921a --- a/src/rl_sar/scripts/actuator_net.py +++ b/src/rl_sar/scripts/actuator_net.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import os import argparse from matplotlib import pyplot as plt @@ -8,8 +9,9 @@ import torch.nn.functional as F from torch.utils.data import Dataset, DataLoader from torch.optim import Adam import pandas as pd +from ament_index_python.packages import get_package_share_directory -BASE_PATH = os.path.join(os.path.dirname(__file__), "../") +BASE_PATH = os.path.join(get_package_share_directory('rl_sar')) class Config: def __init__(self): diff --git a/src/rl_sar/scripts/observation_buffer.py b/src/rl_sar/scripts/observation_buffer.py old mode 100644 new mode 100755 diff --git a/src/rl_sar/scripts/rl_sdk.py b/src/rl_sar/scripts/rl_sdk.py old mode 100644 new mode 100755 index 2761b30..16fb820 --- a/src/rl_sar/scripts/rl_sdk.py +++ b/src/rl_sar/scripts/rl_sdk.py @@ -41,7 +41,7 @@ class RobotState: self.q = [0.0] * 32 self.dq = [0.0] * 32 self.ddq = [0.0] * 32 - self.tauEst = [0.0] * 32 + self.tau_est = [0.0] * 32 self.cur = [0.0] * 32 class STATE(Enum): @@ -123,7 +123,7 @@ class RL: # others self.robot_name = "" self.running_state = STATE.STATE_RL_RUNNING # default running_state set to STATE_RL_RUNNING - self.simulation_running = False + self.simulation_running = True ### protected in cpp ### # rl module @@ -228,10 +228,10 @@ class RL: self.getup_percent = min(self.getup_percent, 1.0) for i in range(self.params.num_of_dofs): command.motor_command.q[i] = (1 - self.getup_percent) * self.now_state.motor_state.q[i] + self.getup_percent * self.params.default_dof_pos[0][i].item() - command.motor_command.dq[i] = 0 + command.motor_command.dq[i] = 0.0 command.motor_command.kp[i] = self.params.fixed_kp[0][i].item() command.motor_command.kd[i] = self.params.fixed_kd[0][i].item() - command.motor_command.tau[i] = 0 + command.motor_command.tau[i] = 0.0 print("\r" + LOGGER.INFO + f"Getting up {self.getup_percent * 100.0:.1f}", end='', flush=True) if self.control.control_state == STATE.STATE_RL_INIT: @@ -261,10 +261,10 @@ class RL: print("\r" + LOGGER.INFO + f"RL Controller x: {self.control.x:.1f} y: {self.control.y:.1f} yaw: {self.control.yaw:.1f}", end='', flush=True) for i in range(self.params.num_of_dofs): command.motor_command.q[i] = self.output_dof_pos[0][i].item() - command.motor_command.dq[i] = 0 + command.motor_command.dq[i] = 0.0 command.motor_command.kp[i] = self.params.rl_kp[0][i].item() command.motor_command.kd[i] = self.params.rl_kd[0][i].item() - command.motor_command.tau[i] = 0 + command.motor_command.tau[i] = 0.0 if self.control.control_state == STATE.STATE_POS_GETDOWN: self.control.control_state = STATE.STATE_WAITING @@ -289,10 +289,10 @@ class RL: self.getdown_percent = min(1.0, self.getdown_percent) for i in range(self.params.num_of_dofs): command.motor_command.q[i] = (1 - self.getdown_percent) * self.now_state.motor_state.q[i] + self.getdown_percent * self.start_state.motor_state.q[i] - command.motor_command.dq[i] = 0 + command.motor_command.dq[i] = 0.0 command.motor_command.kp[i] = self.params.fixed_kp[0][i].item() command.motor_command.kd[i] = self.params.fixed_kd[0][i].item() - command.motor_command.tau[i] = 0 + command.motor_command.tau[i] = 0.0 print("\r" + LOGGER.INFO + f"Getting down {self.getdown_percent * 100.0:.1f}", end='', flush=True) if self.getdown_percent == 1: @@ -424,6 +424,7 @@ class RL: # self.csv_filename += f"_{timestamp}" self.csv_filename += ".csv" + print(LOGGER.INFO + f"Recording motor data in '{os.path.abspath(self.csv_filename)}'") with open(self.csv_filename, 'w', newline='') as file: writer = csv.writer(file) diff --git a/src/rl_sar/scripts/rl_sim.py b/src/rl_sar/scripts/rl_sim.py old mode 100644 new mode 100755 index fc88643..abb7f64 --- a/src/rl_sar/scripts/rl_sim.py +++ b/src/rl_sar/scripts/rl_sim.py @@ -1,90 +1,94 @@ -import sys +#!/usr/bin/env python3 import os import torch import threading import time -import rospy -import numpy as np -from gazebo_msgs.msg import ModelStates -from sensor_msgs.msg import JointState -from geometry_msgs.msg import Twist, Pose -from robot_msgs.msg import MotorCommand -from gazebo_msgs.srv import SetModelState, SetModelStateRequest +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default +from sensor_msgs.msg import Imu +from geometry_msgs.msg import Twist +from robot_msgs.msg import RobotCommand as RobotCommandMsg +from robot_msgs.msg import RobotState as RobotStateMsg +from robot_msgs.msg import MotorCommand as MotorCommandMsg +from robot_msgs.msg import MotorState as MotorStateMsg +from gazebo_msgs.srv import SetModelState from std_srvs.srv import Empty - -path = os.path.abspath(".") -sys.path.insert(0, path + "/src/rl_sar/scripts") +from rcl_interfaces.srv import GetParameters +from ament_index_python.packages import get_package_share_directory from rl_sdk import * from observation_buffer import * -CSV_LOGGER = False +CSV_LOGGER = True -class RL_Sim(RL): +class RL_Sim(RL, Node): def __init__(self): - super().__init__() + Node.__init__(self, "rl_sim_node") + RL.__init__(self) # member variables for RL_Sim - self.vel = Twist() - self.pose = Pose() self.cmd_vel = Twist() + self.gazebo_imu = Imu() + self.robot_state_subscriber_msg = RobotStateMsg() + self.robot_command_publisher_msg = RobotCommandMsg() - # start ros node - rospy.init_node("rl_sim") + self.ros_namespace = self.get_namespace() + + # get params from param_node + self.param_client = self.create_client(GetParameters, '/param_node/get_parameters') + while not self.param_client.wait_for_service(timeout_sec=1.0): + self.get_logger().warn("Waiting for param_node service to be available...") + request = GetParameters.Request() + request.names = ['robot_name', 'gazebo_model_name'] + future = self.param_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + if len(future.result().values) < 2: + self.get_logger().warn("Failed to get all parameters from param_node") + else: + self.robot_name = future.result().values[0].string_value + self.gazebo_model_name = future.result().values[1].string_value + self.get_logger().info(f"Get param robot_name: {self.robot_name}") + self.get_logger().info(f"Get param gazebo_model_name: {self.gazebo_model_name}") # read params from yaml - self.robot_name = rospy.get_param("robot_name", "") self.ReadYaml(self.robot_name) for i in range(len(self.params.observations)): if self.params.observations[i] == "ang_vel": self.params.observations[i] = "ang_vel_world" - # history + # init rl + torch.set_grad_enabled(False) if len(self.params.observations_history) != 0: self.history_obs_buf = ObservationBuffer(1, self.params.num_observations, len(self.params.observations_history)) - - # Due to the fact that the robot_state_publisher sorts the joint names alphabetically, - # the mapping table is established according to the order defined in the YAML file - sorted_joint_controller_names = sorted(self.params.joint_controller_names) - self.sorted_to_original_index = {} - for i in range(len(self.params.joint_controller_names)): - self.sorted_to_original_index[sorted_joint_controller_names[i]] = i - self.mapped_joint_positions = [0.0] * self.params.num_of_dofs - self.mapped_joint_velocities = [0.0] * self.params.num_of_dofs - self.mapped_joint_efforts = [0.0] * self.params.num_of_dofs - - # init - torch.set_grad_enabled(False) - self.joint_publishers_commands = [MotorCommand() for _ in range(self.params.num_of_dofs)] + self.robot_command_publisher_msg.motor_command = [MotorCommandMsg() for _ in range(self.params.num_of_dofs)] + self.robot_state_subscriber_msg.motor_state = [MotorStateMsg() for _ in range(self.params.num_of_dofs)] self.InitObservations() self.InitOutputs() self.InitControl() + self.running_state = STATE.STATE_RL_RUNNING # model - model_path = os.path.join(os.path.dirname(__file__), f"../models/{self.robot_name}/{self.params.model_name}") + model_path = os.path.join(get_package_share_directory('rl_sar'), 'models', self.robot_name, self.params.model_name) self.model = torch.jit.load(model_path) # publisher - self.ros_namespace = rospy.get_param("ros_namespace", "") - self.joint_publishers = {} - for i in range(self.params.num_of_dofs): - topic_name = f"{self.ros_namespace}{self.params.joint_controller_names[i]}/command" - self.joint_publishers[self.params.joint_controller_names[i]] = rospy.Publisher(topic_name, MotorCommand, queue_size=10) + self.robot_command_publisher = self.create_publisher(RobotCommandMsg, self.ros_namespace + "robot_joint_controller/command", qos_profile_system_default) # subscriber - self.cmd_vel_subscriber = rospy.Subscriber("/cmd_vel", Twist, self.CmdvelCallback, queue_size=10) - self.model_state_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.ModelStatesCallback, queue_size=10) - joint_states_topic = f"{self.ros_namespace}joint_states" - self.joint_state_subscriber = rospy.Subscriber(joint_states_topic, JointState, self.JointStatesCallback, queue_size=10) + self.cmd_vel_subscriber = self.create_subscription(Twist, "/cmd_vel", self.CmdvelCallback, qos_profile_system_default) + self.gazebo_imu_subscriber = self.create_subscription(Imu, "/imu", self.GazeboImuCallback, qos_profile_system_default) + self.robot_state_subscriber = self.create_subscription(RobotStateMsg, self.ros_namespace + "robot_joint_controller/state", self.RobotStateCallback, qos_profile_system_default) # service - self.gazebo_model_name = rospy.get_param("gazebo_model_name", "") - self.gazebo_set_model_state_client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) - self.gazebo_pause_physics_client = rospy.ServiceProxy("/gazebo/pause_physics", Empty) - self.gazebo_unpause_physics_client = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) + self.gazebo_set_model_state_client = self.create_client(SetModelState, "/gazebo/set_model_state") + self.gazebo_pause_physics_client = self.create_client(Empty, "/gazebo/pause_physics") + self.gazebo_unpause_physics_client = self.create_client(Empty, "/gazebo/unpause_physics") # loops self.thread_control = threading.Thread(target=self.ThreadControl) self.thread_rl = threading.Thread(target=self.ThreadRL) + self.thread_control.daemon = True + self.thread_rl.daemon = True self.thread_control.start() self.thread_rl.start() @@ -96,87 +100,82 @@ class RL_Sim(RL): if CSV_LOGGER: self.CSVInit(self.robot_name) - print(LOGGER.INFO + "RL_Sim start") + self.get_logger().info("RL_Sim start") def __del__(self): - print(LOGGER.INFO + "RL_Sim exit") + self.get_logger().info("RL_Sim exit") def GetState(self, state): if self.params.framework == "isaacgym": - state.imu.quaternion[3] = self.pose.orientation.w - state.imu.quaternion[0] = self.pose.orientation.x - state.imu.quaternion[1] = self.pose.orientation.y - state.imu.quaternion[2] = self.pose.orientation.z + state.imu.quaternion[3] = self.gazebo_imu.orientation.w + state.imu.quaternion[0] = self.gazebo_imu.orientation.x + state.imu.quaternion[1] = self.gazebo_imu.orientation.y + state.imu.quaternion[2] = self.gazebo_imu.orientation.z elif self.params.framework == "isaacsim": - state.imu.quaternion[0] = self.pose.orientation.w - state.imu.quaternion[1] = self.pose.orientation.x - state.imu.quaternion[2] = self.pose.orientation.y - state.imu.quaternion[3] = self.pose.orientation.z + state.imu.quaternion[0] = self.gazebo_imu.orientation.w + state.imu.quaternion[1] = self.gazebo_imu.orientation.x + state.imu.quaternion[2] = self.gazebo_imu.orientation.y + state.imu.quaternion[3] = self.gazebo_imu.orientation.z - state.imu.gyroscope[0] = self.vel.angular.x - state.imu.gyroscope[1] = self.vel.angular.y - state.imu.gyroscope[2] = self.vel.angular.z + state.imu.gyroscope[0] = self.gazebo_imu.angular_velocity.x + state.imu.gyroscope[1] = self.gazebo_imu.angular_velocity.y + state.imu.gyroscope[2] = self.gazebo_imu.angular_velocity.z - # state.imu.accelerometer + state.imu.accelerometer[0] = self.gazebo_imu.linear_acceleration.x + state.imu.accelerometer[1] = self.gazebo_imu.linear_acceleration.y + state.imu.accelerometer[2] = self.gazebo_imu.linear_acceleration.z for i in range(self.params.num_of_dofs): - state.motor_state.q[i] = self.mapped_joint_positions[i] - state.motor_state.dq[i] = self.mapped_joint_velocities[i] - state.motor_state.tauEst[i] = self.mapped_joint_efforts[i] + state.motor_state.q[i] = self.robot_state_subscriber_msg.motor_state[i].q + state.motor_state.dq[i] = self.robot_state_subscriber_msg.motor_state[i].dq + state.motor_state.tau_est[i] = self.robot_state_subscriber_msg.motor_state[i].tau_est def SetCommand(self, command): for i in range(self.params.num_of_dofs): - self.joint_publishers_commands[i].q = command.motor_command.q[i] - self.joint_publishers_commands[i].dq = command.motor_command.dq[i] - self.joint_publishers_commands[i].kp = command.motor_command.kp[i] - self.joint_publishers_commands[i].kd = command.motor_command.kd[i] - self.joint_publishers_commands[i].tau = command.motor_command.tau[i] + self.robot_command_publisher_msg.motor_command[i].q = float(command.motor_command.q[i]) + self.robot_command_publisher_msg.motor_command[i].dq = float(command.motor_command.dq[i]) + self.robot_command_publisher_msg.motor_command[i].kp = float(command.motor_command.kp[i]) + self.robot_command_publisher_msg.motor_command[i].kd = float(command.motor_command.kd[i]) + self.robot_command_publisher_msg.motor_command[i].tau = float(command.motor_command.tau[i]) - for i in range(self.params.num_of_dofs): - self.joint_publishers[self.params.joint_controller_names[i]].publish(self.joint_publishers_commands[i]) + self.robot_command_publisher.publish(self.robot_command_publisher_msg) def RobotControl(self): - if self.control.control_state == STATE.STATE_RESET_SIMULATION: - set_model_state = SetModelStateRequest().model_state - set_model_state.model_name = self.gazebo_model_name - set_model_state.pose.position.z = 1.0 - set_model_state.reference_frame = "world" - self.gazebo_set_model_state_client(set_model_state) - self.control.control_state = STATE.STATE_WAITING - if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION: - if self.simulation_running: - self.gazebo_pause_physics_client() - print("\r\n" + LOGGER.INFO + "Simulation Stop") - else: - self.gazebo_unpause_physics_client() - print("\r\n" + LOGGER.INFO + "Simulation Start") - self.simulation_running = not self.simulation_running - self.control.control_state = STATE.STATE_WAITING + # NOT AVAILABLE NOW + # if self.control.control_state == STATE.STATE_RESET_SIMULATION: + # set_model_state = SetModelState.Request() + # set_model_state.model_state.model_name = self.gazebo_model_name + # set_model_state.model_state.pose.position.z = 1.0 + # set_model_state.model_state.reference_frame = "world" + # self.gazebo_set_model_state_client.call_async(set_model_state) + # self.control.control_state = STATE.STATE_WAITING + # if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION: + # if self.simulation_running: + # self.gazebo_pause_physics_client.call_async(Empty.Request()) + # self.get_logger().info("Simulation Stop") + # else: + # self.gazebo_unpause_physics_client.call_async(Empty.Request()) + # self.get_logger().info("Simulation Start") + # self.simulation_running = not self.simulation_running + # self.control.control_state = STATE.STATE_WAITING if self.simulation_running: self.GetState(self.robot_state) self.StateController(self.robot_state, self.robot_command) self.SetCommand(self.robot_command) - def ModelStatesCallback(self, msg): - self.vel = msg.twist[2] - self.pose = msg.pose[2] + def GazeboImuCallback(self, msg): + self.gazebo_imu = msg def CmdvelCallback(self, msg): self.cmd_vel = msg - def MapData(self, source_data, target_data): - for i in range(len(source_data)): - target_data[i] = source_data[self.sorted_to_original_index[self.params.joint_controller_names[i]]] - - def JointStatesCallback(self, msg): - self.MapData(msg.position, self.mapped_joint_positions) - self.MapData(msg.velocity, self.mapped_joint_velocities) - self.MapData(msg.effort, self.mapped_joint_efforts) + def RobotStateCallback(self, msg): + self.robot_state_subscriber_msg = msg def RunModel(self): if self.running_state == STATE.STATE_RL_RUNNING and self.simulation_running: - self.obs.lin_vel = torch.tensor([[self.vel.linear.x, self.vel.linear.y, self.vel.linear.z]]) + # self.obs.lin_vel NOT USE self.obs.ang_vel = torch.tensor(self.robot_state.imu.gyroscope).unsqueeze(0) # self.obs.commands = torch.tensor([[self.cmd_vel.linear.x, self.cmd_vel.linear.y, self.cmd_vel.angular.z]]) self.obs.commands = torch.tensor([[self.control.x, self.control.y, self.control.yaw]]) @@ -199,7 +198,9 @@ class RL_Sim(RL): self.output_dof_pos = self.ComputePosition(self.obs.actions) if CSV_LOGGER: - tau_est = torch.tensor(self.mapped_joint_efforts).unsqueeze(0) + tau_est = torch.empty(1, self.params.num_of_dofs, dtype=torch.float32) + for i in range(self.params.num_of_dofs): + tau_est[0, i] = self.robot_state_subscriber_msg.motor_state[i].tau_est self.CSVLogger(self.output_torques, tau_est, self.obs.dof_pos, self.output_dof_pos, self.obs.dof_vel) def Forward(self): @@ -219,21 +220,27 @@ class RL_Sim(RL): def ThreadControl(self): thread_period = self.params.dt thread_name = "thread_control" - print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified") - while not rospy.is_shutdown(): + self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified") + while rclpy.ok(): self.RobotControl() time.sleep(thread_period) - print("[Thread End] named: " + thread_name) + self.get_logger().info("[Thread End] named: " + thread_name) def ThreadRL(self): thread_period = self.params.dt * self.params.decimation thread_name = "thread_rl" - print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified") - while not rospy.is_shutdown(): + self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified") + while rclpy.ok(): self.RunModel() time.sleep(thread_period) - print("[Thread End] named: " + thread_name) + self.get_logger().info("[Thread End] named: " + thread_name) if __name__ == "__main__": + rclpy.init() rl_sim = RL_Sim() - rospy.spin() + try: + rclpy.spin(rl_sim) + except KeyboardInterrupt: + rl_sim.get_logger().info("Shutdown signal received, shutting down...") + rclpy.shutdown() + diff --git a/src/rl_sar/src/rl_real_a1.cpp b/src/rl_sar/src/rl_real_a1.cpp index 1052954..3f54ddd 100644 --- a/src/rl_sar/src/rl_real_a1.cpp +++ b/src/rl_sar/src/rl_real_a1.cpp @@ -116,7 +116,7 @@ void RL_Real::GetState(RobotState *state) { state->motor_state.q[i] = this->unitree_low_state.motorState[state_mapping[i]].q; state->motor_state.dq[i] = this->unitree_low_state.motorState[state_mapping[i]].dq; - state->motor_state.tauEst[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst; + state->motor_state.tau_est[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst; } } @@ -178,7 +178,7 @@ void RL_Real::RunModel() this->output_dof_pos = this->ComputePosition(this->obs.actions); #ifdef CSV_LOGGER - torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0); + torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0); this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel); #endif } diff --git a/src/rl_sar/src/rl_real_go2.cpp b/src/rl_sar/src/rl_real_go2.cpp index f414b1d..c19d4ba 100644 --- a/src/rl_sar/src/rl_real_go2.cpp +++ b/src/rl_sar/src/rl_real_go2.cpp @@ -122,7 +122,7 @@ void RL_Real::GetState(RobotState *state) { state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q(); state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq(); - state->motor_state.tauEst[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est(); + state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est(); } } @@ -183,7 +183,7 @@ void RL_Real::RunModel() this->output_dof_pos = this->ComputePosition(this->obs.actions); #ifdef CSV_LOGGER - torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0); + torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0); this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel); #endif } diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 69ebcc2..57cf071 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -1,14 +1,43 @@ #include "rl_sim.hpp" // #define PLOT -// #define CSV_LOGGER +#define CSV_LOGGER RL_Sim::RL_Sim() + : rclcpp::Node("rl_sim_node") { - ros::NodeHandle nh; + this->ros_namespace = this->get_namespace(); + + // get params from param_node + param_client = this->create_client("/param_node/get_parameters"); + while (!param_client->wait_for_service(std::chrono::seconds(1))) + { + std::cout << LOGGER::WARNING << "Waiting for param_node service to be available..." << std::endl; + } + auto request = std::make_shared(); + request->names.push_back("robot_name"); + request->names.push_back("gazebo_model_name"); + auto future = param_client->async_send_request(request); + rclcpp::spin_until_future_complete(this->get_node_base_interface(), future); + if (future.get()->values.size() < 2) + { + std::cout << LOGGER::WARNING << "Failed to get all parameters from param_node" << std::endl; + } + else + { + this->robot_name = future.get()->values[0].string_value; + this->gazebo_model_name = future.get()->values[1].string_value; + std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl; + std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl; + } + + // // get params from this node + // this->declare_parameter("robot_name", ""); + // this->get_parameter("robot_name", this->robot_name); + // this->declare_parameter("gazebo_model_name", ""); + // this->get_parameter("gazebo_model_name", this->gazebo_model_name); // read params from yaml - nh.param("robot_name", this->robot_name, ""); this->ReadYaml(this->robot_name); for (std::string &observation : this->params.observations) { @@ -18,17 +47,6 @@ RL_Sim::RL_Sim() observation = "ang_vel_world"; } } - // Due to the fact that the robot_state_publisher sorts the joint names alphabetically, - // the mapping table is established according to the order defined in the YAML file - std::vector sorted_joint_controller_names = this->params.joint_controller_names; - std::sort(sorted_joint_controller_names.begin(), sorted_joint_controller_names.end()); - for (size_t i = 0; i < this->params.joint_controller_names.size(); ++i) - { - this->sorted_to_original_index[sorted_joint_controller_names[i]] = i; - } - this->mapped_joint_positions = std::vector(this->params.num_of_dofs, 0.0); - this->mapped_joint_velocities = std::vector(this->params.num_of_dofs, 0.0); - this->mapped_joint_efforts = std::vector(this->params.num_of_dofs, 0.0); // init rl torch::autograd::GradMode::set_enabled(false); @@ -36,7 +54,8 @@ RL_Sim::RL_Sim() { this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size()); } - this->joint_publishers_commands.resize(this->params.num_of_dofs); + this->robot_command_publisher_msg.motor_command.resize(this->params.num_of_dofs); + this->robot_state_subscriber_msg.motor_state.resize(this->params.num_of_dofs); this->InitObservations(); this->InitOutputs(); this->InitControl(); @@ -47,24 +66,26 @@ RL_Sim::RL_Sim() this->model = torch::jit::load(model_path); // publisher - nh.param("ros_namespace", this->ros_namespace, ""); - for (int i = 0; i < this->params.num_of_dofs; ++i) - { - // joint need to rename as xxx_joint - this->joint_publishers[this->params.joint_controller_names[i]] = - nh.advertise(this->ros_namespace + this->params.joint_controller_names[i] + "/command", 10); - } + this->robot_command_publisher = this->create_publisher( + this->ros_namespace + "robot_joint_controller/command", rclcpp::SystemDefaultsQoS()); // subscriber - this->cmd_vel_subscriber = nh.subscribe("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this); - this->model_state_subscriber = nh.subscribe("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this); - this->joint_state_subscriber = nh.subscribe(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this); + this->cmd_vel_subscriber = this->create_subscription( + "/cmd_vel", rclcpp::SystemDefaultsQoS(), + [this] (const geometry_msgs::msg::Twist::SharedPtr msg) {this->CmdvelCallback(msg);} + ); + this->gazebo_imu_subscriber = this->create_subscription( + "/imu", rclcpp::SystemDefaultsQoS(), [this] (const sensor_msgs::msg::Imu::SharedPtr msg) {this->GazeboImuCallback(msg);} + ); + this->robot_state_subscriber = this->create_subscription( + this->ros_namespace + "robot_joint_controller/state", rclcpp::SystemDefaultsQoS(), + [this] (const robot_msgs::msg::RobotState::SharedPtr msg) {this->RobotStateCallback(msg);} + ); // service - nh.param("gazebo_model_name", this->gazebo_model_name, ""); - this->gazebo_set_model_state_client = nh.serviceClient("/gazebo/set_model_state"); - this->gazebo_pause_physics_client = nh.serviceClient("/gazebo/pause_physics"); - this->gazebo_unpause_physics_client = nh.serviceClient("/gazebo/unpause_physics"); + this->gazebo_set_model_state_client = this->create_client("/gazebo/set_model_state"); + this->gazebo_pause_physics_client = this->create_client("/gazebo/pause_physics"); + this->gazebo_unpause_physics_client = this->create_client("/gazebo/unpause_physics"); // loop this->loop_control = std::make_shared("loop_control", this->params.dt, std::bind(&RL_Sim::RobotControl, this)); @@ -107,30 +128,32 @@ void RL_Sim::GetState(RobotState *state) { if (this->params.framework == "isaacgym") { - state->imu.quaternion[3] = this->pose.orientation.w; - state->imu.quaternion[0] = this->pose.orientation.x; - state->imu.quaternion[1] = this->pose.orientation.y; - state->imu.quaternion[2] = this->pose.orientation.z; + state->imu.quaternion[3] = this->gazebo_imu.orientation.w; + state->imu.quaternion[0] = this->gazebo_imu.orientation.x; + state->imu.quaternion[1] = this->gazebo_imu.orientation.y; + state->imu.quaternion[2] = this->gazebo_imu.orientation.z; } else if (this->params.framework == "isaacsim") { - state->imu.quaternion[0] = this->pose.orientation.w; - state->imu.quaternion[1] = this->pose.orientation.x; - state->imu.quaternion[2] = this->pose.orientation.y; - state->imu.quaternion[3] = this->pose.orientation.z; + state->imu.quaternion[0] = this->gazebo_imu.orientation.w; + state->imu.quaternion[1] = this->gazebo_imu.orientation.x; + state->imu.quaternion[2] = this->gazebo_imu.orientation.y; + state->imu.quaternion[3] = this->gazebo_imu.orientation.z; } - state->imu.gyroscope[0] = this->vel.angular.x; - state->imu.gyroscope[1] = this->vel.angular.y; - state->imu.gyroscope[2] = this->vel.angular.z; + state->imu.gyroscope[0] = this->gazebo_imu.angular_velocity.x; + state->imu.gyroscope[1] = this->gazebo_imu.angular_velocity.y; + state->imu.gyroscope[2] = this->gazebo_imu.angular_velocity.z; - // state->imu.accelerometer + state->imu.accelerometer[0] = this->gazebo_imu.linear_acceleration.x; + state->imu.accelerometer[1] = this->gazebo_imu.linear_acceleration.y; + state->imu.accelerometer[2] = this->gazebo_imu.linear_acceleration.z; for (int i = 0; i < this->params.num_of_dofs; ++i) { - state->motor_state.q[i] = this->mapped_joint_positions[i]; - state->motor_state.dq[i] = this->mapped_joint_velocities[i]; - state->motor_state.tauEst[i] = this->mapped_joint_efforts[i]; + state->motor_state.q[i] = this->robot_state_subscriber_msg.motor_state[i].q; + state->motor_state.dq[i] = this->robot_state_subscriber_msg.motor_state[i].dq; + state->motor_state.tau_est[i] = this->robot_state_subscriber_msg.motor_state[i].tau_est; } } @@ -138,49 +161,79 @@ void RL_Sim::SetCommand(const RobotCommand *command) { for (int i = 0; i < this->params.num_of_dofs; ++i) { - this->joint_publishers_commands[i].q = command->motor_command.q[i]; - this->joint_publishers_commands[i].dq = command->motor_command.dq[i]; - this->joint_publishers_commands[i].kp = command->motor_command.kp[i]; - this->joint_publishers_commands[i].kd = command->motor_command.kd[i]; - this->joint_publishers_commands[i].tau = command->motor_command.tau[i]; + this->robot_command_publisher_msg.motor_command[i].q = command->motor_command.q[i]; + this->robot_command_publisher_msg.motor_command[i].dq = command->motor_command.dq[i]; + this->robot_command_publisher_msg.motor_command[i].kp = command->motor_command.kp[i]; + this->robot_command_publisher_msg.motor_command[i].kd = command->motor_command.kd[i]; + this->robot_command_publisher_msg.motor_command[i].tau = command->motor_command.tau[i]; } - for (int i = 0; i < this->params.num_of_dofs; ++i) - { - this->joint_publishers[this->params.joint_controller_names[i]].publish(this->joint_publishers_commands[i]); - } + this->robot_command_publisher->publish(this->robot_command_publisher_msg); } void RL_Sim::RobotControl() { std::lock_guard lock(robot_state_mutex); - if (this->control.control_state == STATE_RESET_SIMULATION) - { - gazebo_msgs::SetModelState set_model_state; - set_model_state.request.model_state.model_name = this->gazebo_model_name; - set_model_state.request.model_state.pose.position.z = 1.0; - set_model_state.request.model_state.reference_frame = "world"; - this->gazebo_set_model_state_client.call(set_model_state); - - this->control.control_state = STATE_WAITING; - } - if (this->control.control_state == STATE_TOGGLE_SIMULATION) - { - std_srvs::Empty empty; - if (simulation_running) - { - this->gazebo_pause_physics_client.call(empty); - std::cout << std::endl << LOGGER::INFO << "Simulation Stop" << std::endl; - } - else - { - this->gazebo_unpause_physics_client.call(empty); - std::cout << std::endl << LOGGER::INFO << "Simulation Start" << std::endl; - } - simulation_running = !simulation_running; - this->control.control_state = STATE_WAITING; - } + // if (this->control.control_state == STATE_RESET_SIMULATION) + // { + // auto set_model_state_request = std::make_shared(); + // set_model_state_request->model_state.model_name = this->gazebo_model_name; + // set_model_state_request->model_state.pose.position.z = 1.0; + // set_model_state_request->model_state.reference_frame = "world"; + // this->gazebo_set_model_state_client->async_send_request(set_model_state_request, + // [this](rclcpp::Client::SharedFuture future) + // { + // if (future.get()) + // { + // std::cout << LOGGER::INFO << "Reset Simulation" << std::endl; + // this->control.control_state = STATE_WAITING; + // } + // else + // { + // std::cout << LOGGER::WARNING << "Failed to reset simulation" << std::endl; + // } + // } + // ); + // } + // if (this->control.control_state == STATE_TOGGLE_SIMULATION) + // { + // auto empty_request = std::make_shared(); + // if (simulation_running) + // { + // this->gazebo_pause_physics_client->async_send_request(empty_request, + // [this](rclcpp::Client::SharedFuture future) + // { + // if (future.get()) + // { + // std::cout << LOGGER::INFO << "Simulation Stop" << std::endl; + // } + // else + // { + // std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl; + // } + // } + // ); + // } + // else + // { + // this->gazebo_unpause_physics_client->async_send_request(empty_request, + // [this](rclcpp::Client::SharedFuture future) + // { + // if (future.get()) + // { + // std::cout << LOGGER::INFO << "Simulation Start" << std::endl; + // } + // else + // { + // std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl; + // } + // } + // ); + // } + // simulation_running = !simulation_running; + // this->control.control_state = STATE_WAITING; + // } if (simulation_running) { this->motiontime++; @@ -190,30 +243,19 @@ void RL_Sim::RobotControl() } } -void RL_Sim::ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg) +void RL_Sim::GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { - this->vel = msg->twist[2]; - this->pose = msg->pose[2]; + this->gazebo_imu = *msg; } -void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg) +void RL_Sim::CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { this->cmd_vel = *msg; } -void RL_Sim::MapData(const std::vector &source_data, std::vector &target_data) +void RL_Sim::RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg) { - for (size_t i = 0; i < source_data.size(); ++i) - { - target_data[i] = source_data[this->sorted_to_original_index[this->params.joint_controller_names[i]]]; - } -} - -void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - MapData(msg->position, this->mapped_joint_positions); - MapData(msg->velocity, this->mapped_joint_velocities); - MapData(msg->effort, this->mapped_joint_efforts); + this->robot_state_subscriber_msg = *msg; } void RL_Sim::RunModel() @@ -222,7 +264,7 @@ void RL_Sim::RunModel() if (this->running_state == STATE_RL_RUNNING && simulation_running) { - this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}}); + // this->obs.lin_vel NOT USE this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0); // this->obs.commands = torch::tensor({{this->cmd_vel.linear.x, this->cmd_vel.linear.y, this->cmd_vel.angular.z}}); this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}}); @@ -247,7 +289,11 @@ void RL_Sim::RunModel() this->output_dof_pos = this->ComputePosition(this->obs.actions); #ifdef CSV_LOGGER - torch::Tensor tau_est = torch::tensor(this->mapped_joint_efforts).unsqueeze(0); + torch::Tensor tau_est = torch::empty({1, this->params.num_of_dofs}, torch::kFloat); + for (int i = 0; i < this->params.num_of_dofs; ++i) + { + tau_est[0][i] = this->robot_state_subscriber_msg.motor_state[i].tau_est; + } this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel); #endif } @@ -291,8 +337,8 @@ void RL_Sim::Plot() { this->plot_real_joint_pos[i].erase(this->plot_real_joint_pos[i].begin()); this->plot_target_joint_pos[i].erase(this->plot_target_joint_pos[i].begin()); - this->plot_real_joint_pos[i].push_back(this->mapped_joint_positions[i]); - this->plot_target_joint_pos[i].push_back(this->joint_publishers_commands[i].q); + this->plot_real_joint_pos[i].push_back(this->robot_state_subscriber_msg.motor_state[i].q); + this->plot_target_joint_pos[i].push_back(this->robot_command_publisher_msg.motor_command[i].q); plt::subplot(4, 3, i + 1); plt::named_plot("_real_joint_pos", this->plot_t, this->plot_real_joint_pos[i], "r"); plt::named_plot("_target_joint_pos", this->plot_t, this->plot_target_joint_pos[i], "b"); @@ -302,17 +348,10 @@ void RL_Sim::Plot() plt::pause(0.01); } -void signalHandler(int signum) -{ - ros::shutdown(); - exit(0); -} - int main(int argc, char **argv) { - signal(SIGINT, signalHandler); - ros::init(argc, argv, "rl_sar"); - RL_Sim rl_sar; - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/src/robot_joint_controller/CMakeLists.txt b/src/robot_joint_controller/CMakeLists.txt index c64c9e9..9e80a17 100644 --- a/src/robot_joint_controller/CMakeLists.txt +++ b/src/robot_joint_controller/CMakeLists.txt @@ -1,31 +1,66 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(robot_joint_controller) -find_package(catkin REQUIRED COMPONENTS - controller_interface - hardware_interface - pluginlib - roscpp - realtime_tools - robot_msgs +if(DEFINED ENV{ROS_DISTRO}) + set(ROS_DISTRO_ENV $ENV{ROS_DISTRO}) + if(ROS_DISTRO_ENV STREQUAL "foxy") + add_definitions(-DROS_DISTRO_FOXY) + elseif(ROS_DISTRO_ENV STREQUAL "humble") + add_definitions(-DROS_DISTRO_HUMBLE) + else() + add_definitions(-DROS_DISTRO_UNKNOWN) + endif() +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(urdf REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(robot_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/robot_joint_controller.cpp + src/robot_joint_controller_group.cpp ) -catkin_package( - CATKIN_DEPENDS - robot_msgs - controller_interface - hardware_interface - pluginlib - roscpp - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} +target_include_directories(${PROJECT_NAME} PRIVATE include) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + urdf + control_toolbox + realtime_tools + hardware_interface + controller_interface + pluginlib + robot_msgs + geometry_msgs + rcl_interfaces ) -include_directories(include ${catkin_INCLUDE_DIRS}) +pluginlib_export_plugin_description_file(controller_interface robot_joint_controller_plugins.xml) -link_directories(${catkin_LIB_DIRS} lib) - -add_library(robot_joint_controller - src/robot_joint_controller.cpp +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} ) -target_link_libraries(robot_joint_controller ${catkin_LIBRARIES}) + +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(rclcpp urdf control_toolbox realtime_tools hardware_interface controller_interface pluginlib robot_msgs geometry_msgs rcl_interfaces) +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_package() diff --git a/src/robot_joint_controller/include/robot_joint_controller.h b/src/robot_joint_controller/include/robot_joint_controller.h deleted file mode 100644 index a58e4fa..0000000 --- a/src/robot_joint_controller/include/robot_joint_controller.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef ROBOT_JOINT_CONTROLLER_H -#define ROBOT_JOINT_CONTROLLER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "robot_msgs/MotorCommand.h" -#include "robot_msgs/MotorState.h" -#include - -#include -#include -#include -#include - -#define PosStopF (2.146E+9f) -#define VelStopF (16000.0f) - -typedef struct -{ - uint8_t mode; - double pos; - double posStiffness; - double vel; - double velStiffness; - double torque; -} ServoCommand; - -namespace robot_joint_controller -{ - class RobotJointController : public controller_interface::Controller - { - private: - hardware_interface::JointHandle joint; - ros::Subscriber sub_command, sub_ft; - control_toolbox::Pid pid_controller_; - std::unique_ptr> controller_state_publisher_; - - public: - std::string name_space; - std::string joint_name; - urdf::JointConstSharedPtr joint_urdf; - realtime_tools::RealtimeBuffer command; - robot_msgs::MotorCommand lastCommand; - robot_msgs::MotorState lastState; - ServoCommand servoCommand; - - RobotJointController(); - ~RobotJointController(); - virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); - virtual void starting(const ros::Time &time); - virtual void update(const ros::Time &time, const ros::Duration &period); - virtual void stopping(); - void setCommandCB(const robot_msgs::MotorCommandConstPtr &msg); - void positionLimits(double &position); - void velocityLimits(double &velocity); - void effortLimits(double &effort); - - void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false); - void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup); - void getGains(double &p, double &i, double &d, double &i_max, double &i_min); - }; -} - -#endif diff --git a/src/robot_joint_controller/include/robot_joint_controller.hpp b/src/robot_joint_controller/include/robot_joint_controller.hpp new file mode 100644 index 0000000..7477a78 --- /dev/null +++ b/src/robot_joint_controller/include/robot_joint_controller.hpp @@ -0,0 +1,92 @@ +#ifndef ROBOT_JOINT_CONTROLLER_HPP +#define ROBOT_JOINT_CONTROLLER_HPP + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include +#include +#include +#include "controller_interface/controller_interface.hpp" +#include +#include +#include "robot_msgs/msg/motor_command.hpp" +#include "robot_msgs/msg/motor_state.hpp" +#include "visibility_control.h" + +#include +#include +#include +#include +#include +#include + +#define PosStopF (2.146E+9f) +#define VelStopF (16000.0f) + +typedef struct +{ + uint8_t mode; + double pos; + double pos_stiffness; + double vel; + double vel_stiffness; + double torque; +} ServoCommand; + +namespace robot_joint_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class RobotJointController : public controller_interface::ControllerInterface +{ +public: + ROBOT_JOINT_CONTROLLER_PUBLIC + RobotJointController(); + +#if defined(ROS_DISTRO_FOXY) + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::return_type update() override; +#elif defined(ROS_DISTRO_HUMBLE) + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; +#endif + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + // ROBOT_JOINT_CONTROLLER_PUBLIC + // CallbackReturn on_init() override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + void UpdateFunc(const double &period_seconds); + void SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg); + void PositionLimit(double &position); + void VelocityLimit(double &velocity); + void EffortLimit(double &effort); + +protected: + std::string name_space_; + std::string joint_name_; + realtime_tools::RealtimeBuffer rt_command_ptr_; + robot_msgs::msg::MotorCommand last_command_; + robot_msgs::msg::MotorState last_state_; + ServoCommand servo_command_; + urdf::JointConstSharedPtr joints_urdf_; + rclcpp::Client::SharedPtr robot_description_client_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; + std::shared_ptr> controller_state_publisher_; +#if defined(ROS_DISTRO_FOXY) + rclcpp::Time previous_update_timestamp_{0}; +#endif +}; + +} // namespace robot_joint_controller + +#endif // ROBOT_JOINT_CONTROLLER_HPP diff --git a/src/robot_joint_controller/include/robot_joint_controller_group.hpp b/src/robot_joint_controller/include/robot_joint_controller_group.hpp new file mode 100644 index 0000000..0efb7cc --- /dev/null +++ b/src/robot_joint_controller/include/robot_joint_controller_group.hpp @@ -0,0 +1,92 @@ +#ifndef ROBOT_JOINT_CONTROLLER_GROUP_HPP +#define ROBOT_JOINT_CONTROLLER_GROUP_HPP + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include +#include +#include +#include "controller_interface/controller_interface.hpp" +#include +#include +#include "robot_msgs/msg/robot_command.hpp" +#include "robot_msgs/msg/robot_state.hpp" +#include "visibility_control.h" + +#include +#include +#include +#include +#include +#include + +#define PosStopF (2.146E+9f) +#define VelStopF (16000.0f) + +typedef struct +{ + uint8_t mode; + double pos; + double pos_stiffness; + double vel; + double vel_stiffness; + double torque; +} ServoCommand; + +namespace robot_joint_controller +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class RobotJointControllerGroup : public controller_interface::ControllerInterface +{ +public: + ROBOT_JOINT_CONTROLLER_PUBLIC + RobotJointControllerGroup(); + +#if defined(ROS_DISTRO_FOXY) + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::return_type update() override; +#elif defined(ROS_DISTRO_HUMBLE) + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override; +#endif + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + ROBOT_JOINT_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + // ROBOT_JOINT_CONTROLLER_PUBLIC + // CallbackReturn on_init() override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + ROBOT_JOINT_CONTROLLER_PUBLIC + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + void UpdateFunc(const double &period_seconds); + void SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg); + void PositionLimit(double &position, int &index); + void VelocityLimit(double &velocity, int &index); + void EffortLimit(double &effort, int &index); + +protected: + std::string name_space_; + std::vector joint_names_; + realtime_tools::RealtimeBuffer rt_command_ptr_; + robot_msgs::msg::RobotCommand last_command_; + robot_msgs::msg::RobotState last_state_; + ServoCommand servo_command_; + std::vector joints_urdf_; + rclcpp::Client::SharedPtr robot_description_client_; + rclcpp::Subscription::SharedPtr joints_command_subscriber_; + std::shared_ptr> controller_state_publisher_; +#if defined(ROS_DISTRO_FOXY) + rclcpp::Time previous_update_timestamp_{0}; +#endif +}; + +} // namespace robot_joint_controller + +#endif // ROBOT_JOINT_CONTROLLER_GROUP_HPP diff --git a/src/robot_joint_controller/include/visibility_control.h b/src/robot_joint_controller/include/visibility_control.h new file mode 100644 index 0000000..b1354d7 --- /dev/null +++ b/src/robot_joint_controller/include/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_ +#define ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((dllexport)) +#define ROBOT_JOINT_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define ROBOT_JOINT_CONTROLLER_EXPORT __declspec(dllexport) +#define ROBOT_JOINT_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef ROBOT_JOINT_CONTROLLER_BUILDING_DLL +#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_EXPORT +#else +#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_IMPORT +#endif +#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE ROBOT_JOINT_CONTROLLER_PUBLIC +#define ROBOT_JOINT_CONTROLLER_LOCAL +#else +#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define ROBOT_JOINT_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define ROBOT_JOINT_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define ROBOT_JOINT_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROBOT_JOINT_CONTROLLER_PUBLIC +#define ROBOT_JOINT_CONTROLLER_LOCAL +#endif +#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_ \ No newline at end of file diff --git a/src/robot_joint_controller/package.xml b/src/robot_joint_controller/package.xml index 4d75904..f27caab 100644 --- a/src/robot_joint_controller/package.xml +++ b/src/robot_joint_controller/package.xml @@ -1,27 +1,27 @@ - - robot_joint_controller - 0.0.0 - The robot_joint_controller package - Ziqi Fan - Apache - catkin - controller_interface - hardware_interface - pluginlib - roscpp - controller_interface - hardware_interface - pluginlib - roscpp - controller_interface - hardware_interface - pluginlib - roscpp - robot_msgs - - - - - - \ No newline at end of file + + robot_joint_controller + 0.0.0 + The robot_joint_controller package + + Ziqi Fan + Apache-2.0 + + ament_cmake + + rclcpp + urdf + control_toolbox + realtime_tools + hardware_interface + controller_interface + pluginlib + robot_msgs + geometry_msgs + rcl_interfaces + + + ament_cmake + + + diff --git a/src/robot_joint_controller/robot_joint_controller_plugins.xml b/src/robot_joint_controller/robot_joint_controller_plugins.xml index 05cf7f2..2cf7c37 100644 --- a/src/robot_joint_controller/robot_joint_controller_plugins.xml +++ b/src/robot_joint_controller/robot_joint_controller_plugins.xml @@ -1,8 +1,12 @@ - - - - The robot joint controller. - + + + The robot joint single controller + + + The robot joint group controller + diff --git a/src/robot_joint_controller/src/robot_joint_controller.cpp b/src/robot_joint_controller/src/robot_joint_controller.cpp index 41d652c..9d372e9 100644 --- a/src/robot_joint_controller/src/robot_joint_controller.cpp +++ b/src/robot_joint_controller/src/robot_joint_controller.cpp @@ -1,198 +1,231 @@ -#include "robot_joint_controller.h" -#include - -// #define rqtTune // use rqt or not - -double clamp(double &value, double min, double max) -{ - if (value < min) - { - value = min; - } - else if (value > max) - { - value = max; - } - return value; -} +#include "robot_joint_controller.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include namespace robot_joint_controller { +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::HW_IF_EFFORT; - RobotJointController::RobotJointController() +RobotJointController::RobotJointController() + : controller_interface::ControllerInterface(), + joints_command_subscriber_(nullptr), + controller_state_publisher_(nullptr) +{ + memset(&last_command_, 0, sizeof(robot_msgs::msg::MotorCommand)); + memset(&last_state_, 0, sizeof(robot_msgs::msg::MotorState)); + memset(&servo_command_, 0, sizeof(ServoCommand)); +} + +// CallbackReturn RobotJointController::on_init() +// { +// return CallbackReturn::SUCCESS; +// } + +CallbackReturn RobotJointController::on_configure(const rclcpp_lifecycle::State &previous_state) +{ + name_space_ = get_node()->get_namespace(); + + if (!get_node()->get_parameter("joints", joint_name_)) { - memset(&lastCommand, 0, sizeof(robot_msgs::MotorCommand)); - memset(&lastState, 0, sizeof(robot_msgs::MotorState)); - memset(&servoCommand, 0, sizeof(ServoCommand)); + RCLCPP_ERROR(get_node()->get_logger(), "No joint given in namespace: '%s'", name_space_.c_str()); + return CallbackReturn::ERROR; + } + else + { + RCLCPP_WARN(get_node()->get_logger(), "joint_name_: %s", joint_name_.c_str()); } - RobotJointController::~RobotJointController() - { - sub_ft.shutdown(); - sub_command.shutdown(); - } + robot_description_client_ = get_node()->create_client("/robot_state_publisher/get_parameters"); - void RobotJointController::setCommandCB(const robot_msgs::MotorCommandConstPtr &msg) - { - lastCommand.q = msg->q; - lastCommand.kp = msg->kp; - lastCommand.dq = msg->dq; - lastCommand.kd = msg->kd; - lastCommand.tau = msg->tau; - // the writeFromNonRT can be used in RT, if you have the guarantee that - // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) - // * there is only one single rt thread - command.writeFromNonRT(lastCommand); - } + auto request = std::make_shared(); + request->names.push_back("robot_description_"); - // Controller initialization in non-realtime - bool RobotJointController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) + while (!robot_description_client_->wait_for_service(std::chrono::seconds(1))) { - name_space = n.getNamespace(); - if (!n.getParam("joint", joint_name)) + if (!rclcpp::ok()) { - ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str()); - return false; + RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting."); + } + RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again..."); + } + + auto response_received_callback = [this](rclcpp::Client::SharedFuture future) + { + std::string robot_description = future.get()->values[0].string_value; + urdf::Model urdf; + if (!urdf.initString(robot_description)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file"); } - // load pid param from ymal only if rqt need -#ifdef rqtTune - // Load PID Controller using gains set on parameter server - if (!pid_controller_.init(ros::NodeHandle(n, "pid"))) - return false; + joints_urdf_ = urdf.getJoint(joint_name_); + if (!joints_urdf_) + { + RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name_.c_str()); + } + }; + auto future_result = robot_description_client_->async_send_request(request, response_received_callback); + + // Start command subscriber + joints_command_subscriber_ = get_node()->create_subscription( + "~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointController::SetCommandCallback, this, std::placeholders::_1)); + + // Start realtime state publisher + controller_state_publisher_ = std::make_shared>( + get_node()->create_publisher(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS())); + +#if defined(ROS_DISTRO_FOXY) + previous_update_timestamp_ = get_node()->get_clock()->now(); #endif - urdf::Model urdf; // Get URDF info about joint - if (!urdf.initParamWithNodeHandle("robot_description", n)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } - joint_urdf = urdf.getJoint(joint_name); - if (!joint_urdf) - { - ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str()); - return false; - } - joint = robot->getHandle(joint_name); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} - // Start command subscriber - sub_command = n.subscribe("command", 20, &RobotJointController::setCommandCB, this); +controller_interface::InterfaceConfiguration RobotJointController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - // Start realtime state publisher - controller_state_publisher_.reset( - new realtime_tools::RealtimePublisher(n, name_space + "/state", 1)); + command_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT); - return true; - } + return command_interfaces_config; - void RobotJointController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup) +} + +controller_interface::InterfaceConfiguration RobotJointController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT); + + return state_interfaces_config; +} + +CallbackReturn RobotJointController::on_activate(const rclcpp_lifecycle::State &previous_state) +{ + double init_pos = state_interfaces_[0].get_value(); + last_command_.q = init_pos; + last_state_.q = init_pos; + last_command_.dq = 0; + last_state_.dq = 0; + last_command_.tau = 0; + last_state_.tau_est = 0; + + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer(); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn RobotJointController::on_deactivate(const rclcpp_lifecycle::State &previous_state) +{ + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer(); + return CallbackReturn::SUCCESS; +} + +#if defined(ROS_DISTRO_FOXY) +controller_interface::return_type RobotJointController::update() +{ + const auto current_time = get_node()->get_clock()->now(); + const auto period_seconds = (current_time - previous_update_timestamp_).seconds(); + previous_update_timestamp_ = current_time; + auto joint_command = rt_command_ptr_.readFromRT(); + // no command received yet + if (!joint_command) { - pid_controller_.setGains(p, i, d, i_max, i_min, antiwindup); + return controller_interface::return_type::OK; } - - void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup) + last_command_ = *(joint_command); + UpdateFunc(period_seconds); + return controller_interface::return_type::OK; +} +#elif defined(ROS_DISTRO_HUMBLE) +controller_interface::return_type RobotJointController::update(const rclcpp::Time &time, const rclcpp::Duration &period) +{ + const auto period_seconds = period.seconds(); + auto joint_command = rt_command_ptr_.readFromRT(); + // no command received yet + if (!joint_command) { - pid_controller_.getGains(p, i, d, i_max, i_min, antiwindup); + return controller_interface::return_type::OK; } - - void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min) - { - bool dummy; - pid_controller_.getGains(p, i, d, i_max, i_min, dummy); - } - - // Controller startup in realtime - void RobotJointController::starting(const ros::Time &time) - { - double init_pos = joint.getPosition(); - lastCommand.q = init_pos; - lastState.q = init_pos; - lastCommand.dq = 0; - lastState.dq = 0; - lastCommand.tau = 0; - lastState.tauEst = 0; - command.initRT(lastCommand); - - pid_controller_.reset(); - } - - // Controller update loop in realtime - void RobotJointController::update(const ros::Time &time, const ros::Duration &period) - { - double currentPos, currentVel, calcTorque; - lastCommand = *(command.readFromRT()); - - // set command data - servoCommand.pos = lastCommand.q; - positionLimits(servoCommand.pos); - servoCommand.posStiffness = lastCommand.kp; - if (fabs(lastCommand.q - PosStopF) < 0.00001) - { - servoCommand.posStiffness = 0; - } - servoCommand.vel = lastCommand.dq; - velocityLimits(servoCommand.vel); - servoCommand.velStiffness = lastCommand.kd; - if (fabs(lastCommand.dq - VelStopF) < 0.00001) - { - servoCommand.velStiffness = 0; - } - servoCommand.torque = lastCommand.tau; - effortLimits(servoCommand.torque); - - // rqt set P D gains -#ifdef rqtTune - double i, i_max, i_min; - getGains(servoCommand.posStiffness, i, servoCommand.velStiffness, i_max, i_min); + last_command_ = *(joint_command); + UpdateFunc(period_seconds); + return controller_interface::return_type::OK; +} #endif - currentPos = joint.getPosition(); - // currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec()); - // calcTorque = computeTorque(currentPos, currentVel, servoCommand); - currentVel = (currentPos - (double)lastState.q) / period.toSec(); - calcTorque = servoCommand.posStiffness * (servoCommand.pos - currentPos) + servoCommand.velStiffness * (servoCommand.vel - currentVel) + servoCommand.torque; - effortLimits(calcTorque); +void RobotJointController::UpdateFunc(const double &period_seconds) +{ + double currentPos = 0.0; + double currentVel = 0.0; + double calcTorque = 0.0; - joint.setCommand(calcTorque); - - lastState.q = currentPos; - lastState.dq = currentVel; - // lastState.tauEst = calcTorque; - lastState.tauEst = joint.getEffort(); - - // publish state - if (controller_state_publisher_ && controller_state_publisher_->trylock()) - { - controller_state_publisher_->msg_.q = lastState.q; - controller_state_publisher_->msg_.dq = lastState.dq; - controller_state_publisher_->msg_.tauEst = lastState.tauEst; - controller_state_publisher_->unlockAndPublish(); - } - } - - // Controller stopping in realtime - void RobotJointController::stopping() {} - - void RobotJointController::positionLimits(double &position) + // set command data + servo_command_.pos = last_command_.q; + PositionLimit(servo_command_.pos); + servo_command_.pos_stiffness = last_command_.kp; + if (fabs(last_command_.q - PosStopF) < 0.00001) { - if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC) - clamp(position, joint_urdf->limits->lower, joint_urdf->limits->upper); + servo_command_.pos_stiffness = 0; } - - void RobotJointController::velocityLimits(double &velocity) + servo_command_.vel = last_command_.dq; + VelocityLimit(servo_command_.vel); + servo_command_.vel_stiffness = last_command_.kd; + if (fabs(last_command_.dq - VelStopF) < 0.00001) { - if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC) - clamp(velocity, -joint_urdf->limits->velocity, joint_urdf->limits->velocity); + servo_command_.vel_stiffness = 0; } + servo_command_.torque = last_command_.tau; + EffortLimit(servo_command_.torque); - void RobotJointController::effortLimits(double &effort) + currentPos = state_interfaces_[0].get_value(); + currentVel = (currentPos - (double)(last_state_.q)) / period_seconds; + calcTorque = servo_command_.pos_stiffness * (servo_command_.pos - currentPos) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel) + servo_command_.torque; + EffortLimit(calcTorque); + + command_interfaces_[0].set_value(calcTorque); + + last_state_.q = currentPos; + last_state_.dq = currentVel; + last_state_.tau_est = state_interfaces_[1].get_value(); + + // publish state + if (controller_state_publisher_ && controller_state_publisher_->trylock()) { - if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC) - clamp(effort, -joint_urdf->limits->effort, joint_urdf->limits->effort); + controller_state_publisher_->msg_ = last_state_; + controller_state_publisher_->unlockAndPublish(); } +} -} // namespace +void RobotJointController::SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg) +{ + last_command_ = *msg; + rt_command_ptr_.writeFromNonRT(last_command_); +} + +void RobotJointController::PositionLimit(double &position) +{ + std::clamp(position, joints_urdf_->limits->lower, joints_urdf_->limits->upper); +} + +void RobotJointController::VelocityLimit(double &velocity) +{ + std::clamp(velocity, -joints_urdf_->limits->velocity, joints_urdf_->limits->velocity); +} + +void RobotJointController::EffortLimit(double &effort) +{ + std::clamp(effort, -joints_urdf_->limits->effort, joints_urdf_->limits->effort); +} + +} // namespace robot_joint_controller // Register controller to pluginlib -PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerBase); +PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerInterface) diff --git a/src/robot_joint_controller/src/robot_joint_controller_group.cpp b/src/robot_joint_controller/src/robot_joint_controller_group.cpp new file mode 100644 index 0000000..7e2bdf3 --- /dev/null +++ b/src/robot_joint_controller/src/robot_joint_controller_group.cpp @@ -0,0 +1,278 @@ +#include "robot_joint_controller_group.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include + +namespace robot_joint_controller +{ +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::HW_IF_EFFORT; + +RobotJointControllerGroup::RobotJointControllerGroup() + : controller_interface::ControllerInterface(), + joints_command_subscriber_(nullptr), + controller_state_publisher_(nullptr) +{ + memset(&servo_command_, 0, sizeof(ServoCommand)); +} + +// CallbackReturn RobotJointControllerGroup::on_init() +// { +// return CallbackReturn::SUCCESS; +// } + +CallbackReturn RobotJointControllerGroup::on_configure(const rclcpp_lifecycle::State &previous_state) +{ + name_space_ = get_node()->get_namespace(); + + joint_names_ = get_node()->get_parameter("joints").as_string_array(); + + if (joint_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return CallbackReturn::ERROR; + } + else + { + for (const auto& joint_name : joint_names_) + { + RCLCPP_WARN(get_node()->get_logger(), "joint_name: %s", joint_name.c_str()); + } + } + + robot_description_client_ = get_node()->create_client("/robot_state_publisher/get_parameters"); + + auto request = std::make_shared(); + request->names.push_back("robot_description"); + + while (!robot_description_client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting."); + } + RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again..."); + } + + auto response_received_callback = [this](rclcpp::Client::SharedFuture future) + { + std::string robot_description = future.get()->values[0].string_value; + urdf::Model urdf; + if (!urdf.initString(robot_description)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file"); + } + + for (const auto& joint_name : joint_names_) + { + auto joint_urdf = urdf.getJoint(joint_name); + if (!joint_urdf) + { + RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name.c_str()); + } + if (joint_urdf) + { + joints_urdf_.push_back(joint_urdf); + } + } + }; + auto future_result = robot_description_client_->async_send_request(request, response_received_callback); + + // Start command subscriber + joints_command_subscriber_ = get_node()->create_subscription( + "~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointControllerGroup::SetCommandCallback, this, std::placeholders::_1)); + + // Start realtime state publisher + controller_state_publisher_ = std::make_shared>( + get_node()->create_publisher(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS())); + +#if defined(ROS_DISTRO_FOXY) + previous_update_timestamp_ = get_node()->get_clock()->now(); +#endif + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration RobotJointControllerGroup::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & joint_name : joint_names_) + { + command_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration RobotJointControllerGroup::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + for (const auto & joint_name : joint_names_) + { + state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_POSITION); + state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT); + } + + return state_interfaces_config; +} + +CallbackReturn RobotJointControllerGroup::on_activate(const rclcpp_lifecycle::State &previous_state) +{ + last_command_ = robot_msgs::msg::RobotCommand(); + last_command_.motor_command.resize(joint_names_.size()); + last_state_ = robot_msgs::msg::RobotState(); + last_state_.motor_state.resize(joint_names_.size()); + + for (int index = 0; index < joint_names_.size(); ++index) + { + double init_pos = state_interfaces_[index * 2].get_value(); + last_command_.motor_command[index].q = init_pos; + last_state_.motor_state[index].q = init_pos; + last_command_.motor_command[index].dq = 0; + last_state_.motor_state[index].dq = 0; + last_command_.motor_command[index].tau = 0; + last_state_.motor_state[index].tau_est = 0; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = realtime_tools::RealtimeBuffer(); + rt_command_ptr_.writeFromNonRT(last_command_); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn RobotJointControllerGroup::on_deactivate(const rclcpp_lifecycle::State &previous_state) +{ + last_command_ = robot_msgs::msg::RobotCommand(); + last_command_.motor_command.resize(joint_names_.size()); + last_state_ = robot_msgs::msg::RobotState(); + last_state_.motor_state.resize(joint_names_.size()); + + // reset command buffer + rt_command_ptr_ = realtime_tools::RealtimeBuffer(); + rt_command_ptr_.writeFromNonRT(last_command_); + return CallbackReturn::SUCCESS; +} + +#if defined(ROS_DISTRO_FOXY) +controller_interface::return_type RobotJointControllerGroup::update() +{ + const auto current_time = get_node()->get_clock()->now(); + const auto period_seconds = (current_time - previous_update_timestamp_).seconds(); + previous_update_timestamp_ = current_time; + auto joint_commands = rt_command_ptr_.readFromRT(); + // no command received yet + if (!joint_commands) + { + return controller_interface::return_type::OK; + } + if (joint_commands->motor_command.size() != joint_names_.size()) + { + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + joint_commands->motor_command.size(), joint_names_.size()); + return controller_interface::return_type::ERROR; + } + last_command_ = *(joint_commands); + UpdateFunc(period_seconds); + return controller_interface::return_type::OK; +} +#elif defined(ROS_DISTRO_HUMBLE) +controller_interface::return_type RobotJointControllerGroup::update(const rclcpp::Time &time, const rclcpp::Duration &period) +{ + const auto period_seconds = period.seconds(); + auto joint_commands = rt_command_ptr_.readFromRT(); + // no command received yet + if (!joint_commands) + { + return controller_interface::return_type::OK; + } + if (joint_commands->motor_command.size() != joint_names_.size()) + { + RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, + "command size (%zu) does not match number of interfaces (%zu)", + joint_commands->motor_command.size(), joint_names_.size()); + return controller_interface::return_type::ERROR; + } + last_command_ = *(joint_commands); + UpdateFunc(period_seconds); + return controller_interface::return_type::OK; +} +#endif + +void RobotJointControllerGroup::UpdateFunc(const double &period_seconds) +{ + std::vector currentPos(joint_names_.size(), 0.0); + std::vector currentVel(joint_names_.size(), 0.0); + std::vector calcTorque(joint_names_.size(), 0.0); + + for (int index = 0; index < joint_names_.size(); ++index) + { + // set command data + servo_command_.pos = last_command_.motor_command[index].q; + PositionLimit(servo_command_.pos, index); + servo_command_.pos_stiffness = last_command_.motor_command[index].kp; + if (fabs(last_command_.motor_command[index].q - PosStopF) < 0.00001) + { + servo_command_.pos_stiffness = 0; + } + servo_command_.vel = last_command_.motor_command[index].dq; + VelocityLimit(servo_command_.vel, index); + servo_command_.vel_stiffness = last_command_.motor_command[index].kd; + if (fabs(last_command_.motor_command[index].dq - VelStopF) < 0.00001) + { + servo_command_.vel_stiffness = 0; + } + servo_command_.torque = last_command_.motor_command[index].tau; + EffortLimit(servo_command_.torque, index); + + currentPos[index] = state_interfaces_[index * 2].get_value(); + currentVel[index] = (currentPos[index] - (double)(last_state_.motor_state[index].q)) / period_seconds; + calcTorque[index] = servo_command_.pos_stiffness * (servo_command_.pos - currentPos[index]) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel[index]) + servo_command_.torque; + EffortLimit(calcTorque[index], index); + + command_interfaces_[index].set_value(calcTorque[index]); + + last_state_.motor_state[index].q = currentPos[index]; + last_state_.motor_state[index].dq = currentVel[index]; + last_state_.motor_state[index].tau_est = state_interfaces_[index + 1].get_value(); + } + + // publish state + if (controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.motor_state = last_state_.motor_state; + controller_state_publisher_->unlockAndPublish(); + } +} + +void RobotJointControllerGroup::SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg) +{ + last_command_ = *msg; + rt_command_ptr_.writeFromNonRT(last_command_); +} + +void RobotJointControllerGroup::PositionLimit(double &position, int &index) +{ + std::clamp(position, joints_urdf_[index]->limits->lower, joints_urdf_[index]->limits->upper); +} + +void RobotJointControllerGroup::VelocityLimit(double &velocity, int &index) +{ + std::clamp(velocity, -joints_urdf_[index]->limits->velocity, joints_urdf_[index]->limits->velocity); +} + +void RobotJointControllerGroup::EffortLimit(double &effort, int &index) +{ + std::clamp(effort, -joints_urdf_[index]->limits->effort, joints_urdf_[index]->limits->effort); +} + +} // namespace robot_joint_controller + +// Register controller to pluginlib +PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointControllerGroup, controller_interface::ControllerInterface) diff --git a/src/robot_msgs/CMakeLists.txt b/src/robot_msgs/CMakeLists.txt index 108236f..e297cca 100644 --- a/src/robot_msgs/CMakeLists.txt +++ b/src/robot_msgs/CMakeLists.txt @@ -1,44 +1,17 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(robot_msgs) -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs - geometry_msgs - sensor_msgs +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MotorCommand.msg" + "msg/MotorState.msg" + "msg/RobotCommand.msg" + "msg/RobotState.msg" + "msg/IMU.msg" ) -add_message_files( - FILES - MotorCommand.msg - MotorState.msg - RobotCommand.msg - RobotState.msg - IMU.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs -) - -catkin_package( - CATKIN_DEPENDS - message_runtime - std_msgs - geometry_msgs - sensor_msgs -) - -############# -## Install ## -############# - -# Mark topic names header files for installation -install( - DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" -) \ No newline at end of file +ament_package() diff --git a/src/robot_msgs/msg/MotorState.msg b/src/robot_msgs/msg/MotorState.msg index fbc5477..a1cd87c 100644 --- a/src/robot_msgs/msg/MotorState.msg +++ b/src/robot_msgs/msg/MotorState.msg @@ -1,5 +1,5 @@ -float32 q # motor current position (rad) -float32 dq # motor current speed (rad/s) -float32 ddq # motor current speed (rad/s) -float32 tauEst # current estimated output torque (N*m) +float32 q # motor current pos (rad) +float32 dq # motor current vel (rad/s) +float32 ddq # motor current acc (rad/s^2) +float32 tau_est # current estimated output torque (N*m) float32 cur # current estimated output cur (N*m) \ No newline at end of file diff --git a/src/robot_msgs/msg/RobotCommand.msg b/src/robot_msgs/msg/RobotCommand.msg index f8f6bec..0515c9a 100644 --- a/src/robot_msgs/msg/RobotCommand.msg +++ b/src/robot_msgs/msg/RobotCommand.msg @@ -1 +1 @@ -MotorCommand[32] motor_command \ No newline at end of file +MotorCommand[] motor_command \ No newline at end of file diff --git a/src/robot_msgs/msg/RobotState.msg b/src/robot_msgs/msg/RobotState.msg index d11a054..60fd506 100644 --- a/src/robot_msgs/msg/RobotState.msg +++ b/src/robot_msgs/msg/RobotState.msg @@ -1,2 +1,2 @@ IMU imu -MotorState[32] motor_state \ No newline at end of file +MotorState[] motor_state \ No newline at end of file diff --git a/src/robot_msgs/package.xml b/src/robot_msgs/package.xml index ba89639..c8ee2c3 100644 --- a/src/robot_msgs/package.xml +++ b/src/robot_msgs/package.xml @@ -1,19 +1,19 @@ - - + + robot_msgs 0.0.0 - - The robot messgaes package. - + The robot messages package. Ziqi Fan - Apache - - catkin - message_runtime - message_generation - std_msgs - geometry_msgs - sensor_msgs - - \ No newline at end of file + Apache 2.0 + + ament_cmake + + + ament_cmake + + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + diff --git a/src/robots/a1_description/CMakeLists.txt b/src/robots/a1_description/CMakeLists.txt index 0415ceb..315164e 100644 --- a/src/robots/a1_description/CMakeLists.txt +++ b/src/robots/a1_description/CMakeLists.txt @@ -1,18 +1,22 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(a1_description) -find_package(catkin REQUIRED COMPONENTS - genmsg - roscpp - std_msgs - tf +find_package(ament_cmake REQUIRED) +find_package(ament_lint_auto REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) +find_package(robot_state_publisher REQUIRED) + +install( + DIRECTORY + config + launch + meshes + urdf + xacro + + DESTINATION + share/${PROJECT_NAME}/ ) -catkin_package( - CATKIN_DEPENDS -) - -include_directories( - # include - ${catkin_INCLUDE_DIRS} -) +ament_package() diff --git a/src/robots/a1_description/config/robot_control.yaml b/src/robots/a1_description/config/robot_control.yaml index 0e044d4..6675ec8 100644 --- a/src/robots/a1_description/config/robot_control.yaml +++ b/src/robots/a1_description/config/robot_control.yaml @@ -1,70 +1,113 @@ -a1_gazebo: - # Publish all joint states ----------------------------------- - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 1000 +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + # use_sim_time: true # If running in simulation - # FL Controllers --------------------------------------- - FL_hip_controller: - type: robot_joint_controller/RobotJointController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/ImuSensorBroadcaster + + # FL Controllers --------------------------------------- + FL_hip_controller: + type: robot_joint_controller/RobotJointController + FL_thigh_controller: + type: robot_joint_controller/RobotJointController + FL_calf_controller: + type: robot_joint_controller/RobotJointController + + # FR Controllers --------------------------------------- + FR_hip_controller: + type: robot_joint_controller/RobotJointController + FR_thigh_controller: + type: robot_joint_controller/RobotJointController + FR_calf_controller: + type: robot_joint_controller/RobotJointController + + # RL Controllers --------------------------------------- + RL_hip_controller: + type: robot_joint_controller/RobotJointController + RL_thigh_controller: + type: robot_joint_controller/RobotJointController + RL_calf_controller: + type: robot_joint_controller/RobotJointController + + # RR Controllers --------------------------------------- + RR_hip_controller: + type: robot_joint_controller/RobotJointController + RR_thigh_controller: + type: robot_joint_controller/RobotJointController + RR_calf_controller: + type: robot_joint_controller/RobotJointController + + +# FL Controllers --------------------------------------- +FL_hip_controller: + ros__parameters: joint: FL_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - FL_thigh_controller: - type: robot_joint_controller/RobotJointController +FL_thigh_controller: + ros__parameters: joint: FL_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - FL_calf_controller: - type: robot_joint_controller/RobotJointController +FL_calf_controller: + ros__parameters: joint: FL_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # FR Controllers --------------------------------------- - FR_hip_controller: - type: robot_joint_controller/RobotJointController +# FR Controllers --------------------------------------- +FR_hip_controller: + ros__parameters: joint: FR_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - FR_thigh_controller: - type: robot_joint_controller/RobotJointController +FR_thigh_controller: + ros__parameters: joint: FR_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - FR_calf_controller: - type: robot_joint_controller/RobotJointController +FR_calf_controller: + ros__parameters: joint: FR_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # RL Controllers --------------------------------------- - RL_hip_controller: - type: robot_joint_controller/RobotJointController +# RL Controllers --------------------------------------- +RL_hip_controller: + ros__parameters: joint: RL_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - RL_thigh_controller: - type: robot_joint_controller/RobotJointController +RL_thigh_controller: + ros__parameters: joint: RL_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - RL_calf_controller: - type: robot_joint_controller/RobotJointController +RL_calf_controller: + ros__parameters: joint: RL_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # RR Controllers --------------------------------------- - RR_hip_controller: - type: robot_joint_controller/RobotJointController +# RR Controllers --------------------------------------- +RR_hip_controller: + ros__parameters: joint: RR_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - RR_thigh_controller: - type: robot_joint_controller/RobotJointController +RR_thigh_controller: + ros__parameters: joint: RR_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - RR_calf_controller: - type: robot_joint_controller/RobotJointController +RR_calf_controller: + ros__parameters: joint: RR_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} +imu_sensor_broadcaster: + ros__parameters: + sensor_name: "imu_sensor" + frame_id: imu_link + diff --git a/src/robots/a1_description/config/robot_control_group.yaml b/src/robots/a1_description/config/robot_control_group.yaml new file mode 100644 index 0000000..84056f3 --- /dev/null +++ b/src/robots/a1_description/config/robot_control_group.yaml @@ -0,0 +1,34 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + # use_sim_time: true # If running in simulation + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/ImuSensorBroadcaster + + robot_joint_controller: + type: robot_joint_controller/RobotJointControllerGroup + +robot_joint_controller: + ros__parameters: + joints: + - FL_hip_joint + - FL_thigh_joint + - FL_calf_joint + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: "imu_sensor" + frame_id: imu_link \ No newline at end of file diff --git a/src/robots/a1_description/launch/a1_rviz.launch b/src/robots/a1_description/launch/a1_rviz.launch deleted file mode 100644 index 6d74b9b..0000000 --- a/src/robots/a1_description/launch/a1_rviz.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/robots/a1_description/launch/a1_rviz.launch.py b/src/robots/a1_description/launch/a1_rviz.launch.py new file mode 100644 index 0000000..db2f9ba --- /dev/null +++ b/src/robots/a1_description/launch/a1_rviz.launch.py @@ -0,0 +1,83 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.conditions import IfCondition + +import xacro + + +def generate_launch_description(): + + ####### DATA INPUT ########## + # urdf_file = 'moonbotX.urdf' + package_description = "a1_description" + + use_sim_time = LaunchConfiguration('use_sim_time') + + use_joint_state_publisher = LaunchConfiguration('use_joint_state_publisher') + + # Process the URDF file + pkg_path = os.path.join(get_package_share_directory(package_description)) + xacro_file = os.path.join(pkg_path,'xacro','robot.xacro') + robot_description_config = xacro.process_file(xacro_file) + + jsp_arg = DeclareLaunchArgument( + 'use_joint_state_publisher', + default_value='True' + ) + + # Create a robot_state_publisher node + params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time} + + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher_node', + emulate_tty=True, + output='screen', + parameters=[params] + ) + + # Joint State Publisher + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + output='screen', + condition=IfCondition(use_joint_state_publisher) + ) + + # RVIZ Configuration + rviz_config_dir = os.path.join( + get_package_share_directory(package_description), + 'launch', + 'check_joint.rviz' + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + output='screen', + name='rviz_node', + parameters=[{'use_sim_time': True}], + arguments=['-d', rviz_config_dir] + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use sim time if true'), + + jsp_arg, + + # map_publisher_node, + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node + + ]) \ No newline at end of file diff --git a/src/robots/a1_description/launch/check_joint.rviz b/src/robots/a1_description/launch/check_joint.rviz index f0f5209..7545fb4 100644 --- a/src/robots/a1_description/launch/check_joint.rviz +++ b/src/robots/a1_description/launch/check_joint.rviz @@ -1,38 +1,33 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /Status1 - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 420 - - Class: rviz/Selection + Tree Height: 617 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -49,8 +44,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -174,92 +177,86 @@ Visualization Manager: Show Trail: false Value: true Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/Axes - Enabled: false - Length: 1 - Name: Axes - Radius: 0.10000000149011612 - Reference Frame: - Value: false - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false Enabled: true Global Options: - Background Color: 238; 238; 238 - Default Light: true + Background Color: 48; 48; 48 Fixed Frame: base Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 1.0307118892669678 + Class: rviz_default_plugins/Orbit + Distance: 1.9769524335861206 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0760490670800209 - Y: 0.11421932280063629 - Z: -0.1576911211013794 - Focal Shape Fixed Size: false + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0047969818115234 + Pitch: 0.21039816737174988 Target Frame: Value: Orbit (rviz) - Yaw: 4.558584690093994 + Yaw: 0.9653980731964111 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 627 + Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false - Time: - collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1108 - X: 812 - Y: 241 + Width: 1200 + X: 617 + Y: 102 diff --git a/src/robots/a1_description/launch/group_gazebo.launch.py b/src/robots/a1_description/launch/group_gazebo.launch.py new file mode 100644 index 0000000..d77031b --- /dev/null +++ b/src/robots/a1_description/launch/group_gazebo.launch.py @@ -0,0 +1,59 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch.event_handlers import OnProcessExit +import xacro + +def generate_launch_description(): + package_name = 'a1_description' + package_path = os.path.join(get_package_share_directory(package_name)) + + xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file).toxml() + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py') + ), + launch_arguments={'verbose': 'true'}.items() + ) + + spawn_entity = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'], + output='screen', + ) + + joint_state_broadcaster_node = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster"], + output='screen', + ) + + robot_joint_controller_node = Node( + package="controller_manager", + executable="spawner.py", + arguments=["robot_joint_controller"], + output='screen', + ) + + return LaunchDescription([ + robot_state_publisher_node, + gazebo, + spawn_entity, + joint_state_broadcaster_node, + robot_joint_controller_node, + ]) diff --git a/src/robots/a1_description/launch/single_gazebo.launch.py b/src/robots/a1_description/launch/single_gazebo.launch.py new file mode 100644 index 0000000..ff847f7 --- /dev/null +++ b/src/robots/a1_description/launch/single_gazebo.launch.py @@ -0,0 +1,59 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch.event_handlers import OnProcessExit +import xacro + +def generate_launch_description(): + package_name = 'a1_description' + package_path = os.path.join(get_package_share_directory(package_name)) + + xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file).toxml() + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name='robot_state_publisher', + output="screen", + parameters=[{"robot_description": robot_description}], + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py') + ), + launch_arguments={'verbose': 'true'}.items() + ) + + spawn_entity = Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'], + output='screen', + ) + + controller_names = [ + "joint_state_broadcaster", + "FL_hip_controller", "FL_thigh_controller", "FL_calf_controller", + "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller", + "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller", + "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller" + ] + + controller_nodes = [Node( + package="controller_manager", + executable="spawner.py", + arguments=[name], + output='screen', + ) for name in controller_names] + + return LaunchDescription([ + robot_state_publisher_node, + gazebo, + spawn_entity, + *controller_nodes, + ]) diff --git a/src/robots/a1_description/package.xml b/src/robots/a1_description/package.xml index 6796895..57ccc47 100644 --- a/src/robots/a1_description/package.xml +++ b/src/robots/a1_description/package.xml @@ -1,14 +1,22 @@ - - a1_description - 0.0.0 - The a1_description package + + a1_description + 2.0.0 + TODO: Package description + Ziqi Fan + Apache-2.0 - unitree - TODO + ament_cmake - catkin - roscpp - std_msgs + urdf + xacro + robot_state_publisher + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robots/a1_description/urdf/a1.urdf b/src/robots/a1_description/urdf/a1.urdf index e133ab4..376481b 100644 --- a/src/robots/a1_description/urdf/a1.urdf +++ b/src/robots/a1_description/urdf/a1.urdf @@ -44,12 +44,20 @@ - + + + + gazebo_ros2_control/GazeboSystem + robot_description + robot_state_publisher_node + $(find a1_description)/config/robot_control.yaml + + diff --git a/src/robots/a1_description/xacro/gazebo.xacro b/src/robots/a1_description/xacro/gazebo.xacro index 7248a42..e03ed0b 100644 --- a/src/robots/a1_description/xacro/gazebo.xacro +++ b/src/robots/a1_description/xacro/gazebo.xacro @@ -1,15 +1,7 @@ - - - - /a1_gazebo - gazebo_ros_control/DefaultRobotHWSim - - - - + - + - true - - true - 1000 - true - __default_topic__ - - trunk_imu - imu_link - 1000.0 - 0.0 - 0 0 0 - 0 0 0 - imu_link - - 0 0 0 0 0 0 - + + + + / + ~/out:=imu + + false + + true + 100 + true + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + diff --git a/src/robots/a1_description/xacro/robot.xacro b/src/robots/a1_description/xacro/robot.xacro old mode 100644 new mode 100755 index d106404..ce1200d --- a/src/robots/a1_description/xacro/robot.xacro +++ b/src/robots/a1_description/xacro/robot.xacro @@ -9,6 +9,7 @@ + @@ -24,7 +25,7 @@ - + @@ -61,7 +62,7 @@ + izz="${trunk_izz}"/> @@ -91,10 +92,10 @@ + izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/> - + diff --git a/src/robots/a1_description/xacro/ros2_control.xacro b/src/robots/a1_description/xacro/ros2_control.xacro new file mode 100644 index 0000000..fef4141 --- /dev/null +++ b/src/robots/a1_description/xacro/ros2_control.xacro @@ -0,0 +1,137 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + + + $(find a1_description)/config/robot_control_group.yaml + + + + \ No newline at end of file diff --git a/src/robots/go2_description/CMakeLists.txt b/src/robots/go2_description/CMakeLists.txt index a0cad9a..2bdafb9 100644 --- a/src/robots/go2_description/CMakeLists.txt +++ b/src/robots/go2_description/CMakeLists.txt @@ -1,14 +1,22 @@ -cmake_minimum_required(VERSION 2.8.3) - +cmake_minimum_required(VERSION 3.8) project(go2_description) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(ament_lint_auto REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) +find_package(robot_state_publisher REQUIRED) -catkin_package() +install( + DIRECTORY + config + launch + meshes + urdf + xacro -find_package(roslaunch) + DESTINATION + share/${PROJECT_NAME}/ +) -foreach(dir config launch meshes urdf) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) +ament_package() diff --git a/src/robots/go2_description/config/robot_control.yaml b/src/robots/go2_description/config/robot_control.yaml old mode 100755 new mode 100644 index 2991707..6675ec8 --- a/src/robots/go2_description/config/robot_control.yaml +++ b/src/robots/go2_description/config/robot_control.yaml @@ -1,70 +1,113 @@ -go2_gazebo: - # Publish all joint states ----------------------------------- - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 1000 +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + # use_sim_time: true # If running in simulation - # FL Controllers --------------------------------------- - FL_hip_controller: - type: robot_joint_controller/RobotJointController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/ImuSensorBroadcaster + + # FL Controllers --------------------------------------- + FL_hip_controller: + type: robot_joint_controller/RobotJointController + FL_thigh_controller: + type: robot_joint_controller/RobotJointController + FL_calf_controller: + type: robot_joint_controller/RobotJointController + + # FR Controllers --------------------------------------- + FR_hip_controller: + type: robot_joint_controller/RobotJointController + FR_thigh_controller: + type: robot_joint_controller/RobotJointController + FR_calf_controller: + type: robot_joint_controller/RobotJointController + + # RL Controllers --------------------------------------- + RL_hip_controller: + type: robot_joint_controller/RobotJointController + RL_thigh_controller: + type: robot_joint_controller/RobotJointController + RL_calf_controller: + type: robot_joint_controller/RobotJointController + + # RR Controllers --------------------------------------- + RR_hip_controller: + type: robot_joint_controller/RobotJointController + RR_thigh_controller: + type: robot_joint_controller/RobotJointController + RR_calf_controller: + type: robot_joint_controller/RobotJointController + + +# FL Controllers --------------------------------------- +FL_hip_controller: + ros__parameters: joint: FL_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - FL_thigh_controller: - type: robot_joint_controller/RobotJointController +FL_thigh_controller: + ros__parameters: joint: FL_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - FL_calf_controller: - type: robot_joint_controller/RobotJointController +FL_calf_controller: + ros__parameters: joint: FL_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # FR Controllers --------------------------------------- - FR_hip_controller: - type: robot_joint_controller/RobotJointController +# FR Controllers --------------------------------------- +FR_hip_controller: + ros__parameters: joint: FR_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - FR_thigh_controller: - type: robot_joint_controller/RobotJointController +FR_thigh_controller: + ros__parameters: joint: FR_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - FR_calf_controller: - type: robot_joint_controller/RobotJointController +FR_calf_controller: + ros__parameters: joint: FR_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # RL Controllers --------------------------------------- - RL_hip_controller: - type: robot_joint_controller/RobotJointController +# RL Controllers --------------------------------------- +RL_hip_controller: + ros__parameters: joint: RL_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - RL_thigh_controller: - type: robot_joint_controller/RobotJointController +RL_thigh_controller: + ros__parameters: joint: RL_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - RL_calf_controller: - type: robot_joint_controller/RobotJointController +RL_calf_controller: + ros__parameters: joint: RL_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} - # RR Controllers --------------------------------------- - RR_hip_controller: - type: robot_joint_controller/RobotJointController +# RR Controllers --------------------------------------- +RR_hip_controller: + ros__parameters: joint: RR_hip_joint pid: {p: 100.0, i: 0.0, d: 5.0} - RR_thigh_controller: - type: robot_joint_controller/RobotJointController +RR_thigh_controller: + ros__parameters: joint: RR_thigh_joint pid: {p: 300.0, i: 0.0, d: 8.0} - RR_calf_controller: - type: robot_joint_controller/RobotJointController +RR_calf_controller: + ros__parameters: joint: RR_calf_joint pid: {p: 300.0, i: 0.0, d: 8.0} +imu_sensor_broadcaster: + ros__parameters: + sensor_name: "imu_sensor" + frame_id: imu_link + diff --git a/src/robots/go2_description/package.xml b/src/robots/go2_description/package.xml index ba52e13..cab7ce4 100644 --- a/src/robots/go2_description/package.xml +++ b/src/robots/go2_description/package.xml @@ -1,21 +1,22 @@ - + + go2_description - 1.0.0 - -

URDF Description package for go2_description

-

This package contains configuration data, 3D models and launch files -for go2_description robot

- - TODO - - BSD - catkin - roslaunch + 2.0.0 + TODO: Package description + Ziqi Fan + Apache-2.0 + + ament_cmake + + urdf + xacro robot_state_publisher - rviz - joint_state_publisher_gui - gazebo + + ament_lint_auto + ament_lint_common + - + ament_cmake + - \ No newline at end of file + diff --git a/src/robots/go2_description/xacro/gazebo.xacro b/src/robots/go2_description/xacro/gazebo.xacro index f449f44..bf9d63a 100644 --- a/src/robots/go2_description/xacro/gazebo.xacro +++ b/src/robots/go2_description/xacro/gazebo.xacro @@ -1,13 +1,5 @@ - - - - /go2_gazebo - gazebo_ros_control/DefaultRobotHWSim - - - - + - + - true - - true - 1000 - true - __default_topic__ - - trunk_imu - imu_link - 1000.0 - 0.0 - 0 0 0 - 0 0 0 - imu_link - - 0 0 0 0 0 0 - + + + + / + ~/out:=imu + + false + + true + 100 + true + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + - - - - - - - + - + false diff --git a/src/robots/go2_description/xacro/robot.xacro b/src/robots/go2_description/xacro/robot.xacro index c03da97..a897dac 100755 --- a/src/robots/go2_description/xacro/robot.xacro +++ b/src/robots/go2_description/xacro/robot.xacro @@ -9,6 +9,7 @@ + @@ -24,7 +25,7 @@ - + @@ -61,7 +62,7 @@ + izz="${trunk_izz}"/> diff --git a/src/robots/go2_description/xacro/ros2_control.xacro b/src/robots/go2_description/xacro/ros2_control.xacro new file mode 100644 index 0000000..21232aa --- /dev/null +++ b/src/robots/go2_description/xacro/ros2_control.xacro @@ -0,0 +1,137 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${hip_torque_max} + ${hip_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${thigh_torque_max} + ${thigh_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + -${calf_torque_max} + ${calf_torque_max} + + + + + + + + + + + $(find go2_description)/config/robot_control_group.yaml + + + + \ No newline at end of file diff --git a/src/robots/gr1t1_description/CMakeLists.txt b/src/robots/gr1t1_description/CMakeLists.txt deleted file mode 100644 index d2927b2..0000000 --- a/src/robots/gr1t1_description/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.0) - -project(gr1t1_description) - -find_package(catkin REQUIRED) - -catkin_package() - -find_package(roslaunch) - -foreach(dir config launch meshes urdf) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) diff --git a/src/robots/gr1t1_description/config/robot_control.yaml b/src/robots/gr1t1_description/config/robot_control.yaml deleted file mode 100644 index d622661..0000000 --- a/src/robots/gr1t1_description/config/robot_control.yaml +++ /dev/null @@ -1,57 +0,0 @@ -gr1t1_gazebo: - # Publish all joint states ----------------------------------- - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 1000 - - # left leg Controllers --------------------------------------- - l_hip_roll_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_roll - pid: {p: 57.0, i: 0.0, d: 5.7} - - l_hip_yaw_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_yaw - pid: {p: 43.0, i: 0.0, d: 4.3} - - l_hip_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - l_knee_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_knee_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - l_ankle_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_ankle_pitch - pid: {p: 15.3, i: 0.0, d: 1.5} - - # right leg Controllers --------------------------------------- - r_hip_roll_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_roll - pid: {p: 57.0, i: 0.0, d: 5.7} - - r_hip_yaw_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_yaw - pid: {p: 43.0, i: 0.0, d: 4.3} - - r_hip_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - r_knee_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_knee_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - r_ankle_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_ankle_pitch - pid: {p: 15.3, i: 0.0, d: 1.5} diff --git a/src/robots/gr1t1_description/launch/display.launch b/src/robots/gr1t1_description/launch/display.launch deleted file mode 100644 index d4cc4fb..0000000 --- a/src/robots/gr1t1_description/launch/display.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t1_description/launch/gazebo.launch b/src/robots/gr1t1_description/launch/gazebo.launch deleted file mode 100644 index ba67d26..0000000 --- a/src/robots/gr1t1_description/launch/gazebo.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t1_description/meshes/base.STL b/src/robots/gr1t1_description/meshes/base.STL deleted file mode 100644 index fd2f20a..0000000 Binary files a/src/robots/gr1t1_description/meshes/base.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/head_pitch.STL b/src/robots/gr1t1_description/meshes/head_pitch.STL deleted file mode 100644 index c17c1a4..0000000 Binary files a/src/robots/gr1t1_description/meshes/head_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/head_roll.STL b/src/robots/gr1t1_description/meshes/head_roll.STL deleted file mode 100644 index b4ff40b..0000000 Binary files a/src/robots/gr1t1_description/meshes/head_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/head_yaw.STL b/src/robots/gr1t1_description/meshes/head_yaw.STL deleted file mode 100644 index 2b152b0..0000000 Binary files a/src/robots/gr1t1_description/meshes/head_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_foot_pitch.STL b/src/robots/gr1t1_description/meshes/l_foot_pitch.STL deleted file mode 100644 index b8d4b4f..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_foot_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_foot_roll.STL b/src/robots/gr1t1_description/meshes/l_foot_roll.STL deleted file mode 100644 index 89ae40c..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_foot_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_hand_pitch.STL b/src/robots/gr1t1_description/meshes/l_hand_pitch.STL deleted file mode 100644 index 40f8d33..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_hand_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_hand_roll.STL b/src/robots/gr1t1_description/meshes/l_hand_roll.STL deleted file mode 100644 index af3f154..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_hand_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_hand_yaw.STL b/src/robots/gr1t1_description/meshes/l_hand_yaw.STL deleted file mode 100644 index 279f5e6..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_hand_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL b/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL deleted file mode 100644 index 9422de6..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_lower_arm_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_shank_pitch.STL b/src/robots/gr1t1_description/meshes/l_shank_pitch.STL deleted file mode 100644 index 301152b..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_shank_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL b/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL deleted file mode 100644 index 0e8d690..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_thigh_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_thigh_roll.STL b/src/robots/gr1t1_description/meshes/l_thigh_roll.STL deleted file mode 100644 index a4d7654..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_thigh_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL b/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL deleted file mode 100644 index f4ea275..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_thigh_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL deleted file mode 100644 index c634251..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_upper_arm_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL deleted file mode 100644 index 0e92ff2..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_upper_arm_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL b/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL deleted file mode 100644 index 7af2b75..0000000 Binary files a/src/robots/gr1t1_description/meshes/l_upper_arm_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_foot_pitch.STL b/src/robots/gr1t1_description/meshes/r_foot_pitch.STL deleted file mode 100644 index 2a053cd..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_foot_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_foot_roll.STL b/src/robots/gr1t1_description/meshes/r_foot_roll.STL deleted file mode 100644 index 3fd1295..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_foot_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_hand_pitch.STL b/src/robots/gr1t1_description/meshes/r_hand_pitch.STL deleted file mode 100644 index 9b7399b..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_hand_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_hand_roll.STL b/src/robots/gr1t1_description/meshes/r_hand_roll.STL deleted file mode 100644 index 9535cb7..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_hand_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_hand_yaw.STL b/src/robots/gr1t1_description/meshes/r_hand_yaw.STL deleted file mode 100644 index dd2de0f..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_hand_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL b/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL deleted file mode 100644 index 7f95ee5..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_lower_arm_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_shank_pitch.STL b/src/robots/gr1t1_description/meshes/r_shank_pitch.STL deleted file mode 100644 index 9cb7e59..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_shank_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL b/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL deleted file mode 100644 index 04712d7..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_thigh_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_thigh_roll.STL b/src/robots/gr1t1_description/meshes/r_thigh_roll.STL deleted file mode 100644 index 365a4a6..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_thigh_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL b/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL deleted file mode 100644 index 49a68fc..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_thigh_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL deleted file mode 100644 index aff0133..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_upper_arm_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL deleted file mode 100644 index 661e9eb..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_upper_arm_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL b/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL deleted file mode 100644 index 58d2471..0000000 Binary files a/src/robots/gr1t1_description/meshes/r_upper_arm_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/waist_pitch.STL b/src/robots/gr1t1_description/meshes/waist_pitch.STL deleted file mode 100644 index d759e9c..0000000 Binary files a/src/robots/gr1t1_description/meshes/waist_pitch.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/waist_roll.STL b/src/robots/gr1t1_description/meshes/waist_roll.STL deleted file mode 100644 index b06c0cb..0000000 Binary files a/src/robots/gr1t1_description/meshes/waist_roll.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/meshes/waist_yaw.STL b/src/robots/gr1t1_description/meshes/waist_yaw.STL deleted file mode 100644 index d6b96e5..0000000 Binary files a/src/robots/gr1t1_description/meshes/waist_yaw.STL and /dev/null differ diff --git a/src/robots/gr1t1_description/package.xml b/src/robots/gr1t1_description/package.xml deleted file mode 100644 index 889a69d..0000000 --- a/src/robots/gr1t1_description/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - gr1t1_description - 1.0.0 - -

URDF Description package for gr1t1_description

-

This package contains configuration data, 3D models and launch files for fldlar_description robot

- - Ziqi Fan - - Apache - catkin - roslaunch - robot_state_publisher - rviz - joint_state_publisher_gui - gazebo - - - - \ No newline at end of file diff --git a/src/robots/gr1t1_description/urdf.rviz b/src/robots/gr1t1_description/urdf.rviz deleted file mode 100644 index d469e8f..0000000 --- a/src/robots/gr1t1_description/urdf.rviz +++ /dev/null @@ -1,225 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 549 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - 10: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 11: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 12: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 13: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 20: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 21: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 22: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 23: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 30: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 31: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 32: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 33: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 40: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 41: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 42: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 43: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.634476661682129 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.0988716408610344 - Y: 0.08353396505117416 - Z: -0.19447802007198334 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5753985047340393 - Target Frame: - Yaw: 0.47539806365966797 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 278 - Y: 96 diff --git a/src/robots/gr1t1_description/urdf/GR1T1.urdf b/src/robots/gr1t1_description/urdf/GR1T1.urdf deleted file mode 100644 index a3faf8a..0000000 --- a/src/robots/gr1t1_description/urdf/GR1T1.urdf +++ /dev/null @@ -1,2023 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf b/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf deleted file mode 100644 index c755c3f..0000000 --- a/src/robots/gr1t1_description/urdf/GR1T1_lower_limb.urdf +++ /dev/null @@ -1,1400 +0,0 @@ - - - - - - /gr1t1_gazebo - gazebo_ros_control/DefaultRobotHWSim - - - - - - - - - base_link - /apply_force/base_link - - - - true - - true - 1000 - true - __default_topic__ - - base_link_imu - imu_link - 1000.0 - 0.0 - 0 0 0 - 0 0 0 - imu_link - - 0 0 0 0 0 0 - - - - - Gazebo/DarkGrey - false - - - 0.2 - 0.2 - - - - - 0.2 - 0.2 - Gazebo/Red - - - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - \ No newline at end of file diff --git a/src/robots/gr1t2_description/CMakeLists.txt b/src/robots/gr1t2_description/CMakeLists.txt deleted file mode 100644 index 90720a1..0000000 --- a/src/robots/gr1t2_description/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.0) - -project(gr1t2_description) - -find_package(catkin REQUIRED) - -catkin_package() - -find_package(roslaunch) - -foreach(dir config launch meshes urdf) - install(DIRECTORY ${dir}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) -endforeach(dir) diff --git a/src/robots/gr1t2_description/config/robot_control.yaml b/src/robots/gr1t2_description/config/robot_control.yaml deleted file mode 100644 index c7c78c1..0000000 --- a/src/robots/gr1t2_description/config/robot_control.yaml +++ /dev/null @@ -1,57 +0,0 @@ -gr1t2_gazebo: - # Publish all joint states ----------------------------------- - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 1000 - - # left leg Controllers --------------------------------------- - l_hip_roll_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_roll - pid: {p: 57.0, i: 0.0, d: 5.7} - - l_hip_yaw_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_yaw - pid: {p: 43.0, i: 0.0, d: 4.3} - - l_hip_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_hip_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - l_knee_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_knee_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - l_ankle_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: l_ankle_pitch - pid: {p: 15.3, i: 0.0, d: 1.5} - - # right leg Controllers --------------------------------------- - r_hip_roll_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_roll - pid: {p: 57.0, i: 0.0, d: 5.7} - - r_hip_yaw_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_yaw - pid: {p: 43.0, i: 0.0, d: 4.3} - - r_hip_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_hip_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - r_knee_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_knee_pitch - pid: {p: 114.0, i: 0.0, d: 11.4} - - r_ankle_pitch_controller: - type: robot_joint_controller/RobotJointController - joint: r_ankle_pitch - pid: {p: 15.3, i: 0.0, d: 1.5} diff --git a/src/robots/gr1t2_description/launch/display.launch b/src/robots/gr1t2_description/launch/display.launch deleted file mode 100644 index ab751d1..0000000 --- a/src/robots/gr1t2_description/launch/display.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t2_description/launch/gazebo.launch b/src/robots/gr1t2_description/launch/gazebo.launch deleted file mode 100644 index 0be4f24..0000000 --- a/src/robots/gr1t2_description/launch/gazebo.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t2_description/meshes/Larm1.STL b/src/robots/gr1t2_description/meshes/Larm1.STL deleted file mode 100644 index 1d9013a..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm2.STL b/src/robots/gr1t2_description/meshes/Larm2.STL deleted file mode 100644 index 8a72f1e..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm3.STL b/src/robots/gr1t2_description/meshes/Larm3.STL deleted file mode 100644 index a2b46b6..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm4.STL b/src/robots/gr1t2_description/meshes/Larm4.STL deleted file mode 100644 index 6b11447..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm4.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm5.STL b/src/robots/gr1t2_description/meshes/Larm5.STL deleted file mode 100644 index 99e2b29..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm5.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm6.STL b/src/robots/gr1t2_description/meshes/Larm6.STL deleted file mode 100644 index 48e0024..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm6.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Larm7.STL b/src/robots/gr1t2_description/meshes/Larm7.STL deleted file mode 100644 index 1741d51..0000000 Binary files a/src/robots/gr1t2_description/meshes/Larm7.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg1.STL b/src/robots/gr1t2_description/meshes/Lleg1.STL deleted file mode 100644 index 459588f..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg2.STL b/src/robots/gr1t2_description/meshes/Lleg2.STL deleted file mode 100644 index 7b7ddaa..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg3.STL b/src/robots/gr1t2_description/meshes/Lleg3.STL deleted file mode 100644 index 9bfa835..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg4.STL b/src/robots/gr1t2_description/meshes/Lleg4.STL deleted file mode 100644 index 3cc16d8..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg4.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg5.STL b/src/robots/gr1t2_description/meshes/Lleg5.STL deleted file mode 100644 index a1b3c5c..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg5.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Lleg6.STL b/src/robots/gr1t2_description/meshes/Lleg6.STL deleted file mode 100644 index af23bf6..0000000 Binary files a/src/robots/gr1t2_description/meshes/Lleg6.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm1.STL b/src/robots/gr1t2_description/meshes/Rarm1.STL deleted file mode 100644 index 17fd984..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm2.STL b/src/robots/gr1t2_description/meshes/Rarm2.STL deleted file mode 100644 index bfa00e4..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm3.STL b/src/robots/gr1t2_description/meshes/Rarm3.STL deleted file mode 100644 index ff78944..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm4.STL b/src/robots/gr1t2_description/meshes/Rarm4.STL deleted file mode 100644 index 26afec1..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm4.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm5.STL b/src/robots/gr1t2_description/meshes/Rarm5.STL deleted file mode 100644 index 4ff1d7c..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm5.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm6.STL b/src/robots/gr1t2_description/meshes/Rarm6.STL deleted file mode 100644 index 18d5905..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm6.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rarm7.STL b/src/robots/gr1t2_description/meshes/Rarm7.STL deleted file mode 100644 index ed77cf7..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rarm7.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg1.STL b/src/robots/gr1t2_description/meshes/Rleg1.STL deleted file mode 100644 index 047945b..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg2.STL b/src/robots/gr1t2_description/meshes/Rleg2.STL deleted file mode 100644 index 7402304..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg3.STL b/src/robots/gr1t2_description/meshes/Rleg3.STL deleted file mode 100644 index de39821..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg4.STL b/src/robots/gr1t2_description/meshes/Rleg4.STL deleted file mode 100644 index c3a85e6..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg4.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg5.STL b/src/robots/gr1t2_description/meshes/Rleg5.STL deleted file mode 100644 index 0c0b594..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg5.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/Rleg6.STL b/src/robots/gr1t2_description/meshes/Rleg6.STL deleted file mode 100644 index d933729..0000000 Binary files a/src/robots/gr1t2_description/meshes/Rleg6.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/base.STL b/src/robots/gr1t2_description/meshes/base.STL deleted file mode 100644 index 3ca22e9..0000000 Binary files a/src/robots/gr1t2_description/meshes/base.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/head1.STL b/src/robots/gr1t2_description/meshes/head1.STL deleted file mode 100644 index a68f0fb..0000000 Binary files a/src/robots/gr1t2_description/meshes/head1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/head2.STL b/src/robots/gr1t2_description/meshes/head2.STL deleted file mode 100644 index b03e84c..0000000 Binary files a/src/robots/gr1t2_description/meshes/head2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/head3.STL b/src/robots/gr1t2_description/meshes/head3.STL deleted file mode 100644 index 8fade2f..0000000 Binary files a/src/robots/gr1t2_description/meshes/head3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/torso.STL b/src/robots/gr1t2_description/meshes/torso.STL deleted file mode 100644 index 992a5c9..0000000 Binary files a/src/robots/gr1t2_description/meshes/torso.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/waist1.STL b/src/robots/gr1t2_description/meshes/waist1.STL deleted file mode 100644 index 7ed86c3..0000000 Binary files a/src/robots/gr1t2_description/meshes/waist1.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/waist2.STL b/src/robots/gr1t2_description/meshes/waist2.STL deleted file mode 100644 index 47bc3df..0000000 Binary files a/src/robots/gr1t2_description/meshes/waist2.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/meshes/waist3.STL b/src/robots/gr1t2_description/meshes/waist3.STL deleted file mode 100644 index 598efa0..0000000 Binary files a/src/robots/gr1t2_description/meshes/waist3.STL and /dev/null differ diff --git a/src/robots/gr1t2_description/package.xml b/src/robots/gr1t2_description/package.xml deleted file mode 100644 index 685ae1a..0000000 --- a/src/robots/gr1t2_description/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - gr1t2_description - 1.0.0 - -

URDF Description package for gr1t2_description

-

This package contains configuration data, 3D models and launch files for fldlar_description robot

- - Ziqi Fan - - Apache - catkin - roslaunch - robot_state_publisher - rviz - joint_state_publisher_gui - gazebo - - - - \ No newline at end of file diff --git a/src/robots/gr1t2_description/urdf.rviz b/src/robots/gr1t2_description/urdf.rviz deleted file mode 100644 index d469e8f..0000000 --- a/src/robots/gr1t2_description/urdf.rviz +++ /dev/null @@ -1,225 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 549 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - 10: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 11: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 12: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 13: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 20: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 21: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 22: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 23: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 30: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 31: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 32: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 33: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 40: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 41: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 42: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - 43: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.634476661682129 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.0988716408610344 - Y: 0.08353396505117416 - Z: -0.19447802007198334 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5753985047340393 - Target Frame: - Yaw: 0.47539806365966797 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1200 - X: 278 - Y: 96 diff --git a/src/robots/gr1t2_description/urdf/GR1T2.urdf b/src/robots/gr1t2_description/urdf/GR1T2.urdf deleted file mode 100644 index b21a881..0000000 --- a/src/robots/gr1t2_description/urdf/GR1T2.urdf +++ /dev/null @@ -1,895 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/robots/gr1t2_description/urdf/GR1T2_simple.urdf b/src/robots/gr1t2_description/urdf/GR1T2_simple.urdf deleted file mode 100644 index 5b76340..0000000 --- a/src/robots/gr1t2_description/urdf/GR1T2_simple.urdf +++ /dev/null @@ -1,1200 +0,0 @@ - - - - - /gr1t2_gazebo - gazebo_ros_control/DefaultRobotHWSim - - - - - - - - - base_link - /apply_force/base_link - - - - true - - true - 1000 - true - __default_topic__ - - base_link_imu - imu_link - 1000.0 - 0.0 - 0 0 0 - 0 0 0 - imu_link - - 0 0 0 0 0 0 - - - - - Gazebo/DarkGrey - false - - - 0.2 - 0.2 - - - - - 0.2 - 0.2 - Gazebo/Red - - - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 0.2 - 0.2 - 1 - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - 1.0 - 1.0 - 1 - - - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - "1" - - - - \ No newline at end of file