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

Adding acceleration scaling factor #634

Merged
merged 3 commits into from
Feb 4, 2016
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
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_, req.max_velocity_scaling_factor))
if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor))
ROS_WARN("Time parametrization for the solution path failed.");
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,13 @@ class MoveGroup
default value of 1.0 internally (i.e. maximum joint velocity) */
void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);

/** \brief Set a scaling factor for optionally reducing the maximum joint acceleration.
Allowed values are in (0,1]. The maximum joint acceleration 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 acceleration) */
void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);

/** \brief Get the number of seconds set by setPlanningTime() */
double getPlanningTime() const;

Expand Down
12 changes: 12 additions & 0 deletions planning_interface/move_group_interface/src/move_group.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,7 @@ class MoveGroup::MoveGroupImpl
planning_time_ = 5.0;
num_planning_attempts_ = 1;
max_velocity_scaling_factor_ = 1.0;
max_acceleration_scaling_factor_ = 1.0;
initializing_constraints_ = false;

if (joint_model_group_->isChain())
Expand Down Expand Up @@ -253,6 +254,11 @@ class MoveGroup::MoveGroupImpl
max_velocity_scaling_factor_ = max_velocity_scaling_factor;
}

void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
{
max_acceleration_scaling_factor_ = max_acceleration_scaling_factor;
}

robot_state::RobotState& getJointStateTarget()
{
return *joint_state_target_;
Expand Down Expand Up @@ -838,6 +844,7 @@ class MoveGroup::MoveGroupImpl
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.max_acceleration_scaling_factor = max_acceleration_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 @@ -1024,6 +1031,7 @@ class MoveGroup::MoveGroupImpl
std::string planner_id_;
unsigned int num_planning_attempts_;
double max_velocity_scaling_factor_;
double max_acceleration_scaling_factor_;
double goal_joint_tolerance_;
double goal_position_tolerance_;
double goal_orientation_tolerance_;
Expand Down Expand Up @@ -1116,6 +1124,10 @@ void moveit::planning_interface::MoveGroup::setMaxVelocityScalingFactor(double m
impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
}

void moveit::planning_interface::MoveGroup::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
{
impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
}

moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::asyncMove()
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,8 @@ 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();
mreq.max_velocity_scaling_factor = ui_->velocity_scaling_factor->value();
mreq.max_acceleration_scaling_factor = ui_->acceleration_scaling_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 @@ -341,7 +342,8 @@ 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());
move_group_->setMaxVelocityScalingFactor(ui_->velocity_scaling_factor->value());
move_group_->setMaxAccelerationScalingFactor(ui_->acceleration_scaling_factor->value());
configureWorkspace();
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>702</width>
<height>395</height>
<height>413</height>
</rect>
</property>
<property name="windowTitle">
Expand Down Expand Up @@ -628,7 +628,31 @@
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="exec_time_factor">
<widget class="QDoubleSpinBox" name="velocity_scaling_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>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_20">
<item>
<widget class="QLabel" name="label_20">
<property name="text">
<string>Acceleration Scaling:</string>
Copy link
Member

Choose a reason for hiding this comment

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

Does this long name widen the UI too much? Screenshot?

Copy link
Member

Choose a reason for hiding this comment

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

Oh, I just saw your screenshot.

If you change this to "Accel Scaling" does it reduce the required screen width? The MoveIt widget is already too wide IMHO so I'd prefer not to do something to make it even wider

Copy link
Author

Choose a reason for hiding this comment

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

I agree with you and thing the whole widget is a bit big. As for the addition, I can do a side by side comparison but I am pretty sure that the widget width went unchanged.

Copy link
Author

Choose a reason for hiding this comment

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

Before and after. No change in width but the widget did get one line taller (expected).

screenshot from 2016-01-14 11 29 47

After

</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="acceleration_scaling_factor">
<property name="maximum">
<double>1.000000000000000</double>
</property>
Expand Down Expand Up @@ -1589,7 +1613,8 @@
<tabstop>wsize_z</tabstop>
<tabstop>planning_time</tabstop>
<tabstop>planning_attempts</tabstop>
<tabstop>exec_time_factor</tabstop>
<tabstop>velocity_scaling_factor</tabstop>
<tabstop>acceleration_scaling_factor</tabstop>
<tabstop>allow_replanning</tabstop>
<tabstop>allow_looking</tabstop>
<tabstop>allow_external_program</tabstop>
Expand Down