Skip to content

Rename variable in MoveGroupInterface tutorial #611

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
felixvd merged 2 commits into from
Mar 19, 2021
Merged

Rename variable in MoveGroupInterface tutorial #611

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b2d063c
Rename variable move_group to move_group_interface
j-kuehn Mar 16, 2021
4c8896c
Run clang-format
j-kuehn Mar 16, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 35 additions & 35 deletions doc/move_group_interface/src/move_group_interface_tutorial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,15 +71,15 @@ int main(int argc, char** argv)

// The :planning_interface:`MoveGroupInterface` class can be easily
// setup using just the name of the planning group you would like to control and plan for.
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);

// We will use the :planning_interface:`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);
move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

// Visualization
// ^^^^^^^^^^^^^
Expand All @@ -106,15 +106,15 @@ int main(int argc, char** argv)
// ^^^^^^^^^^^^^^^^^^^^^^^^^
//
// We can print the name of the reference frame for this robot.
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group_interface.getPlanningFrame().c_str());

// We can also print the name of the end-effector link for this group.
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group_interface.getEndEffectorLink().c_str());

// We can get a list of all the groups in the robot:
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
std::copy(move_group_interface.getJointModelGroupNames().begin(),
move_group_interface.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));

// Start the demo
// ^^^^^^^^^^^^^^^^^^^^^^^^^
Expand All @@ -131,14 +131,14 @@ int main(int argc, char** argv)
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);
move_group_interface.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
// Note that we are just planning, not asking move_group_interface
// to actually move the robot.
moveit::planning_interface::MoveGroupInterface::Plan my_plan;

bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

Expand All @@ -154,7 +154,7 @@ int main(int argc, char** argv)

// 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.execute(my_plan);
// move_group_interface.execute(my_plan);

// Moving to a pose goal
// ^^^^^^^^^^^^^^^^^^^^^
Expand All @@ -164,7 +164,7 @@ int main(int argc, char** argv)
// 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.

// move_group.move();
// move_group_interface.move();

// Planning to a joint-space goal
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand All @@ -174,24 +174,24 @@ int main(int argc, char** argv)
//
// 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();
moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState();
//
// 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] = -tau / 6; // -1/6 turn in radians
move_group.setJointValueTarget(joint_group_positions);
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.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
move_group_interface.setMaxVelocityScalingFactor(0.05);
move_group_interface.setMaxAccelerationScalingFactor(0.05);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
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
Expand Down Expand Up @@ -219,7 +219,7 @@ int main(int argc, char** argv)
// Now, set it as the path constraint for the group.
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
move_group_interface.setPathConstraints(test_constraints);

// Enforce Planning in Joint Space
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand All @@ -241,24 +241,24 @@ int main(int argc, char** argv)
// 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());
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.setStartState(start_state);
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.setPoseTarget(target_pose1);
move_group_interface.setPoseTarget(target_pose1);

// 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.
move_group.setPlanningTime(10.0);
move_group_interface.setPlanningTime(10.0);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

// Visualize the plan in RViz
Expand All @@ -271,7 +271,7 @@ int main(int argc, char** argv)
visual_tools.prompt("next step");

// When done with the path constraint be sure to clear it.
move_group.clearPathConstraints();
move_group_interface.clearPathConstraints();

// Cartesian Paths
// ^^^^^^^^^^^^^^^
Expand Down Expand Up @@ -303,7 +303,7 @@ int main(int argc, char** argv)
moveit_msgs::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);
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

// Visualize the plan in RViz
Expand All @@ -321,21 +321,21 @@ int main(int argc, char** argv)
// Pull requests are welcome.
//
// You can execute a trajectory like this.
move_group.execute(trajectory);
move_group_interface.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());
move_group_interface.setStartState(*move_group_interface.getCurrentState());
geometry_msgs::Pose another_pose;
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);
move_group_interface.setPoseTarget(another_pose);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED");

visual_tools.deleteAllMarkers();
Expand All @@ -351,7 +351,7 @@ int main(int argc, char** argv)
//
// Now let's define a collision object ROS message for the robot to avoid.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
collision_object.header.frame_id = move_group_interface.getPlanningFrame();

// The id of the object is used to identify it.
collision_object.id = "box1";
Expand Down Expand Up @@ -389,7 +389,7 @@ int main(int argc, char** argv)
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);
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "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);
Expand Down Expand Up @@ -417,7 +417,7 @@ int main(int argc, char** argv)
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();
object_to_attach.header.frame_id = move_group_interface.getEndEffectorLink();
geometry_msgs::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;
Expand All @@ -431,7 +431,7 @@ int main(int argc, char** argv)
// Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to.
// You could also use applyAttachedCollisionObject to attach an object to the robot directly.
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(object_to_attach.id, "panda_hand");
move_group_interface.attachObject(object_to_attach.id, "panda_hand");

visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
Expand All @@ -440,8 +440,8 @@ int main(int argc, char** argv)
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);
move_group_interface.setStartStateToCurrentState();
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
Expand All @@ -457,7 +457,7 @@ int main(int argc, char** argv)
//
// Now, let's detach the cylinder from the robot's gripper.
ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
move_group.detachObject(object_to_attach.id);
move_group_interface.detachObject(object_to_attach.id);

// Show text in RViz of status
visual_tools.deleteAllMarkers();
Expand Down