➕ Add moveit_config and robot description
This commit is contained in:
parent
7723d8f7bc
commit
89ba14c294
32 changed files with 2610 additions and 0 deletions
38
roboasm_moveit_config/CMakeLists.txt
Normal file
38
roboasm_moveit_config/CMakeLists.txt
Normal file
|
@ -0,0 +1,38 @@
|
|||
cmake_minimum_required(VERSION 3.5)
|
||||
project(roboasm_moveit_config)
|
||||
|
||||
# Default to C99
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
# Install directories
|
||||
install(DIRECTORY config launch srdf DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
ament_package()
|
35
roboasm_moveit_config/config/joint_limits.yml
Normal file
35
roboasm_moveit_config/config/joint_limits.yml
Normal file
|
@ -0,0 +1,35 @@
|
|||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
||||
|
||||
# For beginners, we downscale velocity and acceleration limits.
|
||||
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
||||
default_velocity_scaling_factor: 0.1
|
||||
default_acceleration_scaling_factor: 0.1
|
||||
|
||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||
joint_limits:
|
||||
joint1:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint3:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint4:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint5:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint6:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
4
roboasm_moveit_config/config/kinematics.yml
Normal file
4
roboasm_moveit_config/config/kinematics.yml
Normal file
|
@ -0,0 +1,4 @@
|
|||
group_name:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.005
|
||||
kinematics_solver_timeout: 0.05
|
203
roboasm_moveit_config/config/ompl_planning.yml
Normal file
203
roboasm_moveit_config/config/ompl_planning.yml
Normal file
|
@ -0,0 +1,203 @@
|
|||
planner_configs:
|
||||
SBLkConfigDefault:
|
||||
type: geometric::SBL
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
ESTkConfigDefault:
|
||||
type: geometric::EST
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
||||
LBKPIECEkConfigDefault:
|
||||
type: geometric::LBKPIECE
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
|
||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
||||
BKPIECEkConfigDefault:
|
||||
type: geometric::BKPIECE
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
|
||||
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
|
||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
||||
KPIECEkConfigDefault:
|
||||
type: geometric::KPIECE
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
||||
border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
|
||||
failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
|
||||
min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
|
||||
RRTkConfigDefault:
|
||||
type: geometric::RRT
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
||||
RRTConnectkConfigDefault:
|
||||
type: geometric::RRTConnect
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
RRTstarkConfigDefault:
|
||||
type: geometric::RRTstar
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
||||
delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
|
||||
TRRTkConfigDefault:
|
||||
type: geometric::TRRT
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
|
||||
max_states_failed: 10 # when to start increasing temp. default: 10
|
||||
temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
|
||||
min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
|
||||
init_temperature: 10e-6 # initial temperature. default: 10e-6
|
||||
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
||||
frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
|
||||
k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup()
|
||||
PRMkConfigDefault:
|
||||
type: geometric::PRM
|
||||
max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
|
||||
PRMstarkConfigDefault:
|
||||
type: geometric::PRMstar
|
||||
FMTkConfigDefault:
|
||||
type: geometric::FMT
|
||||
num_samples: 1000 # number of states that the planner should sample. default: 1000
|
||||
radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1
|
||||
nearest_k: 1 # use Knearest strategy. default: 1
|
||||
cache_cc: 1 # use collision checking cache. default: 1
|
||||
heuristics: 0 # activate cost to go heuristics. default: 0
|
||||
extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
|
||||
BFMTkConfigDefault:
|
||||
type: geometric::BFMT
|
||||
num_samples: 1000 # number of states that the planner should sample. default: 1000
|
||||
radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0
|
||||
nearest_k: 1 # use the Knearest strategy. default: 1
|
||||
balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
|
||||
optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
|
||||
heuristics: 1 # activates cost to go heuristics. default: 1
|
||||
cache_cc: 1 # use the collision checking cache. default: 1
|
||||
extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
|
||||
PDSTkConfigDefault:
|
||||
type: geometric::PDST
|
||||
STRIDEkConfigDefault:
|
||||
type: geometric::STRIDE
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
||||
use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
|
||||
degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
|
||||
max_degree: 18 # max degree of a node in the GNAT. default: 12
|
||||
min_degree: 12 # min degree of a node in the GNAT. default: 12
|
||||
max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6
|
||||
estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0
|
||||
min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2
|
||||
BiTRRTkConfigDefault:
|
||||
type: geometric::BiTRRT
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1
|
||||
init_temperature: 100 # initial temperature. default: 100
|
||||
frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
||||
frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
|
||||
cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
|
||||
LBTRRTkConfigDefault:
|
||||
type: geometric::LBTRRT
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
||||
epsilon: 0.4 # optimality approximation factor. default: 0.4
|
||||
BiESTkConfigDefault:
|
||||
type: geometric::BiEST
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
ProjESTkConfigDefault:
|
||||
type: geometric::ProjEST
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
|
||||
LazyPRMkConfigDefault:
|
||||
type: geometric::LazyPRM
|
||||
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
|
||||
LazyPRMstarkConfigDefault:
|
||||
type: geometric::LazyPRMstar
|
||||
SPARSkConfigDefault:
|
||||
type: geometric::SPARS
|
||||
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
|
||||
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
|
||||
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
|
||||
max_failures: 1000 # maximum consecutive failure limit. default: 1000
|
||||
SPARStwokConfigDefault:
|
||||
type: geometric::SPARStwo
|
||||
stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
|
||||
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
|
||||
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
|
||||
max_failures: 5000 # maximum consecutive failure limit. default: 5000
|
||||
TrajOptDefault:
|
||||
type: geometric::TrajOpt
|
||||
|
||||
panda_arm:
|
||||
planner_configs:
|
||||
- SBLkConfigDefault
|
||||
- ESTkConfigDefault
|
||||
- LBKPIECEkConfigDefault
|
||||
- BKPIECEkConfigDefault
|
||||
- KPIECEkConfigDefault
|
||||
- RRTkConfigDefault
|
||||
- RRTConnectkConfigDefault
|
||||
- RRTstarkConfigDefault
|
||||
- TRRTkConfigDefault
|
||||
- PRMkConfigDefault
|
||||
- PRMstarkConfigDefault
|
||||
- FMTkConfigDefault
|
||||
- BFMTkConfigDefault
|
||||
- PDSTkConfigDefault
|
||||
- STRIDEkConfigDefault
|
||||
- BiTRRTkConfigDefault
|
||||
- LBTRRTkConfigDefault
|
||||
- BiESTkConfigDefault
|
||||
- ProjESTkConfigDefault
|
||||
- LazyPRMkConfigDefault
|
||||
- LazyPRMstarkConfigDefault
|
||||
- SPARSkConfigDefault
|
||||
- SPARStwokConfigDefault
|
||||
- TrajOptDefault
|
||||
panda_arm_hand:
|
||||
planner_configs:
|
||||
- SBLkConfigDefault
|
||||
- ESTkConfigDefault
|
||||
- LBKPIECEkConfigDefault
|
||||
- BKPIECEkConfigDefault
|
||||
- KPIECEkConfigDefault
|
||||
- RRTkConfigDefault
|
||||
- RRTConnectkConfigDefault
|
||||
- RRTstarkConfigDefault
|
||||
- TRRTkConfigDefault
|
||||
- PRMkConfigDefault
|
||||
- PRMstarkConfigDefault
|
||||
- FMTkConfigDefault
|
||||
- BFMTkConfigDefault
|
||||
- PDSTkConfigDefault
|
||||
- STRIDEkConfigDefault
|
||||
- BiTRRTkConfigDefault
|
||||
- LBTRRTkConfigDefault
|
||||
- BiESTkConfigDefault
|
||||
- ProjESTkConfigDefault
|
||||
- LazyPRMkConfigDefault
|
||||
- LazyPRMstarkConfigDefault
|
||||
- SPARSkConfigDefault
|
||||
- SPARStwokConfigDefault
|
||||
- TrajOptDefault
|
||||
hand:
|
||||
planner_configs:
|
||||
- SBLkConfigDefault
|
||||
- ESTkConfigDefault
|
||||
- LBKPIECEkConfigDefault
|
||||
- BKPIECEkConfigDefault
|
||||
- KPIECEkConfigDefault
|
||||
- RRTkConfigDefault
|
||||
- RRTConnectkConfigDefault
|
||||
- RRTstarkConfigDefault
|
||||
- TRRTkConfigDefault
|
||||
- PRMkConfigDefault
|
||||
- PRMstarkConfigDefault
|
||||
- FMTkConfigDefault
|
||||
- BFMTkConfigDefault
|
||||
- PDSTkConfigDefault
|
||||
- STRIDEkConfigDefault
|
||||
- BiTRRTkConfigDefault
|
||||
- LBTRRTkConfigDefault
|
||||
- BiESTkConfigDefault
|
||||
- ProjESTkConfigDefault
|
||||
- LazyPRMkConfigDefault
|
||||
- LazyPRMstarkConfigDefault
|
||||
- SPARSkConfigDefault
|
||||
- SPARStwokConfigDefault
|
||||
- TrajOptDefault
|
288
roboasm_moveit_config/config/rasms.rviz
Normal file
288
roboasm_moveit_config/config/rasms.rviz
Normal file
|
@ -0,0 +1,288 @@
|
|||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 219
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/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
|
||||
- Acceleration_Scaling_Factor: 0.1
|
||||
Class: moveit_rviz_plugin/MotionPlanning
|
||||
Enabled: true
|
||||
Move Group Namespace: ""
|
||||
MoveIt_Allow_Approximate_IK: false
|
||||
MoveIt_Allow_External_Program: false
|
||||
MoveIt_Allow_Replanning: false
|
||||
MoveIt_Allow_Sensor_Positioning: false
|
||||
MoveIt_Planning_Attempts: 10
|
||||
MoveIt_Planning_Time: 5
|
||||
MoveIt_Use_Cartesian_Path: false
|
||||
MoveIt_Use_Constraint_Aware_IK: false
|
||||
MoveIt_Warehouse_Host: 127.0.0.1
|
||||
MoveIt_Warehouse_Port: 33829
|
||||
MoveIt_Workspace:
|
||||
Center:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Size:
|
||||
X: 2
|
||||
Y: 2
|
||||
Z: 2
|
||||
Name: MotionPlanning
|
||||
Planned Path:
|
||||
Color Enabled: false
|
||||
Interrupt Display: false
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Loop Animation: false
|
||||
Robot Alpha: 0.5
|
||||
Robot Color: 150; 50; 150
|
||||
Show Robot Collision: false
|
||||
Show Robot Visual: true
|
||||
Show Trail: false
|
||||
State Display Time: 0.05 s
|
||||
Trail Step Size: 1
|
||||
Trajectory Topic: /display_planned_path
|
||||
Planning Metrics:
|
||||
Payload: 1
|
||||
Show Joint Torques: false
|
||||
Show Manipulability: false
|
||||
Show Manipulability Index: false
|
||||
Show Weight Limit: false
|
||||
TextHeight: 0.07999999821186066
|
||||
Planning Request:
|
||||
Colliding Link Color: 255; 0; 0
|
||||
Goal State Alpha: 1
|
||||
Goal State Color: 250; 128; 0
|
||||
Interactive Marker Size: 0
|
||||
Joint Violation Color: 255; 0; 255
|
||||
Planning Group: roboasm_sgonov_arm
|
||||
Query Goal State: true
|
||||
Query Start State: false
|
||||
Show Workspace: false
|
||||
Start State Alpha: 1
|
||||
Start State Color: 0; 255; 0
|
||||
Planning Scene Topic: /monitored_planning_scene
|
||||
Robot Description: robot_description
|
||||
Scene Geometry:
|
||||
Scene Alpha: 0.8999999761581421
|
||||
Scene Color: 50; 230; 50
|
||||
Scene Display Time: 0.009999999776482582
|
||||
Show Scene Geometry: true
|
||||
Voxel Coloring: Z-Axis
|
||||
Voxel Rendering: Occupied Voxels
|
||||
Scene Robot:
|
||||
Attached Body Color: 150; 50; 150
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Robot Alpha: 1
|
||||
Show Robot Collision: false
|
||||
Show Robot Visual: true
|
||||
Value: true
|
||||
Velocity_Scaling_Factor: 0.1
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.8626145124435425
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -0.09316612780094147
|
||||
Y: -0.08907446265220642
|
||||
Z: 0.0930255576968193
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.8953979015350342
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.6453981399536133
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
"":
|
||||
collapsed: false
|
||||
" - Trajectory Slider":
|
||||
collapsed: false
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001f3000002f4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000166000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff01000001a9000001880000017d00ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001a2000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 225
|
||||
Y: 91
|
182
roboasm_moveit_config/config/robasm_sgonov.rviz
Normal file
182
roboasm_moveit_config/config/robasm_sgonov.rviz
Normal file
|
@ -0,0 +1,182 @@
|
|||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 787
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/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_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.4318417310714722
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.01542382501065731
|
||||
Y: -0.006365143693983555
|
||||
Z: 0.15774454176425934
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.5703980326652527
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.5603898763656616
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
27
roboasm_moveit_config/config/roboasm_controllers.yml
Normal file
27
roboasm_moveit_config/config/roboasm_controllers.yml
Normal file
|
@ -0,0 +1,27 @@
|
|||
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 1000 # Hz
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
position_trajectory_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
position_trajectory_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- joint1
|
||||
- joint2
|
||||
- joint3
|
||||
- joint4
|
||||
- joint5
|
||||
- joint6
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
state_publish_rate: 50.0
|
||||
action_monitor_rate: 20.0
|
||||
|
15
roboasm_moveit_config/config/roboasm_moveit_controllers.yml
Normal file
15
roboasm_moveit_config/config/roboasm_moveit_controllers.yml
Normal file
|
@ -0,0 +1,15 @@
|
|||
controller_names:
|
||||
- position_trajectory_controller
|
||||
|
||||
# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller
|
||||
position_trajectory_controller:
|
||||
action_ns: follow_joint_trajectory
|
||||
type: FollowJointTrajectory
|
||||
default: true
|
||||
joints:
|
||||
- joint1
|
||||
- joint2
|
||||
- joint3
|
||||
- joint4
|
||||
- joint5
|
||||
- joint6
|
161
roboasm_moveit_config/launch/roboasm_bringup.launch.py
Normal file
161
roboasm_moveit_config/launch/roboasm_bringup.launch.py
Normal file
|
@ -0,0 +1,161 @@
|
|||
from launch.actions.declare_launch_argument import DeclareLaunchArgument
|
||||
from launch.launch_description import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
|
||||
from launch.substitutions.launch_configuration import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
import xacro
|
||||
import os
|
||||
from ament_index_python import get_package_share_directory
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
# Evaluate frequently used variables
|
||||
model = LaunchConfiguration("model").perform(context)
|
||||
xacro_file = os.path.join(get_package_share_directory("roboasm_sgonov"),"urdf/","roboasm_sgonov.urdf.xacro")
|
||||
assert os.path.exists(xacro_file), "The xacro file of roboasm_sgonov doesnt exist"+str(xacro_file)
|
||||
robot_description = xacro.process_file(xacro_file)
|
||||
robot_description_content = robot_description.toxml()
|
||||
#print(robot_description_content)
|
||||
|
||||
|
||||
# Load robot description
|
||||
#robot_description_content = Command(
|
||||
# [
|
||||
# FindExecutable(name="xacro"), " ",
|
||||
# PathJoinSubstitution(
|
||||
# [FindPackageShare("roboasm_sgonov"), "urdf/{}.urdf.xacro".format(model)]
|
||||
# ), " "
|
||||
# "robot_name:=", LaunchConfiguration("robot_name"), " ",
|
||||
# "sim:=", LaunchConfiguration("sim")
|
||||
# ]
|
||||
#)
|
||||
|
||||
# Load controls
|
||||
control = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("roboasm_moveit_config"),
|
||||
"launch",
|
||||
"roboasm_control.launch.py"
|
||||
])
|
||||
), launch_arguments=[
|
||||
("robot_description", robot_description_content),
|
||||
("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
|
||||
("controller_configurations", LaunchConfiguration("controller_configurations")),
|
||||
("controller", LaunchConfiguration("controller")),
|
||||
("sim", LaunchConfiguration("sim"))
|
||||
]
|
||||
)
|
||||
|
||||
# Gazebo simulation
|
||||
simulation = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("roboasm_moveit_config"),
|
||||
"launch",
|
||||
"roboasm_simulation.launch.py"
|
||||
])
|
||||
),
|
||||
launch_arguments=[
|
||||
("robot_name", LaunchConfiguration("robot_name"))
|
||||
],
|
||||
condition=IfCondition(LaunchConfiguration("sim"))
|
||||
)
|
||||
|
||||
# Move group
|
||||
move_group = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("roboasm_moveit_config"),
|
||||
"launch",
|
||||
"roboasm_moveit_interface.launch.py"
|
||||
])
|
||||
),
|
||||
launch_arguments=[
|
||||
("robot_description", robot_description_content),
|
||||
("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")),
|
||||
("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")),
|
||||
("model", LaunchConfiguration("model")),
|
||||
("sim", LaunchConfiguration("sim"))
|
||||
]
|
||||
)
|
||||
|
||||
return [
|
||||
simulation,
|
||||
control,
|
||||
move_group
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Launch arguments
|
||||
launch_args = []
|
||||
|
||||
launch_args.append(DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value="roboasm_sgonov",
|
||||
description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
|
||||
))
|
||||
|
||||
launch_args.append(DeclareLaunchArgument(
|
||||
name="robot_name",
|
||||
default_value="rasms",
|
||||
description="Set robot name."
|
||||
))
|
||||
|
||||
launch_args.append(DeclareLaunchArgument(
|
||||
name="sim",
|
||||
default_value="true",
|
||||
description="Launch robot in simulation or on real setup."
|
||||
))
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller_configurations_package",
|
||||
default_value="roboasm_moveit_config",
|
||||
description="Package that contains controller configurations."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller_configurations",
|
||||
default_value="config/roboasm_controllers.yml",
|
||||
description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller",
|
||||
default_value="position_trajectory_controller",
|
||||
description="Robot controller."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="moveit_controller_configurations_package",
|
||||
default_value="roboasm_moveit_config",
|
||||
description="Package that contains MoveIt! controller configurations."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="moveit_controller_configurations",
|
||||
default_value="config/roboasm_moveit_controllers.yml",
|
||||
description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file."
|
||||
)
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
launch_args + [
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
96
roboasm_moveit_config/launch/roboasm_control.launch.py
Normal file
96
roboasm_moveit_config/launch/roboasm_control.launch.py
Normal file
|
@ -0,0 +1,96 @@
|
|||
from launch.launch_description import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.conditions import UnlessCondition
|
||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Launch arguments
|
||||
launch_args = []
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller_configurations_package",
|
||||
default_value="roboasm_moveit_config",
|
||||
description="Package that contains controller configurations."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller_configurations",
|
||||
default_value="config/roboasm_controllers.yml",
|
||||
description="Relative path to controller configurations YAML file."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="robot_description",
|
||||
description="Robot description XML file."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="controller",
|
||||
default_value="position_trajectory_controller",
|
||||
description="Robot controller."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="sim",
|
||||
default_value="true",
|
||||
description="Launch robot in simulation or on real setup."
|
||||
)
|
||||
)
|
||||
|
||||
# Configure robot_description
|
||||
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
|
||||
|
||||
# Load controllers from YAML configuration file
|
||||
controller_configurations = PathJoinSubstitution([
|
||||
FindPackageShare(LaunchConfiguration("controller_configurations_package")),
|
||||
LaunchConfiguration("controller_configurations")
|
||||
])
|
||||
|
||||
# Prepare controller manager and other required nodes
|
||||
controller_manager = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[robot_description, controller_configurations],
|
||||
output="screen",
|
||||
condition=UnlessCondition(LaunchConfiguration("sim"))
|
||||
)
|
||||
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[robot_description]
|
||||
)
|
||||
|
||||
joint_state_broadcaster = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
controller = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
launch_args + [
|
||||
controller_manager,
|
||||
robot_state_publisher,
|
||||
joint_state_broadcaster,
|
||||
controller
|
||||
])
|
202
roboasm_moveit_config/launch/roboasm_moveit_interface.launch.py
Normal file
202
roboasm_moveit_config/launch/roboasm_moveit_interface.launch.py
Normal file
|
@ -0,0 +1,202 @@
|
|||
import os
|
||||
import yaml
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
from ament_index_python import get_package_share_directory
|
||||
|
||||
|
||||
def load_yaml(package_name: str, file_path: str):
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolute_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolute_file_path, "r") as f:
|
||||
return yaml.safe_load(f)
|
||||
except EnvironmentError:
|
||||
return None
|
||||
|
||||
|
||||
def load_file(package_name: str, file_path: str) -> str:
|
||||
package_path = get_package_share_directory(package_name)
|
||||
absolut_file_path = os.path.join(package_path, file_path)
|
||||
|
||||
try:
|
||||
with open(absolut_file_path, "r") as f:
|
||||
return f.read()
|
||||
except EnvironmentError:
|
||||
return None
|
||||
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
# Evaluate frequently used variables
|
||||
model = LaunchConfiguration("model").perform(context)
|
||||
|
||||
# Configure robot_description
|
||||
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
|
||||
|
||||
# Robot semantics SRDF
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": load_file("roboasm_moveit_config", "srdf/{}.srdf".format(model))
|
||||
}
|
||||
|
||||
# Kinematics
|
||||
kinematics_yaml = load_yaml("roboasm_moveit_config", "config/kinematics.yml")
|
||||
|
||||
# Update group name
|
||||
kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"]
|
||||
del kinematics_yaml["group_name"]
|
||||
|
||||
# Joint limits
|
||||
robot_description_planning = {
|
||||
"robot_description_planning": PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("roboasm_moveit_config"),
|
||||
"config/joint_limits.yml"
|
||||
]
|
||||
)
|
||||
}
|
||||
|
||||
# Planning
|
||||
ompl_yaml = load_yaml("roboasm_moveit_config", "config/ompl_planning.yml")
|
||||
|
||||
planning = {
|
||||
"move_group": {
|
||||
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||
"request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
|
||||
"start_state_max_bounds_error": 0.1
|
||||
}
|
||||
}
|
||||
|
||||
# Trajectory execution
|
||||
trajectory_execution = {"allow_trajectory_execution": True,
|
||||
"moveit_manage_controllers": True}
|
||||
|
||||
# Controllers
|
||||
controllers_yaml = load_yaml(
|
||||
LaunchConfiguration("moveit_controller_configurations_package").perform(context),
|
||||
LaunchConfiguration("moveit_controller_configurations").perform(context)
|
||||
)
|
||||
|
||||
moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml,
|
||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}
|
||||
|
||||
# Planning scene
|
||||
planning_scene_monitor_parameters = {"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True}
|
||||
|
||||
# Time configuration
|
||||
use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}
|
||||
|
||||
# Prepare move group node
|
||||
move_group_node = Node(
|
||||
package="moveit_ros_move_group",
|
||||
executable="move_group",
|
||||
output="screen",
|
||||
arguments=["--ros-args"],
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
kinematics_yaml,
|
||||
robot_description_planning,
|
||||
ompl_yaml,
|
||||
planning,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
use_sim_time
|
||||
]
|
||||
)
|
||||
|
||||
robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[robot_description]
|
||||
)
|
||||
|
||||
spawn_controller = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner.py",
|
||||
arguments=["joint_state_broadcaster"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# RViz
|
||||
rviz_config = PathJoinSubstitution([FindPackageShare("roboasm_moveit_config"), "config/robasm_sgonov.rviz"])
|
||||
|
||||
rviz = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
kinematics_yaml,
|
||||
planning,
|
||||
use_sim_time
|
||||
],
|
||||
arguments=[
|
||||
'-d', ""#rviz_config
|
||||
]
|
||||
)
|
||||
|
||||
return [
|
||||
move_group_node,
|
||||
rviz,
|
||||
robot_state_publisher,
|
||||
spawn_controller
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Launch arguments
|
||||
launch_args = []
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="robot_description",
|
||||
description="Robot description XML file."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value="roboasm_sgonov",
|
||||
description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="moveit_controller_configurations_package",
|
||||
default_value="roboasm_moveit_config",
|
||||
description="Package that contains MoveIt! controller configurations."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(
|
||||
DeclareLaunchArgument(
|
||||
name="moveit_controller_configurations",
|
||||
default_value="config/roboasm_moveit_controllers.yml",
|
||||
description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
|
||||
)
|
||||
)
|
||||
|
||||
launch_args.append(DeclareLaunchArgument(
|
||||
name="sim",
|
||||
default_value="true",
|
||||
description="Launch robot in simulation or on real setup."
|
||||
))
|
||||
|
||||
return LaunchDescription(
|
||||
launch_args + [
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
49
roboasm_moveit_config/launch/roboasm_simulation.launch.py
Normal file
49
roboasm_moveit_config/launch/roboasm_simulation.launch.py
Normal file
|
@ -0,0 +1,49 @@
|
|||
from launch.launch_description import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# Launch arguments
|
||||
launch_args = []
|
||||
|
||||
launch_args.append(DeclareLaunchArgument(
|
||||
name="robot_name",
|
||||
default_value="roboasm_sgonov",
|
||||
description="Set robot name."
|
||||
))
|
||||
|
||||
# Launch Gazebo
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare("gazebo_ros"),
|
||||
"launch",
|
||||
"gazebo.launch.py"
|
||||
])
|
||||
)
|
||||
)
|
||||
|
||||
# Note: Environment variable GAZEBO_MODEL_PATH is extended as in
|
||||
# ROS2 control demos via environment hook https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
|
||||
# Also see https://colcon.readthedocs.io/en/released/developer/environment.html#dsv-files
|
||||
# Gazebo launch scripts append GAZEBO_MODEL_PATH with known paths, see https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ab1ae5c05eda62674b36df74eb3be8c93cdc8761/gazebo_ros/launch/gzclient.launch.py#L26
|
||||
spawn_entity = Node(
|
||||
package="gazebo_ros",
|
||||
executable="spawn_entity.py",
|
||||
arguments=[
|
||||
"-topic", "robot_description",
|
||||
"-entity", LaunchConfiguration("robot_name")
|
||||
],
|
||||
output="screen"
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
launch_args + [
|
||||
gazebo,
|
||||
spawn_entity
|
||||
])
|
28
roboasm_moveit_config/package.xml
Normal file
28
roboasm_moveit_config/package.xml
Normal file
|
@ -0,0 +1,28 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>roboasm_moveit_config</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="bill-finger@todo.todo">bill-finger</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>gazebo_ros</exec_depend>
|
||||
<exec_depend>gazebo_ros2_control</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>moveit_ros_move_group</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
41
roboasm_moveit_config/srdf/roboasm_sgonov.srdf
Normal file
41
roboasm_moveit_config/srdf/roboasm_sgonov.srdf
Normal file
|
@ -0,0 +1,41 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--This does not replace URDF, and is not an extension of URDF.
|
||||
This is a format for representing semantic information about the robot structure.
|
||||
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||
-->
|
||||
<robot name="roboasm_sgonov">
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="roboasm_sgonov_arm">
|
||||
<chain base_link="base" tip_link="link6"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state name="pose1" group="roboasm_sgonov_arm">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="0"/>
|
||||
<joint name="joint3" value="0.4339"/>
|
||||
<joint name="joint4" value="0"/>
|
||||
<joint name="joint5" value="0.9199"/>
|
||||
<joint name="joint6" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="zero_pose" group="roboasm_sgonov_arm">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="0"/>
|
||||
<joint name="joint3" value="0"/>
|
||||
<joint name="joint4" value="0"/>
|
||||
<joint name="joint5" value="0"/>
|
||||
<joint name="joint6" value="0"/>
|
||||
</group_state>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="base" link2="link1" reason="Adjacent"/>
|
||||
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
|
||||
<disable_collisions link1="link1" link2="link3" reason="Never"/>
|
||||
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
|
||||
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
|
||||
<disable_collisions link1="link3" link2="link5" reason="Never"/>
|
||||
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
|
||||
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
|
||||
</robot>
|
34
roboasm_sgonov/CMakeLists.txt
Normal file
34
roboasm_sgonov/CMakeLists.txt
Normal file
|
@ -0,0 +1,34 @@
|
|||
cmake_minimum_required(VERSION 3.5)
|
||||
project(roboasm_sgonov)
|
||||
|
||||
# Default to C99
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 99)
|
||||
endif()
|
||||
|
||||
# Default to C++14
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
# Install launch files.
|
||||
install(DIRECTORY
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
|
||||
install(DIRECTORY meshes DESTINATION share/${PROJECT_NAME})
|
||||
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
|
||||
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.dsv")
|
||||
ament_package()
|
1
roboasm_sgonov/config/joint_names_roboarm.yaml
Executable file
1
roboasm_sgonov/config/joint_names_roboarm.yaml
Executable file
|
@ -0,0 +1 @@
|
|||
controller_joint_names: ['', 'Joint1', 'Joint3', 'Joint4', 'Joint5', 'Joint6', 'Joint7', ]
|
182
roboasm_sgonov/config/robasm_sgonov.rviz
Normal file
182
roboasm_sgonov/config/robasm_sgonov.rviz
Normal file
|
@ -0,0 +1,182 @@
|
|||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 787
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/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_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 1.4318417310714722
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.01542382501065731
|
||||
Y: -0.006365143693983555
|
||||
Z: 0.15774454176425934
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.5703980326652527
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.5603898763656616
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1016
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1848
|
||||
X: 72
|
||||
Y: 27
|
102
roboasm_sgonov/launch/rviz.launch.py
Normal file
102
roboasm_sgonov/launch/rviz.launch.py
Normal file
|
@ -0,0 +1,102 @@
|
|||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Get the launch directory
|
||||
bringup_dir = get_package_share_directory('roboasm_sgonov')
|
||||
launch_dir = os.path.join(bringup_dir, 'launch')
|
||||
rviz_file = os.path.join(bringup_dir, 'config/robasm_sgonov.rviz')
|
||||
|
||||
# Launch configuration variables specific to simulation
|
||||
rviz_config_file = LaunchConfiguration('rviz_config_file')
|
||||
use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
|
||||
use_joint_state_pub = LaunchConfiguration('use_joint_state_pub')
|
||||
use_rviz = LaunchConfiguration('use_rviz')
|
||||
urdf_file= LaunchConfiguration('urdf_file')
|
||||
robot_description_content = Command(
|
||||
[
|
||||
FindExecutable(name="xacro"), " ",
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare("roboasm_sgonov"), "urdf/roboasm_sgonov.urdf.xacro"]
|
||||
), " "
|
||||
# "robot_name:=", LaunchConfiguration("robot_name"), " ",
|
||||
# "sim:=", LaunchConfiguration("sim")
|
||||
]
|
||||
)
|
||||
declare_rviz_config_file_cmd = DeclareLaunchArgument(
|
||||
'rviz_config_file',
|
||||
default_value=os.path.join(bringup_dir, 'config', 'robasm_sgonov.rviz'),
|
||||
description= rviz_file)
|
||||
declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
|
||||
'use_robot_state_pub',
|
||||
default_value='True',
|
||||
description='Whether to start the robot state publisher')
|
||||
declare_use_joint_state_pub_cmd = DeclareLaunchArgument(
|
||||
'use_joint_state_pub',
|
||||
default_value='True',
|
||||
description='Whether to start the joint state publisher')
|
||||
declare_use_rviz_cmd = DeclareLaunchArgument(
|
||||
'use_rviz',
|
||||
default_value='True',
|
||||
description='Whether to start RVIZ')
|
||||
|
||||
declare_urdf_cmd = DeclareLaunchArgument(
|
||||
'urdf_file',
|
||||
default_value=os.path.join(bringup_dir, 'urdf', 'roboasm_sgonov.urdf.xacro'),
|
||||
description='Whether to start RVIZ')
|
||||
|
||||
|
||||
start_robot_state_publisher_cmd = Node(
|
||||
condition=IfCondition(use_robot_state_pub),
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
#parameters=[{'use_sim_time': use_sim_time}],
|
||||
arguments=[robot_description_content])
|
||||
|
||||
start_joint_state_publisher_cmd = Node(
|
||||
condition=IfCondition(use_joint_state_pub),
|
||||
package='joint_state_publisher_gui',
|
||||
executable='joint_state_publisher_gui',
|
||||
name='joint_state_publisher_gui',
|
||||
output='screen',
|
||||
arguments=[robot_description_content])
|
||||
|
||||
rviz_cmd = Node(
|
||||
condition=IfCondition(use_rviz),
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
arguments=['-d', rviz_config_file],
|
||||
output='screen')
|
||||
|
||||
|
||||
# Create the launch description and populate
|
||||
ld = LaunchDescription()
|
||||
|
||||
# Declare the launch options
|
||||
ld.add_action(declare_rviz_config_file_cmd)
|
||||
ld.add_action(declare_urdf_cmd)
|
||||
ld.add_action(declare_use_robot_state_pub_cmd)
|
||||
ld.add_action(declare_use_joint_state_pub_cmd)
|
||||
ld.add_action(declare_use_rviz_cmd)
|
||||
|
||||
|
||||
# Add any conditioned actions
|
||||
ld.add_action(start_joint_state_publisher_cmd)
|
||||
ld.add_action(start_robot_state_publisher_cmd)
|
||||
ld.add_action(rviz_cmd)
|
||||
|
||||
return ld
|
BIN
roboasm_sgonov/meshes/base.stl
Executable file
BIN
roboasm_sgonov/meshes/base.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link1.stl
Executable file
BIN
roboasm_sgonov/meshes/link1.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link2.stl
Executable file
BIN
roboasm_sgonov/meshes/link2.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link3.stl
Executable file
BIN
roboasm_sgonov/meshes/link3.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link4.stl
Executable file
BIN
roboasm_sgonov/meshes/link4.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link5.stl
Executable file
BIN
roboasm_sgonov/meshes/link5.stl
Executable file
Binary file not shown.
BIN
roboasm_sgonov/meshes/link6.stl
Executable file
BIN
roboasm_sgonov/meshes/link6.stl
Executable file
Binary file not shown.
23
roboasm_sgonov/package.xml
Normal file
23
roboasm_sgonov/package.xml
Normal file
|
@ -0,0 +1,23 @@
|
|||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>roboasm_sgonov</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="bill-finger@todo.todo">bill-finger</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>xacro</exec_depend>
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
1
roboasm_sgonov/roboasm_sgonov.dsv
Normal file
1
roboasm_sgonov/roboasm_sgonov.dsv
Normal file
|
@ -0,0 +1 @@
|
|||
prepend-non-duplicate;GAZEBO_MODEL_PATH;share
|
55
roboasm_sgonov/urdf/rasms.control.xacro
Normal file
55
roboasm_sgonov/urdf/rasms.control.xacro
Normal file
|
@ -0,0 +1,55 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="rasms_hi">
|
||||
<!-- arg for control mode -->
|
||||
<ros2_control name="rasms_hi" type="system">
|
||||
<hardware>
|
||||
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||
</hardware>
|
||||
<!-- define joints and command/state interfaces for each joint -->
|
||||
<joint name="joint1">
|
||||
<command_interface name="position">
|
||||
<!-- better to use radians as min max first -->
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint2">
|
||||
<command_interface name="position">
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint3">
|
||||
<command_interface name="position">
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint4">
|
||||
<command_interface name="position">
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint5">
|
||||
<command_interface name="position">
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
<joint name="joint6">
|
||||
<command_interface name="position">
|
||||
<param name="min">-1</param>
|
||||
<param name="max"> 1</param>
|
||||
</command_interface>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
</xacro:macro>
|
||||
</robot>
|
69
roboasm_sgonov/urdf/rasms.gazebo.xacro
Normal file
69
roboasm_sgonov/urdf/rasms.gazebo.xacro
Normal file
|
@ -0,0 +1,69 @@
|
|||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
<xacro:macro name="rasms_gazebo">
|
||||
|
||||
<!-- ros_control-plugin -->
|
||||
<gazebo>
|
||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
||||
<parameters>$(find roboasm_moveit_config)/config/roboasm_controllers.yml</parameters>
|
||||
<robotNamespace>/rasms</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 0 -->
|
||||
<gazebo reference="base">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 1 -->
|
||||
<gazebo reference="link1">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 2 -->
|
||||
<gazebo reference="link2">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 3 -->
|
||||
<gazebo reference="link3">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 4 -->
|
||||
<gazebo reference="link4">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 5 -->
|
||||
<gazebo reference="link5">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
|
||||
<!-- link 6 -->
|
||||
<gazebo reference="link6">
|
||||
<mu1>0.2</mu1>
|
||||
<mu2>0.2</mu2>
|
||||
<material>Gazebo/White</material>
|
||||
<gravity>0</gravity>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
315
roboasm_sgonov/urdf/roboasm_sgonov.sdf
Normal file
315
roboasm_sgonov/urdf/roboasm_sgonov.sdf
Normal file
|
@ -0,0 +1,315 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<sdf version='1.7'>
|
||||
<model name='roboasm_sgonov'>
|
||||
<link name='base'>
|
||||
<inertial>
|
||||
<pose>-0.000332 0.000107 -0.036421 0 -0 0</pose>
|
||||
<mass>1.31012</mass>
|
||||
<inertia>
|
||||
<ixx>0.00259584</ixx>
|
||||
<ixy>9.78631e-07</ixy>
|
||||
<ixz>-1.47312e-05</ixz>
|
||||
<iyy>0.00238835</iyy>
|
||||
<iyz>5.81914e-07</iyz>
|
||||
<izz>0.0043068</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='base_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/base.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='base_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/base.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='joint1' type='revolute'>
|
||||
<pose relative_to='base'>0 0 0 1.5708 0 -1.5708</pose>
|
||||
<parent>base</parent>
|
||||
<child>link1</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='link1'>
|
||||
<pose relative_to='joint1'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0.028753 0.080526 0 0 -0 0</pose>
|
||||
<mass>0.52648</mass>
|
||||
<inertia>
|
||||
<ixx>0.00076496</ixx>
|
||||
<ixy>-3.5256e-05</ixy>
|
||||
<ixz>-2.8173e-10</ixz>
|
||||
<iyy>0.00038929</iyy>
|
||||
<iyz>1.9918e-09</iyz>
|
||||
<izz>0.00075521</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link1_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link1.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link1_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link1.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='joint2' type='revolute'>
|
||||
<pose relative_to='link1'>0 0.14172 0 0 -0 0</pose>
|
||||
<parent>link1</parent>
|
||||
<child>link2</child>
|
||||
<axis>
|
||||
<xyz>1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='link2'>
|
||||
<pose relative_to='joint2'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>-0.009842 0.025632 0 0 -0 0</pose>
|
||||
<mass>0.466449</mass>
|
||||
<inertia>
|
||||
<ixx>0.000250149</ixx>
|
||||
<ixy>-6.39625e-06</ixy>
|
||||
<ixz>1.16241e-09</ixz>
|
||||
<iyy>0.000302356</iyy>
|
||||
<iyz>-5.72107e-10</iyz>
|
||||
<izz>0.000264859</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link2_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link2.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link2_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link2.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='joint3' type='revolute'>
|
||||
<pose relative_to='link2'>0 0.081346 0 0 0.007194 0</pose>
|
||||
<parent>link2</parent>
|
||||
<child>link3</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='link3'>
|
||||
<pose relative_to='joint3'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0.02151 0.069442 0 0 -0 0</pose>
|
||||
<mass>0.708169</mass>
|
||||
<inertia>
|
||||
<ixx>0.000864851</ixx>
|
||||
<ixy>-3.6232e-05</ixy>
|
||||
<ixz>-2.95541e-10</ixz>
|
||||
<iyy>0.000482144</iyy>
|
||||
<iyz>2.00354e-09</iyz>
|
||||
<izz>0.000860395</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link3_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link3.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link3_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link3.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='joint4' type='revolute'>
|
||||
<pose relative_to='link3'>0 0.14438 0 0 -0 0</pose>
|
||||
<parent>link3</parent>
|
||||
<child>link4</child>
|
||||
<axis>
|
||||
<xyz>1 0 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='link4'>
|
||||
<pose relative_to='joint4'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>-0.009842 0.025632 0 0 -0 0</pose>
|
||||
<mass>0.466449</mass>
|
||||
<inertia>
|
||||
<ixx>0.000250149</ixx>
|
||||
<ixy>-6.39625e-06</ixy>
|
||||
<ixz>1.15938e-09</ixz>
|
||||
<iyy>0.000302356</iyy>
|
||||
<iyz>-5.70585e-10</iyz>
|
||||
<izz>0.000264859</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link4_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link4.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link4_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link4.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<joint name='joint5' type='revolute'>
|
||||
<pose relative_to='link4'>0 0.081346 0 0 -0 0</pose>
|
||||
<parent>link4</parent>
|
||||
<child>link5</child>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='link5'>
|
||||
<pose relative_to='joint5'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>0.02151 0.069442 0 0 -0 0</pose>
|
||||
<mass>0.708169</mass>
|
||||
<inertia>
|
||||
<ixx>0.000864851</ixx>
|
||||
<ixy>-3.6232e-05</ixy>
|
||||
<ixz>-2.95541e-10</ixz>
|
||||
<iyy>0.000482144</iyy>
|
||||
<iyz>2.00354e-09</iyz>
|
||||
<izz>0.000860395</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link5_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link5.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link5_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link5.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
<link name='link6'>
|
||||
<pose relative_to='joint6'>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose>-0.010796 0.033705 6e-06 0 -0 0</pose>
|
||||
<mass>0.599356</mass>
|
||||
<inertia>
|
||||
<ixx>0.00028631</ixx>
|
||||
<ixy>-5.79904e-06</ixy>
|
||||
<ixz>6.26853e-08</ixz>
|
||||
<iyy>0.000358509</iyy>
|
||||
<iyz>-4.97585e-10</iyz>
|
||||
<izz>0.000295139</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='link6_collision'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link6.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='link6_visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://roboasm_sgonov/meshes/link6.stl</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
374
roboasm_sgonov/urdf/roboasm_sgonov.urdf.xacro
Executable file
374
roboasm_sgonov/urdf/roboasm_sgonov.urdf.xacro
Executable file
|
@ -0,0 +1,374 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="roboasm_sgonov" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
|
||||
<xacro:include filename="$(find roboasm_sgonov)/urdf/rasms.gazebo.xacro"/>
|
||||
<xacro:include filename="$(find roboasm_sgonov)/urdf/rasms.control.xacro"/>
|
||||
<link name="world"/>
|
||||
<joint name="to_world" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base"/>
|
||||
<origin xyz="0.0 0.0 0.07" rpy="0.0 0.0 0.0"/>
|
||||
</joint>
|
||||
<link
|
||||
name="base">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000332122479320982 0.000107427994729828 -0.0364210136165633"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.31011976880718" />
|
||||
<inertia
|
||||
ixx="0.00259583556985636"
|
||||
ixy="9.78631121059142E-07"
|
||||
ixz="-1.47312310265428E-05"
|
||||
iyy="0.00238834687122552"
|
||||
iyz="5.81913913034168E-07"
|
||||
izz="0.00430679870377171" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/base.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/base.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.028753 0.080526 1.3501E-08"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.52648" />
|
||||
<inertia
|
||||
ixx="0.00076496"
|
||||
ixy="-3.5256E-05"
|
||||
ixz="-2.8173E-10"
|
||||
iyy="0.00038929"
|
||||
iyz="1.9918E-09"
|
||||
izz="0.00075521" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link1.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.79216 0.81961 0.93333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link1.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint1"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="1.5708 0 -1.5708" />
|
||||
<parent
|
||||
link="base" />
|
||||
<child
|
||||
link="link1" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
</joint>
|
||||
<link
|
||||
name="link2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0098423932944443 0.025631709550142 2.62062855886029E-08"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.466449031624498" />
|
||||
<inertia
|
||||
ixx="0.000250149264856934"
|
||||
ixy="-6.39625267943634E-06"
|
||||
ixz="1.16240576365026E-09"
|
||||
iyy="0.000302356392836182"
|
||||
iyz="-5.72107071817652E-10"
|
||||
izz="0.000264859050215469" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link2.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link2.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint2"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0.14172 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="link1" />
|
||||
<child
|
||||
link="link2" />
|
||||
<axis
|
||||
xyz="1 0 0" />
|
||||
</joint>
|
||||
<link
|
||||
name="link3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0215100033761789 0.0694423986454957 3.93278649368959E-08"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.70816935413754" />
|
||||
<inertia
|
||||
ixx="0.000864850509477081"
|
||||
ixy="-3.62319530566251E-05"
|
||||
ixz="-2.95541145601842E-10"
|
||||
iyy="0.000482143975333292"
|
||||
iyz="2.00353934934295E-09"
|
||||
izz="0.000860394915615017" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link3.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link3.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint3"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0.081346 0"
|
||||
rpy="0 0.0071935 0" />
|
||||
<parent
|
||||
link="link2" />
|
||||
<child
|
||||
link="link3" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
</joint>
|
||||
<link
|
||||
name="link4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0098423941326931 0.025631710847922 2.62314079923008E-08"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.466449036998311" />
|
||||
<inertia
|
||||
ixx="0.0002501492525602"
|
||||
ixy="-6.39624584934821E-06"
|
||||
ixz="1.15937712750973E-09"
|
||||
iyy="0.000302356392948396"
|
||||
iyz="-5.70585180406975E-10"
|
||||
izz="0.000264859056361031" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link4.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link4.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint4"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0.14438 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="link3" />
|
||||
<child
|
||||
link="link4" />
|
||||
<axis
|
||||
xyz="1 0 0" />
|
||||
</joint>
|
||||
<link
|
||||
name="link5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.0215100033039704 0.0694423990612557 3.93278667377771E-08"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.708169356775496" />
|
||||
<inertia
|
||||
ixx="0.000864850515095"
|
||||
ixy="-3.6231953093431E-05"
|
||||
ixz="-2.95541136548844E-10"
|
||||
iyy="0.000482143976309882"
|
||||
iyz="2.00353938873391E-09"
|
||||
izz="0.000860394921240315" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link5.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link5.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint5"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0.081346 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="link4" />
|
||||
<child
|
||||
link="link5" />
|
||||
<axis
|
||||
xyz="0 1 0" />
|
||||
</joint>
|
||||
<link
|
||||
name="link6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0107961703559938 0.0337046197725978 6.41961769822694E-06"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.599356299495803" />
|
||||
<inertia
|
||||
ixx="0.000286310483854348"
|
||||
ixy="-5.79903953240378E-06"
|
||||
ixz="6.26853068396246E-08"
|
||||
iyy="0.000358508548445319"
|
||||
iyz="-4.97585335654749E-10"
|
||||
izz="0.000295138763544911" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link6.stl" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://roboasm_sgonov/meshes/link6.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint6"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0.14438 0"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="link5" />
|
||||
<child
|
||||
link="link6" />
|
||||
<axis
|
||||
xyz="1 0 0" />
|
||||
</joint>
|
||||
|
||||
<xacro:rasms_hi/>
|
||||
<xacro:rasms_gazebo/>
|
||||
</robot>
|
85
roboasm_sgonov/world/roboasm.sdf
Normal file
85
roboasm_sgonov/world/roboasm.sdf
Normal file
|
@ -0,0 +1,85 @@
|
|||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<sdf version='1.7'>
|
||||
<world name="default">
|
||||
<physics name="1ms" type="ignored">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
<plugin
|
||||
filename="ignition-gazebo-physics-system"
|
||||
name="ignition::gazebo::systems::Physics">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-user-commands-system"
|
||||
name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-scene-broadcaster-system"
|
||||
name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-contact-system"
|
||||
name="ignition::gazebo::systems::Contact">
|
||||
</plugin>
|
||||
<gui>
|
||||
|
||||
<plugin filename="GzScene3D" name="3D View">
|
||||
<ignition-gui>
|
||||
<title>3D View</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="string" key="state">docked</property>
|
||||
</ignition-gui>
|
||||
<engine>ogre2</engine>
|
||||
<scene>scene</scene>
|
||||
<ambient_light>1.0 1.0 1.0</ambient_light>
|
||||
<background_color>0.4 0.6 1.0</background_color>
|
||||
<camera_pose>8.3 7 7.8 0 0.5 -2.4</camera_pose>
|
||||
</plugin>
|
||||
</gui>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
<include>
|
||||
<uri>roboasm_sgonov_world</uri>
|
||||
<pose>0 0 0.07 0 0 0</pose>
|
||||
</include>
|
||||
</world>
|
||||
</sdf>
|
Loading…
Add table
Add a link
Reference in a new issue