From 1f70cd0a1128ad9856e4cbff36a346db2fc5e223 Mon Sep 17 00:00:00 2001 From: hemes Date: Fri, 8 Jan 2016 12:59:34 -0600 Subject: [PATCH 1/3] Adding acceleration scaling factor --- .../src/add_time_parameterization.cpp | 2 +- .../moveit/move_group_interface/move_group.h | 11 ++++++++ .../move_group_interface/src/move_group.cpp | 16 +++++++++++ .../src/motion_planning_frame_planning.cpp | 6 ++-- .../ui/motion_planning_rviz_plugin_frame.ui | 28 +++++++++++++++++-- 5 files changed, 58 insertions(+), 5 deletions(-) diff --git a/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp index 7b712aa9a0..4c63944151 100644 --- a/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ b/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp @@ -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."); } diff --git a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h index 8d92c16095..41d4bff570 100644 --- a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h +++ b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h @@ -166,7 +166,18 @@ class MoveGroup (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); +<<<<<<< HEAD +======= + + /** \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); + +>>>>>>> Adding acceleration scaling factor /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp index 2743f9c700..2c3d8948b5 100644 --- a/planning_interface/move_group_interface/src/move_group.cpp +++ b/planning_interface/move_group_interface/src/move_group.cpp @@ -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()) @@ -252,7 +253,16 @@ class MoveGroup::MoveGroupImpl { max_velocity_scaling_factor_ = max_velocity_scaling_factor; } +<<<<<<< HEAD +======= + + void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) + { + max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; + } + +>>>>>>> Adding acceleration scaling factor robot_state::RobotState& getJointStateTarget() { return *joint_state_target_; @@ -838,6 +848,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_; @@ -1024,6 +1035,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_; @@ -1116,6 +1128,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() { diff --git a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 83ca9efb38..4054d01950 100644 --- a/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -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; @@ -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(); } diff --git a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 13993881ac..3ea6287b91 100644 --- a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -7,7 +7,7 @@ 0 0 702 - 395 + 413 @@ -628,7 +628,31 @@ - + + + 1.000000000000000 + + + 0.010000000000000 + + + 1.000000000000000 + + + + + + + + + + + Acceleration Scaling: + + + + + 1.000000000000000 From 5fb7fa65ac3878e2cbcabbd4eda78fe4e5ae1ab4 Mon Sep 17 00:00:00 2001 From: hemes Date: Wed, 13 Jan 2016 12:18:01 -0600 Subject: [PATCH 2/3] fixed widget naming issues --- .../src/ui/motion_planning_rviz_plugin_frame.ui | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui index 3ea6287b91..10a291056e 100644 --- a/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui +++ b/visualization/motion_planning_rviz_plugin/src/ui/motion_planning_rviz_plugin_frame.ui @@ -643,9 +643,9 @@ - + - + Acceleration Scaling: @@ -1613,7 +1613,8 @@ wsize_z planning_time planning_attempts - exec_time_factor + velocity_scaling_factor + acceleration_scaling_factor allow_replanning allow_looking allow_external_program From 0a9f010474f5f721ba83316975d26bc87168a37e Mon Sep 17 00:00:00 2001 From: hemes Date: Wed, 27 Jan 2016 09:42:29 -0600 Subject: [PATCH 3/3] fixed conflict issues --- .../include/moveit/move_group_interface/move_group.h | 6 +----- planning_interface/move_group_interface/src/move_group.cpp | 6 +----- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h index 41d4bff570..b13568504d 100644 --- a/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h +++ b/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group.h @@ -166,18 +166,14 @@ class MoveGroup (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); -<<<<<<< HEAD -======= - /** \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); - ->>>>>>> Adding acceleration scaling factor + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; diff --git a/planning_interface/move_group_interface/src/move_group.cpp b/planning_interface/move_group_interface/src/move_group.cpp index 2c3d8948b5..f39357904f 100644 --- a/planning_interface/move_group_interface/src/move_group.cpp +++ b/planning_interface/move_group_interface/src/move_group.cpp @@ -253,16 +253,12 @@ class MoveGroup::MoveGroupImpl { max_velocity_scaling_factor_ = max_velocity_scaling_factor; } -<<<<<<< HEAD -======= - void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { max_acceleration_scaling_factor_ = max_acceleration_scaling_factor; } - ->>>>>>> Adding acceleration scaling factor + robot_state::RobotState& getJointStateTarget() { return *joint_state_target_;