mirror of
https://github.com/davesarmoury/GLaDOS.git
synced 2025-11-04 14:49:45 +08:00
brought in z1_description. Started adding to bringup.
This commit is contained in:
parent
abbfa70b8b
commit
5b7f8f9b53
3
glados_bringup/launch/bringup.launch
Normal file
3
glados_bringup/launch/bringup.launch
Normal file
@ -0,0 +1,3 @@
|
||||
<launch>
|
||||
<include file="$(find glados_bringup)/launch/zed.launch"/>
|
||||
</launch>
|
||||
34
glados_bringup/launch/zed.launch
Normal file
34
glados_bringup/launch/zed.launch
Normal file
@ -0,0 +1,34 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="svo_file" default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
|
||||
<arg name="stream" default="" /> <!-- <arg name="stream" default="<ip_address>:<port>"> -->
|
||||
|
||||
<arg name="node_name" default="zed_node" />
|
||||
<arg name="camera_model" default="zed2i" />
|
||||
|
||||
<arg name="camera_name" default="zed2i" />
|
||||
|
||||
<arg name="base_frame" default="world" />
|
||||
|
||||
<arg name="cam_pos_x" default="-0.5" /> <!-- Position respect to base frame (i.e. "base_link) -->
|
||||
<arg name="cam_pos_y" default="0.0" /> <!-- Position respect to base frame (i.e. "base_link) -->
|
||||
<arg name="cam_pos_z" default="0.5" /> <!-- Position respect to base frame (i.e. "base_link) -->
|
||||
<arg name="cam_roll" default="3.14159" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
|
||||
<arg name="cam_pitch" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
|
||||
<arg name="cam_yaw" default="0.0" /> <!-- Orientation respect to base frame (i.e. "base_link) -->
|
||||
|
||||
<arg name="camera_id" default="0" />
|
||||
<arg name="gpu_id" default="-1" />
|
||||
|
||||
<node name="$(arg node_name)" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
|
||||
<rosparam file="$(find glados_bringup)/params/common.yaml" command="load" />
|
||||
<rosparam file="$(find glados_bringup)/params/$(arg camera_model).yaml" command="load" />
|
||||
|
||||
<param name="general/camera_name" value="$(arg camera_name)" />
|
||||
<param name="general/base_frame" value="$(arg base_frame)" />
|
||||
<param name="svo_file" value="$(arg svo_file)" />
|
||||
<param name="stream" value="$(arg stream)" />
|
||||
<param name="general/zed_id" value="$(arg camera_id)" />
|
||||
<param name="general/gpu_id" value="$(arg gpu_id)" />
|
||||
</node>
|
||||
</launch>
|
||||
89
glados_bringup/params/common.yaml
Normal file
89
glados_bringup/params/common.yaml
Normal file
@ -0,0 +1,89 @@
|
||||
# params/common.yaml
|
||||
# Common parameters to Stereolabs ZED and ZED mini cameras
|
||||
---
|
||||
|
||||
# Dynamic parameters cannot have a namespace
|
||||
brightness: 4 # Dynamic
|
||||
contrast: 4 # Dynamic
|
||||
hue: 0 # Dynamic
|
||||
saturation: 4 # Dynamic
|
||||
sharpness: 4 # Dynamic
|
||||
gamma: 8 # Dynamic - Requires SDK >=v3.1
|
||||
auto_exposure_gain: true # Dynamic
|
||||
gain: 100 # Dynamic - works only if `auto_exposure_gain` is false
|
||||
exposure: 100 # Dynamic - works only if `auto_exposure_gain` is false
|
||||
auto_whitebalance: true # Dynamic
|
||||
whitebalance_temperature: 42 # Dynamic - works only if `auto_whitebalance` is false
|
||||
depth_confidence: 50 # Dynamic
|
||||
depth_texture_conf: 100 # Dynamic
|
||||
point_cloud_freq: 10.0 # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
|
||||
|
||||
general:
|
||||
camera_name: zed # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file)
|
||||
zed_id: 0
|
||||
serial_number: 0
|
||||
gpu_id: -1
|
||||
base_frame: 'base_link' # must be equal to the frame_id used in the URDF file
|
||||
sdk_verbose: 1 # Set verbose level of the ZED SDK
|
||||
svo_compression: 2 # `0`: LOSSLESS, `1`: AVCHD, `2`: HEVC
|
||||
self_calib: true # enable/disable self calibration at starting
|
||||
camera_flip: 'AUTO' # camera flip mode: 'OFF', 'ON', 'AUTO'
|
||||
pub_resolution: 'NATIVE' # The resolution used for output. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission
|
||||
pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM'
|
||||
pub_frame_rate: 15.0 # frequency of publishing of video and depth data (see SDK API "InitParameters::grab_compute_capping_fps")
|
||||
svo_realtime: true # if true the input SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting
|
||||
region_of_interest: '[]' # A polygon defining the ROI where the ZED SDK perform the processing ignoring the rest. Coordinates must be normalized to '1.0' to be resolution independent.
|
||||
|
||||
depth:
|
||||
depth_mode: 'ULTRA' # 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL'
|
||||
depth_stabilization: 1 # [0-100] - 0: Disabled
|
||||
openni_depth_mode: false # 'false': 32bit float meter units, 'true': 16bit uchar millimeter units
|
||||
|
||||
pos_tracking:
|
||||
pos_tracking_enabled: false # True to enable positional tracking from start
|
||||
pos_tracking_mode: 'STANDARD' # Matches the ZED SDK setting: 'QUALITY', 'STANDARD' -> Use 'QUALITY' with 'ULTRA' depth mode for improved outdoors performance
|
||||
set_gravity_as_origin: true # If 'true' align the positional tracking world to imu gravity measurement. Keep the yaw from the user initial pose.
|
||||
imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
|
||||
publish_tf: false # publish `odom -> base_link` TF
|
||||
publish_map_tf: false # publish `map -> odom` TF
|
||||
map_frame: 'map' # main frame
|
||||
odometry_frame: 'odom' # odometry frame
|
||||
area_memory_db_path: '' # file loaded when the node starts to restore the "known visual features" map.
|
||||
save_area_memory_db_on_exit: false # save the "known visual features" map when the node is correctly closed to the path indicated by `area_memory_db_path`
|
||||
area_memory: true # Enable to detect loop closure
|
||||
floor_alignment: false # Enable to automatically calculate camera/floor offset
|
||||
initial_base_pose: [0.0,0.0,0.0, 0.0,0.0,0.0] # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]
|
||||
init_odom_with_first_valid_pose: true # Enable to initialize the odometry with the first valid pose
|
||||
path_pub_rate: 2.0 # Camera trajectory publishing frequency
|
||||
path_max_count: -1 # use '-1' for unlimited path size
|
||||
two_d_mode: false # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero
|
||||
fixed_z_value: 0.00 # Value to be used for Z coordinate if `two_d_mode` is true
|
||||
depth_min_range: 0.0 # Set this value for removing fixed zones of the robot in the FoV of the camerafrom the visual odometry evaluation
|
||||
set_as_static: false # If 'true' the camera will be static and not move in the environment
|
||||
|
||||
mapping:
|
||||
mapping_enabled: false # True to enable mapping and fused point cloud publication
|
||||
resolution: 0.05 # maps resolution in meters [0.01f, 0.2f]
|
||||
max_mapping_range: -1 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
|
||||
fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud
|
||||
clicked_point_topic: '/clicked_point' # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection
|
||||
|
||||
sensors:
|
||||
sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame
|
||||
max_pub_rate: 400. # max frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
|
||||
publish_imu_tf: true # publish `IMU -> <cam_name>_left_camera_frame` TF
|
||||
|
||||
object_detection:
|
||||
od_enabled: true # True to enable Object Detection [not available for ZED]
|
||||
model: 'MULTI_CLASS_BOX_ACCURATE' # 'MULTI_CLASS_BOX_FAST', 'MULTI_CLASS_BOX_MEDIUM', 'MULTI_CLASS_BOX_ACCURATE', 'PERSON_HEAD_BOX_FAST', 'PERSON_HEAD_BOX_ACCURATE'
|
||||
max_range: 15. # Maximum detection range
|
||||
allow_reduced_precision_inference: true # Allow inference to run at a lower precision to improve runtime and memory usage
|
||||
prediction_timeout: 0.5 # During this time [sec], the object will have OK state even if it is not detected. Set this parameter to 0 to disable SDK predictions
|
||||
object_tracking_enabled: true # Enable/disable the tracking of the detected objects
|
||||
mc_people: true # Enable/disable the detection of persons for 'MULTI_CLASS_BOX_X' models
|
||||
mc_vehicle: false # Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX_X' models
|
||||
mc_bag: false # Enable/disable the detection of bags for 'MULTI_CLASS_BOX_X' models
|
||||
mc_animal: false # Enable/disable the detection of animals for 'MULTI_CLASS_BOX_X' models
|
||||
mc_electronics: false # Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX_X' models
|
||||
mc_fruit_vegetable: false # Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX_X' models
|
||||
mc_sport: false # Enable/disable the detection of sport-related objects for 'MULTI_CLASS_BOX_X' models
|
||||
12
glados_bringup/params/zed2i.yaml
Normal file
12
glados_bringup/params/zed2i.yaml
Normal file
@ -0,0 +1,12 @@
|
||||
# params/zed2i.yaml
|
||||
# Parameters for Stereolabs ZED 2i camera
|
||||
---
|
||||
|
||||
general:
|
||||
camera_model: 'zed2i'
|
||||
grab_resolution: 'HD1080' # 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
|
||||
grab_frame_rate: 30 # Frequency of frame grabbing for internal SDK operations
|
||||
|
||||
depth:
|
||||
min_depth: 0.3 # Min: 0.3, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory
|
||||
max_depth: 15.0 # Max: 40.0
|
||||
File diff suppressed because one or more lines are too long
@ -98,7 +98,7 @@ def main() -> None:
|
||||
if sound_stream is not None:
|
||||
sound_stream(resp.audio)
|
||||
|
||||
filename = text.translate(str.maketrans('', '', string.punctuation)) + ".wav"
|
||||
filename = "/home/davesarmoury/" + text.translate(str.maketrans('', '', string.punctuation)) + ".wav"
|
||||
filename = filename.replace(" ", "_")
|
||||
out_f = wave.open(filename, 'wb')
|
||||
out_f.setnchannels(nchannels)
|
||||
|
||||
13
z1_description/CMakeLists.txt
Normal file
13
z1_description/CMakeLists.txt
Normal file
@ -0,0 +1,13 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(z1_description)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
urdf
|
||||
xacro
|
||||
)
|
||||
|
||||
catkin_package()
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
40
z1_description/config/robot_control.yaml
Normal file
40
z1_description/config/robot_control.yaml
Normal file
@ -0,0 +1,40 @@
|
||||
z1_gazebo:
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 1000
|
||||
|
||||
Joint01_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint1
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
Joint02_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint2
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
Joint03_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint3
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
Joint04_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint4
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
Joint05_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint5
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
Joint06_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: joint6
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
|
||||
gripper_controller:
|
||||
type: unitree_legged_control/UnitreeJointController
|
||||
joint: jointGripper
|
||||
pid: {p: 300.0, i: 0.0, d: 5.0}
|
||||
234
z1_description/launch/setting.rviz
Normal file
234
z1_description/launch/setting.rviz
Normal file
@ -0,0 +1,234 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
- /RobotModel1/Links1
|
||||
- /TF1
|
||||
- /TF1/Frames1
|
||||
Splitter Ratio: 0.6125907897949219
|
||||
Tree Height: 719
|
||||
- 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: ""
|
||||
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
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
gripperMover:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
gripperStator:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link00:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link01:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link02:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link03:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link04:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link05:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link06:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: false
|
||||
gripperMover:
|
||||
Value: true
|
||||
gripperStator:
|
||||
Value: false
|
||||
link00:
|
||||
Value: true
|
||||
link01:
|
||||
Value: false
|
||||
link02:
|
||||
Value: true
|
||||
link03:
|
||||
Value: true
|
||||
link04:
|
||||
Value: true
|
||||
link05:
|
||||
Value: true
|
||||
link06:
|
||||
Value: true
|
||||
world:
|
||||
Value: true
|
||||
Marker Alpha: 1
|
||||
Marker Scale: 0.5
|
||||
Name: TF
|
||||
Show Arrows: false
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
world:
|
||||
link00:
|
||||
link01:
|
||||
link02:
|
||||
link03:
|
||||
link04:
|
||||
link05:
|
||||
link06:
|
||||
gripperStator:
|
||||
gripperMover:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 255; 255; 255
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
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: 0.9349431395530701
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.7853981852531433
|
||||
Focal Point:
|
||||
X: -0.06341491639614105
|
||||
Y: -0.17587876319885254
|
||||
Z: 0.08242372423410416
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.06539830565452576
|
||||
Target Frame: <Fixed Frame>
|
||||
Yaw: 1.5285662412643433
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000044f0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
||||
24
z1_description/launch/z1_rviz.launch
Normal file
24
z1_description/launch/z1_rviz.launch
Normal file
@ -0,0 +1,24 @@
|
||||
<launch>
|
||||
|
||||
<!-- <arg name="user_debug" default="false"/> -->
|
||||
<arg name="UnitreeGripperYN" default="true"/>
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find z1_description)/xacro/robot.xacro'
|
||||
UnitreeGripper:=$(arg UnitreeGripperYN)"/>
|
||||
|
||||
<!-- for higher robot_state_publisher average rate-->
|
||||
<!-- <param name="rate" value="1000"/> -->
|
||||
|
||||
<!-- send fake joint values -->
|
||||
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui">
|
||||
<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 z1_description)/launch/setting.rviz"/>
|
||||
|
||||
</launch>
|
||||
BIN
z1_description/meshes/collision/z1_GripperMover.STL
Normal file
BIN
z1_description/meshes/collision/z1_GripperMover.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_GripperStator.STL
Normal file
BIN
z1_description/meshes/collision/z1_GripperStator.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link00.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link00.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link01.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link01.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link02.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link02.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link03.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link03.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link04.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link04.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link05.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link05.STL
Normal file
Binary file not shown.
BIN
z1_description/meshes/collision/z1_Link06.STL
Normal file
BIN
z1_description/meshes/collision/z1_Link06.STL
Normal file
Binary file not shown.
2340
z1_description/meshes/visual/z1_GripperMover.dae
Normal file
2340
z1_description/meshes/visual/z1_GripperMover.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_GripperStator.dae
Normal file
2340
z1_description/meshes/visual/z1_GripperStator.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link00.dae
Normal file
2340
z1_description/meshes/visual/z1_Link00.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link01.dae
Normal file
2340
z1_description/meshes/visual/z1_Link01.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link02.dae
Normal file
2340
z1_description/meshes/visual/z1_Link02.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link03.dae
Normal file
2340
z1_description/meshes/visual/z1_Link03.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link04.dae
Normal file
2340
z1_description/meshes/visual/z1_Link04.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link05.dae
Normal file
2340
z1_description/meshes/visual/z1_Link05.dae
Normal file
File diff suppressed because one or more lines are too long
2340
z1_description/meshes/visual/z1_Link06.dae
Normal file
2340
z1_description/meshes/visual/z1_Link06.dae
Normal file
File diff suppressed because one or more lines are too long
19
z1_description/package.xml
Normal file
19
z1_description/package.xml
Normal file
@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>z1_description</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The z1_description package</description>
|
||||
|
||||
<maintainer email="laikago@unitree.cc">unitree</maintainer>
|
||||
|
||||
<license>TODO</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>urdf</build_depend>
|
||||
<build_depend>xacro</build_depend>
|
||||
<build_export_depend>urdf</build_export_depend>
|
||||
<build_export_depend>xacro</build_export_depend>
|
||||
<exec_depend>urdf</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
</package>
|
||||
149
z1_description/xacro/const.xacro
Normal file
149
z1_description/xacro/const.xacro
Normal file
@ -0,0 +1,149 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
|
||||
<!-- Link 00 -->
|
||||
<xacro:property name="L00_Mass" value="0.47247481"/>
|
||||
<xacro:property name="L00_ComX" value="-0.00334984"/>
|
||||
<xacro:property name="L00_ComY" value="-0.00013615"/>
|
||||
<xacro:property name="L00_ComZ" value="0.02495843"/>
|
||||
<xacro:property name="L00_Ixx" value="0.00037937"/>
|
||||
<xacro:property name="L00_Ixy" value="0.00000035"/>
|
||||
<xacro:property name="L00_Ixz" value="0.00001037"/>
|
||||
<xacro:property name="L00_Iyy" value="0.00041521"/>
|
||||
<xacro:property name="L00_Iyz" value="0.00000099"/>
|
||||
<xacro:property name="L00_Izz" value="0.00053066"/>
|
||||
|
||||
<!-- Link 01 -->
|
||||
<xacro:property name="L01_Mass" value="0.67332551"/>
|
||||
<xacro:property name="L01_ComX" value="0.00000247"/>
|
||||
<xacro:property name="L01_ComY" value="-0.00025198"/>
|
||||
<xacro:property name="L01_ComZ" value="0.02317169"/>
|
||||
<xacro:property name="L01_Ixx" value="0.00128328"/>
|
||||
<xacro:property name="L01_Ixy" value="0.00000006"/>
|
||||
<xacro:property name="L01_Ixz" value="0.00000040"/>
|
||||
<xacro:property name="L01_Iyy" value="0.00071931"/>
|
||||
<xacro:property name="L01_Iyz" value="-0.00000050"/>
|
||||
<xacro:property name="L01_Izz" value="0.00083936"/>
|
||||
|
||||
<!-- Link 02 -->
|
||||
<xacro:property name="L02_Mass" value="1.19132258"/>
|
||||
<xacro:property name="L02_ComX" value="-0.11012601"/>
|
||||
<xacro:property name="L02_ComY" value="0.00240029"/>
|
||||
<xacro:property name="L02_ComZ" value="0.00158266"/>
|
||||
<xacro:property name="L02_Ixx" value="0.00102138"/>
|
||||
<xacro:property name="L02_Ixy" value="-0.00062358"/>
|
||||
<xacro:property name="L02_Ixz" value="-0.00000513"/>
|
||||
<xacro:property name="L02_Iyy" value="0.02429457"/>
|
||||
<xacro:property name="L02_Iyz" value="0.00000210"/>
|
||||
<xacro:property name="L02_Izz" value="0.02466114"/>
|
||||
|
||||
<!-- Link 03 -->
|
||||
<xacro:property name="L03_Mass" value="0.83940874"/>
|
||||
<xacro:property name="L03_ComX" value="0.10609208"/>
|
||||
<xacro:property name="L03_ComY" value="-0.00541815"/>
|
||||
<xacro:property name="L03_ComZ" value="0.03476383"/>
|
||||
<xacro:property name="L03_Ixx" value="0.00108061"/>
|
||||
<xacro:property name="L03_Ixy" value="0.00008669"/>
|
||||
<xacro:property name="L03_Ixz" value="0.00208102"/>
|
||||
<xacro:property name="L03_Iyy" value="0.00954238"/>
|
||||
<xacro:property name="L03_Iyz" value="0.00001332"/>
|
||||
<xacro:property name="L03_Izz" value="0.00886621"/>
|
||||
|
||||
<!-- Link 04 -->
|
||||
<xacro:property name="L04_Mass" value="0.56404563"/>
|
||||
<xacro:property name="L04_ComX" value="0.04366681"/>
|
||||
<xacro:property name="L04_ComY" value="0.00364738"/>
|
||||
<xacro:property name="L04_ComZ" value="-0.00170192"/>
|
||||
<xacro:property name="L04_Ixx" value="0.00031576"/>
|
||||
<xacro:property name="L04_Ixy" value="-0.00008130"/>
|
||||
<xacro:property name="L04_Ixz" value="-0.00004091"/>
|
||||
<xacro:property name="L04_Iyy" value="0.00092996"/>
|
||||
<xacro:property name="L04_Iyz" value="0.00000596"/>
|
||||
<xacro:property name="L04_Izz" value="0.00097912"/>
|
||||
|
||||
<!-- Link 05 -->
|
||||
<xacro:property name="L05_Mass" value="0.38938492"/>
|
||||
<xacro:property name="L05_ComX" value="0.03121533"/>
|
||||
<xacro:property name="L05_ComY" value="0.0"/>
|
||||
<xacro:property name="L05_ComZ" value="0.00646316"/>
|
||||
<xacro:property name="L05_Ixx" value="0.00017605"/>
|
||||
<xacro:property name="L05_Ixy" value="-0.00000040"/>
|
||||
<xacro:property name="L05_Ixz" value="-0.00005689"/>
|
||||
<xacro:property name="L05_Iyy" value="0.00055896"/>
|
||||
<xacro:property name="L05_Iyz" value="0.00000013"/>
|
||||
<xacro:property name="L05_Izz" value="0.00053860"/>
|
||||
|
||||
<!-- Link 06 -->
|
||||
<xacro:property name="L06_Mass" value="0.28875807"/>
|
||||
<xacro:property name="L06_ComX" value="0.02415690"/>
|
||||
<xacro:property name="L06_ComY" value="-0.00017355"/>
|
||||
<xacro:property name="L06_ComZ" value="-0.00143876"/>
|
||||
<xacro:property name="L06_Ixx" value="0.00018328"/>
|
||||
<xacro:property name="L06_Ixy" value="-0.00000122"/>
|
||||
<xacro:property name="L06_Ixz" value="-0.00000054"/>
|
||||
<xacro:property name="L06_Iyy" value="0.00014750"/>
|
||||
<xacro:property name="L06_Iyz" value="-0.00000008"/>
|
||||
<xacro:property name="L06_Izz" value="0.00014680"/>
|
||||
|
||||
<!-- Link Gripper Stator -->
|
||||
<xacro:property name="GripperStator_Mass" value="0.52603655"/>
|
||||
<xacro:property name="GripperStator_ComX" value="0.04764427"/>
|
||||
<xacro:property name="GripperStator_ComY" value="-0.00035819"/>
|
||||
<xacro:property name="GripperStator_ComZ" value="-0.00249162"/>
|
||||
<xacro:property name="GripperStator_Ixx" value="0.00038683"/>
|
||||
<xacro:property name="GripperStator_Ixy" value="0.00000359"/>
|
||||
<xacro:property name="GripperStator_Ixz" value="-0.00007662"/>
|
||||
<xacro:property name="GripperStator_Iyy" value="0.00068614"/>
|
||||
<xacro:property name="GripperStator_Iyz" value="-0.00000209"/>
|
||||
<xacro:property name="GripperStator_Izz" value="0.00066293"/>
|
||||
|
||||
<!-- Link Gripper Mover -->
|
||||
<xacro:property name="GripperMover_Mass" value="0.27621302"/>
|
||||
<xacro:property name="GripperMover_ComX" value="0.01320633"/>
|
||||
<xacro:property name="GripperMover_ComY" value="0.00476708"/>
|
||||
<xacro:property name="GripperMover_ComZ" value="0.00380534"/>
|
||||
<xacro:property name="GripperMover_Ixx" value="0.00017716"/>
|
||||
<xacro:property name="GripperMover_Ixy" value="-0.00001683"/>
|
||||
<xacro:property name="GripperMover_Ixz" value="0.00001786"/>
|
||||
<xacro:property name="GripperMover_Iyy" value="0.00026787"/>
|
||||
<xacro:property name="GripperMover_Iyz" value="-0.00000262"/>
|
||||
<xacro:property name="GripperMover_Izz" value="0.00035728"/>
|
||||
|
||||
<!-- joint limits -->
|
||||
<!-- <xacro:property name="jointDamping" value="3.0"/> -->
|
||||
<!-- <xacro:property name="jointFriction" value="2.0"/> -->
|
||||
<xacro:property name="jointDamping" value="1.0"/>
|
||||
<xacro:property name="jointFriction" value="1.0"/>
|
||||
|
||||
<xacro:property name="torqueMax" value="30.0"/>
|
||||
<xacro:property name="velocityMax" value="10"/>
|
||||
<xacro:property name="joint1_PositionMin" value="${-PI*150/180}"/>
|
||||
<xacro:property name="joint1_PositionMax" value="${PI*150/180}"/>
|
||||
<xacro:property name="joint2_PositionMin" value="0.0"/>
|
||||
<xacro:property name="joint2_PositionMax" value="${PI}"/>
|
||||
<xacro:property name="joint3_PositionMin" value="${-PI*274/180}"/>
|
||||
<xacro:property name="joint3_PositionMax" value="0.0"/>
|
||||
<xacro:property name="joint4_PositionMin" value="${-PI*100/180}"/>
|
||||
<xacro:property name="joint4_PositionMax" value="${PI*90/180}"/>
|
||||
<xacro:property name="joint5_PositionMin" value="${-PI*99/180}"/>
|
||||
<xacro:property name="joint5_PositionMax" value="${PI*99/180}"/>
|
||||
<xacro:property name="joint6_PositionMin" value="${-PI*160/180}"/>
|
||||
<xacro:property name="joint6_PositionMax" value="${PI*160/180}"/>
|
||||
<xacro:property name="Gripper_PositionMin" value="${-PI*90/180}"/>
|
||||
<xacro:property name="Gripper_PositionMax" value="0.0"/>
|
||||
|
||||
<!-- collision size -->
|
||||
<xacro:property name="motor_diameter" value="${0.065/2.0}"/>
|
||||
<xacro:property name="motor_height" value="0.051"/>
|
||||
<xacro:property name="arm1_diameter" value="${0.045/2.0}"/>
|
||||
<xacro:property name="arm1_height" value="0.235"/>
|
||||
<xacro:property name="arm2_diameter" value="${0.04/2.0}"/>
|
||||
<xacro:property name="arm2_height" value="0.126"/>
|
||||
<xacro:property name="gripperBox_X" value="0."/>
|
||||
<xacro:property name="gripperBox_Y" value="0."/>
|
||||
<xacro:property name="gripperBox_Z" value="0."/>
|
||||
</robot>
|
||||
24
z1_description/xacro/gazebo.xacro
Normal file
24
z1_description/xacro/gazebo.xacro
Normal file
@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot>
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/z1_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="link04">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="link05">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="link06">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
|
||||
|
||||
</robot>
|
||||
322
z1_description/xacro/robot.xacro
Normal file
322
z1_description/xacro/robot.xacro
Normal file
@ -0,0 +1,322 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:include filename="$(find z1_description)/xacro/const.xacro"/>
|
||||
<xacro:include filename="$(find z1_description)/xacro/gazebo.xacro"/>
|
||||
<xacro:include filename="$(find z1_description)/xacro/transmission.xacro"/>
|
||||
|
||||
<xacro:arg name="UnitreeGripper" default="false"/>
|
||||
|
||||
<link name="world"/>
|
||||
|
||||
<joint name="base_static_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="world"/>
|
||||
<child link="link00"/>
|
||||
</joint>
|
||||
|
||||
<link name="link00">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link00.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 ${motor_height/2.0}"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L00_ComX} ${L00_ComY} ${L00_ComZ}"/>
|
||||
<mass value="${L00_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L00_Ixx}" ixy="${L00_Ixy}" ixz="${L00_Ixz}"
|
||||
iyy="${L00_Iyy}" iyz="${L00_Iyz}"
|
||||
izz="${L00_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0585"/>
|
||||
<parent link="link00"/>
|
||||
<child link="link01"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint1_PositionMin}" upper="${joint1_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link01">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<geometry>
|
||||
<cylinder length="${2*motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision> -->
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L01_ComX} ${L01_ComY} ${L01_ComZ}"/>
|
||||
<mass value="${L01_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L01_Ixx}" ixy="${L01_Ixy}" ixz="${L01_Ixz}"
|
||||
iyy="${L01_Iyy}" iyz="${L01_Iyz}"
|
||||
izz="${L01_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint2" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.045"/>
|
||||
<parent link="link01"/>
|
||||
<child link="link02"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${2*jointDamping}" friction="${2*jointFriction}"/>
|
||||
<limit effort="${2*torqueMax}" velocity="${velocityMax}" lower="${joint2_PositionMin}" upper="${joint2_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link02">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link02.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${2.0*motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${arm1_height}" radius="${arm1_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="${-0.045-arm1_height/2.0} 0 0"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="${PI/2.0} 0 0" xyz="-0.35 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L02_ComX} ${L02_ComY} ${L02_ComZ}"/>
|
||||
<mass value="${L02_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L02_Ixx}" ixy="${L02_Ixy}" ixz="${L02_Ixz}"
|
||||
iyy="${L02_Iyy}" iyz="${L02_Iyz}"
|
||||
izz="${L02_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint3" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.35 0 0"/>
|
||||
<parent link="link02"/>
|
||||
<child link="link03"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint3_PositionMin}" upper="${joint3_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link03">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link03.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${arm2_height-0.01}" radius="${arm2_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="${0.065+arm2_height/2.0} 0 0.055"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.059" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/>
|
||||
<mass value="${L03_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L03_Ixx}" ixy="${L03_Ixy}" ixz="${L03_Ixz}"
|
||||
iyy="${L03_Iyy}" iyz="${L03_Iyz}"
|
||||
izz="${L03_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint4" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.218 0 0.057"/>
|
||||
<parent link="link03"/>
|
||||
<child link="link04"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint4_PositionMin}" upper="${joint4_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link04">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link04.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.073" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="${PI/2.0} 0 0" xyz="0.0472 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/>
|
||||
<mass value="${L04_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L04_Ixx}" ixy="${L04_Ixy}" ixz="${L04_Ixz}"
|
||||
iyy="${L04_Iyy}" iyz="${L04_Iyz}"
|
||||
izz="${L04_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint5" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.07 0.0 0.0"/>
|
||||
<parent link="link04"/>
|
||||
<child link="link05"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint5_PositionMin}" upper="${joint5_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link05">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link05.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L05_ComX} ${L05_ComY} ${L05_ComZ}"/>
|
||||
<mass value="${L05_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L05_Ixx}" ixy="${L05_Ixy}" ixz="${L05_Ixz}"
|
||||
iyy="${L05_Iyy}" iyz="${L05_Iyz}"
|
||||
izz="${L05_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint6" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0492 0.0 0.0"/>
|
||||
<parent link="link05"/>
|
||||
<child link="link06"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint6_PositionMin}" upper="${joint6_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="link06">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_Link06.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 ${PI/2.0} 0" xyz="${motor_height/2.0} 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${L06_ComX} ${L06_ComY} ${L06_ComZ}"/>
|
||||
<mass value="${L06_Mass}"/>
|
||||
<inertia
|
||||
ixx="${L06_Ixx}" ixy="${L06_Ixy}" ixz="${L06_Ixz}"
|
||||
iyy="${L06_Iyy}" iyz="${L06_Iyz}"
|
||||
izz="${L06_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<xacro:if value="$(arg UnitreeGripper)">
|
||||
<joint name="gripperStator" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.051 0.0 0.0"/>
|
||||
<parent link="link06"/>
|
||||
<child link="gripperStator"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripperStator">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_GripperStator.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/collision/z1_GripperStator.STL" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${GripperStator_ComX} ${GripperStator_ComY} ${GripperStator_ComZ}"/>
|
||||
<mass value="${GripperStator_Mass}"/>
|
||||
<inertia
|
||||
ixx="${GripperStator_Ixx}" ixy="${GripperStator_Ixy}" ixz="${GripperStator_Ixz}"
|
||||
iyy="${GripperStator_Iyy}" iyz="${GripperStator_Iyz}"
|
||||
izz="${GripperStator_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="jointGripper" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.049 0.0 0"/>
|
||||
<parent link="gripperStator"/>
|
||||
<child link="gripperMover"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="${jointDamping}" friction="${jointFriction}"/>
|
||||
<limit effort="${torqueMax}" velocity="${velocityMax}" lower="${Gripper_PositionMin}" upper="${Gripper_PositionMax}"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripperMover">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/visual/z1_GripperMover.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/collision/z1_GripperMover.STL" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="${GripperMover_ComX} ${GripperMover_ComY} ${GripperMover_ComZ}"/>
|
||||
<mass value="${GripperMover_Mass}"/>
|
||||
<inertia
|
||||
ixx="${GripperMover_Ixx}" ixy="${GripperMover_Ixy}" ixz="${GripperMover_Ixz}"
|
||||
iyy="${GripperMover_Iyy}" iyz="${GripperMover_Iyz}"
|
||||
izz="${GripperMover_Izz}"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<xacro:motorTransmission name="Gripper"/>
|
||||
|
||||
<gazebo reference="gripperStator">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="gripperMover">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
|
||||
<xacro:motorTransmission name="1"/>
|
||||
<xacro:motorTransmission name="2"/>
|
||||
<xacro:motorTransmission name="3"/>
|
||||
<xacro:motorTransmission name="4"/>
|
||||
<xacro:motorTransmission name="5"/>
|
||||
<xacro:motorTransmission name="6"/>
|
||||
|
||||
</robot>
|
||||
20
z1_description/xacro/transmission.xacro
Normal file
20
z1_description/xacro/transmission.xacro
Normal file
@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="motorTransmission" params="name">
|
||||
|
||||
<transmission name="JointTrans${name}">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint${name}">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator${name}">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
283
z1_description/xacro/z1.urdf
Normal file
283
z1_description/xacro/z1.urdf
Normal file
@ -0,0 +1,283 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from robot.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="z1_description">
|
||||
<!-- ros_control plugin -->
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
|
||||
<robotNamespace>/z1_gazebo</robotNamespace>
|
||||
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="link04">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="link05">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
<gazebo reference="link06">
|
||||
<self_collide>true</self_collide>
|
||||
</gazebo>
|
||||
<link name="world"/>
|
||||
<joint name="base_static_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="world"/>
|
||||
<child link="link00"/>
|
||||
</joint>
|
||||
<link name="link00">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link00.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.051" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0255"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.00334984 -0.00013615 0.02495843"/>
|
||||
<mass value="0.47247481"/>
|
||||
<inertia ixx="0.00037937" ixy="3.5e-07" ixz="1.037e-05" iyy="0.00041521" iyz="9.9e-07" izz="0.00053066"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint1" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.0585"/>
|
||||
<parent link="link00"/>
|
||||
<child link="link01"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30.0" lower="-2.6179938779914944" upper="2.6179938779914944" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link01">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link01.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<!-- <collision>
|
||||
<geometry>
|
||||
<cylinder length="${2*motor_height}" radius="${motor_diameter}"/>
|
||||
</geometry>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
</collision> -->
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="2.47e-06 -0.00025198 0.02317169"/>
|
||||
<mass value="0.67332551"/>
|
||||
<inertia ixx="0.00128328" ixy="6e-08" ixz="4e-07" iyy="0.00071931" iyz="-5e-07" izz="0.00083936"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint2" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0 0 0.045"/>
|
||||
<parent link="link01"/>
|
||||
<child link="link02"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="2.0" friction="2.0"/>
|
||||
<limit effort="60.0" lower="0.0" upper="3.141592653589793" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link02">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link02.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.102" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.235" radius="0.0225"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="-0.16249999999999998 0 0"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.051" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="-0.35 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-0.11012601 0.00240029 0.00158266"/>
|
||||
<mass value="1.19132258"/>
|
||||
<inertia ixx="0.00102138" ixy="-0.00062358" ixz="-5.13e-06" iyy="0.02429457" iyz="2.1e-06" izz="0.02466114"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint3" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="-0.35 0 0"/>
|
||||
<parent link="link02"/>
|
||||
<child link="link03"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30.0" lower="-4.782202150464463" upper="0.0" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link03">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link03.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.116" radius="0.02"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0.128 0 0.055"/>
|
||||
</collision>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.059" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0.2205 0 0.055"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.10609208 -0.00541815 0.03476383"/>
|
||||
<mass value="0.83940874"/>
|
||||
<inertia ixx="0.00108061" ixy="8.669e-05" ixz="0.00208102" iyy="0.00954238" iyz="1.332e-05" izz="0.00886621"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint4" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.218 0 0.057"/>
|
||||
<parent link="link03"/>
|
||||
<child link="link04"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30.0" lower="-1.7453292519943295" upper="1.5707963267948966" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link04">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link04.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.073" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="1.5707963267948966 0 0" xyz="0.0472 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.04366681 0.00364738 -0.00170192"/>
|
||||
<mass value="0.56404563"/>
|
||||
<inertia ixx="0.00031576" ixy="-8.13e-05" ixz="-4.091e-05" iyy="0.00092996" iyz="5.96e-06" izz="0.00097912"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint5" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.07 0.0 0.0"/>
|
||||
<parent link="link04"/>
|
||||
<child link="link05"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30.0" lower="-1.7278759594743864" upper="1.7278759594743864" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link05">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link05.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.03121533 0.0 0.00646316"/>
|
||||
<mass value="0.38938492"/>
|
||||
<inertia ixx="0.00017605" ixy="-4e-07" ixz="-5.689e-05" iyy="0.00055896" iyz="1.3e-07" izz="0.0005386"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint6" type="revolute">
|
||||
<origin rpy="0 0 0" xyz="0.0492 0.0 0.0"/>
|
||||
<parent link="link05"/>
|
||||
<child link="link06"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
<dynamics damping="1.0" friction="1.0"/>
|
||||
<limit effort="30.0" lower="-2.792526803190927" upper="2.792526803190927" velocity="10"/>
|
||||
</joint>
|
||||
<link name="link06">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://z1_description/meshes/z1_Link06.dae" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.051" radius="0.0325"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.5707963267948966 0" xyz="0.0255 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0241569 -0.00017355 -0.00143876"/>
|
||||
<mass value="0.28875807"/>
|
||||
<inertia ixx="0.00018328" ixy="-1.22e-06" ixz="-5.4e-07" iyy="0.0001475" iyz="-8e-08" izz="0.0001468"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<transmission name="JointTrans1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="JointTrans2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="JointTrans3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="JointTrans4">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint4">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator4">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="JointTrans5">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint5">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator5">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="JointTrans6">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="Actuator6">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user