Skip to content
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

Jump to bottom

motion_planning_rviz_plugin : add CartesianPath planning check box #1238

Merged
merged 8 commits into from
Dec 6, 2018
Merged

motion_planning_rviz_plugin : add CartesianPath planning check box #1238

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
2f4206a
motion_planning_rviz_plugin : add CartesianPath planning check box
k-okada Jun 1, 2018
1af111f
remove cartesianPathToggled()
rhaschke Nov 29, 2018
a4922fc
cleanup
rhaschke Nov 29, 2018
77faad5
gracefully handle non-unique end-effector
rhaschke Nov 29, 2018
2e76c28
save/load status of CartesianPath checkbox
rhaschke Nov 29, 2018
989af56
report planning time
rhaschke Nov 29, 2018
d4c761c
disable checkbox if CartesianPath planning is not feasible
rhaschke Nov 29, 2018
d61f8ec
clang format
rhaschke Dec 4, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions ...n_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,8 @@ private Q_SLOTS:
void approximateIKChanged(int state);

// Planning tab
bool computeCartesianPlan();
bool computeJointSpacePlan();
void planButtonClicked();
void executeButtonClicked();
void planAndExecuteButtonClicked();
Expand Down
3 changes: 3 additions & 0 deletions moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1313,6 +1313,8 @@ void MotionPlanningDisplay::load(const rviz::Config& config)
if (config.mapGetFloat("MoveIt_Goal_Tolerance", &d))
frame_->ui_->goal_tolerance->setValue(d);
bool b;
if (config.mapGetBool("MoveIt_Use_Cartesian_Path", &b))
frame_->ui_->use_cartesian_path->setChecked(b);
if (config.mapGetBool("MoveIt_Use_Constraint_Aware_IK", &b))
frame_->ui_->collision_aware_ik->setChecked(b);
if (config.mapGetBool("MoveIt_Allow_Approximate_IK", &b))
Expand Down Expand Up @@ -1384,6 +1386,7 @@ void MotionPlanningDisplay::save(rviz::Config config) const
config.mapSetValue("MoveIt_Allow_Replanning", frame_->ui_->allow_replanning->isChecked());
config.mapSetValue("MoveIt_Allow_Sensor_Positioning", frame_->ui_->allow_looking->isChecked());
config.mapSetValue("MoveIt_Allow_External_Program", frame_->ui_->allow_external_program->isChecked());
config.mapSetValue("MoveIt_Use_Cartesian_Path", frame_->ui_->use_cartesian_path->isChecked());
config.mapSetValue("MoveIt_Use_Constraint_Aware_IK", frame_->ui_->collision_aware_ik->isChecked());
config.mapSetValue("MoveIt_Allow_Approximate_IK", frame_->ui_->approximate_ik->isChecked());

Expand Down
5 changes: 4 additions & 1 deletion moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,8 @@ void MotionPlanningFrame::changePlanningGroupHelper()
std::string group = planning_display_->getCurrentPlanningGroup();
planning_display_->addMainLoopJob(
boost::bind(&MotionPlanningParamWidget::setGroupName, ui_->planner_param_treeview, group));
planning_display_->addMainLoopJob([=](){ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group));});
planning_display_->addMainLoopJob(
[=]() { ui_->planning_group_combo_box->setCurrentText(QString::fromStdString(group)); });

