Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17234] pkg/trunk
|
From: <is...@us...> - 2009-06-17 18:45:52
|
Revision: 17234
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17234&view=rev
Author: isucan
Date: 2009-06-17 18:45:10 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
fixes for move_arm
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -30,7 +30,7 @@
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE. return robot_actions::PREEMPTED;
+* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Sachin Chitta, Ioan Sucan
@@ -91,7 +91,7 @@
delete planningMonitor_;
delete collisionModels_;
}
-
+
robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
{
if (!valid_)
@@ -121,40 +121,41 @@
req.goal_constraints.pose_constraint[0].type == motion_planning_msgs::PoseConstraint::POSITION_XYZ +
motion_planning_msgs::PoseConstraint::ORIENTATION_RPY && // that is active on all 6 DOFs
req.goal_constraints.pose_constraint[0].position_distance < 0.1 && // and the desired position and
- req.goal_constraints.pose_constraint[0].orientation_distance < 0.1 && // orientation distances are small,
- ((arm_ == "left_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back())
- || (arm_ == "right_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back()))) // and acts on the last link of the arm
+ req.goal_constraints.pose_constraint[0].orientation_distance < 0.1) // orientation distances are small
{
- // we can do ik can turn the pose constraint into a joint one
- ROS_INFO("Converting pose constraint to joint constraint using IK...");
-
- std::vector<double> solution;
- if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ planning_models::KinematicModel::Link *link = planningMonitor_->getKinematicModel()->getLink(req.goal_constraints.pose_constraint[0].link_name);
+ if (link && link->before && link->before->name == arm_joint_names_.back())
{
- req.goal_constraints.joint_constraint.resize(1);
- req.goal_constraints.joint_constraint[0].header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
- req.goal_constraints.joint_constraint[0].header.stamp = planningMonitor_->lastStateUpdate();
- unsigned int n = 0;
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ // we can do ik can turn the pose constraint into a joint one
+ ROS_INFO("Converting pose constraint to joint constraint using IK...");
+
+ std::vector<double> solution;
+ if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
{
- motion_planning_msgs::JointConstraint jc;
- jc.joint_name = arm_joint_names_[i];
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
- jc.value.push_back(solution[n + j]);
- jc.toleranceAbove.push_back(0.0);
- jc.toleranceBelow.push_back(0.0);
+ motion_planning_msgs::JointConstraint jc;
+ jc.joint_name = arm_joint_names_[i];
+ jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
+ jc.header.stamp = planningMonitor_->lastStateUpdate();
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ jc.value.push_back(solution[n + j]);
+ jc.toleranceAbove.push_back(0.0);
+ jc.toleranceBelow.push_back(0.0);
+ }
+ n += u;
+ req.goal_constraints.joint_constraint.push_back(jc);
}
- n += u;
- req.goal_constraints.joint_constraint.push_back(jc);
+ req.goal_constraints.pose_constraint.clear();
}
- req.goal_constraints.pose_constraint.clear();
+ else
+ ROS_WARN("Unable to compute IK");
}
- else
- ROS_WARN("Unable to compute IK");
}
-
+
req.times = 1;
req.allowed_time = 0.5;
@@ -226,12 +227,17 @@
// stop the robot if we need to
if (feedback == pr2_robot_actions::MoveArmState::MOVING)
{
- if (result == robot_actions::PREEMPTED || !planningMonitor_->isPathValid(currentPath_))
+ bool safe = planningMonitor_->isEnvironmentSafe();
+ bool valid = planningMonitor_->isPathValid(currentPath_);
+ if (result == robot_actions::PREEMPTED || !safe || !valid)
{
if (result == robot_actions::PREEMPTED)
ROS_INFO("Preempt requested. Stopping arm.");
else
- ROS_INFO("Current path is no longer valid. Stopping & replanning...");
+ if (safe)
+ ROS_WARN("Environment is no longer safe. Cannot decide if path is valid. Stopping & replanning...");
+ else
+ ROS_INFO("Current path is no longer valid. Stopping & replanning...");
if (trajectoryId != -1)
{
@@ -354,8 +360,19 @@
pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
req_query.trajectoryid = -1;
- if (!client_query.call(req_query, res_query))
+ bool result = client_query.call(req_query, res_query);
+
+ if (!result)
{
+ ROS_WARN("Unable to retrieve controller joint names from control query service. Waiting a bit and retrying...");
+ ros::Duration(1.0).sleep();
+ result = client_query.call(req_query, res_query);
+ if (result)
+ ROS_WARN("Retrieved controller joints on second attempt");
+ }
+
+ if (!result)
+ {
ROS_ERROR("Unable to retrieve controller joint names from control query service");
return false;
}
Modified: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -73,6 +73,7 @@
goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+ goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
Modified: pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-17 18:45:10 UTC (rev 17234)
@@ -3,7 +3,5 @@
<node pkg="ompl_planning" type="motion_planner" args="robot_description:=robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen">
<param name="~refresh_interval_collision_map" type="double" value="0.0" />
<param name="~refresh_interval_kinematic_state" type="double" value="0.0" />
- <param name="~refresh_interval_base_pose" type="double" value="0.0" />
- <param name="~kinematic_planning_status_interval" type="double" value="0.02" />
</node>
</launch>
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -56,10 +56,6 @@
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &KinematicPlanning::planToGoal, this);
-
- // determine intervals; a value of 0 means forever
- nodeHandle_.param("~refresh_interval_collision_map", intervalCollisionMap_, 3.0);
- nodeHandle_.param("~refresh_interval_kinematic_state", intervalState_, 0.5);
}
/** Free the memory */
@@ -99,25 +95,6 @@
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
}
- bool isSafeToPlan(bool report)
- {
- if (!planningMonitor_->isMapUpdated(intervalCollisionMap_))
- {
- if (report)
- ROS_WARN("Planning is not safe: map is not up to date");
- return false;
- }
-
- if (!planningMonitor_->isStateUpdated(intervalState_))
- {
- if (report)
- ROS_WARN("Planning is not safe: robot state is not up to date");
- return false;
- }
-
- return true;
- }
-
bool planToGoal(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
ROS_INFO("Received request for planning");
@@ -130,7 +107,7 @@
res.path.model_id = req.params.model_id;
res.path.header.frame_id = planningMonitor_->getFrameId();
res.path.header.stamp = planningMonitor_->lastMapUpdate();
- res.unsafe = isSafeToPlan(true) ? 0 : 1;
+ res.unsafe = planningMonitor_->isEnvironmentSafe() ? 0 : 1;
res.distance = -1.0;
res.approximate = 0;
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -39,17 +39,19 @@
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
{
- if (isMapUpdated(intervalCollisionMap_))
- {
- ROS_WARN("Planning is not safe: map is not up to date");
- return false;
- }
+ if (intervalCollisionMap_ > 0.0)
+ if (isMapUpdated(intervalCollisionMap_))
+ {
+ ROS_WARN("Planning is not safe: map is not up to date");
+ return false;
+ }
- if (isStateUpdated(intervalState_))
- {
- ROS_WARN("Planning is not safe: robot state is not up to date");
- return false;
- }
+ if (intervalState_ > 0.0)
+ if (isStateUpdated(intervalState_))
+ {
+ ROS_WARN("Planning is not safe: robot state is not up to date");
+ return false;
+ }
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
