Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21933] pkg/trunk
|
From: <is...@us...> - 2009-08-15 05:27:12
|
Revision: 21933
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21933&view=rev
Author: isucan
Date: 2009-08-15 05:27:03 +0000 (Sat, 15 Aug 2009)
Log Message:
-----------
bugfix in self collision checking with attached objects
Modified Paths:
--------------
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/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -41,6 +41,8 @@
#include <move_arm/MoveArmAction.h>
#include <move_arm/ActuateGripperAction.h>
+#include <mapping_msgs/AttachedObject.h>
+
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_msgs/JointTraj.h>
@@ -301,6 +303,9 @@
actionlib::SimpleActionClient<move_arm::ActuateGripperAction> gripper(nh, "actuate_gripper_" + group);
ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 1);
+ ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_object", 1);
+
+
std::map<std::string, move_arm::MoveArmGoal> goals;
std::vector<std::string> names(7);
@@ -363,6 +368,50 @@
printPose(p);
}
else
+ if (cmd == "att0")
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header.frame_id = "r_wrist_roll_link";
+ ao.header.stamp = ros::Time::now();
+ ao.link_name = "r_wrist_roll_link";
+ pubAttach.publish(ao);
+ }
+ else
+ if (cmd == "att1")
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header.frame_id = "r_wrist_roll_link";
+ ao.header.stamp = ros::Time::now();
+ ao.link_name = "r_wrist_roll_link";
+
+ mapping_msgs::Object object;
+ object.type = mapping_msgs::Object::CYLINDER;
+ object.dimensions.push_back(0.02); // 4 cm diam
+ object.dimensions.push_back(1.3); // 48 inch
+
+ // identity transform should place the object in the center
+ geometry_msgs::Pose pose;
+ pose.position.x = 0.16;
+ pose.position.y = 0;
+ pose.position.z = 0;
+
+ pose.orientation.x = 0;
+ pose.orientation.y = 0;
+ pose.orientation.z = 0;
+ pose.orientation.w = 1;
+
+ ao.objects.push_back(object);
+ ao.poses.push_back(pose);
+
+ ao.touch_links.push_back("r_gripper_l_finger_link");
+ ao.touch_links.push_back("r_gripper_r_finger_link");
+ ao.touch_links.push_back("r_gripper_l_finger_tip_link");
+ ao.touch_links.push_back("r_gripper_r_finger_tip_link");
+ ao.touch_links.push_back("r_gripper_palm_link");
+
+ pubAttach.publish(ao);
+ }
+ else
if (cmd == "time")
{
std::cout << " Allowed execution time is " << allowed_time << " seconds" << 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-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -546,7 +546,7 @@
ros::Duration eps(0.01);
ros::Duration epsLong(0.1);
- std::valarray<double> velocityHistory(3);
+ std::valarray<double> velocityHistory(20);
unsigned int velocityHistoryIndex = 0;
while (true)
@@ -654,8 +654,9 @@
ROS_INFO("Maximum path contact penetration depth is %f at link %s, sum of all contact depths is %f", ccost.cost, ccost.link.c_str(), ccost.sum);
if (velocityHistoryIndex >= velocityHistory.size())
- {
+ {
double sum = velocityHistory.sum();
+ ROS_DEBUG("vel = %f", sum);
if (sum < 1e-3)
{
ROS_INFO("The total velocity of the robot over the last %d samples is %f. Self-preempting...", (int)velocityHistory.size(), sum);
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -270,6 +270,7 @@
link->attachedBodies.push_back(new planning_models::KinematicModel::AttachedBody(link));
tf::poseMsgToTF(poseP.pose, link->attachedBodies[j]->attachTrans);
link->attachedBodies[j]->shape = shape;
+ link->attachedBodies[j]->touch_links = attachedObject->touch_links;
}
result = true;
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-08-15 05:27:03 UTC (rev 21933)
@@ -253,17 +253,20 @@
}
/** \brief The constant transform applied to the link (needs to be specified by user) */
- btTransform attachTrans;
+ btTransform attachTrans;
/** \brief The geometry of the attached body */
- shapes::Shape *shape;
+ shapes::Shape *shape;
/** \brief The global transform for this link (computed by forward kinematics) */
- btTransform globalTrans;
+ btTransform globalTrans;
/** \brief The link that owns this attached body */
- Link *owner;
+ Link *owner;
+ /** \brief The set of links this body is allowed to touch */
+ std::vector<std::string> touch_links;
+
protected:
/** \brief recompute globalTrans */
void computeTransform(btTransform &parentTrans);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -859,6 +859,7 @@
ab->attachTrans = src->attachedBodies[i]->attachTrans;
ab->shape = shapes::cloneShape(src->attachedBodies[i]->shape);
ab->globalTrans = src->attachedBodies[i]->globalTrans;
+ ab->touch_links = src->attachedBodies[i]->touch_links;
dest->attachedBodies.push_back(ab);
}
for (unsigned int i = 0 ; i < src->after.size() ; ++i)
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-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-08-15 05:27:03 UTC (rev 21933)
@@ -55,6 +55,9 @@
EnvironmentModelODE(void);
virtual ~EnvironmentModelODE(void);
+ /** \brief Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(const std::vector<std::string> &links);
+
/** \brief Get the list of contacts (collisions) */
virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1);
@@ -212,6 +215,7 @@
struct kGeom
{
std::vector<dGeomID> geom;
+ std::vector< std::vector<bool> > allowedTouch;
bool enabled;
planning_models::KinematicModel::Link *link;
unsigned int index;
@@ -329,7 +333,9 @@
dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
void updateGeom(dGeomID geom, const btTransform &pose) const;
void removeCollidingObjects(const dGeomID geom);
-
+
+ void updateAllowedTouch(void);
+
/** \brief Check if thread-specific routines have been called */
void checkThreadInit(void) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-15 05:06:29 UTC (rev 21932)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-08-15 05:27:03 UTC (rev 21933)
@@ -134,6 +134,7 @@
}
m_modelGeom.linkGeom.push_back(kg);
}
+ updateAllowedTouch();
}
dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape)
@@ -275,6 +276,7 @@
kg->geom.push_back(ga);
}
}
+ updateAllowedTouch();
}
void collision_space::EnvironmentModelODE::updateRobotModel(void)
@@ -491,12 +493,40 @@
EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
if (kg1 && kg2)
{
- if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
- return;
+ // if we have two body parts (it is the first in the list of geoms for the link), check if they are allowed to collide
+ if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false && kg1->geom[0] == o1 && kg2->geom[0] == o2)
+ return;
else
{
- cdata->link1 = kg1->link;
- cdata->link2 = kg2->link;
+ // if we are looking at a link and an attached object
+ if ((kg1->geom[0] == o1 && kg2->geom[0] != o2) || (kg1->geom[0] != o1 && kg2->geom[0] == o2))
+ {
+ int p1 = -1, p2 = -1;
+ for (unsigned int i = 0 ; i < kg1->geom.size() ; ++i)
+ if (kg1->geom[i] == o1)
+ {
+ p1 = i;
+ break;
+ }
+ for (unsigned int i = 0 ; i < kg2->geom.size() ; ++i)
+ if (kg2->geom[i] == o2)
+ {
+ p2 = i;
+ break;
+ }
+ assert(p1 >= 0 && p2 >= 0);
+ if (p1 == 0)
+ if (kg2->allowedTouch[p2][kg1->index])
+ return;
+ if (p2 == 0)
+ if (kg1->allowedTouch[p1][kg2->index])
+ return;
+ }
+ else
+ {
+ cdata->link1 = kg1->link;
+ cdata->link2 = kg2->link;
+ }
}
}
}
@@ -787,6 +817,49 @@
m_objects->clearObjects(ns);
}
+void collision_space::EnvironmentModelODE::updateAllowedTouch(void)
+{
+ const unsigned int n = m_modelGeom.linkGeom.size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ kGeom *kg = m_modelGeom.linkGeom[i];
+ kg->allowedTouch.resize(kg->geom.size());
+
+ if (kg->geom.empty())
+ continue;
+
+ kg->allowedTouch[0].resize(n);
+
+ // compute the allowed touch for robot links
+ for (unsigned int j = 0 ; j < n ; ++j)
+ // if self collision checking with link j is disabled, we are allowed to touch link i with geom 0
+ // otherwise, we are not
+ kg->allowedTouch[0][j] = !m_selfCollisionTest[kg->index][j];
+
+ const unsigned int nab = kg->link->attachedBodies.size();
+ for (unsigned int k = 0 ; k < nab ; ++k)
+ {
+ kg->allowedTouch[k + 1].clear();
+ kg->allowedTouch[k + 1].resize(n, false);
+ for (unsigned int j = 0 ; j < kg->link->attachedBodies[k]->touch_links.size() ; ++j)
+ {
+ const std::string &tlink = kg->link->attachedBodies[k]->touch_links[j];
+ std::map<std::string, unsigned int>::const_iterator it = m_collisionLinkIndex.find(tlink);
+ if (it != m_collisionLinkIndex.end())
+ kg->allowedTouch[k + 1][it->second] = true;
+ else
+ m_msg.error("Unknown link '%s'", tlink.c_str());
+ }
+ }
+ }
+}
+
+void collision_space::EnvironmentModelODE::addSelfCollisionGroup(const std::vector<std::string> &links)
+{
+ EnvironmentModel::addSelfCollisionGroup(links);
+ updateAllowedTouch();
+}
+
void collision_space::EnvironmentModelODE::removeCollidingObjects(const shapes::StaticShape *shape)
{
checkThreadInit();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