if (!group.empty() && robot_model)
{
Expand Down Expand Up @@ -351,6 +352,8 @@ void MotionPlanningFrame::changePlanningGroupHelper()
{
move_group_->allowLooking(ui_->allow_looking->isChecked());
move_group_->allowReplanning(ui_->allow_replanning->isChecked());
bool hasUniqueEndeffector = !move_group_->getEndEffectorLink().empty();
planning_display_->addMainLoopJob([=]() { ui_->use_cartesian_path->setEnabled(hasUniqueEndeffector); });
moveit_msgs::PlannerInterfaceDescription desc;
if (move_group_->getInterfaceDescription(desc))
planning_display_->addMainLoopJob(boost::bind(&MotionPlanningFrame::populatePlannersList, this, desc));
Expand Down
78 changes: 70 additions & 8 deletions moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@

#include <std_srvs/Empty.h>
#include <moveit_msgs/RobotState.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include "ui_motion_planning_rviz_plugin_frame.h"

Expand Down Expand Up @@ -108,6 +110,59 @@ void MotionPlanningFrame::onClearOctomapClicked()
clear_octomap_service_client_.call(srv);
}

bool MotionPlanningFrame::computeCartesianPlan()
{
ros::WallTime start = ros::WallTime::now();
// get goal pose
robot_state::RobotState goal = *planning_display_->getQueryGoalState();
std::vector<geometry_msgs::Pose> waypoints;
const std::string& link_name = move_group_->getEndEffectorLink();
const robot_model::LinkModel* link = move_group_->getRobotModel()->getLinkModel(link_name);
if (!link)
{
ROS_ERROR_STREAM("Failed to determine unique end-effector link: " << link_name);
return false;
}
waypoints.push_back(tf2::toMsg(goal.getGlobalLinkTransform(link)));

// setup default params
double cart_step_size = 0.01;
double cart_jump_thresh = 0.0;
bool avoid_collisions = true;

// compute trajectory
moveit_msgs::RobotTrajectory trajectory;
double fraction =
move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions);

if (fraction >= 1.0)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This used to be > 0. With this version, the button fails silently. Is that on purpose?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, this was on purpose: If you cannot reach your target, the planning request should fail.
What do you mean by "fails silently"? It should be written "FAILED" and the partial trajectory should be shown.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My bad, it will display "Failed" in the UI. I don't see how the partial trajectory would be displayed, but I'll go test first.

{
ROS_INFO("Achieved %f %% of Cartesian path", fraction * 100.);

// Compute time parameterization to also provide velocities
// https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4
robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory);
trajectory_processing::IterativeParabolicTimeParameterization iptp;
bool success =
iptp.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value());
ROS_INFO("Computing time stamps %s", success ? "SUCCEDED" : "FAILED");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

SUCCEEDED

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As this is already merged, could you please file a PR?


// Store trajectory in current_plan_
current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
rt.getRobotTrajectoryMsg(current_plan_->trajectory_);
current_plan_->planning_time_ = (ros::WallTime::now() - start).toSec();
return success;
}
return false;
}

bool MotionPlanningFrame::computeJointSpacePlan()
{
current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
return move_group_->plan(*current_plan_) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
}

void MotionPlanningFrame::computePlanButtonClicked()
{
if (!move_group_)
Expand All @@ -117,19 +172,18 @@ void MotionPlanningFrame::computePlanButtonClicked()
ui_->result_label->setText("Planning...");

configureForPlanning();
current_plan_.reset(new moveit::planning_interface::MoveGroupInterface::Plan());
if (move_group_->plan(*current_plan_))
bool success = (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState()) ?
computeCartesianPlan() :
computeJointSpacePlan();

if (success)
{
ui_->execute_button->setEnabled(true);

// Success
ui_->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time_, 'f', 3)));
}
else
{
current_plan_.reset();

// Failure
ui_->result_label->setText("Failed");
}
Q_EMIT planningFinished();
Expand All @@ -156,8 +210,16 @@ void MotionPlanningFrame::computePlanAndExecuteButtonClicked()
// to suppress a warning, we pass an empty state (which encodes "start from current state")
move_group_->setStartStateToCurrentState();
ui_->stop_button->setEnabled(true);
bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
onFinishedExecution(success);
if (ui_->use_cartesian_path->isEnabled() && ui_->use_cartesian_path->checkState())
{
if (computeCartesianPlan())
computeExecuteButtonClicked();
}
else
{
bool success = move_group_->move() == moveit::planning_interface::MoveItErrorCode::SUCCESS;
onFinishedExecution(success);
}
ui_->plan_and_execute_button->setEnabled(true);
}

Expand Down
10 changes: 10 additions & 0 deletions ...ros/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -704,6 +704,16 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="use_cartesian_path">
<property name="text">
<string>Use Cartesian Path</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="collision_aware_ik">
<property name="text">
Expand Down