added go1 model
This commit is contained in:
Bian Zekun
parent
b5d72a0761
commit
2bf4935031
95
README.md
95
README.md
|
|
@ -1,50 +1,25 @@
|
|||
Packages Version: v3.2.1
|
||||
Suitable for unitree_legged_sdk(namely v3.2) and aliengo_sdk(namely v3.1).
|
||||
|
||||
# Introduction
|
||||
Here are the ROS packages of Unitree robots, namely Laikago, Aliengo and A1. You can load robots and joint controllers in Gazebo, so you can do low-level control(control the torque, position and angular velocity) on the robot joints. Please watch out that the Gazebo simulation cannot do high-level control, namely walking. Besides of these simulation functions, you can also control your real robots in ROS by the `unitree_legged_real`. For real robots, you can do high-level and low-level control by our ROS packages.
|
||||
Here are the ROS simulation packages of Unitree robots, You can load robots and joint controllers in Gazebo, so you can do low-level control(control the torque, position and angular velocity) on the robot joints. Please watch out that the Gazebo simulation cannot do high-level control, namely walking. Besides of these simulation functions, you can also control your real robots in ROS by the [unitree_legged_real](https://github.com/unitreerobotics) package. For real robots, you can do high-level and low-level control by our ROS packages.
|
||||
|
||||
## Packages:
|
||||
Robot description: `a1_description`, `aliengo_description`, `laikago_description`
|
||||
Robot description: `go1_description`, `a1_description`, `aliengo_description`, `laikago_description`
|
||||
|
||||
Robot and joints controller: `unitree_controller`
|
||||
|
||||
Basic function: `unitree_legged_msgs`
|
||||
Basic message function: `unitree_legged_msgs`
|
||||
|
||||
Simulation related: `unitree_gazebo`, `unitree_legged_control`
|
||||
|
||||
Real robot control related: `unitree_legged_real`
|
||||
<!-- Real robot control related: `unitree_legged_real` -->
|
||||
|
||||
# Dependencies
|
||||
* [ROS](https://www.ros.org/) melodic or ROS kinetic(has not been tested)
|
||||
* [Gazebo8](http://gazebosim.org/)
|
||||
* [unitree_legged_sdk](https://github.com/unitreerobotics): If your robot is suitable for `unitree_legged_sdk`, then you do not need `aliengo_sdk`.
|
||||
* [aliengo_sdk](https://github.com/unitreerobotics): If your robot is suitable for `aliengo_sdk`, then you do not need `unitree_legged_sdk`.
|
||||
|
||||
# Configuration
|
||||
Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case.
|
||||
If your use `unitree_legged_sdk`, then you need to set `UNITREE_SDK_VERSION=3_2` and the path `UNITREE_LEGGED_SDK_PATH`.
|
||||
Otherwise, if you use `aliengo_sdk`, you need to set `UNITREE_SDK_VERSION=3_1` and the path `ALIENGO_SDK_PATH`.
|
||||
|
||||
```
|
||||
source /opt/ros/melodic/setup.bash
|
||||
source /usr/share/gazebo-8/setup.sh
|
||||
source ~/catkin_ws/devel/setup.bash
|
||||
export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH}
|
||||
export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH}
|
||||
export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH}
|
||||
# 3_1, 3_2
|
||||
export UNITREE_SDK_VERSION=3_2
|
||||
export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk
|
||||
export ALIENGO_SDK_PATH=~/aliengo_sdk
|
||||
# amd64, arm32, arm64
|
||||
export UNITREE_PLATFORM="amd64"
|
||||
```
|
||||
|
||||
# Build
|
||||
If you just need to send message from the ROS on robot's NX or TX2, you can just copy `unitree_legged_msgs` and `unitree_legged_real` to `catkin_ws/src` folder and run `catkin_make` to compile.
|
||||
|
||||
If you would like to fully compile the `unitree_ros`, please run the following command to install relative packages.
|
||||
|
||||
<!-- If you would like to fully compile the `unitree_ros`, please run the following command to install relative packages. -->
|
||||
|
||||
If your ROS is melodic:
|
||||
```
|
||||
|
|
@ -71,17 +46,16 @@ catkin_make
|
|||
|
||||
If you face a dependency problem, you can just run `catkin_make` again.
|
||||
|
||||
|
||||
# Detail of Packages
|
||||
## unitree_legged_control:
|
||||
It contains the joints controllers for Gazebo simulation, which allows user to control joints with position, velocity and torque. Refer to "unitree_ros/unitree_controller/src/servo.cpp" for joint control examples in different modes.
|
||||
It contains the joints controllers for Gazebo simulation, which allows users to control joints with position, velocity and torque. Refer to "unitree_ros/unitree_controller/src/servo.cpp" for joint control examples in different modes.
|
||||
|
||||
## unitree_legged_msgs:
|
||||
ros-type message, including command and state of high-level and low-level control.
|
||||
It would be better if it be compiled firstly, otherwise you may have dependency problems (such as that you can't find the header file).
|
||||
|
||||
## The description of robots:
|
||||
Namely the description of A1, Aliengo and Laikago. Each package include mesh, urdf and xacro files of robot. Take Laikago as an example, you can check the model in Rviz by:
|
||||
Namely the description of Go1, A1, Aliengo and Laikago. Each package include mesh, urdf and xacro files of robot. Take Laikago as an example, you can check the model in Rviz by:
|
||||
```
|
||||
roslaunch laikago_description laikago_rviz.launch
|
||||
```
|
||||
|
|
@ -91,7 +65,7 @@ You can launch the Gazebo simulation by the following command:
|
|||
```
|
||||
roslaunch unitree_gazebo normal.launch rname:=a1 wname:=stairs
|
||||
```
|
||||
Where the `rname` means robot name, which can be `laikago`, `aliengo` or `a1`. The `wname` means world name, which can be `earth`, `space` or `stairs`. And the default value of `rname` is `laikago`, while the default value of `wname` is `earth`. In Gazebo, the robot should be lying on the ground with joints not activated.
|
||||
Where the `rname` means robot name, which can be `laikago`, `aliengo`, `a1` or `go1`. The `wname` means world name, which can be `earth`, `space` or `stairs`. And the default value of `rname` is `laikago`, while the default value of `wname` is `earth`. In Gazebo, the robot should be lying on the ground with joints not activated.
|
||||
|
||||
### Stand controller
|
||||
After launching the gazebo simulation, you can start to control the robot:
|
||||
|
|
@ -110,53 +84,4 @@ Then run the position and pose publisher in another terminal:
|
|||
```
|
||||
rosrun unitree_controller unitree_move_kinetic
|
||||
```
|
||||
The robot will turn around the origin, which is the movement under the world coordinate. And inside of the source file `move_publisher`, we also offered the method to move robot under robot coordinate. You can change the value of `def_frame` to `coord::ROBOT` and run the catkin_make again, then the `unitree_move_publisher` will move robot under its own coordinate.
|
||||
|
||||
## unitree_legged_real
|
||||
### Setup the net connection
|
||||
First, please connect the network cable between your PC and robot. Then run `ifconfig` in a terminal, you will find your port name. For example, `enx000ec6612921`.
|
||||
|
||||
Then, open the `ipconfig.sh` file under the folder `unitree_legged_real`, modify the port name to your own. And run the following commands:
|
||||
```
|
||||
sudo chmod +x ipconfig.sh
|
||||
sudo ./ipconfig.sh
|
||||
```
|
||||
If you run the `ifconfig` again, you will find that port has `inet` and `netmask` now.
|
||||
In order to set your port automatically, you can modify `interfaces`:
|
||||
```
|
||||
sudo gedit /etc/network/interfaces
|
||||
```
|
||||
And add the following 4 lines at the end:
|
||||
```
|
||||
auto enx000ec6612921
|
||||
iface enx000ec6612921 inet static
|
||||
address 192.168.123.162
|
||||
netmask 255.255.255.0
|
||||
```
|
||||
Where the port name have to change to your own.
|
||||
|
||||
|
||||
### Run the package
|
||||
You can control your real robot(only A1 and Aliengo) from ROS by this package.
|
||||
|
||||
First you have to run the `real_launch` under root account:
|
||||
```
|
||||
sudo su
|
||||
source /home/yourUserName/catkin_ws/devel/setup.bash
|
||||
roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=highlevel firmwork:=3_2
|
||||
```
|
||||
Please watchout that the `/home/yourUserName` means the home directory of yourself. These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And the `ctrl_level` means the control level, which can be `lowlevel` or `highlevel`(case does not matter), and the default value is `highlevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose. The `firmwork` means the firmwork version of the robot. The default value is `3_2` Now all the A1's firmwork version is `3_2`, and most Aliengo's firmwork version is `3_1`.(will update in the future)
|
||||
|
||||
To do so, you need to run the controller in another terminal(also under root account):
|
||||
```
|
||||
rosrun unitree_legged_real position_lcm
|
||||
```
|
||||
We offered some examples. When you run the low level controller, please make sure the robot is hanging up. The low level contains:
|
||||
```
|
||||
position_lcm
|
||||
velocity_lcm
|
||||
torque_lcm
|
||||
```
|
||||
The `velocity_lcm` and `torque_lcm` have to run under root account too. Please use the same method as runing `real_launch`.
|
||||
|
||||
And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`.
|
||||
The robot will turn around the origin, which is the movement under the world coordinate. And inside of the source file `move_publisher`, we also offered the method to move robot under robot coordinate. You can change the value of `def_frame` to `coord::ROBOT` and run the catkin_make again, then the `unitree_move_publisher` will move robot under its own coordinate.
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(go1_description)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
genmsg
|
||||
roscpp
|
||||
std_msgs
|
||||
tf
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS
|
||||
)
|
||||
|
||||
include_directories(
|
||||
# include
|
||||
${Boost_INCLUDE_DIR}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
go1_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
|
||||
# FL Controllers ---------------------------------------
|
||||
FL_hip_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FL_thigh_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FL_calf_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# FR Controllers ---------------------------------------
|
||||
FR_hip_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
FR_thigh_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
FR_calf_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: FR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RL Controllers ---------------------------------------
|
||||
RL_hip_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RL_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RL_thigh_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RL_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RL_calf_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RL_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
# RR Controllers ---------------------------------------
|
||||
RR_hip_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RR_hip_joint
|
||||
pid: {p: 100.0, i: 0.0, d: 5.0}
|
||||
|
||||
RR_thigh_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RR_thigh_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
RR_calf_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: RR_calf_joint
|
||||
pid: {p: 300.0, i: 0.0, d: 8.0}
|
||||
|
||||
|
|
@ -0,0 +1,475 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 796
|
||||
- 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
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: PointCloud2
|
||||
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: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
FL_calf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FL_foot:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FL_hip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FL_thigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FL_thigh_shoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FR_calf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FR_foot:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FR_hip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FR_thigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
FR_thigh_shoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
RL_calf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RL_foot:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RL_hip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RL_thigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RL_thigh_shoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RR_calf:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RR_foot:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RR_hip:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RR_thigh:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
RR_thigh_shoulder:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_chin:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_face:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_laserscan_link_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_laserscan_link_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_optical_chin:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_optical_face:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_optical_left:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_optical_rearDown:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_optical_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
camera_rearDown:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_right:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
imu_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
trunk:
|
||||
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
|
||||
- Class: rviz/Axes
|
||||
Enabled: false
|
||||
Length: 1
|
||||
Name: Axes
|
||||
Radius: 0.10000000149011612
|
||||
Reference Frame: <Fixed 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
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /cam1/point_cloud_face
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /cam2/point_cloud_chin
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /cam3/point_cloud_left
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /cam4/point_cloud_right
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Decay Time: 0
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud2
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Flat Squares
|
||||
Topic: /cam5/point_cloud_rearDown
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: false
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 238; 238; 238
|
||||
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: 3.724609613418579
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.29964715242385864
|
||||
Y: -0.07829883694648743
|
||||
Z: -0.04122250899672508
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.3647973835468292
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 2.6735990047454834
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1025
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001c7000003a7fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000570000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1853
|
||||
X: 1987
|
||||
Y: 27
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
<launch>
|
||||
|
||||
<arg name="user_debug" default="false"/>
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find go1_description)/xacro/robot.xacro'
|
||||
DEBUG:=$(arg user_debug)"/>
|
||||
|
||||
<!-- for higher robot_state_publisher average rate-->
|
||||
<!-- <param name="rate" value="1000"/> -->
|
||||
|
||||
<!-- send fake joint values -->
|
||||
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
|
||||
<param name="use_gui" value="TRUE"/>
|
||||
</node>
|
||||
|
||||
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
|
||||
<param name="publish_frequency" type="double" value="1000.0"/>
|
||||
</node>
|
||||
|
||||
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
|
||||
args="-d $(find go1_description)/launch/check_joint.rviz"/>
|
||||
|
||||
</launch>
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,13 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>go1_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The go1_description package</description>
|
||||
|
||||
<maintainer email="laikago@unitree.cc">unitree</maintainer>
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
</package>
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="go1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
<xacro:property name="stick_mass" value="0.00001"/>
|
||||
|
||||
<!-- simplified collision value -->
|
||||
<xacro:property name="trunk_width" value="0.0935"/>
|
||||
<xacro:property name="trunk_length" value="0.3762"/>
|
||||
<xacro:property name="trunk_height" value="0.114"/>
|
||||
<xacro:property name="hip_radius" value="0.046"/>
|
||||
<xacro:property name="hip_length" value="0.04"/>
|
||||
<xacro:property name="thigh_shoulder_radius" value="0.041"/>
|
||||
<xacro:property name="thigh_shoulder_length" value="0.032"/>
|
||||
<xacro:property name="thigh_width" value="0.0245"/>
|
||||
<xacro:property name="thigh_height" value="0.034"/>
|
||||
<xacro:property name="calf_width" value="0.016"/>
|
||||
<xacro:property name="calf_height" value="0.016"/>
|
||||
<xacro:property name="foot_radius" value="0.02"/>
|
||||
<xacro:property name="stick_radius" value="0.01"/>
|
||||
<xacro:property name="stick_length" value="0.2"/>
|
||||
|
||||
<!-- kinematic value -->
|
||||
<xacro:property name="thigh_offset" value="0.08"/>
|
||||
<xacro:property name="thigh_length" value="0.213"/>
|
||||
<xacro:property name="calf_length" value="0.213"/>
|
||||
|
||||
<!-- leg offset from trunk center value -->
|
||||
<xacro:property name="leg_offset_x" value="0.1881"/>
|
||||
<xacro:property name="leg_offset_y" value="0.04675"/>
|
||||
<!-- <xacro:property name="trunk_offset_z" value="0.01675"/> -->
|
||||
<xacro:property name="hip_offset" value="0.08"/>
|
||||
|
||||
<!-- joint limits -->
|
||||
<xacro:property name="damping" value="0.01"/>
|
||||
<xacro:property name="friction" value="0.2"/>
|
||||
<xacro:property name="hip_max" value="46"/>
|
||||
<xacro:property name="hip_min" value="-46"/>
|
||||
<xacro:property name="hip_velocity_max" value="21"/>
|
||||
<xacro:property name="hip_torque_max" value="33.5"/>
|
||||
<xacro:property name="thigh_max" value="240"/>
|
||||
<xacro:property name="thigh_min" value="-60"/>
|
||||
<xacro:property name="thigh_velocity_max" value="21"/>
|
||||
<xacro:property name="thigh_torque_max" value="33.5"/>
|
||||
<xacro:property name="calf_max" value="-52.5"/>
|
||||
<xacro:property name="calf_min" value="-154.5"/>
|
||||
<xacro:property name="calf_velocity_max" value="21"/>
|
||||
<xacro:property name="calf_torque_max" value="33.5"/>
|
||||
|
||||
<!-- dynamics inertial value -->
|
||||
<!-- trunk -->
|
||||
<xacro:property name="trunk_mass" value="4.8"/>
|
||||
<xacro:property name="trunk_com_x" value="0.011611"/>
|
||||
<xacro:property name="trunk_com_y" value="0.004437"/>
|
||||
<xacro:property name="trunk_com_z" value="0.000108"/>
|
||||
<xacro:property name="trunk_ixx" value="0.016130741919"/>
|
||||
<xacro:property name="trunk_ixy" value="0.000593180607"/>
|
||||
<xacro:property name="trunk_ixz" value="0.000007324662"/>
|
||||
<xacro:property name="trunk_iyy" value="0.036507810812"/>
|
||||
<xacro:property name="trunk_iyz" value="0.000020969537"/>
|
||||
<xacro:property name="trunk_izz" value="0.044693872053"/>
|
||||
|
||||
<!-- hip (left front) -->
|
||||
<xacro:property name="hip_mass" value="0.510299"/>
|
||||
<xacro:property name="hip_com_x" value="-0.00541"/>
|
||||
<xacro:property name="hip_com_y" value="-0.00074"/>
|
||||
<xacro:property name="hip_com_z" value="0.000006"/>
|
||||
<xacro:property name="hip_ixx" value="0.00030528937"/>
|
||||
<xacro:property name="hip_ixy" value="-0.000007788013"/>
|
||||
<xacro:property name="hip_ixz" value="0.000000220160"/>
|
||||
<xacro:property name="hip_iyy" value="0.000590894859"/>
|
||||
<xacro:property name="hip_iyz" value="-0.000000017175"/>
|
||||
<xacro:property name="hip_izz" value="0.000396594572"/>
|
||||
|
||||
<!-- thigh -->
|
||||
<xacro:property name="thigh_mass" value="0.898919"/>
|
||||
<xacro:property name="thigh_com_x" value="-0.003468"/>
|
||||
<xacro:property name="thigh_com_y" value="-0.018947"/>
|
||||
<xacro:property name="thigh_com_z" value="-0.032736"/>
|
||||
<xacro:property name="thigh_ixx" value="0.005395867678"/>
|
||||
<xacro:property name="thigh_ixy" value="0.000000102809"/>
|
||||
<xacro:property name="thigh_ixz" value="0.000337529085"/>
|
||||
<xacro:property name="thigh_iyy" value="0.005142451046"/>
|
||||
<xacro:property name="thigh_iyz" value="-0.000005816563"/>
|
||||
<xacro:property name="thigh_izz" value="0.00102478732"/>
|
||||
|
||||
<!-- calf -->
|
||||
<xacro:property name="calf_mass" value="0.158015"/>
|
||||
<xacro:property name="calf_com_x" value="0.006286"/>
|
||||
<xacro:property name="calf_com_y" value="0.001307"/>
|
||||
<xacro:property name="calf_com_z" value="-0.122269"/>
|
||||
<xacro:property name="calf_ixx" value="0.003607648222"/>
|
||||
<xacro:property name="calf_ixy" value="0.000001494971"/>
|
||||
<xacro:property name="calf_ixz" value="-0.000132778525"/>
|
||||
<xacro:property name="calf_iyy" value="0.003626771492"/>
|
||||
<xacro:property name="calf_iyz" value="-0.000028638535"/>
|
||||
<xacro:property name="calf_izz" value="0.000035148003"/>
|
||||
|
||||
<!-- foot -->
|
||||
<xacro:property name="foot_mass" value="0.06"/>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="depthCamera" params="camID name *origin">
|
||||
<joint name="camera_joint_${name}" type="fixed">
|
||||
<xacro:insert_block name="origin"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="camera_${name}"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_${name}">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size=".001 .001 .001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://go1_description/meshes/depthCamera.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
||||
<inertial>
|
||||
<mass value="1e-5" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="camera_optical_joint_${name}" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
|
||||
<parent link="camera_${name}"/>
|
||||
<child link="camera_optical_${name}"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_optical_${name}">
|
||||
</link>
|
||||
|
||||
<gazebo reference="camera_${name}">
|
||||
<!-- <material>Gazebo/Black</material> -->
|
||||
<sensor name="camera_${name}_camera" type="depth">
|
||||
<update_rate>16</update_rate>
|
||||
<camera>
|
||||
<horizontal_fov>2.094</horizontal_fov>
|
||||
<image>
|
||||
<width>928</width>
|
||||
<height>800</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.1</near>
|
||||
<far>5</far>
|
||||
</clip>
|
||||
</camera>
|
||||
<plugin name="camera_${name}_controller" filename="libgazebo_ros_openni_kinect.so">
|
||||
<baseline>0.025</baseline>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>camera_${name}_ir</cameraName>
|
||||
<imageTopicName>/camera_${name}/color/image_raw</imageTopicName>
|
||||
<cameraInfoTopicName>/camera_${name}/color/camera_info</cameraInfoTopicName>
|
||||
<depthImageTopicName>/camera_${name}/depth/image_raw</depthImageTopicName>
|
||||
<depthImageInfoTopicName>/camera_${name}/depth/camera_info</depthImageInfoTopicName>
|
||||
<!-- <pointCloudTopicName>/camera_${name}/depth/points</pointCloudTopicName> -->
|
||||
<pointCloudTopicName>/cam${camID}/point_cloud_${name}</pointCloudTopicName>
|
||||
<frameName>camera_optical_${name}</frameName>
|
||||
<pointCloudCutoff>0.1</pointCloudCutoff>
|
||||
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
<CxPrime>0</CxPrime>
|
||||
<Cx>0.0045</Cx>
|
||||
<Cy>0.0039</Cy>
|
||||
<focalLength>0</focalLength>
|
||||
<hackBaseline>0</hackBaseline>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,385 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/go1_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- Show the trajectory of trunk center. -->
|
||||
<gazebo>
|
||||
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
|
||||
<frequency>10</frequency>
|
||||
<plot>
|
||||
<link>base</link>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<material>Gazebo/Yellow</material>
|
||||
</plot>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
|
||||
<frequency>100</frequency>
|
||||
<plot>
|
||||
<link>FL_foot</link>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<material>Gazebo/Green</material>
|
||||
</plot>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
|
||||
<bodyName>trunk</bodyName>
|
||||
<topicName>/apply_force/trunk</topicName>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="imu_link">
|
||||
<gravity>true</gravity>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>true</always_on>
|
||||
<update_rate>1000</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>__default_topic__</topic>
|
||||
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
|
||||
<topicName>trunk_imu</topicName>
|
||||
<bodyName>imu_link</bodyName>
|
||||
<updateRateHZ>1000.0</updateRateHZ>
|
||||
<gaussianNoise>0.0</gaussianNoise>
|
||||
<xyzOffset>0 0 0</xyzOffset>
|
||||
<rpyOffset>0 0 0</rpyOffset>
|
||||
<frameName>imu_link</frameName>
|
||||
</plugin>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<!-- Depth camera -->
|
||||
<!-- <gazebo>
|
||||
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
|
||||
<baseline>0.025</baseline>
|
||||
<always_on>true</always_on>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>unitree_camera_left</cameraName>
|
||||
<frameName>depthCamera_link_left</frameName>
|
||||
<imageTopicName>rgb/imageRaw_left</imageTopicName>
|
||||
<depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
|
||||
<pointCloudTopicName>depth/points_left</pointCloudTopicName>
|
||||
<cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>
|
||||
<depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>
|
||||
<pointCloudCutoff>0.1</pointCloudCutoff>
|
||||
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
<CxPrime>0.0</CxPrime>
|
||||
<Cx>0.0045</Cx>
|
||||
<Cy>0.0039</Cy>
|
||||
<focalLength>0.004</focalLength>
|
||||
<hackBaseline>0.0</hackBaseline>
|
||||
</plugin>
|
||||
</gazebo> -->
|
||||
|
||||
<!-- <gazebo reference="depthCamera_link_left">
|
||||
<sensor name="unitree_camera_left" type="depth_camera">
|
||||
<update_rate>16</update_rate>
|
||||
<always_on>true</always_on>
|
||||
<visualize>true</visualize>
|
||||
<camera>
|
||||
<horizontal_fov>2.094</horizontal_fov>
|
||||
<image>
|
||||
<width>928</width>
|
||||
<height>800</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<depth_camera></depth_camera>
|
||||
<clip>
|
||||
<near>0.1</near>
|
||||
<far>100</far>
|
||||
</clip>
|
||||
</camera>
|
||||
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
|
||||
<baseline>0.025</baseline>
|
||||
<always_on>true</always_on>
|
||||
<updateRate>0.0</updateRate>
|
||||
<cameraName>unitree_camera_left</cameraName>
|
||||
<frameName>depthCamera_link_left</frameName>
|
||||
<imageTopicName>rgb/imageRaw_left</imageTopicName>
|
||||
<depthImageTopicName>depth/imageRaw_left</depthImageTopicName>
|
||||
<pointCloudTopicName>depth/points_left</pointCloudTopicName>
|
||||
<cameraInfoTopicName>rgb/cameraInfo_left</cameraInfoTopicName>
|
||||
<depthImageCameraInfoTopicName>depth/cameraInfo_left</depthImageCameraInfoTopicName>
|
||||
<pointCloudCutoff>0.1</pointCloudCutoff>
|
||||
<pointCloudCutoffMax>1.5</pointCloudCutoffMax>
|
||||
<distortionK1>0.0</distortionK1>
|
||||
<distortionK2>0.0</distortionK2>
|
||||
<distortionK3>0.0</distortionK3>
|
||||
<distortionT1>0.0</distortionT1>
|
||||
<distortionT2>0.0</distortionT2>
|
||||
<CxPrime>0.0</CxPrime>
|
||||
<Cx>0.0045</Cx>
|
||||
<Cy>0.0039</Cy>
|
||||
<focalLength>0.004</focalLength>
|
||||
<hackBaseline>0.0</hackBaseline>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo> -->
|
||||
|
||||
<gazebo reference="depthCamera_link_left">
|
||||
<sensor name="camera_link_camera" type="depth">
|
||||
<update_rate>20</update_rate>
|
||||
<camera>
|
||||
<horizontal_fov>1.047198</horizontal_fov>
|
||||
<image>
|
||||
<width>640</width>
|
||||
<height>480</height>
|
||||
<format>R8G8B8</format>
|
||||
</image>
|
||||
<clip>
|
||||
<near>0.05</near>
|
||||
<far>3</far>
|
||||
</clip>
|
||||
</camera>
|
||||
<plugin name="camera_link_controller" filename="libgazebo_ros_openni_kinect.so">
|
||||
<baseline>0.2</baseline>
|
||||
<alwaysOn>true</alwaysOn>
|
||||
<updateRate>1.0</updateRate>
|
||||
<cameraName>camera_link_ir</cameraName>
|
||||
<imageTopicName>/camera_link/color/image_raw</imageTopicName>
|
||||
<cameraInfoTopicName>/camera_link/color/camera_info</cameraInfoTopicName>
|
||||
<depthImageTopicName>/camera_link/depth/image_raw</depthImageTopicName>
|
||||
<depthImageInfoTopicName>/camera_link/depth/camera_info</depthImageInfoTopicName>
|
||||
<pointCloudTopicName>/camera_link/depth/points</pointCloudTopicName>
|
||||
<frameName>depthCamera_link_left</frameName>
|
||||
<pointCloudCutoff>0.5</pointCloudCutoff>
|
||||
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
|
||||
<distortionK1>0.00000001</distortionK1>
|
||||
<distortionK2>0.00000001</distortionK2>
|
||||
<distortionK3>0.00000001</distortionK3>
|
||||
<distortionT1>0.00000001</distortionT1>
|
||||
<distortionT2>0.00000001</distortionT2>
|
||||
<CxPrime>0</CxPrime>
|
||||
<Cx>0</Cx>
|
||||
<Cy>0</Cy>
|
||||
<focalLength>0</focalLength>
|
||||
<hackBaseline>0</hackBaseline>
|
||||
</plugin>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<!-- Foot contacts. -->
|
||||
<gazebo reference="FR_calf">
|
||||
<sensor name="FR_foot_contact" type="contact">
|
||||
<update_rate>100</update_rate>
|
||||
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
|
||||
<contact>
|
||||
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<gazebo reference="FL_calf">
|
||||
<sensor name="FL_foot_contact" type="contact">
|
||||
<update_rate>100</update_rate>
|
||||
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
|
||||
<contact>
|
||||
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<gazebo reference="RR_calf">
|
||||
<sensor name="RR_foot_contact" type="contact">
|
||||
<update_rate>100</update_rate>
|
||||
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
|
||||
<contact>
|
||||
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
<gazebo reference="RL_calf">
|
||||
<sensor name="RL_foot_contact" type="contact">
|
||||
<update_rate>100</update_rate>
|
||||
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
|
||||
<contact>
|
||||
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
|
||||
<!-- Visualization of Foot contacts. -->
|
||||
<gazebo reference="FR_foot">
|
||||
<visual>
|
||||
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
|
||||
<topicName>FR_foot_contact</topicName>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
<gazebo reference="FL_foot">
|
||||
<visual>
|
||||
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
|
||||
<topicName>FL_foot_contact</topicName>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
<gazebo reference="RR_foot">
|
||||
<visual>
|
||||
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
|
||||
<topicName>RR_foot_contact</topicName>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
<gazebo reference="RL_foot">
|
||||
<visual>
|
||||
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
|
||||
<topicName>RL_foot_contact</topicName>
|
||||
</plugin>
|
||||
</visual>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base">
|
||||
<material>Gazebo/Green</material>
|
||||
<turnGravityOff>false</turnGravityOff>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="trunk">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="stick_link">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="imu_link">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<!-- FL leg -->
|
||||
<gazebo reference="FL_hip">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
<gazebo reference="FL_thigh">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="FL_calf">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="FL_foot">
|
||||
<mu1>0.6</mu1>
|
||||
<mu2>0.6</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
|
||||
<!-- FR leg -->
|
||||
<gazebo reference="FR_hip">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
<gazebo reference="FR_thigh">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="FR_calf">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="FR_foot">
|
||||
<mu1>0.6</mu1>
|
||||
<mu2>0.6</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
|
||||
<!-- RL leg -->
|
||||
<gazebo reference="RL_hip">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
<gazebo reference="RL_thigh">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="RL_calf">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="RL_foot">
|
||||
<mu1>0.6</mu1>
|
||||
<mu2>0.6</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
|
||||
<!-- RR leg -->
|
||||
<gazebo reference="RR_hip">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
</gazebo>
|
||||
<gazebo reference="RR_thigh">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
<gazebo reference="RR_calf">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="RR_foot">
|
||||
<mu1>0.6</mu1>
|
||||
<mu2>0.6</mu2>
|
||||
<self_collide>1</self_collide>
|
||||
<material>Gazebo/DarkGrey</material>
|
||||
<kp value="1000000.0"/>
|
||||
<kd value="1.0"/>
|
||||
</gazebo>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,176 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
|
||||
|
||||
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
|
||||
|
||||
<joint name="${name}_hip_joint" type="revolute">
|
||||
<xacro:insert_block name="origin"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="${name}_hip"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="${damping}" friction="${friction}"/>
|
||||
<xacro:if value="${(mirror_dae == True)}">
|
||||
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_min*PI/180.0}" upper="${hip_max*PI/180.0}"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${(mirror_dae == False)}">
|
||||
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_max*PI/180.0}" upper="${-hip_min*PI/180.0}"/>
|
||||
</xacro:if>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_hip">
|
||||
<visual>
|
||||
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
|
||||
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
|
||||
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
|
||||
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
|
||||
</xacro:if>
|
||||
<geometry>
|
||||
<mesh filename="package://go1_description/meshes/hip.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${hip_length}" radius="${hip_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
|
||||
<mass value="${hip_mass}"/>
|
||||
<inertia
|
||||
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
|
||||
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
|
||||
izz="${hip_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_hip_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 ${(hip_offset)*mirror} 0"/>
|
||||
<parent link="${name}_hip"/>
|
||||
<child link="${name}_thigh_shoulder"/>
|
||||
</joint>
|
||||
|
||||
<!-- this link is only for collision -->
|
||||
<link name="${name}_thigh_shoulder">
|
||||
<collision>
|
||||
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${thigh_shoulder_length}" radius="${thigh_shoulder_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_thigh_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
|
||||
<parent link="${name}_hip"/>
|
||||
<child link="${name}_thigh"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${damping}" friction="${friction}"/>
|
||||
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_min*PI/180.0}" upper="${thigh_max*PI/180.0}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_thigh">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<xacro:if value="${mirror_dae == True}">
|
||||
<mesh filename="package://go1_description/meshes/thigh.dae" scale="1 1 1"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${mirror_dae == False}">
|
||||
<mesh filename="package://go1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
|
||||
</xacro:if>
|
||||
</geometry>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
|
||||
<geometry>
|
||||
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
|
||||
<mass value="${thigh_mass}"/>
|
||||
<inertia
|
||||
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
|
||||
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
|
||||
izz="${thigh_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_calf_joint" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
|
||||
<parent link="${name}_thigh"/>
|
||||
<child link="${name}_calf"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${damping}" friction="${friction}"/>
|
||||
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_min*PI/180.0}" upper="${calf_max*PI/180.0}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_calf">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://go1_description/meshes/calf.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
|
||||
<geometry>
|
||||
<box size="${calf_length} ${calf_width} ${calf_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
|
||||
<mass value="${calf_mass}"/>
|
||||
<inertia
|
||||
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
|
||||
iyy="${calf_iyy}" iyz="${calf_iyz}"
|
||||
izz="${calf_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${name}_foot_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
|
||||
<parent link="${name}_calf"/>
|
||||
<child link="${name}_foot"/>
|
||||
</joint>
|
||||
|
||||
<link name="${name}_foot">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${foot_radius-0.01}"/>
|
||||
</geometry>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${foot_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="${foot_mass}"/>
|
||||
<inertia
|
||||
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
|
||||
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<xacro:leg_transmission name="${name}"/>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="grey">
|
||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="silver">
|
||||
<color rgba="${233/255} ${233/255} ${216/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="orange">
|
||||
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="brown">
|
||||
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,191 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="go1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:include filename="$(find go1_description)/xacro/const.xacro"/>
|
||||
<xacro:include filename="$(find go1_description)/xacro/materials.xacro"/>
|
||||
<xacro:include filename="$(find go1_description)/xacro/leg.xacro"/>
|
||||
<!-- <xacro:include filename="$(find go1_description)/xacro/stairs.xacro"/> -->
|
||||
<xacro:include filename="$(find go1_description)/xacro/gazebo.xacro"/>
|
||||
<xacro:include filename="$(find go1_description)/xacro/depthCamera.xacro"/>
|
||||
<xacro:include filename="$(find go1_description)/xacro/ultraSound.xacro"/>
|
||||
<!-- <xacro:include filename="$(find go1_gazebo)/launch/stairs.urdf.xacro"/> -->
|
||||
|
||||
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
|
||||
|
||||
<!-- Rollover Protection mode will add an additional stick on the top, use "true" or "false" to switch it. -->
|
||||
<xacro:property name="rolloverProtection" value="false"/>
|
||||
|
||||
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
|
||||
<xacro:if value="$(arg DEBUG)">
|
||||
<link name="world"/>
|
||||
<joint name="base_static_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
</joint>
|
||||
</xacro:if>
|
||||
|
||||
<link name="base">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
<joint name="floating_base" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="base"/>
|
||||
<child link="trunk"/>
|
||||
</joint>
|
||||
|
||||
<link name="trunk">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://go1_description/meshes/trunk.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
|
||||
<mass value="${trunk_mass}"/>
|
||||
<inertia
|
||||
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
|
||||
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
|
||||
izz="${trunk_izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<xacro:if value="${(rolloverProtection == 'true')}">
|
||||
<joint name="stick_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
<child link="stick_link"/>
|
||||
<origin rpy="0 0 0" xyz="${0.18} 0 ${stick_length/2.0+0.08}"/>
|
||||
</joint>
|
||||
|
||||
<link name="stick_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${stick_length}" radius="${stick_radius}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<!-- <material name="white"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${stick_length}" radius="${stick_radius}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="${stick_mass}"/>
|
||||
<inertia
|
||||
ixx="${stick_mass / 2.0 * (stick_radius*stick_radius)}" ixy="0.0" ixz="0.0"
|
||||
iyy="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}" iyz="0.0"
|
||||
izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</xacro:if>
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="trunk"/>
|
||||
<child link="imu_link"/>
|
||||
<origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
|
||||
</joint>
|
||||
|
||||
<link name="imu_link">
|
||||
<inertial>
|
||||
<mass value="0.001"/>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<!-- <material name="red"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size=".001 .001 .001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
|
||||
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
|
||||
</xacro:leg>
|
||||
|
||||
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
|
||||
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
|
||||
</xacro:leg>
|
||||
|
||||
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
|
||||
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
|
||||
</xacro:leg>
|
||||
|
||||
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
|
||||
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
|
||||
</xacro:leg>
|
||||
|
||||
<xacro:depthCamera camID="1" name="face">
|
||||
<origin rpy="${PI} 0 0" xyz="0.2785 0.0125 0.0167"/>
|
||||
</xacro:depthCamera>
|
||||
|
||||
<xacro:depthCamera camID="2" name="chin">
|
||||
<origin rpy="${PI} ${PI/2} 0" xyz="0.2522 0.0125 -0.0436"/>
|
||||
</xacro:depthCamera>
|
||||
|
||||
<xacro:depthCamera camID="3" name="left">
|
||||
<origin rpy="${PI} 0.2618 ${PI/2}" xyz="-0.066 0.082 -0.0176"/>
|
||||
</xacro:depthCamera>
|
||||
|
||||
<xacro:depthCamera camID="4" name="right">
|
||||
<origin rpy="${PI} 0.2618 ${-PI/2}" xyz="-0.041 -0.082 -0.0176"/>
|
||||
</xacro:depthCamera>
|
||||
|
||||
<xacro:depthCamera camID="5" name="rearDown">
|
||||
<origin rpy="${PI} ${PI/2} 0" xyz="-0.0825 0.0125 -0.04365"/>
|
||||
</xacro:depthCamera>
|
||||
|
||||
<joint name="camera_laserscan_joint_left" type="fixed">
|
||||
<origin rpy="0 0.2618 0" xyz="0 0 0"/>
|
||||
<parent link="camera_left"/>
|
||||
<child link="camera_laserscan_link_left"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_laserscan_link_left">
|
||||
</link>
|
||||
|
||||
<joint name="camera_laserscan_joint_right" type="fixed">
|
||||
<origin rpy="0 0.2618 0" xyz="0 0 0"/>
|
||||
<parent link="camera_right"/>
|
||||
<child link="camera_laserscan_link_right"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_laserscan_link_right">
|
||||
</link>
|
||||
|
||||
<xacro:ultraSound name="left">
|
||||
<origin rpy="0 0.2618 ${PI/2}" xyz="-0.0535 0.0826 0.00868"/>
|
||||
</xacro:ultraSound>
|
||||
|
||||
<xacro:ultraSound name="right">
|
||||
<origin rpy="0 0.2618 ${-PI/2}" xyz="-0.0535 -0.0826 0.00868"/>
|
||||
</xacro:ultraSound>
|
||||
|
||||
<xacro:ultraSound name="face">
|
||||
<origin rpy="0 0 0" xyz="0.2747 0.0 -0.0088"/>
|
||||
</xacro:ultraSound>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="leg_transmission" params="name">
|
||||
|
||||
<transmission name="${name}_hip_tran">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="${name}_hip_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="${name}_hip_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="${name}_thigh_tran">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="${name}_thigh_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="${name}_thigh_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
<transmission name="${name}_calf_tran">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="${name}_calf_joint">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="${name}_calf_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="ultraSound" params="name *origin">
|
||||
<joint name="ultraSound_joint_${name}" type="fixed">
|
||||
<xacro:insert_block name="origin"/>
|
||||
<parent link="trunk"/>
|
||||
<child link="ultraSound_${name}"/>
|
||||
</joint>
|
||||
<link name="ultraSound_${name}">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<box size=".001 .001 .001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://go1_description/meshes/ultraSound.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
|
||||
<inertial>
|
||||
<mass value="1e-5" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
|
||||
</inertial>
|
||||
</link>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
|
@ -1,61 +0,0 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(unitree_legged_real)
|
||||
|
||||
add_compile_options(-std=c++11)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
geometry_msgs
|
||||
unitree_legged_msgs
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${Boost_INCLUDE_DIR}
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "-O3")
|
||||
|
||||
if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1")
|
||||
include_directories($ENV{ALIENGO_SDK_PATH}/include)
|
||||
link_directories($ENV{ALIENGO_SDK_PATH}/lib)
|
||||
string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so)
|
||||
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
|
||||
|
||||
add_definitions(-DSDK3_1)
|
||||
|
||||
add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp)
|
||||
target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
elseif( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_2")
|
||||
include_directories($ENV{UNITREE_LEGGED_SDK_PATH}/include)
|
||||
link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib)
|
||||
string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so)
|
||||
set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
|
||||
|
||||
add_definitions(-DSDK3_2)
|
||||
|
||||
add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
|
||||
target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
endif()
|
||||
|
||||
add_executable(position_lcm src/exe/position_mode.cpp)
|
||||
target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(position_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(velocity_lcm src/exe/velocity_mode.cpp)
|
||||
target_link_libraries(velocity_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(velocity_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(torque_lcm src/exe/torque_mode.cpp)
|
||||
target_link_libraries(torque_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(torque_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_executable(walk_lcm src/exe/walk_mode.cpp)
|
||||
target_link_libraries(walk_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
|
@ -1,338 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#ifndef _CONVERT_H_
|
||||
#define _CONVERT_H_
|
||||
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include <unitree_legged_msgs/MotorCmd.h>
|
||||
#include <unitree_legged_msgs/MotorState.h>
|
||||
#include <unitree_legged_msgs/IMU.h>
|
||||
|
||||
#ifdef SDK3_1
|
||||
#include "aliengo_sdk/aliengo_sdk.hpp"
|
||||
|
||||
unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){
|
||||
unitree_legged_msgs::IMU ros;
|
||||
ros.quaternion[0] = lcm.quaternion[0];
|
||||
ros.quaternion[1] = lcm.quaternion[1];
|
||||
ros.quaternion[2] = lcm.quaternion[2];
|
||||
ros.quaternion[3] = lcm.quaternion[3];
|
||||
ros.gyroscope[0] = lcm.gyroscope[0];
|
||||
ros.gyroscope[1] = lcm.gyroscope[1];
|
||||
ros.gyroscope[2] = lcm.gyroscope[2];
|
||||
ros.accelerometer[0] = lcm.acceleration[0];
|
||||
ros.accelerometer[1] = lcm.acceleration[1];
|
||||
ros.accelerometer[2] = lcm.acceleration[2];
|
||||
// ros.rpy[0] = lcm.rpy[0];
|
||||
// ros.rpy[1] = lcm.rpy[1];
|
||||
// ros.rpy[2] = lcm.rpy[2];
|
||||
ros.temperature = lcm.temp;
|
||||
return ros;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){
|
||||
unitree_legged_msgs::MotorState ros;
|
||||
ros.mode = lcm.mode;
|
||||
ros.q = lcm.position;
|
||||
ros.dq = lcm.velocity;
|
||||
ros.ddq = 0;
|
||||
ros.tauEst = lcm.torque;
|
||||
ros.q_raw = 0;
|
||||
ros.dq_raw = 0;
|
||||
ros.ddq_raw = 0;
|
||||
ros.temperature = lcm.temperature;
|
||||
return ros;
|
||||
}
|
||||
|
||||
aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){
|
||||
aliengo::MotorCmd lcm;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.position = ros.q;
|
||||
lcm.velocity = ros.dq;
|
||||
lcm.positionStiffness = ros.Kp;
|
||||
lcm.velocityStiffness = ros.Kd;
|
||||
lcm.torque = ros.tau;
|
||||
return lcm;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){
|
||||
unitree_legged_msgs::LowState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = 0;
|
||||
ros.robotID = 0;
|
||||
ros.SN = 0;
|
||||
ros.bandWidth = 0;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
for(int i = 0; i < 20; i++){
|
||||
ros.motorState[i] = ToRos(lcm.motorState[i]);
|
||||
}
|
||||
for(int i = 0; i < 4; i++){
|
||||
ros.footForce[i] = lcm.footForce[i];
|
||||
ros.footForceEst[i] = 0;
|
||||
}
|
||||
ros.tick = lcm.tick;
|
||||
for(int i = 0; i < 40; i++){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
}
|
||||
ros.crc = lcm.crc;
|
||||
|
||||
return ros;
|
||||
}
|
||||
|
||||
aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){
|
||||
aliengo::LowCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
for(int i = 0; i < 20; i++){
|
||||
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
|
||||
}
|
||||
for(int i = 0; i < 4; i++){
|
||||
lcm.led[i].r = ros.led[i].r;
|
||||
lcm.led[i].g = ros.led[i].g;
|
||||
lcm.led[i].b = ros.led[i].b;
|
||||
}
|
||||
for(int i = 0; i < 40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
}
|
||||
lcm.crc = ros.crc;
|
||||
return lcm;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){
|
||||
unitree_legged_msgs::HighState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = 0;
|
||||
ros.robotID = 0;
|
||||
ros.SN = 0;
|
||||
ros.bandWidth = 0;
|
||||
ros.mode = lcm.mode;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
ros.forwardSpeed = lcm.forwardSpeed;
|
||||
ros.sideSpeed = lcm.sideSpeed;
|
||||
ros.rotateSpeed = lcm.rotateSpeed;
|
||||
ros.bodyHeight = lcm.bodyHeight;
|
||||
ros.updownSpeed = lcm.updownSpeed;
|
||||
ros.forwardPosition = lcm.forwardPosition.x;
|
||||
ros.sidePosition = lcm.sidePosition.y;
|
||||
for(int i = 0; i < 4; i++){
|
||||
ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
|
||||
ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
|
||||
ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
|
||||
ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
|
||||
ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
|
||||
ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
|
||||
ros.footForce[i] = lcm.footForce[i];
|
||||
ros.footForceEst[i] = 0;
|
||||
}
|
||||
ros.tick = lcm.tick;
|
||||
for(int i = 0; i<40; i++){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
}
|
||||
ros.reserve = 0;
|
||||
ros.crc = lcm.crc;
|
||||
return ros;
|
||||
}
|
||||
|
||||
aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){
|
||||
aliengo::HighCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.forwardSpeed = ros.forwardSpeed;
|
||||
lcm.sideSpeed = ros.sideSpeed;
|
||||
lcm.rotateSpeed = ros.rotateSpeed;
|
||||
lcm.bodyHeight = ros.bodyHeight;
|
||||
lcm.footRaiseHeight = ros.footRaiseHeight;
|
||||
lcm.yaw = ros.yaw;
|
||||
lcm.pitch = ros.pitch;
|
||||
lcm.roll = ros.roll;
|
||||
for(int i = 0; i < 4; i++){
|
||||
lcm.led[i].r = ros.led[i].r;
|
||||
lcm.led[i].g = ros.led[i].g;
|
||||
lcm.led[i].b = ros.led[i].b;
|
||||
}
|
||||
for(int i = 0; i < 40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
}
|
||||
lcm.crc = ros.crc;
|
||||
return lcm;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SDK3_2
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
|
||||
unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
|
||||
{
|
||||
unitree_legged_msgs::IMU ros;
|
||||
ros.quaternion[0] = lcm.quaternion[0];
|
||||
ros.quaternion[1] = lcm.quaternion[1];
|
||||
ros.quaternion[2] = lcm.quaternion[2];
|
||||
ros.quaternion[3] = lcm.quaternion[3];
|
||||
ros.gyroscope[0] = lcm.gyroscope[0];
|
||||
ros.gyroscope[1] = lcm.gyroscope[1];
|
||||
ros.gyroscope[2] = lcm.gyroscope[2];
|
||||
ros.accelerometer[0] = lcm.accelerometer[0];
|
||||
ros.accelerometer[1] = lcm.accelerometer[1];
|
||||
ros.accelerometer[2] = lcm.accelerometer[2];
|
||||
// ros.rpy[0] = lcm.rpy[0];
|
||||
// ros.rpy[1] = lcm.rpy[1];
|
||||
// ros.rpy[2] = lcm.rpy[2];
|
||||
ros.temperature = lcm.temperature;
|
||||
return ros;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
|
||||
{
|
||||
unitree_legged_msgs::MotorState ros;
|
||||
ros.mode = lcm.mode;
|
||||
ros.q = lcm.q;
|
||||
ros.dq = lcm.dq;
|
||||
ros.ddq = lcm.ddq;
|
||||
ros.tauEst = lcm.tauEst;
|
||||
ros.q_raw = lcm.q_raw;
|
||||
ros.dq_raw = lcm.dq_raw;
|
||||
ros.ddq_raw = lcm.ddq_raw;
|
||||
ros.temperature = lcm.temperature;
|
||||
ros.reserve[0] = lcm.reserve[0];
|
||||
ros.reserve[1] = lcm.reserve[1];
|
||||
return ros;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::MotorCmd lcm;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.q = ros.q;
|
||||
lcm.dq = ros.dq;
|
||||
lcm.tau = ros.tau;
|
||||
lcm.Kp = ros.Kp;
|
||||
lcm.Kd = ros.Kd;
|
||||
lcm.reserve[0] = ros.reserve[0];
|
||||
lcm.reserve[1] = ros.reserve[1];
|
||||
lcm.reserve[2] = ros.reserve[2];
|
||||
return lcm;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
|
||||
{
|
||||
unitree_legged_msgs::LowState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = lcm.commVersion;
|
||||
ros.robotID = lcm.robotID;
|
||||
ros.SN = lcm.SN;
|
||||
ros.bandWidth = lcm.bandWidth;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
for(int i = 0; i<20; i++){
|
||||
ros.motorState[i] = ToRos(lcm.motorState[i]);
|
||||
}
|
||||
for(int i = 0; i<4; i++){
|
||||
ros.footForce[i] = lcm.footForce[i];
|
||||
ros.footForceEst[i] = lcm.footForceEst[i];
|
||||
}
|
||||
ros.tick = lcm.tick;
|
||||
for(int i = 0; i<40; i++){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
}
|
||||
ros.reserve = lcm.reserve;
|
||||
ros.crc = lcm.crc;
|
||||
return ros;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::LowCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
lcm.commVersion = ros.commVersion;
|
||||
lcm.robotID = ros.robotID;
|
||||
lcm.SN = ros.SN;
|
||||
lcm.bandWidth = ros.bandWidth;
|
||||
for(int i = 0; i<20; i++){
|
||||
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
|
||||
}
|
||||
for(int i = 0; i<4; i++){
|
||||
lcm.led[i].r = ros.led[i].r;
|
||||
lcm.led[i].g = ros.led[i].g;
|
||||
lcm.led[i].b = ros.led[i].b;
|
||||
}
|
||||
for(int i = 0; i<40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
}
|
||||
lcm.reserve = ros.reserve;
|
||||
lcm.crc = ros.crc;
|
||||
return lcm;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
|
||||
{
|
||||
unitree_legged_msgs::HighState ros;
|
||||
ros.levelFlag = lcm.levelFlag;
|
||||
ros.commVersion = lcm.commVersion;
|
||||
ros.robotID = lcm.robotID;
|
||||
ros.SN = lcm.SN;
|
||||
ros.bandWidth = lcm.bandWidth;
|
||||
ros.mode = lcm.mode;
|
||||
ros.imu = ToRos(lcm.imu);
|
||||
ros.forwardSpeed = lcm.forwardSpeed;
|
||||
ros.sideSpeed = lcm.sideSpeed;
|
||||
ros.rotateSpeed = lcm.rotateSpeed;
|
||||
ros.bodyHeight = lcm.bodyHeight;
|
||||
ros.updownSpeed = lcm.updownSpeed;
|
||||
ros.forwardPosition = lcm.forwardPosition;
|
||||
ros.sidePosition = lcm.sidePosition;
|
||||
for(int i = 0; i<4; i++){
|
||||
ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
|
||||
ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
|
||||
ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
|
||||
ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
|
||||
ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
|
||||
ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
|
||||
ros.footForce[i] = lcm.footForce[i];
|
||||
ros.footForceEst[i] = lcm.footForceEst[i];
|
||||
}
|
||||
ros.tick = lcm.tick;
|
||||
for(int i = 0; i<40; i++){
|
||||
ros.wirelessRemote[i] = lcm.wirelessRemote[i];
|
||||
}
|
||||
ros.reserve = lcm.reserve;
|
||||
ros.crc = lcm.crc;
|
||||
return ros;
|
||||
}
|
||||
|
||||
UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType)
|
||||
{
|
||||
UNITREE_LEGGED_SDK::HighCmd lcm;
|
||||
lcm.levelFlag = ros.levelFlag;
|
||||
lcm.commVersion = ros.commVersion;
|
||||
lcm.robotID = ros.robotID;
|
||||
lcm.SN = ros.SN;
|
||||
lcm.bandWidth = ros.bandWidth;
|
||||
lcm.mode = ros.mode;
|
||||
lcm.forwardSpeed = ros.forwardSpeed;
|
||||
lcm.sideSpeed = ros.sideSpeed;
|
||||
lcm.rotateSpeed = ros.rotateSpeed;
|
||||
lcm.bodyHeight = ros.bodyHeight;
|
||||
lcm.footRaiseHeight = ros.footRaiseHeight;
|
||||
lcm.yaw = ros.yaw;
|
||||
lcm.pitch = ros.pitch;
|
||||
lcm.roll = ros.roll;
|
||||
for(int i = 0; i<4; i++){
|
||||
lcm.led[i].r = ros.led[i].r;
|
||||
lcm.led[i].g = ros.led[i].g;
|
||||
lcm.led[i].b = ros.led[i].b;
|
||||
}
|
||||
for(int i = 0; i<40; i++){
|
||||
lcm.wirelessRemote[i] = ros.wirelessRemote[i];
|
||||
lcm.AppRemote[i] = ros.AppRemote[i];
|
||||
}
|
||||
lcm.reserve = ros.reserve;
|
||||
lcm.crc = ros.crc;
|
||||
return lcm;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
|
|
@ -1,8 +0,0 @@
|
|||
#!/bin/bash
|
||||
|
||||
sudo ifconfig lo multicast
|
||||
sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo
|
||||
|
||||
sudo ifconfig enx000ec6612921 down
|
||||
sudo ifconfig enx000ec6612921 up 192.168.123.162 netmask 255.255.255.0
|
||||
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
<launch>
|
||||
<arg name="rname" default="a1"/>
|
||||
<arg name="ctrl_level" default="highlevel"/>
|
||||
<arg name="firmwork" default="3_2"/>
|
||||
|
||||
<node pkg="unitree_legged_real" type="lcm_server_$(arg firmwork)" name="node_lcm_server"
|
||||
respawn="false" output="screen" args="$(arg rname) $(arg ctrl_level)" />
|
||||
|
||||
<param name="robot_name" value="$(arg rname)"/>
|
||||
<param name="control_level" value="$(arg ctrl_level)"/>
|
||||
<param name="firmwork" value="$(arg firmwork)"/>
|
||||
</launch>
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>unitree_legged_real</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The unitree_legged_real package</description>
|
||||
|
||||
<maintainer email="laikago@unitree.cc">unitree</maintainer>
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<depend>unitree_legged_msgs</depend>
|
||||
<depend>eigen</depend>
|
||||
|
||||
</package>
|
||||
|
|
@ -1,190 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
#ifdef SDK3_1
|
||||
using namespace aliengo;
|
||||
#endif
|
||||
#ifdef SDK3_2
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
#endif
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
double jointLinearInterpolation(double initPos, double targetPos, double rate)
|
||||
{
|
||||
double p;
|
||||
rate = std::min(std::max(rate, 0.0), 1.0);
|
||||
p = initPos*(1-rate) + targetPos*rate;
|
||||
return p;
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime = 0;
|
||||
int rate_count = 0;
|
||||
int sin_count = 0;
|
||||
float qInit[3]={0};
|
||||
float qDes[3]={0};
|
||||
float sin_mid_q[3] = {0.0, 1.2, -2.0};
|
||||
float Kp[3] = {0};
|
||||
float Kd[3] = {0};
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
bool initiated_flag = false; // initiate need time
|
||||
int count = 0;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
SendLowROS.motorCmd[i].q = PosStopF; // 禁止位置环
|
||||
SendLowROS.motorCmd[i].Kp = 0;
|
||||
SendLowROS.motorCmd[i].dq = VelStopF; // 禁止速度环
|
||||
SendLowROS.motorCmd[i].Kd = 0;
|
||||
SendLowROS.motorCmd[i].tau = 0;
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("FR_2 position: %f\n", RecvLowROS.motorState[FR_2].q);
|
||||
|
||||
if(initiated_flag == true){
|
||||
motiontime++;
|
||||
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
// printf("%d\n", motiontime);
|
||||
// printf("%d %f %f %f\n", FR_0, RecvLowROS.motorState[FR_0].q, RecvLowROS.motorState[FR_1].q, RecvLowROS.motorState[FR_2].q);
|
||||
// printf("%f %f \n", RecvLowROS.motorState[FR_0].mode, RecvLowROS.motorState[FR_1].mode);
|
||||
if( motiontime >= 0){
|
||||
// first, get record initial position
|
||||
// if( motiontime >= 100 && motiontime < 500){
|
||||
if( motiontime >= 0 && motiontime < 10){
|
||||
qInit[0] = RecvLowROS.motorState[FR_0].q;
|
||||
qInit[1] = RecvLowROS.motorState[FR_1].q;
|
||||
qInit[2] = RecvLowROS.motorState[FR_2].q;
|
||||
}
|
||||
if( motiontime >= 10 && motiontime < 400){
|
||||
// printf("%f %f %f\n", );
|
||||
rate_count++;
|
||||
double rate = rate_count/200.0; // needs count to 200
|
||||
Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0;
|
||||
Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0;
|
||||
|
||||
qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
|
||||
qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
|
||||
qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
|
||||
}
|
||||
double sin_joint1, sin_joint2;
|
||||
// last, do sine wave
|
||||
if( motiontime >= 1700){
|
||||
sin_count++;
|
||||
sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
|
||||
sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0);
|
||||
qDes[0] = sin_mid_q[0];
|
||||
qDes[1] = sin_mid_q[1];
|
||||
qDes[2] = sin_mid_q[2] + sin_joint2;
|
||||
// qDes[2] = sin_mid_q[2];
|
||||
}
|
||||
|
||||
SendLowROS.motorCmd[FR_0].q = qDes[0];
|
||||
SendLowROS.motorCmd[FR_0].dq = 0;
|
||||
SendLowROS.motorCmd[FR_0].Kp = Kp[0];
|
||||
SendLowROS.motorCmd[FR_0].Kd = Kd[0];
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
|
||||
SendLowROS.motorCmd[FR_1].q = qDes[1];
|
||||
SendLowROS.motorCmd[FR_1].dq = 0;
|
||||
SendLowROS.motorCmd[FR_1].Kp = Kp[1];
|
||||
SendLowROS.motorCmd[FR_1].Kd = Kd[1];
|
||||
SendLowROS.motorCmd[FR_1].tau = 0.0f;
|
||||
|
||||
SendLowROS.motorCmd[FR_2].q = qDes[2];
|
||||
SendLowROS.motorCmd[FR_2].dq = 0;
|
||||
SendLowROS.motorCmd[FR_2].Kp = Kp[2];
|
||||
SendLowROS.motorCmd[FR_2].Kd = Kd[2];
|
||||
SendLowROS.motorCmd[FR_2].tau = 0.0f;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
|
||||
count++;
|
||||
if(count > 10){
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "position_ros_mode");
|
||||
std::string firmwork;
|
||||
ros::param::get("/firmwork", firmwork);
|
||||
|
||||
#ifdef SDK3_1
|
||||
aliengo::Control control(aliengo::LOWLEVEL);
|
||||
aliengo::LCM roslcm;
|
||||
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
|
||||
#ifdef SDK3_2
|
||||
std::string robot_name;
|
||||
UNITREE_LEGGED_SDK::LeggedType rname;
|
||||
ros::param::get("/robot_name", robot_name);
|
||||
if(strcasecmp(robot_name.c_str(), "A1") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
|
||||
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
|
||||
|
||||
// UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
|
||||
// UNITREE_LEGGED_SDK::InitEnvironment();
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1,114 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
#ifdef SDK3_1
|
||||
using namespace aliengo;
|
||||
#endif
|
||||
#ifdef SDK3_2
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
#endif
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime=0;
|
||||
float torque = 0;
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime++;
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("FL_1 position: %f\n", RecvLowROS.motorState[FL_1].q);
|
||||
|
||||
// gravity compensation
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
if( motiontime >= 500){
|
||||
torque = (0 - RecvLowROS.motorState[FL_1].q)*10.0f + (0 - RecvLowROS.motorState[FL_1].dq)*1.0f;
|
||||
if(torque > 5.0f) torque = 5.0f;
|
||||
if(torque < -5.0f) torque = -5.0f;
|
||||
|
||||
SendLowROS.motorCmd[FL_1].q = PosStopF;
|
||||
SendLowROS.motorCmd[FL_1].dq = VelStopF;
|
||||
SendLowROS.motorCmd[FL_1].Kp = 0;
|
||||
SendLowROS.motorCmd[FL_1].Kd = 0;
|
||||
SendLowROS.motorCmd[FL_1].tau = torque;
|
||||
}
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "torque_ros_mode");
|
||||
std::string firmwork;
|
||||
ros::param::get("/firmwork", firmwork);
|
||||
|
||||
#ifdef SDK3_1
|
||||
aliengo::Control control(aliengo::LOWLEVEL);
|
||||
aliengo::LCM roslcm;
|
||||
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
|
||||
#ifdef SDK3_2
|
||||
std::string robot_name;
|
||||
UNITREE_LEGGED_SDK::LeggedType rname;
|
||||
ros::param::get("/robot_name", robot_name);
|
||||
if(strcasecmp(robot_name.c_str(), "A1") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
|
||||
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
|
||||
|
||||
// UNITREE_LEGGED_SDK::InitEnvironment();
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1,118 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/LowCmd.h>
|
||||
#include <unitree_legged_msgs/LowState.h>
|
||||
#include "convert.h"
|
||||
|
||||
#ifdef SDK3_1
|
||||
using namespace aliengo;
|
||||
#endif
|
||||
#ifdef SDK3_2
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
#endif
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to LOW-level." << std::endl
|
||||
<< "Make sure the robot is hung up." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
long motiontime=0;
|
||||
float Kv = 1.5;
|
||||
float speed = 0;
|
||||
unsigned long int Tpi =0;
|
||||
TCmd SendLowLCM = {0};
|
||||
TState RecvLowLCM = {0};
|
||||
unitree_legged_msgs::LowCmd SendLowROS;
|
||||
unitree_legged_msgs::LowState RecvLowROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
SendLowROS.levelFlag = LOWLEVEL;
|
||||
for(int i = 0; i<12; i++){
|
||||
SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
|
||||
}
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime++;
|
||||
roslcm.Get(RecvLowLCM);
|
||||
RecvLowROS = ToRos(RecvLowLCM);
|
||||
printf("motorState[FL_2] velocity: %f\n", RecvLowROS.motorState[FL_2].dq);
|
||||
|
||||
// gravity compensation
|
||||
SendLowROS.motorCmd[FR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[FL_0].tau = +0.65f;
|
||||
SendLowROS.motorCmd[RR_0].tau = -0.65f;
|
||||
SendLowROS.motorCmd[RL_0].tau = +0.65f;
|
||||
|
||||
if( motiontime >= 500){
|
||||
SendLowROS.motorCmd[FL_2].q = PosStopF;
|
||||
SendLowROS.motorCmd[FL_2].dq = speed;
|
||||
SendLowROS.motorCmd[FL_2].Kp = 0;
|
||||
SendLowROS.motorCmd[FL_2].Kd = Kv;
|
||||
SendLowROS.motorCmd[FL_2].tau = 0.0f;
|
||||
Tpi++;
|
||||
// try 1 or 3
|
||||
speed = 3 * sin(4*M_PI*Tpi/1000.0);
|
||||
}
|
||||
SendLowLCM = ToLcm(SendLowROS, SendLowLCM);
|
||||
roslcm.Send(SendLowLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "velocity_ros_mode");
|
||||
std::string firmwork;
|
||||
ros::param::get("/firmwork", firmwork);
|
||||
|
||||
#ifdef SDK3_1
|
||||
aliengo::Control control(aliengo::LOWLEVEL);
|
||||
aliengo::LCM roslcm;
|
||||
mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
|
||||
#ifdef SDK3_2
|
||||
std::string robot_name;
|
||||
UNITREE_LEGGED_SDK::LeggedType rname;
|
||||
ros::param::get("/robot_name", robot_name);
|
||||
if(strcasecmp(robot_name.c_str(), "A1") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
|
||||
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
|
||||
|
||||
// UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
|
||||
// UNITREE_LEGGED_SDK::InitEnvironment();
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -1,162 +0,0 @@
|
|||
/************************************************************************
|
||||
Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
Use of this source code is governed by the MPL-2.0 license, see LICENSE.
|
||||
************************************************************************/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pthread.h>
|
||||
#include <string>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <unitree_legged_msgs/HighCmd.h>
|
||||
#include <unitree_legged_msgs/HighState.h>
|
||||
#include "convert.h"
|
||||
|
||||
#ifdef SDK3_1
|
||||
using namespace aliengo;
|
||||
#endif
|
||||
#ifdef SDK3_2
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
#endif
|
||||
|
||||
template<typename TLCM>
|
||||
void* update_loop(void* param)
|
||||
{
|
||||
TLCM *data = (TLCM *)param;
|
||||
while(ros::ok){
|
||||
data->Recv();
|
||||
usleep(2000);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename TCmd, typename TState, typename TLCM>
|
||||
int mainHelper(int argc, char *argv[], TLCM &roslcm)
|
||||
{
|
||||
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
|
||||
<< "Make sure the robot is standing on the ground." << std::endl
|
||||
<< "Press Enter to continue..." << std::endl;
|
||||
std::cin.ignore();
|
||||
|
||||
ros::NodeHandle n;
|
||||
ros::Rate loop_rate(500);
|
||||
|
||||
// SetLevel(HIGHLEVEL);
|
||||
long motiontime = 0;
|
||||
TCmd SendHighLCM = {0};
|
||||
TState RecvHighLCM = {0};
|
||||
unitree_legged_msgs::HighCmd SendHighROS;
|
||||
unitree_legged_msgs::HighState RecvHighROS;
|
||||
|
||||
roslcm.SubscribeState();
|
||||
|
||||
pthread_t tid;
|
||||
pthread_create(&tid, NULL, update_loop<TLCM>, &roslcm);
|
||||
|
||||
while (ros::ok()){
|
||||
motiontime = motiontime+2;
|
||||
roslcm.Get(RecvHighLCM);
|
||||
RecvHighROS = ToRos(RecvHighLCM);
|
||||
// printf("%f\n", RecvHighROS.forwardSpeed);
|
||||
|
||||
SendHighROS.forwardSpeed = 0.0f;
|
||||
SendHighROS.sideSpeed = 0.0f;
|
||||
SendHighROS.rotateSpeed = 0.0f;
|
||||
SendHighROS.bodyHeight = 0.0f;
|
||||
|
||||
SendHighROS.mode = 0;
|
||||
SendHighROS.roll = 0;
|
||||
SendHighROS.pitch = 0;
|
||||
SendHighROS.yaw = 0;
|
||||
|
||||
if(motiontime>1000 && motiontime<1500){
|
||||
SendHighROS.mode = 1;
|
||||
// SendHighROS.roll = 0.3f;
|
||||
}
|
||||
|
||||
if(motiontime>1500 && motiontime<2000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.pitch = 0.3f;
|
||||
}
|
||||
|
||||
if(motiontime>2000 && motiontime<2500){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.yaw = 0.2f;
|
||||
}
|
||||
|
||||
if(motiontime>2500 && motiontime<3000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = -0.3f;
|
||||
}
|
||||
|
||||
if(motiontime>3000 && motiontime<3500){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = 0.3f;
|
||||
}
|
||||
|
||||
if(motiontime>3500 && motiontime<4000){
|
||||
SendHighROS.mode = 1;
|
||||
SendHighROS.bodyHeight = 0.0f;
|
||||
}
|
||||
|
||||
if(motiontime>4000 && motiontime<5000){
|
||||
SendHighROS.mode = 2;
|
||||
}
|
||||
|
||||
if(motiontime>5000 && motiontime<8500){
|
||||
SendHighROS.mode = 2;
|
||||
SendHighROS.forwardSpeed = 0.1f; // -1 ~ +1
|
||||
}
|
||||
|
||||
if(motiontime>8500 && motiontime<12000){
|
||||
SendHighROS.mode = 2;
|
||||
SendHighROS.forwardSpeed = -0.2f; // -1 ~ +1
|
||||
}
|
||||
|
||||
if(motiontime>12000 && motiontime<16000){
|
||||
SendHighROS.mode = 2;
|
||||
SendHighROS.rotateSpeed = 0.1f; // turn
|
||||
}
|
||||
|
||||
if(motiontime>16000 && motiontime<20000){
|
||||
SendHighROS.mode = 2;
|
||||
SendHighROS.rotateSpeed = -0.1f; // turn
|
||||
}
|
||||
|
||||
if(motiontime>20000 && motiontime<21000){
|
||||
SendHighROS.mode = 1;
|
||||
}
|
||||
|
||||
SendHighLCM = ToLcm(SendHighROS, SendHighLCM);
|
||||
roslcm.Send(SendHighLCM);
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
ros::init(argc, argv, "walk_ros_mode");
|
||||
std::string firmwork;
|
||||
ros::param::get("/firmwork", firmwork);
|
||||
|
||||
#ifdef SDK3_1
|
||||
aliengo::Control control(aliengo::HIGHLEVEL);
|
||||
aliengo::LCM roslcm;
|
||||
mainHelper<aliengo::HighCmd, aliengo::HighState, aliengo::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
|
||||
#ifdef SDK3_2
|
||||
std::string robot_name;
|
||||
UNITREE_LEGGED_SDK::LeggedType rname;
|
||||
ros::param::get("/robot_name", robot_name);
|
||||
if(strcasecmp(robot_name.c_str(), "A1") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::A1;
|
||||
else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
|
||||
rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
|
||||
|
||||
// UNITREE_LEGGED_SDK::InitEnvironment();
|
||||
UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL);
|
||||
mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
|
||||
#endif
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue