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 16f731dc5..bb5c9731e 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -175,6 +175,13 @@ int main(int argc, char** argv) 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); ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); @@ -207,7 +214,7 @@ int main(int argc, char** argv) // 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 + // satisfies the path constraints. So we need to set the start // state to a new pose. robot_state::RobotState start_state(*move_group.getCurrentState()); geometry_msgs::Pose start_pose2; @@ -263,11 +270,6 @@ int main(int argc, char** argv) target_pose3.position.x -= 0.2; waypoints.push_back(target_pose3); // up and left - // Cartesian motions are frequently needed to be slower for actions such as approach and retreat - // grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor - // of the maxiumum speed of each joint. Note this is not the speed of the end effector point. - move_group.setMaxVelocityScalingFactor(0.1); - // 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. @@ -288,6 +290,16 @@ int main(int argc, char** argv) 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 by wrapping it in a plan like this. + moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan; + cartesian_plan.trajectory_ = trajectory; + move_group.execute(cartesian_plan); + // Adding/Removing Objects and Attaching/Detaching Objects // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // @@ -328,7 +340,7 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - // Wait for MoveGroup to recieve and process the collision object message + // Wait for MoveGroup to receive and process the collision object message 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 @@ -358,7 +370,7 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - /* Wait for MoveGroup to recieve and process the attached collision object message */ + /* 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 attaches to the " "robot"); @@ -370,7 +382,7 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - /* Wait for MoveGroup to recieve and process the attached collision object message */ + /* 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 detaches to the " "robot"); @@ -384,7 +396,7 @@ int main(int argc, char** argv) visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); - /* Wait for MoveGroup to recieve and process the attached collision object message */ + /* 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 diff --git a/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst b/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst index c9d852988..b03ce7747 100644 --- a/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst +++ b/doc/quickstart_in_rviz/quickstart_in_rviz_tutorial.rst @@ -129,6 +129,13 @@ Note what happens when you try to move an end-effector out of its reachable work .. image:: rviz_plugin_invalid.png :width: 700px +Moving Joints or in Null Space +++++++++++++++++++++++++++++++ +You can use the **Joints** tab to move single joints and the redundant joints of 7-DOF robots. Try moving the "null space exploration" slider as shown in the animation below. + +.. image:: rviz_joints_nullspace.webm + :width: 700px + Step 4: Use Motion Planning with the Panda ------------------------------------------- @@ -152,7 +159,7 @@ Step 4: Use Motion Planning with the Panda Introspecting Trajectory Waypoints ++++++++++++++++++++++++++++++++++ -You can visually introspect trajectory point by point on RViz. +You can visually introspect trajectories point by point in RViz. * From "`Panels`" menu, select "`MotionPlanning - Slider`". You'll see a new Slider panel on RViz. @@ -165,6 +172,28 @@ NOTE: Once you placed your EEF to a new goal, be sure to run `Plan` before runni .. image:: rviz_plugin_slider.png :width: 700px +Plan Cartesian motions +++++++++++++++++++++++ + +If the "Use Cartesian Path" checkbox is activated, the robot will attempt to move the end effector linearly in cartesian space. + +.. image:: rviz_plan_free.png + :width: 700px + +.. image:: rviz_plan_cartesian.png + :width: 700px + + +Executing Trajectories, Adjusting Speed ++++++++++++++++++++++++++++++++++++++++ + +Clicking "Plan & Execute" or "Execute" after a successful plan will send the trajectory to the robot - in this tutorial, since you used `demo.launch`, the robot is only simulated. + +Initially, the default velocity and acceleration are scaled to 10% (`0.1`) of the robot's maximum. You can change these scaling factors in the Planning tab shown below, or change these default values in the `moveit_config` of your robot (in `joint_limits.yaml`). + +.. image:: rviz_plugin_collision_aware_ik_checkbox.png + :width: 700px + Next Steps ---------- @@ -175,7 +204,7 @@ Many of the tutorials use ``moveit_visual_tools`` to step through a demo. Before From "`Panels`" menu, select "`RvizVisualToolsGui`". You'll see the new panel added to RViz. -.. image:: rviz_pannels.png +.. image:: rviz_panels.png :width: 700px Saving Your Configuration diff --git a/doc/quickstart_in_rviz/rviz_joints_nullspace.webm b/doc/quickstart_in_rviz/rviz_joints_nullspace.webm new file mode 100644 index 000000000..d626dd165 Binary files /dev/null and b/doc/quickstart_in_rviz/rviz_joints_nullspace.webm differ diff --git a/doc/quickstart_in_rviz/rviz_pannels.png b/doc/quickstart_in_rviz/rviz_panels.png similarity index 100% rename from doc/quickstart_in_rviz/rviz_pannels.png rename to doc/quickstart_in_rviz/rviz_panels.png diff --git a/doc/quickstart_in_rviz/rviz_plan_cartesian.png b/doc/quickstart_in_rviz/rviz_plan_cartesian.png new file mode 100644 index 000000000..028067738 Binary files /dev/null and b/doc/quickstart_in_rviz/rviz_plan_cartesian.png differ diff --git a/doc/quickstart_in_rviz/rviz_plan_free.png b/doc/quickstart_in_rviz/rviz_plan_free.png new file mode 100644 index 000000000..5b8ddc86a Binary files /dev/null and b/doc/quickstart_in_rviz/rviz_plan_free.png differ diff --git a/doc/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png b/doc/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png index c978b2446..62cd61813 100644 Binary files a/doc/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png and b/doc/quickstart_in_rviz/rviz_plugin_collision_aware_ik_checkbox.png differ