Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20935] pkg/trunk
|
From: <is...@us...> - 2009-08-06 22:12:04
|
Revision: 20935
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20935&view=rev
Author: isucan
Date: 2009-08-06 22:11:57 +0000 (Thu, 06 Aug 2009)
Log Message:
-----------
updates to move_arm; dealing with alloweble contacts in a nicer way (more general)
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/launch/move_larm.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/highlevel/move_arm/launch/move_larm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-08-06 22:11:57 UTC (rev 20935)
@@ -19,9 +19,6 @@
<param name="group" type="string" value="left_arm" />
<param name="show_collisions" type="bool" value="true" />
- <param name="allowed_touch" type="string" value="l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link" />
- <param name="max_penetration_depth" type="double" value="0.01" />
-
<param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-06 22:11:57 UTC (rev 20935)
@@ -19,9 +19,6 @@
<param name="group" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
- <param name="allowed_touch" type="string" value="r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link" />
- <param name="max_penetration_depth" type="double" value="0.01" />
-
<param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -178,6 +178,17 @@
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
+
+ goal.contacts.resize(1);
+ goal.contacts[0].links.push_back("r_gripper_l_finger_link");
+ goal.contacts[0].links.push_back("r_gripper_r_finger_link");
+ goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
+ goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
+ goal.contacts[0].links.push_back("r_gripper_palm_link");
+ goal.contacts[0].depth = 0.025;
+ goal.contacts[0].bound.type = mapping_msgs::Object::SPHERE;
+ goal.contacts[0].bound.dimensions.push_back(0.3);
+ goal.contacts[0].pose = goal.goal_constraints.pose_constraint[0].pose;
}
void setConfig(const planning_models::StateParams *_sp, const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -246,13 +246,6 @@
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
{
@@ -282,41 +275,15 @@
};
- /** \brief The allowed, ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning
+ /** \brief The 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)
+ void contactFound(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;
- }
- }
-
+ if (ccost.cost < cdepth)
+ ccost.cost = cdepth;
ccost.sum += cdepth;
-
if (display)
{
static int count = 0;
@@ -348,23 +315,24 @@
/** \brief Evaluate the cost of a state, in terms of collisions */
double computeStateCollisionCost(const planning_models::StateParams *sp)
{
- std::vector<AllowedContact> allowed; // leave this empty
- CollisionCost ccost;
+ CollisionCost ccost;
- planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, allowed, ccost, false, _1), 0);
+ std::vector<collision_space::EnvironmentModel::AllowedContact> ac = planningMonitor_->getAllowedContacts();
+ planningMonitor_->clearAllowedContacts();
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, ccost, false, _1), 0);
// check for collision, getting all contacts
planningMonitor_->isStateValid(sp, planning_environment::PlanningMonitor::COLLISION_TEST);
planningMonitor_->setOnCollisionContactCallback(NULL);
-
+ planningMonitor_->setAllowedContacts(ac);
+
return ccost.sum;
}
/** \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(const std::vector<AllowedContact> &allowed,
- std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus solveGoalComplex(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 ...");
@@ -412,7 +380,7 @@
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
- return runLRplanner(allowed, req, feedback);
+ return runLRplanner(req, feedback);
}
else
{
@@ -430,7 +398,7 @@
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
- robot_actions::ResultStatus result = runLRplanner(allowed, req, feedback);
+ robot_actions::ResultStatus result = runLRplanner(req, feedback);
req.goal_constraints = kc;
planningMonitor_->setGoalConstraints(req.goal_constraints);
@@ -438,24 +406,24 @@
// if reaching the intermediate state was succesful
if (result == robot_actions::SUCCESS)
// run the short range planner to the original goal
- return runSRplanner(allowed, states, req, feedback);
+ return runSRplanner(states, req, feedback);
else
return result;
}
}
else
- return runLRplanner(allowed, states, req, feedback);
+ return runLRplanner(states, req, feedback);
}
else
{
ROS_ERROR("Service for searching for valid states failed");
- return runLRplanner(allowed, states, req, feedback);
+ return runLRplanner(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(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoalJoints(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal to set of joints ...");
@@ -471,13 +439,13 @@
// try to skip straight to planning
if (planningMonitor_->isStateValidAtGoal(sp.get()))
- return runLRplanner(allowed, req, feedback);
+ return runLRplanner(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(allowed, states, req, feedback);
+ return solveGoalJoints(states, req, feedback);
}
}
@@ -502,15 +470,14 @@
}
/** \brief Find a plan to given request, given a set of hint states in the goal region */
- robot_actions::ResultStatus solveGoalJoints(const std::vector<AllowedContact> &allowed,
- std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus solveGoalJoints(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(allowed, states, req, feedback);
+ return solveGoalComplex(states, req, feedback);
std::vector<CostState> cstates(states.size());
@@ -530,7 +497,7 @@
if (planningMonitor_->isStateValidAtGoal(cstates[0].state))
{
updateRequest(req, cstates[0].state);
- return runLRplanner(allowed, req, feedback);
+ return runLRplanner(req, feedback);
}
else
{
@@ -538,7 +505,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(allowed, states, req, feedback);
+ return solveGoalComplex(states, req, feedback);
}
}
@@ -552,7 +519,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(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoalPose(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal to pose ...");
@@ -593,13 +560,13 @@
}
if (states.empty())
- return solveGoalComplex(allowed, states, req, feedback);
+ return solveGoalComplex(states, req, feedback);
else
- return solveGoalJoints(allowed, states, req, feedback);
+ return solveGoalJoints(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(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus solveGoal(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ROS_DEBUG("Acting on goal...");
@@ -609,31 +576,30 @@
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(allowed, req, feedback);
+ return solveGoalPose(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(allowed, req, feedback);
+ return solveGoalJoints(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(allowed, states, req, feedback);
+ return solveGoalComplex(states, req, feedback);
}
- robot_actions::ResultStatus runLRplanner(const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runLRplanner(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- return runLRplanner(allowed, states, req, feedback);
+ return runLRplanner(states, req, feedback);
}
- robot_actions::ResultStatus runLRplanner(const std::vector<AllowedContact> &allowed,
- std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus runLRplanner(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, allowed, req, feedback);
+ robot_actions::ResultStatus result = runPlanner(clientPlan, 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
@@ -642,27 +608,25 @@
// 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, allowed, req, feedback);
+ result = runPlanner(clientPlan, req, feedback);
}
return result;
}
- robot_actions::ResultStatus runSRplanner(const std::vector<AllowedContact> &allowed,
- motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runSRplanner(motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
std::vector< boost::shared_ptr<planning_models::StateParams> > states;
- return runSRplanner(allowed, states, req, feedback);
+ return runSRplanner(states, req, feedback);
}
- robot_actions::ResultStatus runSRplanner(const std::vector<AllowedContact> &allowed,
- std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ robot_actions::ResultStatus runSRplanner(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, allowed, req, feedback);
+ robot_actions::ResultStatus result = runPlanner(clientPlan, 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
@@ -671,13 +635,13 @@
// 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, allowed, req, feedback);
+ result = runPlanner(clientPlan, req, feedback);
}
return result;
}
- robot_actions::ResultStatus runPlanner(ros::ServiceClient &clientPlan, const std::vector<AllowedContact> &allowed, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
+ robot_actions::ResultStatus runPlanner(ros::ServiceClient &clientPlan, motion_planning_msgs::GetMotionPlan::Request &req, int32_t& feedback)
{
ResultStatus result = robot_actions::SUCCESS;
@@ -785,19 +749,14 @@
}
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);
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, ccost, true, _1), 0);
+ valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST +
+ planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
+ planningMonitor_->setOnCollisionContactCallback(NULL);
if (!valid)
- {
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);
}
if (result == robot_actions::PREEMPTED || !safe || !valid)
@@ -913,36 +872,18 @@
robot_actions::ResultStatus execute(const move_arm::MoveArmGoal& goal, int32_t& feedback)
{
+ planningMonitor_->setAllowedContacts(goal.contacts);
- 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)
req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
+ req.params.contacts = goal.contacts;
// forward the goal & path constraints
req.goal_constraints = goal.goal_constraints;
req.path_constraints = goal.path_constraints;
-
+
// transform them to the local coordinate frame since we may be updating this request later on
planningMonitor_->transformConstraintsToFrame(req.goal_constraints, planningMonitor_->getFrameId());
planningMonitor_->transformConstraintsToFrame(req.path_constraints, planningMonitor_->getFrameId());
@@ -960,7 +901,7 @@
// fill the starting state
fillStartState(req.start_state);
- robot_actions::ResultStatus result = solveGoal(contactRegions, req, feedback);
+ robot_actions::ResultStatus result = solveGoal(req, feedback);
if (result == robot_actions::SUCCESS)
ROS_INFO("Goal was reached");
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-08-06 22:11:57 UTC (rev 20935)
@@ -40,6 +40,7 @@
#include "planning_environment/models/collision_models.h"
#include "planning_environment/monitors/kinematic_model_state_monitor.h"
+#include <motion_planning_msgs/AcceptableContact.h>
#include <mapping_msgs/CollisionMap.h>
#include <mapping_msgs/ObjectInMap.h>
#include <boost/thread/mutex.hpp>
@@ -142,11 +143,24 @@
return pointcloud_padd_;
}
- /** \brief If the used modified some instance of an environment model, this function provides the means to obtain a collision map (the set of boxes)
+ /** \brief Convert an allowed contact message into the structure accepted by the collision space */
+ bool computeAllowedContact(const motion_planning_msgs::AcceptableContact &allowedContactMsg, collision_space::EnvironmentModel::AllowedContact &allowedContact) const;
+
+ /** \brief This function provides the means to obtain a collision map (the set of boxes)
+ * from the current environment model */
+ void recoverCollisionMap(mapping_msgs::CollisionMap &cmap);
+
+ /** \brief If the user modified some instance of an environment model, this function provides the means to obtain a collision map (the set of boxes)
* from that environment model */
void recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap);
- /** \brief If the used modified some instance of an
+
+ /** \brief This function provides the means to
+ obtain a set of objects in map (all objects that are not
+ in the namespace the collision map was added to) from the current environment */
+ void recoverObjectsInMap(std::vector<mapping_msgs::ObjectInMap> &omap);
+
+ /** \brief If the user modified some instance of an
environment model, this function provides the means to
obtain a set of objects in map (all objects that are not
in the namespace the collision map was added to) */
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h 2009-08-06 22:11:57 UTC (rev 20935)
@@ -133,6 +133,18 @@
/** \brief Transform the kinematic joint to the frame requested */
bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
+ /** \brief Set the set of contacts allowed when collision checking */
+ void setAllowedContacts(const std::vector<motion_planning_msgs::AcceptableContact> &allowedContacts);
+
+ /** \brief Set the set of contacts allowed when collision checking */
+ void setAllowedContacts(const std::vector<collision_space::EnvironmentModel::AllowedContact> &allowedContacts);
+
+ /** \brief Get the set of contacts allowed when collision checking */
+ const std::vector<collision_space::EnvironmentModel::AllowedContact>& getAllowedContacts(void) const;
+
+ /** \brief Clear the set of allowed contacts */
+ void clearAllowedContacts(void);
+
/** \brief Set a callback to be called when a collision is found */
void setOnCollisionContactCallback(const boost::function<void(collision_space::EnvironmentModel::Contact&)> &callback, unsigned int maxContacts = 1)
{
@@ -158,6 +170,8 @@
boost::function<void(collision_space::EnvironmentModel::Contact&)> onCollisionContact_;
unsigned int maxCollisionContacts_;
+ std::vector<collision_space::EnvironmentModel::AllowedContact> allowedContacts_;
+
motion_planning_msgs::KinematicConstraints kcPath_;
motion_planning_msgs::KinematicConstraints kcGoal_;
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -367,6 +367,35 @@
return result;
}
+bool planning_environment::CollisionSpaceMonitor::computeAllowedContact(const motion_planning_msgs::AcceptableContact &allowedContactMsg,
+ collision_space::EnvironmentModel::AllowedContact &allowedContact) const
+{
+ shapes::Shape *shape = constructObject(allowedContactMsg.bound);
+ if (shape)
+ {
+ boost::shared_ptr<bodies::Body> body(bodies::createBodyFromShape(shape));
+ geometry_msgs::PoseStamped pose;
+ tf_->transformPose(getFrameId(), allowedContactMsg.pose, pose);
+ btTransform tr;
+ tf::poseMsgToTF(pose.pose, tr);
+ body->setPose(tr);
+ allowedContact.bound = body;
+ allowedContact.links = allowedContactMsg.links;
+ allowedContact.depth = allowedContactMsg.depth;
+ delete shape;
+ return true;
+ }
+ else
+ return false;
+}
+
+void planning_environment::CollisionSpaceMonitor::recoverCollisionMap(mapping_msgs::CollisionMap &cmap)
+{
+ getEnvironmentModel()->lock();
+ recoverCollisionMap(getEnvironmentModel(), cmap);
+ getEnvironmentModel()->unlock();
+}
+
void planning_environment::CollisionSpaceMonitor::recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap)
{
cmap.header.frame_id = getFrameId();
@@ -398,6 +427,13 @@
}
}
+void planning_environment::CollisionSpaceMonitor::recoverObjectsInMap(std::vector<mapping_msgs::ObjectInMap> &omap)
+{
+ getEnvironmentModel()->lock();
+ recoverObjectsInMap(getEnvironmentModel(), omap);
+ getEnvironmentModel()->unlock();
+}
+
void planning_environment::CollisionSpaceMonitor::recoverObjectsInMap(const collision_space::EnvironmentModel *env, std::vector<mapping_msgs::ObjectInMap> &omap)
{
omap.clear();
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -315,7 +315,7 @@
// check for collision
std::vector<collision_space::EnvironmentModel::Contact> contacts;
if (test & COLLISION_TEST)
- valid = !getEnvironmentModel()->getCollisionContacts(contacts, maxCollisionContacts_);
+ valid = !getEnvironmentModel()->getCollisionContacts(allowedContacts_, contacts, maxCollisionContacts_);
if (valid && (test & (PATH_CONSTRAINTS_TEST | GOAL_CONSTRAINTS_TEST)))
{
@@ -506,7 +506,7 @@
// check for collision
std::vector<collision_space::EnvironmentModel::Contact> contacts;
if (test & COLLISION_TEST)
- valid = !getEnvironmentModel()->getCollisionContacts(contacts, remainingContacts);
+ valid = !getEnvironmentModel()->getCollisionContacts(allowedContacts_, contacts, remainingContacts);
if (onCollisionContact_)
for (unsigned int i = 0 ; i < contacts.size() ; ++i)
@@ -548,3 +548,31 @@
return valid;
}
+
+void planning_environment::PlanningMonitor::setAllowedContacts(const std::vector<collision_space::EnvironmentModel::AllowedContact> &allowedContacts)
+{
+ allowedContacts_ = allowedContacts;
+}
+
+void planning_environment::PlanningMonitor::setAllowedContacts(const std::vector<motion_planning_msgs::AcceptableContact> &allowedContacts)
+{
+ allowedContacts_.clear();
+ for (unsigned int i = 0 ; i < allowedContacts.size() ; ++i)
+ {
+ collision_space::EnvironmentModel::AllowedContact ac;
+ if (computeAllowedContact(allowedContacts[i], ac))
+ allowedContacts_.push_back(ac);
+ }
+}
+
+const std::vector<collision_space::EnvironmentModel::AllowedContact>& planning_environment::PlanningMonitor::getAllowedContacts(void) const
+{
+ return allowedContacts_;
+}
+
+void planning_environment::PlanningMonitor::clearAllowedContacts(void)
+{
+ allowedContacts_.clear();
+}
+
+
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-08-06 22:11:57 UTC (rev 20935)
@@ -40,6 +40,7 @@
#include "collision_space/environment_objects.h"
#include <planning_models/kinematic.h>
#include <planning_models/output.h>
+#include <geometric_shapes/bodies.h>
#include <LinearMath/btVector3.h>
#include <boost/thread/mutex.hpp>
#include <boost/shared_ptr.hpp>
@@ -75,7 +76,20 @@
/** \brief if the contact is between two links, this is not NULL */
planning_models::KinematicModel::Link *link2;
};
-
+
+ /** \brief Definition of a contact that is allowed */
+ struct AllowedContact
+ {
+ /// the bound where the contact is allowed
+ boost::shared_ptr<bodies::Body> bound;
+
+ /// the set of link names that are allowed to make contact
+ std::vector<std::string> links;
+
+ /// tha maximum depth for the contact
+ double depth;
+ };
+
EnvironmentModel(void)
{
m_selfCollision = true;
@@ -140,8 +154,8 @@
virtual bool isSelfCollision(void) = 0;
/** \brief Get the list of contacts (collisions). The maximum number of contacts to be returned can be specified. If the value is 0, all found contacts are returned. */
- virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
-
+ virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
+ bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
/**********************************************************************/
/* Collision Bodies */
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-08-06 22:11:57 UTC (rev 20935)
@@ -66,7 +66,7 @@
}
/** \brief Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
+ virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
/** \brief Check if a model is in collision */
virtual bool isCollision(void);
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-08-06 22:11:57 UTC (rev 20935)
@@ -56,7 +56,7 @@
virtual ~EnvironmentModelODE(void);
/** \brief Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
+ virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
/** \brief Check if a model is in collision */
virtual bool isCollision(void);
@@ -296,6 +296,7 @@
contacts = NULL;
selfCollisionTest = NULL;
link1 = link2 = NULL;
+ allowed = NULL;
}
bool done;
@@ -303,9 +304,11 @@
bool collides;
unsigned int max_contacts;
std::vector<EnvironmentModelODE::Contact> *contacts;
- std::vector< std::vector<bool> > *selfCollisionTest;
+ const std::vector< std::vector<bool> > *selfCollisionTest;
dSpaceID selfSpace;
+ const std::vector<AllowedContact> *allowed;
+
planning_models::KinematicModel::Link *link1;
planning_models::KinematicModel::Link *link2;
};
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -36,6 +36,12 @@
#include "collision_space/environment.h"
+bool collision_space::EnvironmentModel::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
+{
+ std::vector<AllowedContact> allowed;
+ return getCollisionContacts(allowed, contacts, max_count);
+}
+
bool collision_space::EnvironmentModel::getVerbose(void) const
{
return m_verbose;
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -231,7 +231,7 @@
return false;
}
-bool collision_space::EnvironmentModelBullet::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
+bool collision_space::EnvironmentModelBullet::getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count)
{
m_world->getPairCache()->setOverlapFilterCallback(&m_genericCollisionFilterCallback);
m_world->performDiscreteCollisionDetection();
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-06 22:04:58 UTC (rev 20934)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-06 22:11:57 UTC (rev 20935)
@@ -511,16 +511,58 @@
&contact[0].geom, sizeof(dContact));
if (numc)
{
- cdata->collides = true;
for (int i = 0 ; i < numc ; ++i)
{
if (cdata->max_contacts == 0 || cdata->contacts->size() < cdata->max_contacts)
{
+ btVector3 pos(contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2]);
+
+ if (cdata->allowed)
+ {
+ dSpaceID s1 = dGeomGetSpace(o1);
+ dSpaceID s2 = dGeomGetSpace(o2);
+
+ bool b1 = s1 == cdata->selfSpace;
+ bool b2 = s2 == cdata->selfSpace;
+
+ if (b1 != b2)
+ {
+ std::string link_name;
+ if (b1)
+ {
+ EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
+ link_name = kg1->link->name;
+ }
+ else
+ {
+ EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
+ link_name = kg2->link->name;
+ }
+
+ bool allow = false;
+ for (unsigned int j = 0 ; !allow && j < cdata->allowed->size() ; ++j)
+ {
+ if (cdata->allowed->at(j).bound->containsPoint(pos) && cdata->allowed->at(j).depth < fabs(contact[i].geom.depth))
+ {
+ for (unsigned int k = 0 ; k < cdata->allowed->at(j).links.size() ; ++k)
+ if (cdata->allowed->at(j).links[k] == link_name)
+ {
+ allow = true;
+ break;
+ }
+ }
+ }
+
+ if (allow)
+ continue;
+ }
+ }
+
+ cdata->collides = true;
+
collision_space::EnvironmentModelODE::Contact add;
- add.pos.setX(contact[i].geom.pos[0]);
- add.pos.setY(contact[i].geom.pos[1]);
- add.pos.setZ(contact[i].geom.pos[2]);
+ add.pos = pos;
add.normal.setX(contact[i].geom.normal[0]);
add.normal.setY(contact[i].geom.normal[1]);
@@ -555,11 +597,14 @@
}
}
-bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
+bool collision_space::EnvironmentModelODE::getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
+ if (!allowedContacts.empty())
+ cdata.allowed = &allowedContacts;
+ cdata.selfSpace = m_modelGeom.space;
contacts.clear();
checkThreadInit();
testCollision(&cdata);
@@ -569,6 +614,7 @@
bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
+ cdata.selfSpace = m_modelGeom.space;
checkThreadInit();
testCollision(&cdata);
return cdata.collides;
@@ -576,7 +622,8 @@
bool collision_space::EnvironmentModelODE::isSelfCollision(void)
{
- CollisionData cdata;
+ CollisionData cdata;
+ cdata.selfSpace = m_modelGeom.space;
checkThreadInit();
testSelfCollision(&cdata);
return cdata.collides;
@@ -585,7 +632,6 @@
void collision_space::EnvironmentModelODE::testSelfCollision(CollisionData *cdata)
{
cdata->selfCollisionTest = &m_selfCollisionTest;
- cdata->selfSpace = m_modelGeom.space;
dSpaceCollide(m_modelGeom.space, cdata, nearCallbackFn);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
