🔨 add move_group cpp interface that's work
This commit is contained in:
parent
b1ae818aa0
commit
4fe0324b25
10 changed files with 1135 additions and 60 deletions
366
rasms_description/urdf/rasms_description.urdf
Normal file
366
rasms_description/urdf/rasms_description.urdf
Normal file
|
@ -0,0 +1,366 @@
|
|||
<robot name="rasms">
|
||||
<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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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://rasms_description/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>
|
||||
</robot>
|
|
@ -62,10 +62,18 @@ target_include_directories(rasms_moveit PUBLIC include)
|
|||
ament_target_dependencies(rasms_moveit
|
||||
${THIS_PACKAGE_INCLUDE_DEPENDS} Boost
|
||||
)
|
||||
|
||||
install(TARGETS rasms_moveit
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
add_executable(rasms_move_group_interface src/rasms_move_group_interface.cpp)
|
||||
target_include_directories(rasms_move_group_interface PUBLIC include)
|
||||
ament_target_dependencies(rasms_move_group_interface
|
||||
${THIS_PACKAGE_INCLUDE_DEPENDS} Boost
|
||||
)
|
||||
install(TARGETS rasms_move_group_interface
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
#install(TARGETS rasms_moveit
|
||||
# EXPORT export_${PROJECT_NAME}
|
||||
# LIBRARY DESTINATION lib
|
||||
|
|
501
rasms_moveit_actions/src/rasms_move_group_interface.cpp
Normal file
501
rasms_moveit_actions/src/rasms_move_group_interface.cpp
Normal file
|
@ -0,0 +1,501 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2013, SRI International
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of SRI International nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
|
||||
|
||||
#include <moveit/move_group_interface/move_group_interface.h>
|
||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||
|
||||
#include <moveit_msgs/msg/display_robot_state.hpp>
|
||||
#include <moveit_msgs/msg/display_trajectory.hpp>
|
||||
|
||||
#include <moveit_msgs/msg/attached_collision_object.hpp>
|
||||
#include <moveit_msgs/msg/collision_object.hpp>
|
||||
|
||||
#include <moveit_visual_tools/moveit_visual_tools.h>
|
||||
|
||||
// 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("move_group_demo");
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.automatically_declare_parameters_from_overrides(true);
|
||||
auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", 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(move_group_node);
|
||||
std::thread([&executor]() { executor.spin(); }).detach();
|
||||
|
||||
// BEGIN_TUTORIAL
|
||||
//
|
||||
// Setup
|
||||
// ^^^^^
|
||||
//
|
||||
// MoveIt operates on sets of joints called "planning groups" and stores them in an object called
|
||||
// the `JointModelGroup`. Throughout MoveIt, the terms "planning group" and "joint model group"
|
||||
// are used interchangably.
|
||||
static const std::string PLANNING_GROUP = "rasms_arm_group";
|
||||
|
||||
// The
|
||||
// :moveit_codedir:`MoveGroupInterface<moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h>`
|
||||
// class can be easily set up using just the name of the planning group you would like to control and plan for.
|
||||
moveit::planning_interface::MoveGroupInterface move_group(move_group_node, PLANNING_GROUP);
|
||||
|
||||
// We will use the
|
||||
// :moveit_codedir:`PlanningSceneInterface<moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h>`
|
||||
// class to add and remove collision objects in our "virtual world" scene
|
||||
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
|
||||
|
||||
// Raw pointers are frequently used to refer to the planning group for improved performance.
|
||||
const moveit::core::JointModelGroup* joint_model_group =
|
||||
move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
|
||||
|
||||
// Visualization
|
||||
// ^^^^^^^^^^^^^
|
||||
namespace rvt = rviz_visual_tools;
|
||||
moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base", "move_group_tutorial",
|
||||
move_group.getRobotModel());
|
||||
|
||||
visual_tools.deleteAllMarkers();
|
||||
|
||||
/* Remote control is an introspection tool that allows users to step through a high level script */
|
||||
/* via buttons and keyboard shortcuts in RViz */
|
||||
visual_tools.loadRemoteControl();
|
||||
|
||||
// RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
|
||||
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
|
||||
text_pose.translation().z() = 1.0;
|
||||
visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE);
|
||||
|
||||
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
|
||||
visual_tools.trigger();
|
||||
|
||||
// Getting Basic Information
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// We can print the name of the reference frame for this robot.
|
||||
RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str());
|
||||
|
||||
// We can also print the name of the end-effector link for this group.
|
||||
RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str());
|
||||
|
||||
// We can get a list of all the groups in the robot:
|
||||
RCLCPP_INFO(LOGGER, "Available Planning Groups:");
|
||||
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
|
||||
std::ostream_iterator<std::string>(std::cout, ", "));
|
||||
|
||||
// Start the demo
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
|
||||
|
||||
// .. _move_group_interface-planning-to-pose-goal:
|
||||
//
|
||||
// Planning to a Pose goal
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^
|
||||
// We can plan a motion for this group to a desired pose for the
|
||||
// end-effector.
|
||||
geometry_msgs::msg::Pose target_pose1;
|
||||
target_pose1.orientation.w = 1.0;
|
||||
target_pose1.position.x = 0.28;
|
||||
target_pose1.position.y = -0.2;
|
||||
target_pose1.position.z = 0.5;
|
||||
move_group.setPoseTarget(target_pose1);
|
||||
|
||||
// Now, we call the planner to compute the plan and visualize it.
|
||||
// Note that we are just planning, not asking move_group
|
||||
// to actually move the robot.
|
||||
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
|
||||
|
||||
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
|
||||
|
||||
// Visualizing plans
|
||||
// ^^^^^^^^^^^^^^^^^
|
||||
// We can also visualize the plan as a line with markers in RViz.
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line");
|
||||
visual_tools.publishAxisLabeled(target_pose1, "pose1");
|
||||
visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||
|
||||
// Moving to a pose goal
|
||||
// ^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// Moving to a pose goal is similar to the step above
|
||||
// except we now use the ``move()`` function. Note that
|
||||
// the pose goal we had set earlier is still active
|
||||
// and so the robot will try to move to that goal. We will
|
||||
// not use that function in this tutorial since it is
|
||||
// a blocking function and requires a controller to be active
|
||||
// and report success on execution of a trajectory.
|
||||
|
||||
/* Uncomment below line when working with a real robot */
|
||||
/* move_group.move(); */
|
||||
|
||||
// Planning to a joint-space goal
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// Let's set a joint space goal and move towards it. This will replace the
|
||||
// pose target we set above.
|
||||
//
|
||||
// To start, we'll create an pointer that references the current robot's state.
|
||||
// RobotState is the object that contains all the current position/velocity/acceleration data.
|
||||
moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10);
|
||||
//
|
||||
// Next get the current set of joint values for the group.
|
||||
std::vector<double> joint_group_positions;
|
||||
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
|
||||
|
||||
// Now, let's modify one of the joints, plan to the new joint space goal, and visualize the plan.
|
||||
joint_group_positions[0] = -1.0; // radians
|
||||
move_group.setJointValueTarget(joint_group_positions);
|
||||
|
||||
// We lower the allowed maximum velocity and acceleration to 5% of their maximum.
|
||||
// The default values are 10% (0.1).
|
||||
// Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
|
||||
// or set explicit factors in your code if you need your robot to move faster.
|
||||
move_group.setMaxVelocityScalingFactor(0.05);
|
||||
move_group.setMaxAccelerationScalingFactor(0.05);
|
||||
|
||||
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
|
||||
|
||||
// Visualize the plan in RViz:
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||
|
||||
// Planning with Path Constraints
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// Path constraints can easily be specified for a link on the robot.
|
||||
// Let's specify a path constraint and a pose goal for our group.
|
||||
// First define the path constraint.
|
||||
moveit_msgs::msg::OrientationConstraint ocm;
|
||||
ocm.link_name = "link6";
|
||||
ocm.header.frame_id = "base";
|
||||
ocm.orientation.w = 1.0;
|
||||
ocm.absolute_x_axis_tolerance = 0.1;
|
||||
ocm.absolute_y_axis_tolerance = 0.1;
|
||||
ocm.absolute_z_axis_tolerance = 0.1;
|
||||
ocm.weight = 1.0;
|
||||
|
||||
// Now, set it as the path constraint for the group.
|
||||
moveit_msgs::msg::Constraints test_constraints;
|
||||
test_constraints.orientation_constraints.push_back(ocm);
|
||||
move_group.setPathConstraints(test_constraints);
|
||||
|
||||
// Enforce Planning in Joint Space
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// Depending on the planning problem MoveIt chooses between
|
||||
// ``joint space`` and ``cartesian space`` for problem representation.
|
||||
// Setting the group parameter ``enforce_joint_model_state_space:true`` in
|
||||
// the ompl_planning.yaml file enforces the use of ``joint space`` for all plans.
|
||||
//
|
||||
// By default, planning requests with orientation path constraints
|
||||
// are sampled in ``cartesian space`` so that invoking IK serves as a
|
||||
// generative sampler.
|
||||
//
|
||||
// By enforcing ``joint space``, the planning process will use rejection
|
||||
// sampling to find valid requests. Please note that this might
|
||||
// increase planning time considerably.
|
||||
//
|
||||
// We will reuse the old goal that we had and plan to it.
|
||||
// Note that this will only work if the current state already
|
||||
// satisfies the path constraints. So we need to set the start
|
||||
// state to a new pose.
|
||||
moveit::core::RobotState start_state(*move_group.getCurrentState());
|
||||
geometry_msgs::msg::Pose start_pose2;
|
||||
start_pose2.orientation.w = 1.0;
|
||||
start_pose2.position.x = 0.55;
|
||||
start_pose2.position.y = -0.05;
|
||||
start_pose2.position.z = 0.8;
|
||||
start_state.setFromIK(joint_model_group, start_pose2);
|
||||
move_group.setStartState(start_state);
|
||||
|
||||
// Now, we will plan to the earlier pose target from the new
|
||||
// start state that we just created.
|
||||
move_group.setPoseTarget(target_pose1);
|
||||
|
||||
// Planning with constraints can be slow because every sample must call an inverse kinematics solver.
|
||||
// Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
|
||||
move_group.setPlanningTime(10.0);
|
||||
|
||||
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
|
||||
|
||||
// Visualize the plan in RViz:
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.publishAxisLabeled(start_pose2, "start");
|
||||
visual_tools.publishAxisLabeled(target_pose1, "goal");
|
||||
visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||
|
||||
// When done with the path constraint, be sure to clear it.
|
||||
move_group.clearPathConstraints();
|
||||
|
||||
// Cartesian Paths
|
||||
// ^^^^^^^^^^^^^^^
|
||||
// You can plan a Cartesian path directly by specifying a list of waypoints
|
||||
// for the end-effector to go through. Note that we are starting
|
||||
// from the new start state above. The initial pose (start state) does not
|
||||
// need to be added to the waypoint list but adding it can help with visualizations
|
||||
std::vector<geometry_msgs::msg::Pose> waypoints;
|
||||
waypoints.push_back(start_pose2);
|
||||
|
||||
geometry_msgs::msg::Pose target_pose3 = start_pose2;
|
||||
|
||||
target_pose3.position.z -= 0.2;
|
||||
waypoints.push_back(target_pose3); // down
|
||||
|
||||
target_pose3.position.y -= 0.2;
|
||||
waypoints.push_back(target_pose3); // right
|
||||
|
||||
target_pose3.position.z += 0.2;
|
||||
target_pose3.position.y += 0.2;
|
||||
target_pose3.position.x -= 0.2;
|
||||
waypoints.push_back(target_pose3); // up and left
|
||||
|
||||
// We want the Cartesian path to be interpolated at a resolution of 1 cm
|
||||
// which is why we will specify 0.01 as the max step in Cartesian
|
||||
// translation. We will specify the jump threshold as 0.0, effectively disabling it.
|
||||
// Warning - disabling the jump threshold while operating real hardware can cause
|
||||
// large unpredictable motions of redundant joints and could be a safety issue
|
||||
moveit_msgs::msg::RobotTrajectory trajectory;
|
||||
const double jump_threshold = 0.0;
|
||||
const double eef_step = 0.01;
|
||||
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
|
||||
|
||||
// Visualize the plan in RViz
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
|
||||
for (std::size_t i = 0; i < waypoints.size(); ++i)
|
||||
visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||
|
||||
// Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian
|
||||
// plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
|
||||
// the trajectory manually, as described `here <https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4>`_.
|
||||
// Pull requests are welcome.
|
||||
//
|
||||
// You can execute a trajectory like this.
|
||||
/* move_group.execute(trajectory); */
|
||||
|
||||
// Adding objects to the environment
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// First, let's plan to another simple goal with no objects in the way.
|
||||
move_group.setStartState(*move_group.getCurrentState());
|
||||
geometry_msgs::msg::Pose another_pose;
|
||||
another_pose.orientation.w = 0;
|
||||
another_pose.orientation.x = -1.0;
|
||||
another_pose.position.x = 0.7;
|
||||
another_pose.position.y = 0.0;
|
||||
another_pose.position.z = 0.59;
|
||||
move_group.setPoseTarget(another_pose);
|
||||
|
||||
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");
|
||||
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishAxisLabeled(another_pose, "goal");
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
|
||||
|
||||
// The result may look like this:
|
||||
//
|
||||
// .. image:: ./move_group_interface_tutorial_clear_path.gif
|
||||
// :alt: animation showing the arm moving relatively straight toward the goal
|
||||
//
|
||||
// Now, let's define a collision object ROS message for the robot to avoid.
|
||||
moveit_msgs::msg::CollisionObject collision_object;
|
||||
collision_object.header.frame_id = move_group.getPlanningFrame();
|
||||
|
||||
// The id of the object is used to identify it.
|
||||
collision_object.id = "box1";
|
||||
|
||||
// Define a box to add to the world.
|
||||
shape_msgs::msg::SolidPrimitive primitive;
|
||||
primitive.type = primitive.BOX;
|
||||
primitive.dimensions.resize(3);
|
||||
primitive.dimensions[primitive.BOX_X] = 0.1;
|
||||
primitive.dimensions[primitive.BOX_Y] = 1.5;
|
||||
primitive.dimensions[primitive.BOX_Z] = 0.5;
|
||||
|
||||
// Define a pose for the box (specified relative to frame_id).
|
||||
geometry_msgs::msg::Pose box_pose;
|
||||
box_pose.orientation.w = 1.0;
|
||||
box_pose.position.x = 0.48;
|
||||
box_pose.position.y = 0.0;
|
||||
box_pose.position.z = 0.25;
|
||||
|
||||
collision_object.primitives.push_back(primitive);
|
||||
collision_object.primitive_poses.push_back(box_pose);
|
||||
collision_object.operation = collision_object.ADD;
|
||||
|
||||
std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
|
||||
collision_objects.push_back(collision_object);
|
||||
|
||||
// Now, let's add the collision object into the world
|
||||
// (using a vector that could contain additional objects)
|
||||
RCLCPP_INFO(LOGGER, "Add an object into the world");
|
||||
planning_scene_interface.addCollisionObjects(collision_objects);
|
||||
|
||||
// Show text in RViz of status and wait for MoveGroup to receive and process the collision object message
|
||||
visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
|
||||
|
||||
// Now, when we plan a trajectory it will avoid the obstacle.
|
||||
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED");
|
||||
visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
|
||||
|
||||
// The result may look like this:
|
||||
//
|
||||
// .. image:: ./move_group_interface_tutorial_avoid_path.gif
|
||||
// :alt: animation showing the arm moving avoiding the new obstacle
|
||||
//
|
||||
// Attaching objects to the robot
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// You can attach an object to the robot, so that it moves with the robot geometry.
|
||||
// This simulates picking up the object for the purpose of manipulating it.
|
||||
// The motion planning should avoid collisions between objects as well.
|
||||
moveit_msgs::msg::CollisionObject object_to_attach;
|
||||
object_to_attach.id = "cylinder1";
|
||||
|
||||
shape_msgs::msg::SolidPrimitive cylinder_primitive;
|
||||
cylinder_primitive.type = primitive.CYLINDER;
|
||||
cylinder_primitive.dimensions.resize(2);
|
||||
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
|
||||
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;
|
||||
|
||||
// We define the frame/pose for this cylinder so that it appears in the gripper.
|
||||
object_to_attach.header.frame_id = move_group.getEndEffectorLink();
|
||||
geometry_msgs::msg::Pose grab_pose;
|
||||
grab_pose.orientation.w = 1.0;
|
||||
grab_pose.position.z = 0.2;
|
||||
|
||||
// First, we add the object to the world (without using a vector).
|
||||
object_to_attach.primitives.push_back(cylinder_primitive);
|
||||
object_to_attach.primitive_poses.push_back(grab_pose);
|
||||
object_to_attach.operation = object_to_attach.ADD;
|
||||
planning_scene_interface.applyCollisionObject(object_to_attach);
|
||||
|
||||
// Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to.
|
||||
// We also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper.
|
||||
// You could also use applyAttachedCollisionObject to attach an object to the robot directly.
|
||||
RCLCPP_INFO(LOGGER, "Attach the object to the robot");
|
||||
std::vector<std::string> touch_links;
|
||||
touch_links.push_back("panda_rightfinger");
|
||||
touch_links.push_back("panda_leftfinger");
|
||||
move_group.attachObject(object_to_attach.id, "panda_hand", touch_links);
|
||||
|
||||
visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.trigger();
|
||||
|
||||
/* Wait for MoveGroup to receive and process the attached collision object message */
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");
|
||||
|
||||
// Replan, but now with the object in hand.
|
||||
move_group.setStartStateToCurrentState();
|
||||
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
|
||||
RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
|
||||
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
|
||||
visual_tools.trigger();
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");
|
||||
|
||||
// The result may look something like this:
|
||||
//
|
||||
// .. image:: ./move_group_interface_tutorial_attached_object.gif
|
||||
// :alt: animation showing the arm moving differently once the object is attached
|
||||
//
|
||||
// Detaching and Removing Objects
|
||||
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
//
|
||||
// Now, let's detach the cylinder from the robot's gripper.
|
||||
RCLCPP_INFO(LOGGER, "Detach the object from the robot");
|
||||
move_group.detachObject(object_to_attach.id);
|
||||
|
||||
// Show text in RViz of status
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.trigger();
|
||||
|
||||
/* Wait for MoveGroup to receive and process the attached collision object message */
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot");
|
||||
|
||||
// Now, let's remove the objects from the world.
|
||||
RCLCPP_INFO(LOGGER, "Remove the objects from the world");
|
||||
std::vector<std::string> object_ids;
|
||||
object_ids.push_back(collision_object.id);
|
||||
object_ids.push_back(object_to_attach.id);
|
||||
planning_scene_interface.removeCollisionObjects(object_ids);
|
||||
|
||||
// Show text in RViz of status
|
||||
visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE);
|
||||
visual_tools.trigger();
|
||||
|
||||
/* Wait for MoveGroup to receive and process the attached collision object message */
|
||||
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
|
||||
|
||||
// END_TUTORIAL
|
||||
visual_tools.deleteAllMarkers();
|
||||
visual_tools.trigger();
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
|
@ -39,7 +39,7 @@ int main(int argc, char** argv)
|
|||
// Setup
|
||||
// ^^^^^
|
||||
//
|
||||
static const std::string PLANNING_GROUP = "rasms_description_arm";
|
||||
static const std::string PLANNING_GROUP = "rasms_arm_group";
|
||||
static const std::string LOGNAME = "moveit_interface";
|
||||
|
||||
/* Otherwise robot with zeros joint_states */
|
||||
|
|
|
@ -1,4 +1,4 @@
|
|||
group_name:
|
||||
rasms_arm_group:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.005
|
||||
kinematics_solver_timeout: 0.05
|
||||
|
|
|
@ -123,8 +123,7 @@ planner_configs:
|
|||
TrajOptDefault:
|
||||
type: geometric::TrajOpt
|
||||
|
||||
rasms_description_arm:
|
||||
longest_valid_fraction: "0.001"
|
||||
rasms_arm_group:
|
||||
planner_configs:
|
||||
- SBLkConfigDefault
|
||||
- ESTkConfigDefault
|
||||
|
|
|
@ -7,7 +7,7 @@ Panels:
|
|||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 743
|
||||
Tree Height: 197
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
|
@ -53,7 +53,7 @@ Visualization Manager:
|
|||
Scene Geometry:
|
||||
Scene Alpha: 0.8999999761581421
|
||||
Scene Color: 50; 230; 50
|
||||
Scene Display Time: 0.009999999776482582
|
||||
Scene Display Time: 0.20000000298023224
|
||||
Show Scene Geometry: true
|
||||
Voxel Coloring: Z-Axis
|
||||
Voxel Rendering: Occupied Voxels
|
||||
|
@ -108,44 +108,6 @@ Visualization Manager:
|
|||
Show Robot Collision: false
|
||||
Show Robot Visual: true
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base:
|
||||
Value: true
|
||||
link1:
|
||||
Value: true
|
||||
link2:
|
||||
Value: true
|
||||
link3:
|
||||
Value: true
|
||||
link4:
|
||||
Value: true
|
||||
link5:
|
||||
Value: true
|
||||
link6:
|
||||
Value: true
|
||||
world:
|
||||
Value: true
|
||||
Marker Scale: 0.5
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
world:
|
||||
base:
|
||||
link1:
|
||||
link2:
|
||||
link3:
|
||||
link4:
|
||||
link5:
|
||||
link6:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Class: moveit_rviz_plugin/Trajectory
|
||||
Color Enabled: false
|
||||
Enabled: true
|
||||
|
@ -207,10 +169,183 @@ Visualization Manager:
|
|||
Trail Step Size: 1
|
||||
Trajectory Topic: /display_planned_path
|
||||
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: true
|
||||
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: /move_group/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: rasms_arm_group
|
||||
Query Goal State: false
|
||||
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
|
||||
- Class: rviz_default_plugins/MarkerArray
|
||||
Enabled: true
|
||||
Name: MarkerArray
|
||||
Namespaces:
|
||||
{}
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /move_group_tutorial
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: world
|
||||
Fixed Frame: base
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
|
@ -250,25 +385,25 @@ Visualization Manager:
|
|||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 2.2047624588012695
|
||||
Distance: 3.8008623123168945
|
||||
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
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.7153979539871216
|
||||
Pitch: 0.72039794921875
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.7453981041908264
|
||||
Yaw: 0.2953982949256897
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
|
@ -276,7 +411,11 @@ Window Geometry:
|
|||
Height: 1043
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 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
|
||||
MotionPlanning:
|
||||
collapsed: false
|
||||
MotionPlanning - Trajectory Slider:
|
||||
collapsed: false
|
||||
QMainWindow State: 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
|
||||
RvizVisualToolsGui:
|
||||
collapsed: false
|
||||
Selection:
|
||||
|
|
|
@ -70,8 +70,8 @@ def generate_launch_description():
|
|||
kinematics_yaml = load_yaml("rasms_moveit_config", "config/kinematics.yml")
|
||||
|
||||
# Update group name
|
||||
kinematics_yaml["rasms_description_arm"] = kinematics_yaml["group_name"]
|
||||
del kinematics_yaml["group_name"]
|
||||
#kinematics_yaml["rasms_arm_group"] = kinematics_yaml["group_name"]
|
||||
#del kinematics_yaml["group_name"]
|
||||
|
||||
# Joint limits
|
||||
robot_description_planning = {
|
||||
|
@ -185,8 +185,8 @@ def generate_launch_description():
|
|||
|
||||
launch_nodes = []
|
||||
launch_nodes.append(rviz)
|
||||
#launch_nodes.append(move_group_node)
|
||||
launch_nodes.append(moveit_cpp_node)
|
||||
launch_nodes.append(move_group_node)
|
||||
#launch_nodes.append(moveit_cpp_node)
|
||||
|
||||
|
||||
return LaunchDescription(
|
||||
|
|
62
rasms_moveit_config/launch/test_mg_interface.launch.py
Normal file
62
rasms_moveit_config/launch/test_mg_interface.launch.py
Normal file
|
@ -0,0 +1,62 @@
|
|||
import os
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import xacro
|
||||
|
||||
|
||||
def load_file(package_name, file_path):
|
||||
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 file:
|
||||
return file.read()
|
||||
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def load_yaml(package_name, file_path):
|
||||
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 file:
|
||||
return yaml.safe_load(file)
|
||||
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
||||
return None
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# planning_context
|
||||
robot_description_config = xacro.process_file(
|
||||
os.path.join(
|
||||
get_package_share_directory("rasms_description"),
|
||||
"urdf",
|
||||
"rasms_description.urdf.xacro",
|
||||
)
|
||||
)
|
||||
robot_description = {"robot_description": robot_description_config.toxml()}
|
||||
|
||||
robot_description_semantic_config = load_file(
|
||||
"rasms_moveit_config", "srdf/rasms_description.srdf"
|
||||
)
|
||||
robot_description_semantic = {
|
||||
"robot_description_semantic": robot_description_semantic_config
|
||||
}
|
||||
|
||||
kinematics_yaml = load_yaml(
|
||||
"rasms_moveit_config", "config/kinematics.yml"
|
||||
)
|
||||
|
||||
# MoveGroupInterface demo executable
|
||||
move_group_demo = Node(
|
||||
name="move_group_interface_tutorial",
|
||||
package="rasms_moveit_actions",
|
||||
executable="rasms_move_group_interface",
|
||||
output="screen",
|
||||
parameters=[robot_description, robot_description_semantic, kinematics_yaml],
|
||||
)
|
||||
|
||||
return LaunchDescription([move_group_demo])
|
|
@ -9,11 +9,11 @@
|
|||
<!--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="rasms_description_arm">
|
||||
<group name="rasms_arm_group">
|
||||
<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="zero_pose" group="rasms_description_arm">
|
||||
<group_state name="zero_pose" group="rasms_arm_group">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="0"/>
|
||||
<joint name="joint3" value="0"/>
|
||||
|
@ -21,7 +21,7 @@
|
|||
<joint name="joint5" value="0"/>
|
||||
<joint name="joint6" value="0"/>
|
||||
</group_state>
|
||||
<group_state name="pose1" group="rasms_description_arm">
|
||||
<group_state name="pose1" group="rasms_arm_group">
|
||||
<joint name="joint1" value="0"/>
|
||||
<joint name="joint2" value="1"/>
|
||||
<joint name="joint3" value="0.4339"/>
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue