Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21861] pkg/trunk
|
From: <is...@us...> - 2009-08-14 05:28:01
|
Revision: 21861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21861&view=rev
Author: isucan
Date: 2009-08-14 05:27:52 +0000 (Fri, 14 Aug 2009)
Log Message:
-----------
monitoring robot velocity to decide when to stop motion (if for instance, the robot bumps into something)
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-14 05:27:52 UTC (rev 21861)
@@ -52,6 +52,7 @@
#include <planning_environment/util/construct_object.h>
#include <geometric_shapes/bodies.h>
+#include <valarray>
#include <algorithm>
#include <cstdlib>
@@ -545,6 +546,9 @@
ros::Duration eps(0.01);
ros::Duration epsLong(0.1);
+ std::valarray<double> velocityHistory(3);
+ unsigned int velocityHistoryIndex = 0;
+
while (true)
{
// if we have to stop, do so
@@ -624,7 +628,6 @@
if (feedback->mode == move_arm::MoveArmFeedback::MOVING)
{
bool safe = planningMonitor_->isEnvironmentSafe();
- bool valid = true;
// we don't want to check the part of the path that was already executed
currentPos = planningMonitor_->closestStateOnPath(currentPath, currentPos, currentPath.states.size() - 1, planningMonitor_->getRobotState());
@@ -637,14 +640,24 @@
CollisionCost ccost;
planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, &ccost, true, _1), 0);
planningMonitor_->setAllowedContacts(allowed_contacts);
- valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST +
- planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
+ bool valid = planningMonitor_->isPathValid(currentPath, currentPos, currentPath.states.size() - 1, planning_environment::PlanningMonitor::COLLISION_TEST +
+ planning_environment::PlanningMonitor::PATH_CONSTRAINTS_TEST, false);
planningMonitor_->clearAllowedContacts();
planningMonitor_->setOnCollisionContactCallback(NULL);
if (!valid)
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();
+ 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);
+ state = PREEMPTED;
+ }
+ }
+
if (state == PREEMPTED || !safe || !valid)
{
if (state == PREEMPTED)
@@ -703,6 +716,7 @@
break;
}
ROS_INFO("Sent trajectory %d to controller", trajectoryId);
+ velocityHistoryIndex = 0;
}
else
{
@@ -711,6 +725,11 @@
break;
}
}
+ else
+ {
+ velocityHistory[velocityHistoryIndex % velocityHistory.size()] = planningMonitor_->getTotalVelocity();
+ velocityHistoryIndex++;
+ }
// monitor controller execution by calling trajectory query
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-08-14 05:27:52 UTC (rev 21861)
@@ -46,6 +46,7 @@
#include <boost/bind.hpp>
#include <vector>
#include <string>
+#include <map>
namespace planning_environment
{
@@ -125,6 +126,12 @@
return robotState_;
}
+ /** \brief Return the maintained robot velocity (square root of sum of squares of velocity at each joint)*/
+ double getTotalVelocity(void) const
+ {
+ return robotVelocity_;
+ }
+
/** \brief Return the transform listener */
tf::TransformListener *getTransformListener(void) const
{
@@ -204,6 +211,7 @@
*attachedBodyNotifier_;
planning_models::StateParams *robotState_;
+ double robotVelocity_;
tf::Pose pose_;
std::string robot_frame_;
std::string frame_id_;
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-14 05:16:09 UTC (rev 21860)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-08-14 05:27:52 UTC (rev 21861)
@@ -49,7 +49,8 @@
onAfterAttachBody_ = NULL;
attachedBodyNotifier_ = NULL;
havePose_ = haveJointState_ = false;
-
+ robotVelocity_ = 0.0;
+
if (rm_->loadedModels())
{
kmodel_ = rm_->getKinematicModel().get();
@@ -118,7 +119,8 @@
bool change = !haveJointState_;
static bool first_time = true;
-
+
+ double totalv = 0.0;
unsigned int n = jointState->get_joints_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
@@ -128,6 +130,8 @@
if (joint->usedParams == 1)
{
double pos = jointState->joints[i].position;
+ double vel = jointState->joints[i].velocity;
+ totalv += vel * vel;
planning_models::KinematicModel::RevoluteJoint* rjoint = dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(joint);
if (rjoint)
if (rjoint->continuous)
@@ -147,7 +151,8 @@
ROS_ERROR("Unknown joint: %s", jointState->joints[i].name.c_str());
}
}
-
+ robotVelocity_ = sqrt(totalv);
+
first_time = false;
lastJointStateUpdate_ = jointState->header.stamp;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
