From 41688c4497b35c6d1723f93aecc51e6c1945bb47 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 28 May 2021 10:14:14 -0600 Subject: [PATCH 1/2] Change comment style on move/execute example commands --- .../src/move_group_interface_tutorial.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 6d81543b4..d0f2f400d 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -152,9 +152,11 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - // Finally, to execute the trajectory stored in my_plan, you could use the following method call: + // Finally, to execute the trajectory stored in my_plan, you could use the following method call. // Note that this can lead to problems if the robot moved in the meanwhile. - // move_group_interface.execute(my_plan); + // This tutorial does not actually move the robot, so this line is commented out + + /* move_group_interface.execute(my_plan); */ // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ @@ -163,8 +165,9 @@ int main(int argc, char** argv) // the following is a more robust combination of the two-step plan+execute pattern shown above // and should be preferred. Note that the pose goal we had set earlier is still active, // so the robot will try to move to that goal. + // This tutorial does not actually move the robot, so this line is commented out - // move_group_interface.move(); + /* move_group_interface.move(); */ // Planning to a joint-space goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -320,8 +323,9 @@ int main(int argc, char** argv) // the trajectory manually, as described `here `_. // Pull requests are welcome. // - // You can execute a trajectory like this. - move_group_interface.execute(trajectory); + // You can execute a trajectory like this. This tutorial does not actually move the robot, so this line is commented out + + /* move_group_interface.execute(trajectory); */ // Adding objects to the environment // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 58d4ed7615e031867e2de84b2aefd87fe42f89e7 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Wed, 9 Jun 2021 15:59:21 -0600 Subject: [PATCH 2/2] Update move group interface tutorial to move the simulated robot --- .../move_group_interface_tutorial.rst | 6 +- .../src/move_group_interface_tutorial.cpp | 126 ++++++++---------- 2 files changed, 58 insertions(+), 74 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 9227c6567..4d1926189 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -30,9 +30,9 @@ Expected Output --------------- See the `YouTube video `_ at the top of this tutorial for expected output. In RViz, we should be able to see the following: 1. The robot moves its arm to the pose goal to its front. - 2. The robot moves its arm to the joint goal at its side. - 3. The robot moves its arm back to a new pose goal while maintaining the end-effector level. - 4. The robot moves its arm along the desired Cartesian path (a triangle down, right, up+left). + 2. The robot moves its arm back to a new pose goal while maintaining the end-effector level. + 3. The robot moves its arm along the desired Cartesian path (a triangle down, right, up+left). + 4. The robot moves its arm to the joint goal back at it's starting position. 5. A box object is added into the environment to the right of the arm. |B| diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index d0f2f400d..8e34f159c 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -116,6 +116,9 @@ int main(int argc, char** argv) std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(), std::ostream_iterator(std::cout, ", ")); + // We can get the current state of the robot as well + moveit::core::RobotStatePtr initial_state = move_group_interface.getCurrentState(); + // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); @@ -152,56 +155,11 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - // Finally, to execute the trajectory stored in my_plan, you could use the following method call. - // Note that this can lead to problems if the robot moved in the meanwhile. - // This tutorial does not actually move the robot, so this line is commented out - - /* move_group_interface.execute(my_plan); */ - // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ - // - // If you do not want to inspect the planned trajectory, - // the following is a more robust combination of the two-step plan+execute pattern shown above - // and should be preferred. Note that the pose goal we had set earlier is still active, - // so the robot will try to move to that goal. - // This tutorial does not actually move the robot, so this line is commented out - - /* move_group_interface.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_interface.getCurrentState(); - // - // 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] = -tau / 6; // -1/6 turn in radians - move_group_interface.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_interface.setMaxVelocityScalingFactor(0.05); - move_group_interface.setMaxAccelerationScalingFactor(0.05); - - success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "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(); + // Finally, to execute the trajectory stored in :code:`my_plan`, you can use the following method call. + // Note that this can lead to problems if the robot moved between planning and this call. + move_group_interface.execute(my_plan); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // Planning with Path Constraints @@ -240,22 +198,18 @@ int main(int argc, char** argv) // 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. + // We will plan to a new pose goal. // 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_interface.getCurrentState()); - geometry_msgs::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_interface.setStartState(start_state); - - // Now we will plan to the earlier pose target from the new - // start state that we have just created. - move_group_interface.setPoseTarget(target_pose1); + // satisfies the path constraints. + geometry_msgs::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; + + // Now we will plan from the current state to the new target pose that we have just created. + move_group_interface.setStartStateToCurrentState(); + move_group_interface.setPoseTarget(target_pose2); // Planning with constraints can be slow because every sample must call an inverse kinematics solver. // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed. @@ -266,12 +220,16 @@ int main(int argc, char** argv) // Visualize the plan in RViz visual_tools.deleteAllMarkers(); - visual_tools.publishAxisLabeled(start_pose2, "start"); + visual_tools.publishAxisLabeled(target_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("next step"); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // Execute the previous plan + move_group_interface.execute(my_plan); + 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_interface.clearPathConstraints(); @@ -280,12 +238,10 @@ int main(int argc, char** argv) // ^^^^^^^^^^^^^^^ // 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 + // from the new start target 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::Pose target_pose3 = start_pose2; + geometry_msgs::Pose target_pose3 = target_pose2; target_pose3.position.z -= 0.2; waypoints.push_back(target_pose3); // down @@ -322,10 +278,38 @@ int main(int argc, char** argv) // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time // the trajectory manually, as described `here `_. // Pull requests are welcome. + move_group_interface.execute(trajectory); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // Planning to a joint-space goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // - // You can execute a trajectory like this. This tutorial does not actually move the robot, so this line is commented out + // Let's set a joint space goal and move towards it. We will pick the initial state the robot started in at the + // beginning of the tutorial + std::vector joint_group_positions; + initial_state->copyJointGroupPositions(joint_model_group, joint_group_positions); + + // We can directly set a joint state target + move_group_interface.setJointValueTarget(joint_group_positions); + move_group_interface.setStartStateToCurrentState(); + + // 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_interface.setMaxVelocityScalingFactor(0.05); + move_group_interface.setMaxAccelerationScalingFactor(0.05); - /* move_group_interface.execute(trajectory); */ + // Visualize the plan in RViz + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Joint Space Move", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); + + // If you do not want to inspect the planned trajectory, the following is a more robust combination of the two-step + // plan+execute pattern shown above and should be preferred. + move_group_interface.move(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // Adding objects to the environment // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^