Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20925] pkg/trunk
|
From: <is...@us...> - 2009-08-06 20:11:02
|
Revision: 20925
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20925&view=rev
Author: isucan
Date: 2009-08-06 20:10:39 +0000 (Thu, 06 Aug 2009)
Log Message:
-----------
adding definition for acceptable contacts
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/CMakeLists.txt
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
pkg/trunk/stacks/pr2/pr2_robot_actions/manifest.xml
Added Paths:
-----------
pkg/trunk/highlevel/move_arm/msg/
pkg/trunk/highlevel/move_arm/msg/AcceptableContact.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg
Removed Paths:
-------------
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmGoal.msg
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmState.msg
Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-08-06 20:10:39 UTC (rev 20925)
@@ -6,6 +6,8 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+genmsg()
+
# main executable
rospack_add_executable(move_arm_action src/move_arm_action.cpp)
rospack_link_boost(move_arm_action thread)
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-06 20:10:39 UTC (rev 20925)
@@ -14,11 +14,19 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="motion_planning_msgs"/>
+ <depend package="geometry_msgs"/>
+ <depend package="mapping_msgs"/>
<depend package="manipulation_srvs"/>
<depend package="visualization_msgs"/>
+ <depend package="geometric_shapes" />
<depend package="planning_models" />
<depend package="planning_environment" />
<depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
<depend package="pr2_mechanism_controllers"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp" />
+ </export>
+
</package>
Added: pkg/trunk/highlevel/move_arm/msg/AcceptableContact.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/AcceptableContact.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/AcceptableContact.msg 2009-08-06 20:10:39 UTC (rev 20925)
@@ -0,0 +1,14 @@
+# This message defines a region of space where contacts
+# with certain links are allowed, up to a certain depth
+
+# The shape of the bound where contacts are allowed
+mapping_msgs/Object bound
+
+# The pose of the box defining the region where contacts are allowed
+geometry_msgs/PoseStamped pose
+
+# The set of links that are allowed to touch obstacles
+string[] links
+
+# The maximum penetration depth
+float64 depth
Copied: pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg (from rev 20909, pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmGoal.msg)
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg 2009-08-06 20:10:39 UTC (rev 20925)
@@ -0,0 +1,10 @@
+# The goal state for the model to plan for. The state is not necessarily explicit.
+# The goal is considered achieved if all the constraints are satisfied.
+# An explicit state can be specified by setting joint constraints to a specific value.
+motion_planning_msgs/KinematicConstraints goal_constraints
+
+# The set of constraints that need to be satisfied by the entire solution path.
+motion_planning_msgs/KinematicConstraints path_constraints
+
+# The set of allowed contacts
+AcceptableContact[] contacts
Property changes on: pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/pr2_robot_actions/msg/MoveArmGoal.msg:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg (from rev 20909, pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmState.msg)
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg 2009-08-06 20:10:39 UTC (rev 20925)
@@ -0,0 +1,9 @@
+# Status
+robot_actions/ActionStatus status
+
+MoveArmGoal goal
+
+int32 PLANNING=1 # when arm is stopped and planning is being performed
+int32 MOVING=2 # when we are executing a plan
+
+int32 feedback
Property changes on: pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/pr2_robot_actions/msg/MoveArmState.msg:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-06 20:10:39 UTC (rev 20925)
@@ -42,8 +42,8 @@
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_srvs/IKService.h>
-#include <pr2_robot_actions/MoveArmGoal.h>
-#include <pr2_robot_actions/MoveArmState.h>
+#include <move_arm/MoveArmGoal.h>
+#include <move_arm/MoveArmState.h>
#include <pr2_robot_actions/ActuateGripperState.h>
#include <std_msgs/Float64.h>
@@ -100,7 +100,7 @@
std::cout << " -rotation [x, y, z, w] = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl;
}
-void goalToState(const pr2_robot_actions::MoveArmGoal &goal, planning_models::StateParams &sp)
+void goalToState(const move_arm::MoveArmGoal &goal, planning_models::StateParams &sp)
{
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
{
@@ -109,7 +109,7 @@
}
}
-btTransform effPosition(const planning_environment::KinematicModelStateMonitor &km, const pr2_robot_actions::MoveArmGoal &goal)
+btTransform effPosition(const planning_environment::KinematicModelStateMonitor &km, const move_arm::MoveArmGoal &goal)
{
planning_models::StateParams sp(*km.getRobotState());
goalToState(goal, sp);
@@ -117,19 +117,19 @@
return km.getKinematicModel()->getJoint(goal.goal_constraints.joint_constraint.back().joint_name)->after->globalTrans;
}
-void printConfig(const pr2_robot_actions::MoveArmGoal &goal)
+void printConfig(const move_arm::MoveArmGoal &goal)
{
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
std::cout << " " << goal.goal_constraints.joint_constraint[i].joint_name << " = " << goal.goal_constraints.joint_constraint[i].value[0] << std::endl;
}
-void printConfigs(const std::map<std::string, pr2_robot_actions::MoveArmGoal> &goals)
+void printConfigs(const std::map<std::string, move_arm::MoveArmGoal> &goals)
{
- for (std::map<std::string, pr2_robot_actions::MoveArmGoal>::const_iterator it = goals.begin() ; it != goals.end() ; ++it)
+ for (std::map<std::string, move_arm::MoveArmGoal>::const_iterator it = goals.begin() ; it != goals.end() ; ++it)
std::cout << " " << it->first << std::endl;
}
-void setupGoal(const std::vector<std::string> &names, pr2_robot_actions::MoveArmGoal &goal)
+void setupGoal(const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
{
goal.goal_constraints.joint_constraint.resize(names.size());
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
@@ -146,7 +146,7 @@
}
}
-void setupGoalEEf(const std::string &link, const std::vector<double> &pz, pr2_robot_actions::MoveArmGoal &goal)
+void setupGoalEEf(const std::string &link, const std::vector<double> &pz, move_arm::MoveArmGoal &goal)
{
goal.goal_constraints.pose_constraint.resize(1);
goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
@@ -167,7 +167,7 @@
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
@@ -177,10 +177,10 @@
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_importance = 0.01;
+ goal.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
}
-void setConfig(const planning_models::StateParams *_sp, const std::vector<std::string> &names, pr2_robot_actions::MoveArmGoal &goal)
+void setConfig(const planning_models::StateParams *_sp, const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
{
setupGoal(names, goal);
planning_models::StateParams sp(*_sp);
@@ -192,7 +192,7 @@
}
}
-void diffConfig(const planning_environment::KinematicModelStateMonitor &km, pr2_robot_actions::MoveArmGoal &goal)
+void diffConfig(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal)
{
std::vector<std::string> names;
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
@@ -204,7 +204,7 @@
}
btTransform pose1 = effPosition(km, goal);
- pr2_robot_actions::MoveArmGoal temp;
+ move_arm::MoveArmGoal temp;
setConfig(km.getRobotState(), names, temp);
btTransform pose2 = effPosition(km, temp);
std::cout << std::endl;
@@ -236,7 +236,7 @@
view.publish(kp);
}
-void setConfigJoint(const unsigned int pos, const double value, pr2_robot_actions::MoveArmGoal &goal)
+void setConfigJoint(const unsigned int pos, const double value, move_arm::MoveArmGoal &goal)
{
goal.goal_constraints.joint_constraint[pos].value[0] = value;
}
@@ -256,12 +256,12 @@
arm = "l";
ros::NodeHandle nh;
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm(arm == "r" ? "move_right_arm" : "move_left_arm");
+ robot_actions::ActionClient<move_arm::MoveArmGoal, move_arm::MoveArmState, int32_t> move_arm(arm == "r" ? "move_right_arm" : "move_left_arm");
robot_actions::ActionClient<std_msgs::Float64, pr2_robot_actions::ActuateGripperState, std_msgs::Float64> gripper(arm == "r" ? "actuate_gripper_right_arm" : "actuate_gripper_left_arm");
ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 1);
int32_t feedback;
- std::map<std::string, pr2_robot_actions::MoveArmGoal> goals;
+ std::map<std::string, move_arm::MoveArmGoal> goals;
std::vector<std::string> names(7);
names[0] = arm + "_shoulder_pan_joint";
@@ -315,7 +315,7 @@
else
if (cmd == "current")
{
- pr2_robot_actions::MoveArmGoal temp;
+ move_arm::MoveArmGoal temp;
setConfig(km.getRobotState(), names, temp);
printConfig(temp);
std::cout << std::endl;
@@ -525,7 +525,7 @@
std::string link = km.getKinematicModel()->getJoint(names.back())->after->name;
std::cout << "Moving " << link << " to " << nrs[0] << ", " << nrs[1] << ", " << nrs[2] << ", " <<
nrs[3] << ", " << nrs[4] << ", " << nrs[5] << ", " << nrs[6] << "..." << std::endl;
- pr2_robot_actions::MoveArmGoal g;
+ move_arm::MoveArmGoal g;
setupGoalEEf(link, nrs, g);
if (move_arm.execute(g, feedback, ros::Duration(allowed_time)) != robot_actions::SUCCESS)
std::cerr << "Failed achieving goal" << std::endl;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-06 20:10:39 UTC (rev 20925)
@@ -40,8 +40,8 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <pr2_robot_actions/MoveArmState.h>
-#include <pr2_robot_actions/MoveArmGoal.h>
+#include <move_arm/MoveArmState.h>
+#include <move_arm/MoveArmGoal.h>
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_srvs/IKService.h>
@@ -57,6 +57,9 @@
#include <visualization_msgs/Marker.h>
#include <planning_environment/monitors/planning_monitor.h>
+#include <planning_environment/util/construct_object.h>
+#include <geometric_shapes/bodies.h>
+
#include <algorithm>
#include <cstdlib>
@@ -147,27 +150,9 @@
if (unsafe_paths_)
ROS_WARN("Paths will NOT be monitored for collision once they have been sent to the controller");
- std::string touch;
- nodeHandle_.param<std::string>("~allowed_touch", touch, std::string());
- std::stringstream ss(touch);
- while (ss.good() & !ss.eof())
- {
- std::string link; ss >> link;
- allowedTouch_[link] = true;
- }
-
- if (!touch.empty())
- ROS_INFO("Touching is allowed for links: %s", touch.c_str());
- else
- ROS_INFO("No link is allowed to touch objects");
-
- nodeHandle_.param<double>("~max_penetration_depth", maxPenetrationDepth_, 0.0);
- if (!touch.empty())
- ROS_INFO("Maximum penetration depth is %f", maxPenetrationDepth_);
-
return true;
}
-
+
protected:
bool getControlJointNames(std::vector<std::string> &joint_names)
@@ -234,30 +219,24 @@
bool unsafe_paths_;
bool show_collisions_;
- /// the set of links that are allowed to touch objects
- std::map<std::string, bool> allowedTouch_;
-
- /// the maximum penetration allowed when touching objects
- double maxPenetrationDepth_;
-
};
-class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
+class MoveArm : public robot_actions::Action<move_arm::MoveArmGoal, int32_t>
{
public:
- MoveArm(MoveBodyCore &core) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_" + core.group_), core_(core)
+ MoveArm(MoveBodyCore &core) : Action<move_arm::MoveArmGoal, int32_t>("move_" + core.group_), core_(core)
{
if (core_.show_collisions_)
visMarkerPublisher_ = core_.nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
// advertise the topic for displaying kinematic plans
- displayPathPublisher_ = core_.nodeHandle_.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 10);
+ displayPathPublisher_ = core_.nodeHandle_.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 10);
- validatingPath_ = false;
planningMonitor_ = core_.planningMonitor_;
- planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, _1), 0);
+ tf_ = &core_.tf_;
+
planningMonitor_->getEnvironmentModel()->setVerbose(false);
}
@@ -266,32 +245,126 @@
}
private:
+
+ struct AllowedContact
+ {
+ boost::shared_ptr<bodies::Body> bound;
+ std::vector<std::string> links;
+ double depth;
+ };
+ // construct a list of states with cost
+ struct CostState
+ {
+ planning_models::StateParams *state;
+ double cost;
+ unsigned int index;
+ };
+
+ struct CostStateOrder
+ {
+ bool operator()(const CostState& a, const CostState& b) const
+ {
+ return a.cost < b.cost;
+ }
+ };
+
+ struct CollisionCost
+ {
+ CollisionCost(void)
+ {
+ cost = 0.0;
+ sum = 0.0;
+ }
+
+ double cost;
+ double sum;
+ };
+
+
+ /** \brief The allowed, ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning
+ * environment when a collision is found */
+ void contactFound(const std::vector<AllowedContact> &allowed, CollisionCost &ccost, bool display, collision_space::EnvironmentModel::Contact &contact)
+ {
+ double cdepth = fabs(contact.depth);
+
+ if (contact.link1 && contact.link2) // self colliison
+ ccost.cost = INFINITY;
+ else
+ {
+ if (contact.link1 || contact.link2)
+ {
+ std::string link = contact.link1 ? contact.link1->name : contact.link2->name;
+ double thisDepth = INFINITY;
+
+ for (unsigned int i = 0 ; i < allowed.size() ; ++i)
+ if (allowed[i].bound->containsPoint(contact.pos) && cdepth < allowed[i].depth)
+ {
+ for (unsigned int j = 0 ; j < allowed[i].links.size() ; ++j)
+ if (allowed[i].links[j] == link)
+ {
+ if (thisDepth > cdepth)
+ thisDepth = cdepth;
+ break;
+ }
+ }
+
+ if (thisDepth > ccost.cost)
+ ccost.cost = thisDepth;
+ }
+ }
+
+ ccost.sum += cdepth;
+
+
+ if (display)
+ {
+ static int count = 0;
+ visualization_msgs::Marker mk;
+ mk.header.stamp = planningMonitor_->lastMapUpdate();
+ mk.header.frame_id = planningMonitor_->getFrameId();
+ mk.ns = ros::this_node::getName();
+ mk.id = count++;
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = contact.pos.x();
+ mk.pose.position.y = contact.pos.y();
+ mk.pose.position.z = contact.pos.z();
+ mk.pose.orientation.w = 1.0;
+
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
+
+ mk.color.a = 0.6;
+ mk.color.r = 1.0;
+ mk.color.g = 0.04;
+ mk.color.b = 0.04;
+
+ mk.lifetime = ros::Duration(30.0);
+
+ visMarkerPublisher_.publish(mk);
+ }
+ }
+
/** \brief Evaluate the cost of a state, in terms of collisions */
double computeStateCollisionCost(const planning_models::StateParams *sp)
{
- // clear collision data where callback adds to
- collisionCost_ = 0.0;
- collisionLinks_.clear();
-
+ std::vector<AllowedContact> allowed; // leave this empty
+ CollisionCost ccost;
+
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, allowed, ccost, false, _1), 0);
+
// check for collision, getting all contacts
planningMonitor_->isStateValid(sp, planning_environment::PlanningMonitor::COLLISION_TEST);
- return evaluateLastCollisionCost();
+ planningMonitor_->setOnCollisionContactCallback(NULL);
+
+ return ccost.sum;
}
-
- /** \brief Evaluate the cost of the last set of collision checks */
- double evaluateLastCollisionCost(void)
- {
- // check we are only touching with allowed links
- for (std::map<std::string, int>::iterator it = collisionLinks_.begin() ; it != collisionLinks_.end() ; ++it)
- if (core_.allowedTouch_.find(it->first) == core_.allowedTouch_.end())
- return INFINITY;
- return collisionCost_;
- }
+
/** \brief If we have a complex goal for which we have not yet found a valid goal state, we use this function*/
- robot_actions::ResultStatus solveGoalComplex(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus solveGoalComplex(const std::vector<AllowedContact> &allowed,
+ std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal with unknown valid goal state ...");
@@ -339,7 +412,7 @@
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
- return runLRplanner(req, feedback);
+ return runLRplanner(allowed, req, feedback);
}
else
{
@@ -357,7 +430,7 @@
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
- robot_actions::ResultStatus result = runLRplanner(req, feedback);
+ robot_actions::ResultStatus result = runLRplanner(allowed, req, feedback);
req.goal_constraints = kc;
planningMonitor_->setGoalConstraints(req.goal_constraints);
@@ -365,24 +438,24 @@
// if reaching the intermediate state was succesful
if (result == robot_actions::SUCCESS)
// run the short range planner to the original goal
- return runSRplanner(states, req, feedback);
+ return runSRplanner(allowed, states, req, feedback);
else
return result;
}
}
else
- return runLRplanner(states, req, feedback);
+ return runLRplanner(allowed, states, req, feedback);
}
else
{
ROS_ERROR("Service for searching for valid states failed");
- return runLRplanner(states, req, feedback);
+ return runLRplanner(allowed, states, req, feedback);
}
}
/** \brief Extract the state specified by the goal and run a planner towards it, if it is valid */
- robot_actions::ResultStatus solveGoalJoints(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoalJoints(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal to set of joints ...");
@@ -398,32 +471,16 @@
// try to skip straight to planning
if (planningMonitor_->isStateValidAtGoal(sp.get()))
- return runLRplanner(req, feedback);
+ return runLRplanner(allowed, req, feedback);
else
{
// if we can't, go to the more generic joint solver
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
states.push_back(sp);
- return solveGoalJoints(states, req, feedback);
+ return solveGoalJoints(allowed, states, req, feedback);
}
}
-
- // construct a list of states with cost
- struct myState
- {
- planning_models::StateParams *state;
- double cost;
- unsigned int index;
- };
-
- struct myStateComp
- {
- bool operator()(const myState& a, const myState& b) const
- {
- return a.cost < b.cost;
- }
- };
-
+
void updateRequest(motion_planning_msgs::GetMotionPlan::Request &req, const planning_models::StateParams *sp)
{
// update request
@@ -445,16 +502,17 @@
}
/** \brief Find a plan to given request, given a set of hint states in the goal region */
- robot_actions::ResultStatus solveGoalJoints(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus solveGoalJoints(const std::vector<AllowedContact> &allowed,
+ std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal to set of joints pointing to potentially invalid state ...");
// just in case we received no states (this should not happen)
if (states.empty())
- return solveGoalComplex(states, req, feedback);
+ return solveGoalComplex(allowed, states, req, feedback);
- std::vector<myState> cstates(states.size());
+ std::vector<CostState> cstates(states.size());
for (unsigned int i = 0 ; i < states.size() ; ++i)
{
@@ -464,7 +522,7 @@
}
// find the state with minimal cost
- std::sort(cstates.begin(), cstates.end(), myStateComp());
+ std::sort(cstates.begin(), cstates.end(), CostStateOrder());
for (unsigned int i = 0 ; i < cstates.size() ; ++i)
ROS_DEBUG("Cost of hint state %d is %f", i, cstates[i].cost);
@@ -472,7 +530,7 @@
if (planningMonitor_->isStateValidAtGoal(cstates[0].state))
{
updateRequest(req, cstates[0].state);
- return runLRplanner(req, feedback);
+ return runLRplanner(allowed, req, feedback);
}
else
{
@@ -480,7 +538,7 @@
std::vector< boost::shared_ptr<planning_models::StateParams> > backup = states;
for (unsigned int i = 0 ; i < cstates.size() ; ++i)
states[i] = backup[cstates[i].index];
- return solveGoalComplex(states, req, feedback);
+ return solveGoalComplex(allowed, states, req, feedback);
}
}
@@ -494,7 +552,7 @@
reach a maximal number of steps. If we have candidate
solutions, we forward this to the joint-goal solver. If not,
the complex goal solver is to be used. */
- robot_actions::ResultStatus solveGoalPose(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoalPose(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal to pose ...");
@@ -535,13 +593,13 @@
}
if (states.empty())
- return solveGoalComplex(states, req, feedback);
+ return solveGoalComplex(allowed, states, req, feedback);
else
- return solveGoalJoints(states, req, feedback);
+ return solveGoalJoints(allowed, states, req, feedback);
}
/** \brief Depending on the type of constraint, decide whether or not to use IK, decide which planners to use */
- robot_actions::ResultStatus solveGoal(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoal(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal...");
@@ -551,30 +609,31 @@
req.goal_constraints.pose_constraint[0].type ==
motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y) // that is active on all 6 DOFs
- return solveGoalPose(req, feedback);
+ return solveGoalPose(allowed, req, feedback);
// if we have only joint constraints, we call the method that gets us to a goal state
if (req.goal_constraints.pose_constraint.empty())
- return solveGoalJoints(req, feedback);
+ return solveGoalJoints(allowed, req, feedback);
// otherwise, more complex constraints, run a generic method; we have no hint states
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- return solveGoalComplex(states, req, feedback);
+ return solveGoalComplex(allowed, states, req, feedback);
}
- robot_actions::ResultStatus runLRplanner(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runLRplanner(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- return runLRplanner(states, req, feedback);
+ return runLRplanner(allowed, states, req, feedback);
}
- robot_actions::ResultStatus runLRplanner(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus runLRplanner(const std::vector<AllowedContact> &allowed,
+ std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Running long range planner...");
ros::ServiceClient clientPlan = core_.nodeHandle_.serviceClient<motion_planning_msgs::GetMotionPlan>(LR_MOTION_PLAN_NAME, true);
- robot_actions::ResultStatus result = runPlanner(clientPlan, req, feedback);
+ robot_actions::ResultStatus result = runPlanner(clientPlan, allowed, req, feedback);
// if the planner aborted and we have an idea about an invalid state that
// may be in the goal region, we make one last try using the short range planner
@@ -583,25 +642,27 @@
// set the goal to be a state
ROS_INFO("Trying again with a state in the goal region (although the state is invalid)...");
updateRequest(req, states[0].get());
- result = runPlanner(clientPlan, req, feedback);
+ result = runPlanner(clientPlan, allowed, req, feedback);
}
return result;
}
- robot_actions::ResultStatus runSRplanner(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runSRplanner(const std::vector<AllowedContact> &allowed,
+ motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- return runSRplanner(states, req, feedback);
+ return runSRplanner(allowed, states, req, feedback);
}
- robot_actions::ResultStatus runSRplanner(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus runSRplanner(const std::vector<AllowedContact> &allowed,
+ std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Running short range planner...");
ros::ServiceClient clientPlan = core_.nodeHandle_.serviceClient<motion_planning_msgs::GetMotionPlan>(SR_MOTION_PLAN_NAME, true);
- robot_actions::ResultStatus result = runPlanner(clientPlan, req, feedback);
+ robot_actions::ResultStatus result = runPlanner(clientPlan, allowed, req, feedback);
// if the planner aborted and we have an idea about an invalid state that
// may be in the goal region, we make one last try using the short range planner
@@ -610,17 +671,17 @@
// set the goal to be a state
ROS_INFO("Trying again with a state in the goal region (although the state is invalid)...");
updateRequest(req, states[0].get());
- result = runPlanner(clientPlan, req, feedback);
+ result = runPlanner(clientPlan, allowed, req, feedback);
}
return result;
}
- robot_actions::ResultStatus runPlanner(ros::ServiceClient &clientPlan, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runPlanner(ros::ServiceClient &clientPlan, const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ResultStatus result = robot_actions::SUCCESS;
- feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ feedback = move_arm::MoveArmState::PLANNING;
update(feedback);
motion_planning_msgs::GetMotionPlan::Response res;
@@ -643,7 +704,7 @@
result = robot_actions::PREEMPTED;
// if we have to plan, do so
- if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
+ if (result == robot_actions::SUCCESS && feedback == move_arm::MoveArmState::PLANNING)
{
if (!planningMonitor_->isEnvironmentSafe())
{
@@ -683,8 +744,8 @@
if (planningMonitor_->transformPathToFrame(currentPath, planningMonitor_->getFrameId()))
{
displayPathPublisher_.publish(currentPath);
- printPath(currentPath);
- feedback = pr2_robot_actions::MoveArmState::MOVING;
+ // printPath(currentPath);
+ feedback = move_arm::MoveArmState::MOVING;
update(feedback);
}
}
@@ -704,11 +765,11 @@
result = robot_actions::PREEMPTED;
// if preeemt was requested while we are planning, terminate
- if (result != robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
+ if (result != robot_actions::SUCCESS && feedback == move_arm::MoveArmState::PLANNING)
break;
// stop the robot if we need to
- if (feedback == pr2_robot_actions::MoveArmState::MOVING)
+ if (feedback == move_arm::MoveArmState::MOVING)
{
bool safe = planningMonitor_->isEnvironmentSafe();
bool valid = true;
@@ -723,19 +784,18 @@
currentPos = 0;
}
- validatingPath_ = true;
- collisionCost_ = 0.0;
- collisionLinks_.clear();
+ CollisionCost ccost;
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, allowed, ccost, true, _1), 0);
valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST, false);
- validatingPath_ = false;
-
+
if (!valid)
{
- double cost = evaluateLastCollisionCost();
- ROS_INFO("Path contact penetration depth: %f", cost);
- if (cost < core_.maxPenetrationDepth_)
+ ROS_INFO("Path contact penetration depth: %f", ccost.cost);
+ if (ccost.cost < INFINITY) // this will always be infinity if we have collisions we do not allow
valid = true;
}
+
+ // and check the path constraints
if (valid)
valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
}
@@ -766,7 +826,7 @@
if (result != robot_actions::PREEMPTED)
{
// if we were not preempted
- feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ feedback = move_arm::MoveArmState::PLANNING;
update(feedback);
continue;
}
@@ -776,7 +836,7 @@
}
// execute & monitor a path if we need to
- if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::MOVING)
+ if (result == robot_actions::SUCCESS && feedback == move_arm::MoveArmState::MOVING)
{
// start the controller if we have to, using trajectory start
if (trajectoryId == -1)
@@ -820,7 +880,7 @@
if (approx && !planningMonitor_->isStateValidAtGoal(planningMonitor_->getRobotState()))
{
ROS_INFO("Completed approximate path (trajectory %d). Trying again to reach goal...", trajectoryId);
- feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ feedback = move_arm::MoveArmState::PLANNING;
update(feedback);
trajectoryId = -1;
continue;
@@ -850,9 +910,30 @@
return result;
}
- robot_actions::ResultStatus execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
+
+ robot_actions::ResultStatus execute(const move_arm::MoveArmGoal& goal, int32_t& feedback)
{
+ std::vector<AllowedContact> contactRegions;
+ for (unsigned int i = 0 ; i < goal.contacts.size() ; ++i)
+ {
+ shapes::Shape *shape = planning_environment::constructObject(goal.contacts[i].bound);
+ if (shape)
+ {
+ boost::shared_ptr<bodies::Body> body(bodies::createBodyFromShape(shape));
+ geometry_msgs::PoseStamped pose;
+ tf_->transformPose(planningMonitor_->getFrameId(), goal.contacts[i].pose, pose);
+ btTransform tr;
+ tf::poseMsgToTF(pose.pose, tr);
+ body->setPose(tr);
+ AllowedContact ac;
+ ac.bound = body;
+ ac.links = goal.contacts[i].links;
+ ac.depth = goal.contacts[i].depth;
+ contactRegions.push_back(ac);
+ }
+ }
+
motion_planning_msgs::GetMotionPlan::Request req;
req.params.model_id = core_.group_; // the model to plan for (should be defined in planning.yaml)
@@ -879,7 +960,7 @@
// fill the starting state
fillStartState(req.start_state);
- robot_actions::ResultStatus result = solveGoal(req, feedback);
+ robot_actions::ResultStatus result = solveGoal(contactRegions, req, feedback);
if (result == robot_actions::SUCCESS)
ROS_INFO("Goal was reached");
@@ -905,8 +986,6 @@
void printPath(const motion_planning_msgs::KinematicPath &path)
{
-// return;
-
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
std::stringstream ss;
@@ -916,44 +995,6 @@
}
}
- void contactFound(collision_space::EnvironmentModel::Contact &contact)
- {
- if (contact.link1)
- collisionLinks_[contact.link1->name]++;
- if (contact.link2)
- collisionLinks_[contact.link2->name]++;
- double depth = fabs(contact.depth);
- if (collisionCost_ < depth)
- collisionCost_ = depth;
-
- if (validatingPath_ && core_.show_collisions_)
- {
- static int count = 0;
- visualization_msgs::Marker mk;
- mk.header.stamp = planningMonitor_->lastMapUpdate();
- mk.header.frame_id = planningMonitor_->getFrameId();
- mk.ns = ros::this_node::getName();
- mk.id = count++;
- mk.type = visualization_msgs::Marker::SPHERE;
- mk.action = visualization_msgs::Marker::ADD;
- mk.pose.position.x = contact.pos.x();
- mk.pose.position.y = contact.pos.y();
- mk.pose.position.z = contact.pos.z();
- mk.pose.orientation.w = 1.0;
-
- mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
-
- mk.color.a = 0.6;
- mk.color.r = 1.0;
- mk.color.g = 0.04;
- mk.color.b = 0.04;
-
- mk.lifetime = ros::Duration(30.0);
-
- visMarkerPublisher_.publish(mk);
- }
- }
-
bool fixStartState(planning_models::StateParams &st)
{
bool result = true;
@@ -1063,16 +1104,13 @@
private:
- MoveBodyCore &core_;
- planning_environment::PlanningMonitor *planningMonitor_;
+ MoveBodyCore &core_;
+ planning_environment::PlanningMonitor *planningMonitor_;
+ tf::TransformListener *tf_;
- ros::Publisher displayPathPublisher_;
- ros::Publisher visMarkerPublisher_;
+ ros::Publisher displayPathPublisher_;
+ ros::Publisher visMarkerPublisher_;
- double collisionCost_;
- std::map<std::string, int> collisionLinks_;
-
- bool validatingPath_;
};
@@ -1088,7 +1126,7 @@
MoveArm move_arm(core);
robot_actions::ActionRunner runner(20.0);
- runner.connect<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t>(move_arm);
+ runner.connect<move_arm::MoveArmGoal, move_arm::MoveArmState, int32_t>(move_arm);
runner.run();
ros::spin();
}
Modified: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-08-06 20:10:39 UTC (rev 20925)
@@ -39,8 +39,8 @@
#include <ros/ros.h>
#include <robot_actions/action_client.h>
-#include <pr2_robot_actions/MoveArmGoal.h>
-#include <pr2_robot_actions/MoveArmState.h>
+#include <move_arm/MoveArmGoal.h>
+#include <move_arm/MoveArmState.h>
#include <vector>
#include <string>
@@ -50,11 +50,11 @@
ros::init(argc, argv, "test_move_right_arm");
ros::NodeHandle nh;
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_right_arm");
+ robot_actions::ActionClient<move_arm::MoveArmGoal, move_arm::MoveArmState, int32_t> move_arm("move_right_arm");
int32_t feedback;
- pr2_robot_actions::MoveArmGoal goalA;
- pr2_robot_actions::MoveArmGoal goalB;
+ move_arm::MoveArmGoal goalA;
+ move_arm::MoveArmGoal goalB;
goalA.goal_constraints.set_pose_constraint_size(1);
goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
Modified: pkg/trunk/stacks/pr2/pr2_robot_actions/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/manifest.xml 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/manifest.xml 2009-08-06 20:10:39 UTC (rev 20925)
@@ -12,7 +12,6 @@
<depend package="door_msgs"/>
<depend package="pr2_msgs"/>
<depend package="pr2_srvs"/>
- <depend package="motion_planning_msgs"/>
<depend package="plugs_msgs"/>
<export>
Deleted: pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmGoal.msg 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmGoal.msg 2009-08-06 20:10:39 UTC (rev 20925)
@@ -1,7 +0,0 @@
-# The goal state for the model to plan for. The state is not necessarily explicit.
-# The goal is considered achieved if all the constraints are satisfied.
-# An explicit state can be specified by setting joint constraints to a specific value.
-motion_planning_msgs/KinematicConstraints goal_constraints
-
-# The set of constraints that need to be satisfied by the entire solution path.
-motion_planning_msgs/KinematicConstraints path_constraints
Deleted: pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmState.msg 2009-08-06 20:09:46 UTC (rev 20924)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/msg/MoveArmState.msg 2009-08-06 20:10:39 UTC (rev 20925)
@@ -1,9 +0,0 @@
-# Status
-robot_actions/ActionStatus status
-
-pr2_robot_actions/MoveArmGoal goal
-
-int32 PLANNING=1 # when arm is stopped and planning is being performed
-int32 MOVING=2 # when we are executing a plan
-
-int32 feedback
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
