➕ add test MoveitCpp node
This commit is contained in:
parent
b2a89ecbde
commit
554a33fc43
15 changed files with 653 additions and 430 deletions
|
|
@ -1,3 +1,7 @@
|
||||||
# Robossembler ROS2
|
# Robossembler ROS2
|
||||||
|
|
||||||
Repo for ROS2 packages related to Robossembler
|
Repo for ROS2 packages related to Robossembler
|
||||||
|
|
||||||
|
## Known issues
|
||||||
|
|
||||||
|
Controller manager can not loaded with `gazebo_ros2_control/GazeboSystem` but MoveitCpp and controller manager completely loaded with `fake_components/GenericSystem` in `ros2_control` tag in file `rasms_descroption/urdf/rasms.control.xacro`
|
||||||
|
|
@ -6,7 +6,6 @@ Panels:
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Global Options1
|
- /Global Options1
|
||||||
- /Status1
|
- /Status1
|
||||||
- /RobotModel1
|
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 787
|
Tree Height: 787
|
||||||
- Class: rviz_common/Selection
|
- Class: rviz_common/Selection
|
||||||
|
|
@ -43,68 +42,74 @@ Visualization Manager:
|
||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: <Fixed Frame>
|
Reference Frame: <Fixed Frame>
|
||||||
Value: true
|
Value: true
|
||||||
- Alpha: 1
|
- Class: moveit_rviz_plugin/PlanningScene
|
||||||
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
|
Enabled: true
|
||||||
Links:
|
Move Group Namespace: ""
|
||||||
All Links Enabled: true
|
Name: PlanningScene
|
||||||
Expand Joint Details: false
|
Planning Scene Topic: /monitored_planning_scene
|
||||||
Expand Link Details: false
|
Robot Description: robot_description
|
||||||
Expand Tree: false
|
Scene Geometry:
|
||||||
Link Tree Style: Links in Alphabetic Order
|
Scene Alpha: 0.8999999761581421
|
||||||
base:
|
Scene Color: 50; 230; 50
|
||||||
Alpha: 1
|
Scene Display Time: 0.009999999776482582
|
||||||
Show Axes: false
|
Show Scene Geometry: true
|
||||||
Show Trail: false
|
Voxel Coloring: Z-Axis
|
||||||
Value: true
|
Voxel Rendering: Occupied Voxels
|
||||||
link1:
|
Scene Robot:
|
||||||
Alpha: 1
|
Attached Body Color: 150; 50; 150
|
||||||
Show Axes: false
|
Links:
|
||||||
Show Trail: false
|
All Links Enabled: true
|
||||||
Value: true
|
Expand Joint Details: false
|
||||||
link2:
|
Expand Link Details: false
|
||||||
Alpha: 1
|
Expand Tree: false
|
||||||
Show Axes: false
|
Link Tree Style: Links in Alphabetic Order
|
||||||
Show Trail: false
|
base:
|
||||||
Value: true
|
Alpha: 1
|
||||||
link3:
|
Show Axes: false
|
||||||
Alpha: 1
|
Show Trail: false
|
||||||
Show Axes: false
|
Value: true
|
||||||
Show Trail: false
|
link1:
|
||||||
Value: true
|
Alpha: 1
|
||||||
link4:
|
Show Axes: false
|
||||||
Alpha: 1
|
Show Trail: false
|
||||||
Show Axes: false
|
Value: true
|
||||||
Show Trail: false
|
link2:
|
||||||
Value: true
|
Alpha: 1
|
||||||
link5:
|
Show Axes: false
|
||||||
Alpha: 1
|
Show Trail: false
|
||||||
Show Axes: false
|
Value: true
|
||||||
Show Trail: false
|
link3:
|
||||||
Value: true
|
Alpha: 1
|
||||||
link6:
|
Show Axes: false
|
||||||
Alpha: 1
|
Show Trail: false
|
||||||
Show Axes: false
|
Value: true
|
||||||
Show Trail: false
|
link4:
|
||||||
Value: true
|
Alpha: 1
|
||||||
Name: RobotModel
|
Show Axes: false
|
||||||
TF Prefix: ""
|
Show Trail: false
|
||||||
Update Interval: 0
|
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
|
Value: true
|
||||||
Visual Enabled: true
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
Fixed Frame: base
|
Fixed Frame: world
|
||||||
Frame Rate: 30
|
Frame Rate: 30
|
||||||
Name: root
|
Name: root
|
||||||
Tools:
|
Tools:
|
||||||
|
|
@ -144,25 +149,25 @@ Visualization Manager:
|
||||||
Views:
|
Views:
|
||||||
Current:
|
Current:
|
||||||
Class: rviz_default_plugins/Orbit
|
Class: rviz_default_plugins/Orbit
|
||||||
Distance: 1.4318417310714722
|
Distance: 2.053321123123169
|
||||||
Enable Stereo Rendering:
|
Enable Stereo Rendering:
|
||||||
Stereo Eye Separation: 0.05999999865889549
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
Stereo Focal Distance: 1
|
Stereo Focal Distance: 1
|
||||||
Swap Stereo Eyes: false
|
Swap Stereo Eyes: false
|
||||||
Value: false
|
Value: false
|
||||||
Focal Point:
|
Focal Point:
|
||||||
X: 0.01542382501065731
|
X: 0.35200047492980957
|
||||||
Y: -0.006365143693983555
|
Y: 0.026240859180688858
|
||||||
Z: 0.15774454176425934
|
Z: 0.2903156876564026
|
||||||
Focal Shape Fixed Size: true
|
Focal Shape Fixed Size: true
|
||||||
Focal Shape Size: 0.05000000074505806
|
Focal Shape Size: 0.05000000074505806
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.009999999776482582
|
||||||
Pitch: 0.5703980326652527
|
Pitch: 0.7903980612754822
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Value: Orbit (rviz)
|
Value: Orbit (rviz)
|
||||||
Yaw: 0.5603898763656616
|
Yaw: 5.348577976226807
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
Displays:
|
Displays:
|
||||||
|
|
@ -5,6 +5,7 @@
|
||||||
<!-- ros_control-plugin -->
|
<!-- ros_control-plugin -->
|
||||||
<gazebo>
|
<gazebo>
|
||||||
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
|
||||||
|
<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
|
||||||
<parameters>$(find rasms_moveit_config)/config/rasms_controllers.yml</parameters>
|
<parameters>$(find rasms_moveit_config)/config/rasms_controllers.yml</parameters>
|
||||||
<robotNamespace>/rasms</robotNamespace>
|
<robotNamespace>/rasms</robotNamespace>
|
||||||
</plugin>
|
</plugin>
|
||||||
|
|
|
||||||
|
|
@ -1,146 +0,0 @@
|
||||||
<?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>
|
|
||||||
<plugin
|
|
||||||
filename="ignition-gazebo-sensors-system"
|
|
||||||
name="ignition::gazebo::systems::Sensors">
|
|
||||||
<render_enfine>ogre2</render_enfine>
|
|
||||||
</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>rasms_model</uri>
|
|
||||||
</include>
|
|
||||||
<!--include>
|
|
||||||
<uri>realsense_camera</uri>
|
|
||||||
<pose>1 0 1 0 0.97 3.14159</pose>
|
|
||||||
<static>true</static>
|
|
||||||
</include-->
|
|
||||||
|
|
||||||
<model name="3d_camera">
|
|
||||||
<static>true</static>
|
|
||||||
<link name="kinect_sensor">
|
|
||||||
<pose>1 0 1 0 1 3.14159</pose>
|
|
||||||
<collision name="collision">
|
|
||||||
<geometry>
|
|
||||||
<box>
|
|
||||||
<size>0.05 0.05 0.05</size>
|
|
||||||
</box>
|
|
||||||
</geometry>
|
|
||||||
</collision>
|
|
||||||
<visual name="visual">
|
|
||||||
<geometry>
|
|
||||||
<box>
|
|
||||||
<size>0.05 0.05 0.05</size>
|
|
||||||
</box>
|
|
||||||
</geometry>
|
|
||||||
</visual>
|
|
||||||
<sensor name="camera" type="rgbd_camera">
|
|
||||||
<topic>rgbd_camera</topic>
|
|
||||||
<always_on>1</always_on>
|
|
||||||
<update_rate>30</update_rate>
|
|
||||||
<camera name="rgbd_sens">
|
|
||||||
<image>
|
|
||||||
<width>212</width>
|
|
||||||
<heigth>120</heigth>
|
|
||||||
</image>
|
|
||||||
<horizontal_fov>1.567821</horizontal_fov>
|
|
||||||
<vertical_fov>1.022238</vertical_fov>
|
|
||||||
<clip>
|
|
||||||
<near>0.01</near>
|
|
||||||
<far>1000.0</far>
|
|
||||||
</clip>
|
|
||||||
<depth_camera>
|
|
||||||
<clip>
|
|
||||||
<near>0.01</near>
|
|
||||||
<far>10.0</far>
|
|
||||||
</clip>
|
|
||||||
</depth_camera>
|
|
||||||
<noise>
|
|
||||||
<type>gaussian</type>
|
|
||||||
<mean>0</mean>
|
|
||||||
<stddev>0</stddev>
|
|
||||||
</noise>
|
|
||||||
<visibility_mask>2</visibility_mask>
|
|
||||||
</camera>
|
|
||||||
|
|
||||||
</sensor>
|
|
||||||
</link>
|
|
||||||
</model>
|
|
||||||
|
|
||||||
</world>
|
|
||||||
</sdf>
|
|
||||||
78
rasms_moveit_actions/CMakeLists.txt
Normal file
78
rasms_moveit_actions/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,78 @@
|
||||||
|
cmake_minimum_required(VERSION 3.5)
|
||||||
|
project(rasms_moveit_actions)
|
||||||
|
|
||||||
|
#find_package(moveit_common REQUIRED)
|
||||||
|
#moveit_package()
|
||||||
|
|
||||||
|
# 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(Eigen3 REQUIRED)
|
||||||
|
find_package(Boost REQUIRED system filesystem date_time thread)
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(control_msgs REQUIRED)
|
||||||
|
find_package(moveit_core REQUIRED)
|
||||||
|
find_package(moveit_ros_planning REQUIRED)
|
||||||
|
find_package(moveit_ros_planning_interface REQUIRED)
|
||||||
|
find_package(moveit_ros_perception REQUIRED)
|
||||||
|
find_package(moveit_servo REQUIRED)
|
||||||
|
find_package(moveit_visual_tools REQUIRED)
|
||||||
|
find_package(geometric_shapes REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(rclcpp_action REQUIRED)
|
||||||
|
find_package(pluginlib REQUIRED)
|
||||||
|
find_package(tf2_ros REQUIRED)
|
||||||
|
find_package(tf2_eigen REQUIRED)
|
||||||
|
find_package(tf2_geometry_msgs REQUIRED)
|
||||||
|
|
||||||
|
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||||
|
ament_cmake
|
||||||
|
rclcpp
|
||||||
|
rclcpp_action
|
||||||
|
tf2_geometry_msgs
|
||||||
|
tf2_ros
|
||||||
|
moveit_core
|
||||||
|
moveit_visual_tools
|
||||||
|
moveit_ros_planning_interface
|
||||||
|
tf2_geometry_msgs
|
||||||
|
moveit_ros_planning
|
||||||
|
pluginlib
|
||||||
|
Eigen3
|
||||||
|
Boost
|
||||||
|
control_msgs
|
||||||
|
moveit_servo
|
||||||
|
)
|
||||||
|
|
||||||
|
include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
|
||||||
|
|
||||||
|
add_executable(rasms_moveit src/run_moveit.cpp)
|
||||||
|
#target_include_directories(rasms_moveit PRIVATE include)
|
||||||
|
ament_target_dependencies(rasms_moveit
|
||||||
|
${THIS_PACKAGE_INCLUDE_DEPENDS}
|
||||||
|
)
|
||||||
|
|
||||||
|
install(TARGETS rasms_moveit
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
#install(TARGETS rasms_moveit
|
||||||
|
# EXPORT export_${PROJECT_NAME}
|
||||||
|
# LIBRARY DESTINATION lib
|
||||||
|
# ARCHIVE DESTINATION lib
|
||||||
|
# RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||||
|
# INCLUDES DESTINATION include
|
||||||
|
#)
|
||||||
|
#ament_export_targets(export_${PROJECT_NAME})
|
||||||
|
|
||||||
|
ament_package()
|
||||||
48
rasms_moveit_actions/package.xml
Normal file
48
rasms_moveit_actions/package.xml
Normal file
|
|
@ -0,0 +1,48 @@
|
||||||
|
<?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>rasms_moveit_actions</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<!--build_depend>moveit_common</build_depend-->
|
||||||
|
|
||||||
|
<depend>control_msgs</depend>
|
||||||
|
<depend>moveit_core</depend>
|
||||||
|
<depend>moveit_ros_perception</depend>
|
||||||
|
<depend>moveit_ros_planning_interface</depend>
|
||||||
|
<depend>moveit_servo</depend>
|
||||||
|
<depend>moveit_visual_tools</depend>
|
||||||
|
<depend>pluginlib</depend>
|
||||||
|
<depend>rviz_visual_tools</depend>
|
||||||
|
<depend>tf2_eigen</depend>
|
||||||
|
<depend>tf2_geometry_msgs</depend>
|
||||||
|
<depend>tf2_ros</depend>
|
||||||
|
|
||||||
|
<build_depend>eigen</build_depend>
|
||||||
|
<build_depend>geometric_shapes</build_depend>
|
||||||
|
<build_depend>moveit_common</build_depend>
|
||||||
|
<build_depend>moveit_ros_planning</build_depend>
|
||||||
|
|
||||||
|
<exec_depend>controller_manager</exec_depend>
|
||||||
|
<exec_depend>gripper_controllers</exec_depend>
|
||||||
|
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||||
|
<exec_depend>joint_state_publisher</exec_depend>
|
||||||
|
<exec_depend>joint_trajectory_controller</exec_depend>
|
||||||
|
<exec_depend>moveit</exec_depend>
|
||||||
|
<exec_depend>robot_state_publisher</exec_depend>
|
||||||
|
<exec_depend>rviz2</exec_depend>
|
||||||
|
<exec_depend>warehouse_ros_mongo</exec_depend>
|
||||||
|
<exec_depend>xacro</exec_depend>
|
||||||
|
|
||||||
|
<!--test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend-->
|
||||||
|
<test_depend>ament_cmake_gtest</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
314
rasms_moveit_actions/src/run_moveit.cpp
Normal file
314
rasms_moveit_actions/src/run_moveit.cpp
Normal file
|
|
@ -0,0 +1,314 @@
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <memory>
|
||||||
|
// MoveitCpp
|
||||||
|
#include <moveit/moveit_cpp/moveit_cpp.h>
|
||||||
|
#include <moveit/moveit_cpp/planning_component.h>
|
||||||
|
|
||||||
|
#include <geometry_msgs/msg/point_stamped.h>
|
||||||
|
|
||||||
|
#include <moveit_visual_tools/moveit_visual_tools.h>
|
||||||
|
//#include <urdf/urdf_world_parser.hpp>
|
||||||
|
|
||||||
|
namespace rvt = rviz_visual_tools;
|
||||||
|
|
||||||
|
// All source files that use ROS logging should define a file-specific
|
||||||
|
// static const rclcpp::Logger named LOGGER, located at the top of the file
|
||||||
|
// and inside the namespace with the narrowest scope (if there is one)
|
||||||
|
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_tutorial");
|
||||||
|
|
||||||
|
int main(int argc, char** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::NodeOptions node_options;
|
||||||
|
RCLCPP_INFO(LOGGER, "Initialize node");
|
||||||
|
|
||||||
|
// This enables loading undeclared parameters
|
||||||
|
// best practice would be to declare parameters in the corresponding classes
|
||||||
|
// and provide descriptions about expected use
|
||||||
|
node_options.automatically_declare_parameters_from_overrides(true);
|
||||||
|
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("run_moveit_cpp", "", node_options);
|
||||||
|
|
||||||
|
// We spin up a SingleThreadedExecutor for the current state monitor to get information
|
||||||
|
// about the robot's state.
|
||||||
|
rclcpp::executors::SingleThreadedExecutor executor;
|
||||||
|
executor.add_node(node);
|
||||||
|
std::thread([&executor]() { executor.spin(); }).detach();
|
||||||
|
|
||||||
|
// BEGIN_TUTORIAL
|
||||||
|
//
|
||||||
|
// Setup
|
||||||
|
// ^^^^^
|
||||||
|
//
|
||||||
|
static const std::string PLANNING_GROUP = "rasms";
|
||||||
|
static const std::string LOGNAME = "moveit_interface";
|
||||||
|
|
||||||
|
/* Otherwise robot with zeros joint_states */
|
||||||
|
rclcpp::sleep_for(std::chrono::seconds(1));
|
||||||
|
|
||||||
|
RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");
|
||||||
|
|
||||||
|
auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
|
||||||
|
moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
|
||||||
|
|
||||||
|
auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
|
||||||
|
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
|
||||||
|
auto robot_start_state = planning_components->getStartState();
|
||||||
|
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
|
||||||
|
|
||||||
|
// Visualization
|
||||||
|
// ^^^^^^^^^^^^^
|
||||||
|
//
|
||||||
|
// The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
|
||||||
|
// and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
|
||||||
|
moveit_visual_tools::MoveItVisualTools visual_tools(node, "base", "moveit_cpp_tutorial",
|
||||||
|
moveit_cpp_ptr->getPlanningSceneMonitor());
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.loadRemoteControl();
|
||||||
|
|
||||||
|
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
|
||||||
|
text_pose.translation().z() = 1.75;
|
||||||
|
visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
// Start the demo
|
||||||
|
// ^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||||
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
|
||||||
|
|
||||||
|
// Planning with MoveItCpp
|
||||||
|
// ^^^^^^^^^^^^^^^^^^^^^^^
|
||||||
|
// There are multiple ways to set the start and the goal states of the plan
|
||||||
|
// they are illustrated in the following plan examples
|
||||||
|
//
|
||||||
|
// Plan #1
|
||||||
|
// ^^^^^^^
|
||||||
|
//
|
||||||
|
// We can set the start state of the plan to the current state of the robot
|
||||||
|
planning_components->setStartStateToCurrentState();
|
||||||
|
|
||||||
|
// The first way to set the goal of the plan is by using geometry_msgs::PoseStamped ROS message type as follow
|
||||||
|
geometry_msgs::msg::PoseStamped target_pose1;
|
||||||
|
target_pose1.header.frame_id = "base";
|
||||||
|
target_pose1.pose.orientation.w = 1.0;
|
||||||
|
target_pose1.pose.position.x = 0.28;
|
||||||
|
target_pose1.pose.position.y = -0.2;
|
||||||
|
target_pose1.pose.position.z = 0.5;
|
||||||
|
planning_components->setGoal(target_pose1, "link6");
|
||||||
|
|
||||||
|
// Now, we call the PlanningComponents to compute the plan and visualize it.
|
||||||
|
// Note that we are just planning
|
||||||
|
auto plan_solution1 = planning_components->plan();
|
||||||
|
|
||||||
|
// Check if PlanningComponents succeeded in finding the plan
|
||||||
|
if (plan_solution1)
|
||||||
|
{
|
||||||
|
// Visualize the start pose in Rviz
|
||||||
|
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base"), "zero_pose");
|
||||||
|
// Visualize the goal pose in Rviz
|
||||||
|
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
|
||||||
|
visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE);
|
||||||
|
// Visualize the trajectory in Rviz
|
||||||
|
visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
/* Uncomment if you want to execute the plan */
|
||||||
|
/* planning_components->execute(); // Execute the plan */
|
||||||
|
}
|
||||||
|
|
||||||
|
// Plan #1 visualization:
|
||||||
|
//
|
||||||
|
// .. image:: images/moveitcpp_plan1.png
|
||||||
|
// :width: 250pt
|
||||||
|
// :align: center
|
||||||
|
//
|
||||||
|
// Start the next plan
|
||||||
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
// Plan #2
|
||||||
|
// ^^^^^^^
|
||||||
|
//
|
||||||
|
// Here we will set the current state of the plan using
|
||||||
|
// moveit::core::RobotState
|
||||||
|
auto start_state = *(moveit_cpp_ptr->getCurrentState());
|
||||||
|
geometry_msgs::msg::Pose start_pose;
|
||||||
|
start_pose.orientation.w = 1.0;
|
||||||
|
start_pose.position.x = 0.55;
|
||||||
|
start_pose.position.y = 0.0;
|
||||||
|
start_pose.position.z = 0.6;
|
||||||
|
|
||||||
|
start_state.setFromIK(joint_model_group_ptr, start_pose);
|
||||||
|
|
||||||
|
planning_components->setStartState(start_state);
|
||||||
|
|
||||||
|
// We will reuse the old goal that we had and plan to it.
|
||||||
|
auto plan_solution2 = planning_components->plan();
|
||||||
|
if (plan_solution2)
|
||||||
|
{
|
||||||
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
||||||
|
moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);
|
||||||
|
|
||||||
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
||||||
|
visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
|
||||||
|
visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);
|
||||||
|
visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
/* Uncomment if you want to execute the plan */
|
||||||
|
/* planning_components->execute(); // Execute the plan */
|
||||||
|
}
|
||||||
|
|
||||||
|
// Plan #2 visualization:
|
||||||
|
//
|
||||||
|
// .. image:: images/moveitcpp_plan2.png
|
||||||
|
// :width: 250pt
|
||||||
|
// :align: center
|
||||||
|
//
|
||||||
|
// Start the next plan
|
||||||
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
// Plan #3
|
||||||
|
// ^^^^^^^
|
||||||
|
//
|
||||||
|
// We can also set the goal of the plan using
|
||||||
|
// moveit::core::RobotState
|
||||||
|
auto target_state = *robot_start_state;
|
||||||
|
geometry_msgs::msg::Pose target_pose2;
|
||||||
|
target_pose2.orientation.w = 1.0;
|
||||||
|
target_pose2.position.x = 0.55;
|
||||||
|
target_pose2.position.y = -0.05;
|
||||||
|
target_pose2.position.z = 0.8;
|
||||||
|
|
||||||
|
target_state.setFromIK(joint_model_group_ptr, target_pose2);
|
||||||
|
|
||||||
|
planning_components->setGoal(target_state);
|
||||||
|
|
||||||
|
// We will reuse the old start that we had and plan from it.
|
||||||
|
auto plan_solution3 = planning_components->plan();
|
||||||
|
if (plan_solution3)
|
||||||
|
{
|
||||||
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
||||||
|
moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);
|
||||||
|
|
||||||
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
||||||
|
visual_tools.publishAxisLabeled(target_pose2, "target_pose");
|
||||||
|
visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);
|
||||||
|
visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
/* Uncomment if you want to execute the plan */
|
||||||
|
/* planning_components->execute(); // Execute the plan */
|
||||||
|
}
|
||||||
|
|
||||||
|
// Plan #3 visualization:
|
||||||
|
//
|
||||||
|
// .. image:: images/moveitcpp_plan3.png
|
||||||
|
// :width: 250pt
|
||||||
|
// :align: center
|
||||||
|
//
|
||||||
|
// Start the next plan
|
||||||
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
// Plan #4
|
||||||
|
// ^^^^^^^
|
||||||
|
//
|
||||||
|
// We can set the start state of the plan to the current state of the robot
|
||||||
|
// We can set the goal of the plan using the name of a group states
|
||||||
|
// for panda robot we have one named robot state for "panda_arm" planning group called "ready"
|
||||||
|
// see `panda_arm.xacro
|
||||||
|
// <https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/config/panda_arm.xacro#L13>`_
|
||||||
|
|
||||||
|
/* // Set the start state of the plan from a named robot state */
|
||||||
|
/* planning_components->setStartState("ready"); // Not implemented yet */
|
||||||
|
// Set the goal state of the plan from a named robot state
|
||||||
|
planning_components->setGoal("ready");
|
||||||
|
|
||||||
|
// Again we will reuse the old start that we had and plan from it.
|
||||||
|
auto plan_solution4 = planning_components->plan();
|
||||||
|
if (plan_solution4)
|
||||||
|
{
|
||||||
|
moveit::core::RobotState robot_state(robot_model_ptr);
|
||||||
|
moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);
|
||||||
|
|
||||||
|
visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("link6"), "start_pose");
|
||||||
|
visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("link6"), "target_pose");
|
||||||
|
visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);
|
||||||
|
visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
/* Uncomment if you want to execute the plan */
|
||||||
|
/* planning_components->execute(); // Execute the plan */
|
||||||
|
}
|
||||||
|
|
||||||
|
// Plan #4 visualization:
|
||||||
|
//
|
||||||
|
// .. image:: images/moveitcpp_plan4.png
|
||||||
|
// :width: 250pt
|
||||||
|
// :align: center
|
||||||
|
//
|
||||||
|
// Start the next plan
|
||||||
|
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
// Plan #5
|
||||||
|
// ^^^^^^^
|
||||||
|
//
|
||||||
|
// We can also generate motion plans around objects in the collision scene.
|
||||||
|
//
|
||||||
|
// First we create the collision object
|
||||||
|
moveit_msgs::msg::CollisionObject collision_object;
|
||||||
|
collision_object.header.frame_id = "base";
|
||||||
|
collision_object.id = "box";
|
||||||
|
|
||||||
|
shape_msgs::msg::SolidPrimitive box;
|
||||||
|
box.type = box.BOX;
|
||||||
|
box.dimensions = { 0.1, 0.4, 0.1 };
|
||||||
|
|
||||||
|
geometry_msgs::msg::Pose box_pose;
|
||||||
|
box_pose.position.x = 0.4;
|
||||||
|
box_pose.position.y = 0.0;
|
||||||
|
box_pose.position.z = 1.0;
|
||||||
|
|
||||||
|
collision_object.primitives.push_back(box);
|
||||||
|
collision_object.primitive_poses.push_back(box_pose);
|
||||||
|
collision_object.operation = collision_object.ADD;
|
||||||
|
|
||||||
|
// Add object to planning scene
|
||||||
|
{ // Lock PlanningScene
|
||||||
|
planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitor());
|
||||||
|
scene->processCollisionObjectMsg(collision_object);
|
||||||
|
} // Unlock PlanningScene
|
||||||
|
planning_components->setStartStateToCurrentState();
|
||||||
|
planning_components->setGoal("extended");
|
||||||
|
|
||||||
|
auto plan_solution5 = planning_components->plan();
|
||||||
|
if (plan_solution5)
|
||||||
|
{
|
||||||
|
visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);
|
||||||
|
visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr);
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
/* Uncomment if you want to execute the plan */
|
||||||
|
/* planning_components->execute(); // Execute the plan */
|
||||||
|
}
|
||||||
|
|
||||||
|
// Plan #5 visualization:
|
||||||
|
//
|
||||||
|
// .. image:: images/moveitcpp_plan5.png
|
||||||
|
// :width: 250pt
|
||||||
|
// :align: center
|
||||||
|
//
|
||||||
|
// END_TUTORIAL
|
||||||
|
visual_tools.prompt("Press 'next' to end the demo");
|
||||||
|
visual_tools.deleteAllMarkers();
|
||||||
|
visual_tools.trigger();
|
||||||
|
|
||||||
|
RCLCPP_INFO(LOGGER, "Shutting down.");
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
20
rasms_moveit_config/config/moveit_cpp.yml
Normal file
20
rasms_moveit_config/config/moveit_cpp.yml
Normal file
|
|
@ -0,0 +1,20 @@
|
||||||
|
rasms_moveit:
|
||||||
|
ros__parameters:
|
||||||
|
planning_scene_monitor_options:
|
||||||
|
name: "planning_scene_monitor"
|
||||||
|
robot_description: "robot_description"
|
||||||
|
joint_state_topic: "/joint_states"
|
||||||
|
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
|
||||||
|
publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
|
||||||
|
monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
|
||||||
|
wait_for_initial_state_timeout: 10.0
|
||||||
|
|
||||||
|
planning_pipelines:
|
||||||
|
#namespace: "moveit_cpp" # optional, default is ~
|
||||||
|
pipeline_names: ["ompl"]
|
||||||
|
|
||||||
|
plan_request_params:
|
||||||
|
planning_attempts: 10
|
||||||
|
planning_pipeline: ompl
|
||||||
|
max_velocity_scaling_factor: 1.0
|
||||||
|
max_acceleration_scaling_factor: 1.0
|
||||||
|
|
@ -123,59 +123,8 @@ planner_configs:
|
||||||
TrajOptDefault:
|
TrajOptDefault:
|
||||||
type: geometric::TrajOpt
|
type: geometric::TrajOpt
|
||||||
|
|
||||||
panda_arm:
|
rasms_description_arm:
|
||||||
planner_configs:
|
longest_valid_fraction: "0.001"
|
||||||
- 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:
|
planner_configs:
|
||||||
- SBLkConfigDefault
|
- SBLkConfigDefault
|
||||||
- ESTkConfigDefault
|
- ESTkConfigDefault
|
||||||
|
|
|
||||||
|
|
@ -7,7 +7,7 @@ Panels:
|
||||||
- /Global Options1
|
- /Global Options1
|
||||||
- /Status1
|
- /Status1
|
||||||
Splitter Ratio: 0.5
|
Splitter Ratio: 0.5
|
||||||
Tree Height: 219
|
Tree Height: 814
|
||||||
- Class: rviz_common/Selection
|
- Class: rviz_common/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz_common/Tool Properties
|
- Class: rviz_common/Tool Properties
|
||||||
|
|
@ -42,106 +42,10 @@ Visualization Manager:
|
||||||
Plane Cell Count: 10
|
Plane Cell Count: 10
|
||||||
Reference Frame: <Fixed Frame>
|
Reference Frame: <Fixed Frame>
|
||||||
Value: true
|
Value: true
|
||||||
- Acceleration_Scaling_Factor: 0.1
|
- Class: moveit_rviz_plugin/PlanningScene
|
||||||
Class: moveit_rviz_plugin/MotionPlanning
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Move Group Namespace: ""
|
Move Group Namespace: ""
|
||||||
MoveIt_Allow_Approximate_IK: false
|
Name: PlanningScene
|
||||||
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
|
Planning Scene Topic: /monitored_planning_scene
|
||||||
Robot Description: robot_description
|
Robot Description: robot_description
|
||||||
Scene Geometry:
|
Scene Geometry:
|
||||||
|
|
@ -202,7 +106,6 @@ Visualization Manager:
|
||||||
Show Robot Collision: false
|
Show Robot Collision: false
|
||||||
Show Robot Visual: true
|
Show Robot Visual: true
|
||||||
Value: true
|
Value: true
|
||||||
Velocity_Scaling_Factor: 0.1
|
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 48; 48; 48
|
Background Color: 48; 48; 48
|
||||||
|
|
@ -261,28 +164,24 @@ Visualization Manager:
|
||||||
Invert Z Axis: false
|
Invert Z Axis: false
|
||||||
Name: Current View
|
Name: Current View
|
||||||
Near Clip Distance: 0.009999999776482582
|
Near Clip Distance: 0.009999999776482582
|
||||||
Pitch: 0.8953979015350342
|
Pitch: 0.84539794921875
|
||||||
Target Frame: <Fixed Frame>
|
Target Frame: <Fixed Frame>
|
||||||
Value: Orbit (rviz)
|
Value: Orbit (rviz)
|
||||||
Yaw: 0.6453981399536133
|
Yaw: 0.6953980922698975
|
||||||
Saved: ~
|
Saved: ~
|
||||||
Window Geometry:
|
Window Geometry:
|
||||||
"":
|
|
||||||
collapsed: false
|
|
||||||
" - Trajectory Slider":
|
|
||||||
collapsed: false
|
|
||||||
Displays:
|
Displays:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Height: 846
|
Height: 1043
|
||||||
Hide Left Dock: false
|
Hide Left Dock: false
|
||||||
Hide Right Dock: false
|
Hide Right Dock: false
|
||||||
QMainWindow State: 000000ff00000000fd0000000400000000000001f3000002f4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000166000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffbffffffff01000001a9000001880000017d00ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001a2000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
QMainWindow State: 000000ff00000000fd0000000400000000000001f3000003b9fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b2000002290000000000000000000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000472000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
Selection:
|
Selection:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Tool Properties:
|
Tool Properties:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Views:
|
Views:
|
||||||
collapsed: false
|
collapsed: false
|
||||||
Width: 1200
|
Width: 1920
|
||||||
X: 225
|
X: 2560
|
||||||
Y: 91
|
Y: 360
|
||||||
|
|
|
||||||
|
|
@ -34,7 +34,6 @@ def launch_setup(context, *args, **kwargs):
|
||||||
("robot_description", robot_description_content),
|
("robot_description", robot_description_content),
|
||||||
("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
|
("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
|
||||||
("controller_configurations", LaunchConfiguration("controller_configurations")),
|
("controller_configurations", LaunchConfiguration("controller_configurations")),
|
||||||
("controller", LaunchConfiguration("controller")),
|
|
||||||
("sim", LaunchConfiguration("sim"))
|
("sim", LaunchConfiguration("sim"))
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|
@ -118,14 +117,6 @@ def generate_launch_description():
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
launch_args.append(
|
|
||||||
DeclareLaunchArgument(
|
|
||||||
name="controller",
|
|
||||||
default_value="position_trajectory_controller",
|
|
||||||
description="Robot controller."
|
|
||||||
)
|
|
||||||
)
|
|
||||||
|
|
||||||
launch_args.append(
|
launch_args.append(
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
name="moveit_controller_configurations_package",
|
name="moveit_controller_configurations_package",
|
||||||
|
|
|
||||||
|
|
@ -1,9 +1,14 @@
|
||||||
|
import os
|
||||||
from launch.launch_description import LaunchDescription
|
from launch.launch_description import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument
|
from launch.actions import DeclareLaunchArgument
|
||||||
from launch.conditions import UnlessCondition
|
from launch.conditions import UnlessCondition
|
||||||
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch.actions import ExecuteProcess
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
|
@ -54,18 +59,26 @@ def generate_launch_description():
|
||||||
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
|
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
|
||||||
|
|
||||||
# Load controllers from YAML configuration file
|
# Load controllers from YAML configuration file
|
||||||
controller_configurations = PathJoinSubstitution([
|
"""controller_configurations_path = PathJoinSubstitution([
|
||||||
FindPackageShare(LaunchConfiguration("controller_configurations_package")),
|
FindPackageShare(LaunchConfiguration("controller_configurations_package")),
|
||||||
LaunchConfiguration("controller_configurations")
|
LaunchConfiguration("controller_configurations")
|
||||||
])
|
])"""
|
||||||
|
print(robot_description)
|
||||||
|
ros2_controllers_path = os.path.join(
|
||||||
|
get_package_share_directory("rasms_moveit_config"),
|
||||||
|
"config",
|
||||||
|
"rasms_controllers.yml",
|
||||||
|
)
|
||||||
|
|
||||||
# Prepare controller manager and other required nodes
|
# Prepare controller manager and other required nodes
|
||||||
controller_manager = Node(
|
controller_manager = Node(
|
||||||
package="controller_manager",
|
package="controller_manager",
|
||||||
executable="ros2_control_node",
|
executable="ros2_control_node",
|
||||||
parameters=[robot_description, controller_configurations],
|
parameters=[robot_description, ros2_controllers_path],
|
||||||
output="screen",
|
output={
|
||||||
condition=UnlessCondition(LaunchConfiguration("sim"))
|
"stdout": "screen",
|
||||||
|
"stderr": "screen"
|
||||||
|
},
|
||||||
)
|
)
|
||||||
|
|
||||||
robot_state_publisher = Node(
|
robot_state_publisher = Node(
|
||||||
|
|
@ -75,7 +88,15 @@ def generate_launch_description():
|
||||||
parameters=[robot_description]
|
parameters=[robot_description]
|
||||||
)
|
)
|
||||||
|
|
||||||
joint_state_broadcaster = Node(
|
static_tf = Node(
|
||||||
|
package="tf2_ros",
|
||||||
|
executable="static_transform_publisher",
|
||||||
|
name="static_transform_publisher",
|
||||||
|
output="log",
|
||||||
|
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"],
|
||||||
|
)
|
||||||
|
|
||||||
|
"""joint_state_broadcaster = Node(
|
||||||
package="controller_manager",
|
package="controller_manager",
|
||||||
executable="spawner.py",
|
executable="spawner.py",
|
||||||
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||||
|
|
@ -84,13 +105,27 @@ def generate_launch_description():
|
||||||
controller = Node(
|
controller = Node(
|
||||||
package="controller_manager",
|
package="controller_manager",
|
||||||
executable="spawner.py",
|
executable="spawner.py",
|
||||||
arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"],
|
arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"],
|
||||||
)
|
)"""
|
||||||
|
|
||||||
|
load_controllers = []
|
||||||
|
for controller in [
|
||||||
|
"position_trajectory_controller",
|
||||||
|
"joint_state_broadcaster",
|
||||||
|
]:
|
||||||
|
load_controllers += [
|
||||||
|
ExecuteProcess(
|
||||||
|
cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
|
||||||
|
shell=True,
|
||||||
|
output="screen",
|
||||||
|
)
|
||||||
|
]
|
||||||
|
|
||||||
return LaunchDescription(
|
return LaunchDescription(
|
||||||
launch_args + [
|
launch_args +
|
||||||
|
[
|
||||||
controller_manager,
|
controller_manager,
|
||||||
robot_state_publisher,
|
robot_state_publisher,
|
||||||
joint_state_broadcaster,
|
static_tf
|
||||||
controller
|
]
|
||||||
])
|
+ load_controllers)
|
||||||
|
|
|
||||||
|
|
@ -63,7 +63,7 @@ def launch_setup(context, *args, **kwargs):
|
||||||
}
|
}
|
||||||
|
|
||||||
# Planning
|
# Planning
|
||||||
ompl_yaml = load_yaml("rasms_moveit_config", "config/ompl_planning.yml")
|
#ompl_yaml = load_yaml("rasms_moveit_config", "config/ompl_planning.yml")
|
||||||
|
|
||||||
planning = {
|
planning = {
|
||||||
"move_group": {
|
"move_group": {
|
||||||
|
|
@ -78,49 +78,79 @@ def launch_setup(context, *args, **kwargs):
|
||||||
"moveit_manage_controllers": True}
|
"moveit_manage_controllers": True}
|
||||||
|
|
||||||
# Controllers
|
# Controllers
|
||||||
controllers_yaml = load_yaml(
|
moveit_simple_controller_yml = load_yaml(
|
||||||
LaunchConfiguration("moveit_controller_configurations_package").perform(context),
|
LaunchConfiguration("moveit_controller_configurations_package").perform(context),
|
||||||
LaunchConfiguration("moveit_controller_configurations").perform(context)
|
LaunchConfiguration("moveit_controller_configurations").perform(context)
|
||||||
)
|
)
|
||||||
|
|
||||||
moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml,
|
moveit_controllers = {"moveit_simple_controller_manager": moveit_simple_controller_yml,
|
||||||
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}
|
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}
|
||||||
|
|
||||||
|
ompl_planning_pipeline_config = {
|
||||||
|
"ompl": {
|
||||||
|
"planning_plugin": "ompl_interface/OMPLPlanner",
|
||||||
|
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization 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,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
ompl_planning_yml = load_yaml(
|
||||||
|
"rasms_moveit_config", "config/ompl_planning.yml"
|
||||||
|
)
|
||||||
|
ompl_planning_pipeline_config["ompl"].update(ompl_planning_yml)
|
||||||
# Planning scene
|
# Planning scene
|
||||||
planning_scene_monitor_parameters = {"publish_planning_scene": True,
|
planning_scene_monitor_parameters = {"publish_planning_scene": True,
|
||||||
"publish_geometry_updates": True,
|
"publish_geometry_updates": True,
|
||||||
"publish_state_updates": True,
|
"publish_state_updates": True,
|
||||||
"publish_transforms_updates": True}
|
"publish_transforms_updates": True}
|
||||||
|
|
||||||
|
moveit_cpp_yaml_file_name = (
|
||||||
|
get_package_share_directory("rasms_moveit_config") + "/config/moveit_cpp.yml"
|
||||||
|
)
|
||||||
|
|
||||||
# Time configuration
|
# Time configuration
|
||||||
use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}
|
use_sim_time = {"use_sime_time": LaunchConfiguration("sim")}
|
||||||
|
|
||||||
# Prepare move group node
|
# Prepare move group node
|
||||||
move_group_node = Node(
|
#"""move_group_node = Node(
|
||||||
package="moveit_ros_move_group",
|
# package="moveit_ros_move_group",
|
||||||
executable="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
|
||||||
|
# ]
|
||||||
|
#)"""
|
||||||
|
|
||||||
|
moveit_cpp_node = Node(
|
||||||
|
name="moveit_cpp_actions",
|
||||||
|
package="rasms_moveit_actions",
|
||||||
|
executable="rasms_moveit",
|
||||||
output="screen",
|
output="screen",
|
||||||
arguments=["--ros-args"],
|
|
||||||
parameters=[
|
parameters=[
|
||||||
|
moveit_cpp_yaml_file_name,
|
||||||
robot_description,
|
robot_description,
|
||||||
robot_description_semantic,
|
robot_description_semantic,
|
||||||
kinematics_yaml,
|
kinematics_yaml,
|
||||||
robot_description_planning,
|
ompl_planning_pipeline_config,
|
||||||
ompl_yaml,
|
|
||||||
planning,
|
|
||||||
trajectory_execution,
|
|
||||||
moveit_controllers,
|
moveit_controllers,
|
||||||
planning_scene_monitor_parameters,
|
],
|
||||||
use_sim_time
|
|
||||||
]
|
|
||||||
)
|
)
|
||||||
|
|
||||||
robot_state_publisher = Node(
|
"""robot_state_publisher = Node(
|
||||||
package="robot_state_publisher",
|
package="robot_state_publisher",
|
||||||
executable="robot_state_publisher",
|
executable="robot_state_publisher",
|
||||||
output="screen",
|
output="screen",
|
||||||
parameters=[robot_description]
|
parameters=[robot_description]
|
||||||
)
|
)"""
|
||||||
|
|
||||||
spawn_controller = Node(
|
spawn_controller = Node(
|
||||||
package="controller_manager",
|
package="controller_manager",
|
||||||
|
|
@ -130,7 +160,7 @@ def launch_setup(context, *args, **kwargs):
|
||||||
)
|
)
|
||||||
|
|
||||||
# RViz
|
# RViz
|
||||||
rviz_config = PathJoinSubstitution([FindPackageShare("rasms_moveit_config"), "config/robasm_sgonov.rviz"])
|
rviz_config = PathJoinSubstitution([FindPackageShare("rasms_moveit_config"), "config/rasms.rviz"])
|
||||||
|
|
||||||
rviz = Node(
|
rviz = Node(
|
||||||
package="rviz2",
|
package="rviz2",
|
||||||
|
|
@ -138,8 +168,6 @@ def launch_setup(context, *args, **kwargs):
|
||||||
parameters=[
|
parameters=[
|
||||||
robot_description,
|
robot_description,
|
||||||
robot_description_semantic,
|
robot_description_semantic,
|
||||||
kinematics_yaml,
|
|
||||||
planning,
|
|
||||||
use_sim_time
|
use_sim_time
|
||||||
],
|
],
|
||||||
arguments=[
|
arguments=[
|
||||||
|
|
@ -148,9 +176,10 @@ def launch_setup(context, *args, **kwargs):
|
||||||
)
|
)
|
||||||
|
|
||||||
return [
|
return [
|
||||||
move_group_node,
|
#move_group_node,
|
||||||
|
moveit_cpp_node,
|
||||||
rviz,
|
rviz,
|
||||||
robot_state_publisher,
|
#robot_state_publisher,
|
||||||
spawn_controller
|
spawn_controller
|
||||||
]
|
]
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -28,10 +28,6 @@ def generate_launch_description():
|
||||||
)
|
)
|
||||||
)
|
)
|
||||||
|
|
||||||
# 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(
|
spawn_entity = Node(
|
||||||
package="gazebo_ros",
|
package="gazebo_ros",
|
||||||
executable="spawn_entity.py",
|
executable="spawn_entity.py",
|
||||||
|
|
|
||||||
|
|
@ -3,7 +3,7 @@
|
||||||
This is a format for representing semantic information about the robot structure.
|
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
|
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||||
-->
|
-->
|
||||||
<robot name="rasms_description">
|
<robot name="rasms">
|
||||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
<!--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-->
|
<!--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-->
|
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||||
|
|
@ -13,14 +13,6 @@
|
||||||
<chain base_link="base" tip_link="link6"/>
|
<chain base_link="base" tip_link="link6"/>
|
||||||
</group>
|
</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 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="rasms_description_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="rasms_description_arm">
|
<group_state name="zero_pose" group="rasms_description_arm">
|
||||||
<joint name="joint1" value="0"/>
|
<joint name="joint1" value="0"/>
|
||||||
<joint name="joint2" value="0"/>
|
<joint name="joint2" value="0"/>
|
||||||
|
|
@ -29,6 +21,14 @@
|
||||||
<joint name="joint5" value="0"/>
|
<joint name="joint5" value="0"/>
|
||||||
<joint name="joint6" value="0"/>
|
<joint name="joint6" value="0"/>
|
||||||
</group_state>
|
</group_state>
|
||||||
|
<group_state name="pose1" group="rasms_description_arm">
|
||||||
|
<joint name="joint1" value="0"/>
|
||||||
|
<joint name="joint2" value="1"/>
|
||||||
|
<joint name="joint3" value="0.4339"/>
|
||||||
|
<joint name="joint4" value="0"/>
|
||||||
|
<joint name="joint5" value="0.9199"/>
|
||||||
|
<joint name="joint6" value="-1"/>
|
||||||
|
</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: 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="base" link2="link1" reason="Adjacent"/>
|
||||||
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
|
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue