diff --git a/rasms_description/urdf/rasms_description.urdf b/rasms_description/urdf/rasms_description.urdf
new file mode 100644
index 0000000..a87e326
--- /dev/null
+++ b/rasms_description/urdf/rasms_description.urdf
@@ -0,0 +1,366 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/rasms_moveit_actions/CMakeLists.txt b/rasms_moveit_actions/CMakeLists.txt
index 46857ee..2b60d1e 100644
--- a/rasms_moveit_actions/CMakeLists.txt
+++ b/rasms_moveit_actions/CMakeLists.txt
@@ -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
diff --git a/rasms_moveit_actions/src/rasms_move_group_interface.cpp b/rasms_moveit_actions/src/rasms_move_group_interface.cpp
new file mode 100644
index 0000000..9aae6ae
--- /dev/null
+++ b/rasms_moveit_actions/src/rasms_move_group_interface.cpp
@@ -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
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+
+// 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`
+ // 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`
+ // 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::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 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 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 `_.
+ // 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 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 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 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;
+}
diff --git a/rasms_moveit_actions/src/run_moveit.cpp b/rasms_moveit_actions/src/run_moveit.cpp
index d75bd85..1ade3a7 100644
--- a/rasms_moveit_actions/src/run_moveit.cpp
+++ b/rasms_moveit_actions/src/run_moveit.cpp
@@ -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 */
diff --git a/rasms_moveit_config/config/kinematics.yml b/rasms_moveit_config/config/kinematics.yml
index 7ccf6ce..29496db 100644
--- a/rasms_moveit_config/config/kinematics.yml
+++ b/rasms_moveit_config/config/kinematics.yml
@@ -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
diff --git a/rasms_moveit_config/config/ompl_planning.yml b/rasms_moveit_config/config/ompl_planning.yml
index 2b8e8fc..661cf4c 100644
--- a/rasms_moveit_config/config/ompl_planning.yml
+++ b/rasms_moveit_config/config/ompl_planning.yml
@@ -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
diff --git a/rasms_moveit_config/config/rasms.rviz b/rasms_moveit_config/config/rasms.rviz
index 55bcdb5..fec5abb 100644
--- a/rasms_moveit_config/config/rasms.rviz
+++ b/rasms_moveit_config/config/rasms.rviz
@@ -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:
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:
diff --git a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py
index 3f4afdd..b1ce3ea 100644
--- a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py
+++ b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py
@@ -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(
diff --git a/rasms_moveit_config/launch/test_mg_interface.launch.py b/rasms_moveit_config/launch/test_mg_interface.launch.py
new file mode 100644
index 0000000..d82aff8
--- /dev/null
+++ b/rasms_moveit_config/launch/test_mg_interface.launch.py
@@ -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])
\ No newline at end of file
diff --git a/rasms_moveit_config/srdf/rasms_description.srdf b/rasms_moveit_config/srdf/rasms_description.srdf
index 985b4a7..8349da5 100644
--- a/rasms_moveit_config/srdf/rasms_description.srdf
+++ b/rasms_moveit_config/srdf/rasms_description.srdf
@@ -9,11 +9,11 @@
-
+
-
+
@@ -21,7 +21,7 @@
-
+