Skip to content
This repository has been archived by the owner on Nov 13, 2017. It is now read-only.

Add time factor support for trajectory time parametrization #547

Merged
merged 4 commits into from
Jan 12, 2015
Merged

Add time factor support for trajectory time parametrization #547

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
df6e830
Add time factor support for iterative_time_parametrization
skohlbr Dec 22, 2014
6b9dba6
Revert accidental minor changes to ui file
skohlbr Dec 23, 2014
b60bd0e
Rename velocity scaling factor, update documentation
skohlbr Dec 27, 2014
e5163a9
Rename velocity scaling label in motion planning ui
skohlbr Jan 12, 2015
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
2 changes: 1 addition & 1 deletion planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,7 +157,7 @@ class AddTimeParameterization : public planning_request_adapter::PlanningRequest
if (result && res.trajectory_)
{
ROS_DEBUG("Running '%s'", getDescription().c_str());
if (!time_param_.computeTimeStamps(*res.trajectory_))
if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor))
ROS_WARN("Time parametrization for the solution path failed.");
}

Expand Down
7 changes: 7 additions & 0 deletions planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,13 @@ class MoveGroup

/** \brief Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1.*/
void setNumPlanningAttempts(unsigned int num_planning_attempts);

/** \brief Set a scaling factor for optionally reducing the maximum joint velocity.
Allowed values are in (0,1]. The maximum joint velocity specified
in the robot model is multiplied by the factor. If outside valid range
(imporantly, this includes it being set to 0.0), the factor is set to a
default value of 1.0 internally (i.e. maximum joint velocity) */
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);

/** \brief Get the number of seconds set by setPlanningTime() */
double getPlanningTime() const;
Expand Down
14 changes: 14 additions & 0 deletions planning_interface/move_group_interface/src/move_group.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ class MoveGroup::MoveGroupImpl
goal_orientation_tolerance_ = 1e-3; // ~0.1 deg
planning_time_ = 5.0;
num_planning_attempts_ = 1;
max_velocity_scaling_factor_ = 1.0;
initializing_constraints_ = false;

if (joint_model_group_->isChain())
Expand Down Expand Up @@ -235,6 +236,11 @@ class MoveGroup::MoveGroupImpl
{
num_planning_attempts_ = num_planning_attempts;
}

void setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
{
max_velocity_scaling_factor_ = max_velocity_scaling_factor;
}

robot_state::RobotState& getJointStateTarget()
{
Expand Down Expand Up @@ -820,6 +826,7 @@ class MoveGroup::MoveGroupImpl
moveit_msgs::MoveGroupGoal goal;
goal.request.group_name = opt_.group_name_;
goal.request.num_planning_attempts = num_planning_attempts_;
goal.request.max_velocity_scaling_factor = max_velocity_scaling_factor_;
goal.request.allowed_planning_time = planning_time_;
goal.request.planner_id = planner_id_;
goal.request.workspace_parameters = workspace_parameters_;
Expand Down Expand Up @@ -1005,6 +1012,7 @@ class MoveGroup::MoveGroupImpl
double planning_time_;
std::string planner_id_;
unsigned int num_planning_attempts_;
double max_velocity_scaling_factor_;
double goal_joint_tolerance_;
double goal_position_tolerance_;
double goal_orientation_tolerance_;
Expand Down Expand Up @@ -1077,6 +1085,12 @@ void moveit::planning_interface::MoveGroup::setNumPlanningAttempts(unsigned int
impl_->setNumPlanningAttempts(num_planning_attempts);
}

void moveit::planning_interface::MoveGroup::setMaxVelocityScalingFactor(double max_velocity_scaling_factor)
{
impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
}


moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
{
return impl_->move(false);
Expand Down
2 changes: 2 additions & 0 deletions 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 @@ -274,6 +274,7 @@ void MotionPlanningFrame::constructPlanningRequest(moveit_msgs::MotionPlanReques
mreq.group_name = planning_display_->getCurrentPlanningGroup();
mreq.num_planning_attempts = ui_->planning_attempts->value();
mreq.allowed_planning_time = ui_->planning_time->value();
mreq.max_velocity_scaling_factor = ui_->exec_time_factor->value();
robot_state::robotStateToRobotStateMsg(*planning_display_->getQueryStartState(), mreq.start_state);
mreq.workspace_parameters.min_corner.x = ui_->wcenter_x->value() - ui_->wsize_x->value() / 2.0;
mreq.workspace_parameters.min_corner.y = ui_->wcenter_y->value() - ui_->wsize_y->value() / 2.0;
Expand Down Expand Up @@ -334,6 +335,7 @@ void MotionPlanningFrame::configureForPlanning()
move_group_->setJointValueTarget(*planning_display_->getQueryGoalState());
move_group_->setPlanningTime(ui_->planning_time->value());
move_group_->setNumPlanningAttempts(ui_->planning_attempts->value());
move_group_->setMaxVelocityScalingFactor(ui_->exec_time_factor->value());
configureWorkspace();
}

Expand Down
34 changes: 29 additions & 5 deletions 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 @@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>688</width>
<height>377</height>
<width>702</width>
<height>395</height>
</rect>
</property>
<property name="windowTitle">
Expand Down Expand Up @@ -310,7 +310,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>218</width>
<width>109</width>
<height>78</height>
</rect>
</property>
Expand Down Expand Up @@ -360,7 +360,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>218</width>
<width>205</width>
<height>78</height>
</rect>
</property>
Expand Down Expand Up @@ -613,7 +613,31 @@
</property>
<property name="value">
<number>10</number>
</property>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
<widget class="QLabel" name="label_11">
<property name="text">
<string>Velocity Scaling:</string>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="exec_time_factor">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
</layout>
Expand Down