Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17313] pkg/trunk
|
From: <is...@us...> - 2009-06-18 22:01:31
|
Revision: 17313
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17313&view=rev
Author: isucan
Date: 2009-06-18 22:01:25 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
changing member in KinematicPath, at Ben's request. also split kinematic model into two files (one for maintaining state)
Modified Paths:
--------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-18 22:01:25 UTC (rev 17313)
@@ -2,8 +2,7 @@
Header header
# The full state of the robot at the start of the path
-# As described by KinematicModel::StateParams
-KinematicState start_state
+KinematicJoint[] start_state
# The model for which this plan was computed
string model_id
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -59,17 +59,17 @@
{
}
- bool isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req);
+ bool isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req);
/* Check and compute a motion plan. Return true if the plan was succesfully computed */
- bool computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool computePlan(ModelMap &models, const planning_models::StateParams *start, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
protected:
/** Set up all the data needed by motion planning based on a request and lock the planner setup
* using this data */
- void configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup);
+ void configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup);
/** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -39,7 +39,7 @@
#include <sstream>
/** Check if request looks valid */
-bool kinematic_planning::RKPRequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req)
+bool kinematic_planning::RKPRequestHandler::isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -76,12 +76,6 @@
return false;
}
- if (m->kmodel->getModelInfo().stateDimension != startState.get_vals_size())
- {
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, startState.get_vals_size());
- return false;
- }
-
// check headers
for (unsigned int i = 0 ; i < req.goal_constraints.pose_constraint.size() ; ++i)
if (!m->planningMonitor->getTransformListener()->frameExists(req.goal_constraints.pose_constraint[i].pose.header.frame_id))
@@ -114,7 +108,7 @@
return true;
}
-void kinematic_planning::RKPRequestHandler::configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup)
+void kinematic_planning::RKPRequestHandler::configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup)
{
/* clear memory */
psetup->si->clearGoal();
@@ -139,19 +133,15 @@
if (psetup->model->groupID >= 0)
{
/* set the pose of the whole robot */
- psetup->model->kmodel->computeTransforms(&startState.vals[0]);
+ psetup->model->kmodel->computeTransforms(startState->getParams());
psetup->model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
- for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = startState.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
+ startState->copyParamsGroup(start->values, psetup->model->groupID);
}
else
- {
/* the start state is the complete state */
- for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = startState.vals[i];
- }
+ startState->copyParams(start->values);
psetup->si->addStartState(start);
@@ -186,9 +176,10 @@
ROS_DEBUG("=======================================");
}
-bool kinematic_planning::RKPRequestHandler::computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool kinematic_planning::RKPRequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start,
+ motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
- if (!isRequestValid(models, res.path.start_state, req))
+ if (!isRequestValid(models, req))
return false;
ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
@@ -200,7 +191,7 @@
RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
// configure the planner
- configure(res.path.start_state, req, psetup);
+ configure(start, req, psetup);
/* compute actual motion plan */
ompl::sb::PathKinematic *bestPath = NULL;
@@ -221,6 +212,15 @@
/* copy the solution to the result */
if (bestPath)
{
+ std::vector<planning_models::KinematicModel::Joint*> joints;
+ psetup->model->kmodel->getJoints(joints);
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ {
+ res.path.start_state[i].header = res.path.header;
+ res.path.start_state[i].joint_name = joints[i]->name;
+ start->copyParamsJoint(res.path.start_state[i].value, joints[i]->name);
+ }
+
res.path.states.resize(bestPath->states.size());
res.path.times.resize(bestPath->states.size());
res.path.names.clear();
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -95,6 +95,40 @@
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
}
+ planning_models::StateParams* fillStartState(const std::vector<motion_planning_msgs::KinematicJoint> &given)
+ {
+ planning_models::StateParams *s = planningMonitor_->getKinematicModel()->newStateParams();
+ for (unsigned int i = 0 ; i < given.size() ; ++i)
+ {
+ if (!planningMonitor_->getTransformListener()->frameExists(given[i].header.frame_id))
+ {
+ ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", given[i].header.frame_id.c_str(), given[i].joint_name.c_str());
+ continue;
+ }
+
+ motion_planning_msgs::KinematicJoint kj = given[i];
+ if (planningMonitor_->transformJointToFrame(kj, planningMonitor_->getFrameId()))
+ s->setParamsJoint(kj.value, kj.joint_name);
+ }
+
+ if (s->seenAll())
+ return s;
+ else
+ {
+ if (planningMonitor_->haveState())
+ {
+ std::vector<planning_models::KinematicModel::Joint*> joints;
+ planningMonitor_->getKinematicModel()->getJoints(joints);
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ if (!s->seenJoint(joints[i]->name))
+ s->setParamsJoint(planningMonitor_->getRobotState()->getParamsJoint(joints[i]->name), joints[i]->name);
+ return s;
+ }
+ }
+ delete s;
+ return NULL;
+ }
+
bool planToGoal(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
ROS_INFO("Received request for planning");
@@ -103,7 +137,7 @@
res.path.states.clear();
res.path.names.clear();
res.path.times.clear();
- res.path.start_state.vals.clear();
+ res.path.start_state.clear();
res.path.model_id = req.params.model_id;
res.path.header.frame_id = planningMonitor_->getFrameId();
res.path.header.stamp = planningMonitor_->lastMapUpdate();
@@ -111,34 +145,12 @@
res.distance = -1.0;
res.approximate = 0;
- if (planningMonitor_->haveState())
+ planning_models::StateParams *startState = fillStartState(req.start_state);
+
+ if (startState)
{
- // get current state
- planningMonitor_->getRobotState()->copyParams(res.path.start_state.vals);
-
- // apply changes as indicated in request
- for (unsigned int i = 0 ; i < req.start_state.size() ; ++i)
- {
- if (!planningMonitor_->getTransformListener()->frameExists(req.start_state[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", req.start_state[i].header.frame_id.c_str(), req.start_state[i].joint_name.c_str());
- continue;
- }
-
- planningMonitor_->transformJointToFrame(req.start_state[i], planningMonitor_->getFrameId());
- int idx = planningMonitor_->getKinematicModel()->getJointIndex(req.start_state[i].joint_name);
- if (idx >= 0)
- {
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(req.start_state[i].joint_name)->usedParams;
- if (u == req.start_state[i].value.size())
- for (unsigned int j = 0 ; j < u ; ++j)
- res.path.start_state.vals[idx + j] = req.start_state[i].value[j];
- else
- ROS_ERROR("Incorrect number of parameters for joint '%s': expected %u, got %u", req.start_state[i].joint_name.c_str(), u, (unsigned int)req.start_state[i].value.size());
- }
- }
-
- st = requestHandler_.computePlan(models_, req, res);
+ st = requestHandler_.computePlan(models_, startState, req, res);
+ delete startState;
}
else
ROS_ERROR("Starting robot state is unknown. Cannot start plan.");
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -95,7 +95,7 @@
fprintf(stderr, "%s:\n", joint->name.c_str());
// check the value of the joint at small increments
- planning_models::KinematicModel::StateParams *sp = kmsm_->getKinematicModel()->newStateParams();
+ planning_models::StateParams *sp = kmsm_->getKinematicModel()->newStateParams();
for (double val = joint->limit[0] ; val <= joint->limit[1] ; val += 0.1)
{
double to_send[controllerDim];
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -118,7 +118,7 @@
return haveMap_;
}
- /** \brief Return true if a map update has been received in the last sec seconds */
+ /** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isMapUpdated(double sec) const;
/** \brief Return the last update time for the map */
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -106,7 +106,7 @@
}
/** \brief Return a pointer to the maintained robot state */
- const planning_models::KinematicModel::StateParams* getRobotState(void) const
+ const planning_models::StateParams* getRobotState(void) const
{
return robotState_;
}
@@ -138,7 +138,7 @@
/** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** \brief Return true if a full mechanism state has been received in the last sec seconds */
+ /** \brief Return true if a full mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isStateUpdated(double sec) const;
/** \brief Return true if the pose is included in the state */
@@ -156,29 +156,28 @@
void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
- RobotModels *rm_;
- bool includePose_;
- planning_models::KinematicModel *kmodel_;
- std::string planarJoint_;
- std::string floatingJoint_;
+ RobotModels *rm_;
+ bool includePose_;
+ planning_models::KinematicModel *kmodel_;
+ std::string planarJoint_;
+ std::string floatingJoint_;
- ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
- tf::TransformListener *tf_;
+ ros::NodeHandle nh_;
+ ros::Subscriber mechanismStateSubscriber_;
+ tf::TransformListener *tf_;
- planning_models::KinematicModel::StateParams *robotState_;
- tf::Pose pose_;
- std::string robot_frame_;
- std::string frame_id_;
+ planning_models::StateParams *robotState_;
+ tf::Pose pose_;
+ std::string robot_frame_;
+ std::string frame_id_;
- boost::function<void(void)> onStateUpdate_;
+ boost::function<void(void)> onStateUpdate_;
- bool havePose_;
- bool haveMechanismState_;
- ros::Time lastStateUpdate_;
+ bool havePose_;
+ bool haveMechanismState_;
+ ros::Time lastStateUpdate_;
};
}
#endif
-
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -72,10 +72,10 @@
bool isEnvironmentSafe(void) const;
/** Check if the full state of the robot is valid */
- bool isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const;
+ bool isStateValidOnPath(const planning_models::StateParams *state) const;
/** Check if the full state of the robot is valid */
- bool isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const;
+ bool isStateValidAtGoal(const planning_models::StateParams *state) const;
/** Check if the path is valid */
bool isPathValid(const motion_planning_msgs::KinematicPath &path) const;
@@ -87,18 +87,18 @@
void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
/** Transform the frames in which constraints are specified to the one requested */
- void transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
+ bool transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
/** Transform the kinematic path to the frame requested */
- void transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
+ bool transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
/** Transform the kinematic joint to the frame requested */
- void transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
+ bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
protected:
/** Transform the joint parameters (if needed) to a target frame */
- void transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const;
+ bool transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const;
/** Check the path assuming it is in the frame of the model */
bool isPathValidAux(const motion_planning_msgs::KinematicPath &path) const;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -38,6 +38,7 @@
#define PLANNING_ENVIRONMENT_ROBOT_MODELS_
#include <planning_models/kinematic.h>
+#include <planning_models/kinematic_state_params.h>
#include <ros/ros.h>
#include <ros/node.h>
#include <boost/shared_ptr.hpp>
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -69,7 +69,6 @@
if (includePose_)
{
tf_ = new tf::TransformListener();
- tf_->setExtrapolationLimit(ros::Duration(1.0));
ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
}
else
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -36,6 +36,7 @@
#include "planning_environment/planning_monitor.h"
#include "planning_environment/kinematic_state_constraint_evaluator.h"
+#include <boost/scoped_ptr.hpp>
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
{
@@ -65,8 +66,9 @@
transformConstraintsToFrame(kcPath_, getFrameId());
}
-void planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const
{
+ bool res = true;
for (unsigned int i = 0; i < kc.pose_constraint.size() ; ++i)
tf_->transformPose(target, kc.pose_constraint[i].pose, kc.pose_constraint[i].pose);
@@ -74,41 +76,44 @@
if (getKinematicModel()->getModelInfo().planarJoints.size() > 0 || getKinematicModel()->getModelInfo().floatingJoints.size() > 0)
{
for (unsigned int i = 0; i < kc.joint_constraint.size() ; ++i)
- transformJoint(kc.joint_constraint[i].joint_name, 0, kc.joint_constraint[i].value, kc.joint_constraint[i].header, target);
+ if (!transformJoint(kc.joint_constraint[i].joint_name, 0, kc.joint_constraint[i].value, kc.joint_constraint[i].header, target))
+ res = false;
}
+ return res;
}
-void planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const
{
// if there are no planar of floating transforms, there is nothing to do
if (getKinematicModel()->getModelInfo().planarJoints.empty() && getKinematicModel()->getModelInfo().floatingJoints.empty())
{
kp.header.frame_id = target;
- return;
+ return true;
}
roslib::Header updatedHeader = kp.header;
-
+ updatedHeader.frame_id = target;
+
// transform start state
- std::vector<planning_models::KinematicModel::Joint*> joints;
- getKinematicModel()->getJoints(joints);
- unsigned int u = 0;
- for (unsigned int i = 0 ; i < joints.size() ; ++i)
- if (joints[i]->usedParams > 0)
- {
- roslib::Header header = kp.header;
- transformJoint(joints[i]->name, u, kp.start_state.vals, header, target);
- updatedHeader = header;
- u += joints[i]->usedParams;
- }
-
-
+ for (unsigned int i = 0 ; i < kp.start_state.size() ; ++i)
+ if (!transformJointToFrame(kp.start_state[i], target))
+ return false;
+
+
// transform the rest of the states
// get the joints this path is for
+ std::vector<planning_models::KinematicModel::Joint*> joints;
joints.resize(kp.names.size());
for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
joints[j] = getKinematicModel()->getJoint(kp.names[j]);
+ if (joints[j] == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s' found on path", kp.names[j].c_str());
+ return false;
+ }
+ }
// iterate through the states
for (unsigned int i = 0 ; i < kp.states.size() ; ++i)
@@ -117,24 +122,37 @@
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
roslib::Header header = kp.header;
- transformJoint(joints[j]->name, u, kp.states[i].vals, header, target);
+ if (!transformJoint(joints[j]->name, u, kp.states[i].vals, header, target))
+ return false;
updatedHeader = header;
u += joints[j]->usedParams;
}
}
kp.header = updatedHeader;
+ return true;
}
-void planning_environment::PlanningMonitor::transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const
{
- transformJoint(kj.joint_name, 0, kj.value, kj.header, target);
+ return transformJoint(kj.joint_name, 0, kj.value, kj.header, target);
}
-void planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const
+bool planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const
{
// planar joints and floating joints may need to be transformed
planning_models::KinematicModel::Joint *joint = getKinematicModel()->getJoint(name);
+ if (joint == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s'", name.c_str());
+ return false;
+ }
+ else
+ if (params.size() < index + joint->usedParams)
+ {
+ ROS_ERROR("Insufficient parameters for joint '%s'", name.c_str());
+ return false;
+ }
if (dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(joint))
{
@@ -174,9 +192,10 @@
}
else
header.frame_id = target;
+ return true;
}
-bool planning_environment::PlanningMonitor::isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const
+bool planning_environment::PlanningMonitor::isStateValidOnPath(const planning_models::StateParams *state) const
{
getEnvironmentModel()->lock();
getKinematicModel()->lock();
@@ -203,7 +222,7 @@
return valid;
}
-bool planning_environment::PlanningMonitor::isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const
+bool planning_environment::PlanningMonitor::isStateValidAtGoal(const planning_models::StateParams *state) const
{
getEnvironmentModel()->lock();
getKinematicModel()->lock();
@@ -238,8 +257,10 @@
if (path.header.frame_id != getFrameId())
{
motion_planning_msgs::KinematicPath pathT = path;
- transformPathToFrame(pathT, getFrameId());
- return isPathValidAux(pathT);
+ if (transformPathToFrame(pathT, getFrameId()))
+ return isPathValidAux(pathT);
+ else
+ return false;
}
else
return isPathValidAux(path);
@@ -247,9 +268,17 @@
bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path) const
{
- planning_models::KinematicModel::StateParams *sp = getKinematicModel()->newStateParams();
- sp->setParams(path.start_state.vals);
-
+ boost::scoped_ptr<planning_models::StateParams> sp(getKinematicModel()->newStateParams());
+
+ for (unsigned int i = 0 ; i < path.start_state.size() ; ++i)
+ sp->setParamsJoint(path.start_state[i].value, path.start_state[i].joint_name);
+
+ if (!sp->seenAll())
+ {
+ ROS_WARN("Incomplete start state specification in path");
+ return false;
+ }
+
KinematicConstraintEvaluatorSet ks;
ks.add(getKinematicModel(), kcPath_.joint_constraint);
ks.add(getKinematicModel(), kcPath_.pose_constraint);
@@ -267,7 +296,15 @@
// get the joints this path is for
std::vector<planning_models::KinematicModel::Joint*> joints(path.names.size());
for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
joints[j] = getKinematicModel()->getJoint(path.names[j]);
+ if (joints[j] == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s' found on path", path.names[j].c_str());
+ valid = false;
+ break;
+ }
+ }
// check every state
for (unsigned int i = 0 ; valid && i < path.states.size() ; ++i)
@@ -275,6 +312,13 @@
unsigned int u = 0;
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
+ if (path.states[i].vals.size() < u + joints[j]->usedParams)
+ {
+ ROS_ERROR("Incorrect state specification on path");
+ valid = false;
+ break;
+ }
+
/* copy the parameters that describe the joint */
std::vector<double> params;
for (unsigned int k = 0 ; k < joints[j]->usedParams ; ++k)
@@ -306,6 +350,5 @@
getKinematicModel()->unlock();
getEnvironmentModel()->unlock();
- delete sp;
return valid;
}
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-18 22:01:25 UTC (rev 17313)
@@ -6,6 +6,7 @@
set(ROS_BUILD_TYPE Debug)
rospack_add_library(planning_models src/kinematic.cpp
+ src/kinematic_state_params.cpp
src/load_mesh.cpp
src/output.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -39,6 +39,7 @@
#include "planning_models/shapes.h"
#include "planning_models/output.h"
+
#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <boost/thread/mutex.hpp>
@@ -47,13 +48,14 @@
#include <vector>
#include <string>
#include <map>
-#include <cassert>
/** Describing a kinematic robot model loaded from URDF. Visual geometry is ignored */
/** Main namespace */
namespace planning_models
{
+
+ class StateParams;
/** Definition of a kinematic model. This class is not thread
safe, however multiple instances can be created */
@@ -430,138 +432,7 @@
bool inRobotFrame;
};
- /** A class that can hold the named parameters of this planning model */
- class StateParams
- {
- public:
- StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo())
- {
- assert(model->isBuilt());
- m_params = m_mi.stateDimension > 0 ? new double[m_mi.stateDimension] : NULL;
- setAll(0);
- reset();
- }
-
- virtual ~StateParams(void)
- {
- if (m_params)
- delete[] m_params;
- }
-
- /** Mark all values as unseen */
- void reset(void);
-
- /** Mark all values in a group as unseen */
- void resetGroup(int groupID);
-
- /** Mark all values in a group as unseen */
- void resetGroup(const std::string &group);
-
- /** Set all the parameters to a given value */
- void setAll(const double value);
-
- /** Set all the parameters from a group to a given value */
- void setAllInGroup(const double value, const std::string &group);
-
- /** Set all the parameters from a group to a given value */
- void setAllInGroup(const double value, int groupID);
-
- /** Set all planar & floating joints to 0, so that the robot is in its own frame */
- void setInRobotFrame(void);
-
- /** Set the parameters for the complete robot. */
- bool setParams(const std::vector<double> ¶ms);
-
- /** Set the parameters for the complete robot. */
- bool setParams(const double *params);
-
- /** Set the parameters for a given group. Return true if
- any change was observed in either of the set
- values. */
- bool setParamsGroup(const std::vector<double> ¶ms, const std::string &group);
-
- /** Set the parameters for a given group. Return true if
- any change was observed in either of the set
- values. */
- bool setParamsGroup(const std::vector<double> ¶ms, int groupID);
-
- /** Set the parameters for a given group. Return true if
- any change was observed in either of the set
- values. */
- bool setParamsGroup(const double *params, const std::string &group);
-
- /** Set the parameters for a given group. Return true if
- any change was observed in either of the set
- values. */
- bool setParamsGroup(const double *params, int groupID);
-
- /** Given the name of a joint, set the values of the
- parameters describing the joint. Return true if any
- change was observed in the set value */
- bool setParamsJoint(const double *params, const std::string &name);
-
- /** Given the name of a joint, set the values of the
- parameters describing the joint. Return true if any
- change was observed in the set value */
- bool setParamsJoint(const std::vector<double> ¶ms, const std::string &name);
-
- /** Given the name of a joint, get the values of the
- parameters describing the joint. */
- const double* getParamsJoint(const std::string &name) const;
-
- /** Return the current value of the params */
- const double* getParams(void) const;
-
- /** Copy the parameters for a given group to a destination address */
- void copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const;
-
- /** Copy the parameters for a given group to a destination address */
- void copyParamsGroup(std::vector<double> ¶ms, int groupID) const;
-
- /** Copy the parameters for a given group to a destination address */
- void copyParamsGroup(double *params, const std::string &group) const;
-
- /** Copy the parameters for a given group to a destination address */
- void copyParamsGroup(double *params, int groupID) const;
-
- /** Copy all parameters to a destination address */
- void copyParams(double *params) const;
-
- /** Copy all parameters to a destination address */
- void copyParams(std::vector<double> ¶ms) const;
-
- /** Copy the parameters describing a given joint */
- void copyParamsJoint(double *params, const std::string &name) const;
-
- /** Copy the parameters describing a given joint */
- void copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const;
-
- /** Check if all params in a group were seen */
- bool seenAllGroup(const std::string &group) const;
-
- /** Check if all params in a group were seen */
- bool seenAllGroup(int groupID) const;
-
- /** Check if all params were seen */
- bool seenAll(void) const;
-
- /** Print the data from the state to screen */
- void print(std::ostream &out = std::cout) const;
-
- /** Print the missing joint names */
- void missing(std::ostream &out = std::cout);
-
- protected:
-
- KinematicModel *m_owner;
- msg::Interface m_msg;
- ModelInfo &m_mi;
- double *m_params;
- std::map<unsigned int, bool> m_seen;
- };
-
-
KinematicModel(void)
{
m_mi.stateDimension = 0;
Added: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h (rev 0)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -0,0 +1,178 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef PLANNING_MODELS_KINEMATIC_STATE_PARAMS_
+#define PLANNING_MODELS_KINEMATIC_STATE_PARAMS_
+
+#include "planning_models/kinematic.h"
+#include "planning_models/output.h"
+#include <vector>
+#include <string>
+#include <map>
+#include <iostream>
+
+/** Maintaining a kinematic state */
+
+/** Main namespace */
+namespace planning_models
+{
+
+ /** \brief A class that can hold the named parameters of this planning model */
+ class StateParams
+ {
+ public:
+
+ StateParams(KinematicModel *model);
+ virtual ~StateParams(void);
+
+ /** \brief Mark all values as unseen */
+ void reset(void);
+
+ /** \brief Mark all values in a group as unseen */
+ void resetGroup(int groupID);
+
+ /** \brief Mark all values in a group as unseen */
+ void resetGroup(const std::string &group);
+
+ /** \brief Set all the parameters to a given value */
+ void setAll(const double value);
+
+ /** \brief Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, const std::string &group);
+
+ /** \brief Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, int groupID);
+
+ /** \brief Set all planar & floating joints to 0, so that the robot is in its own frame */
+ void setInRobotFrame(void);
+
+ /** \brief Set the parameters for the complete robot. */
+ bool setParams(const std::vector<double> ¶ms);
+
+ /** \brief Set the parameters for the complete robot. */
+ bool setParams(const double *params);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const std::vector<double> ¶ms, const std::string &group);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const std::vector<double> ¶ms, int groupID);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, const std::string &group);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, int groupID);
+
+ /** \brief Given the name of a joint, set the values of the
+ parameters describing the joint. Return true if any
+ change was observed in the set value */
+ bool setParamsJoint(const double *params, const std::string &name);
+
+ /** \brief Given the name of a joint, set the values of the
+ parameters describing the joint. Return true if any
+ change was observed in the set value */
+ bool setParamsJoint(const std::vector<double> ¶ms, const std::string &name);
+
+ /** \brief Given the name of a joint, get the values of the
+ parameters describing the joint. */
+ const double* getParamsJoint(const std::string &name) const;
+
+ /** \brief Return the current value of the params */
+ const double* getParams(void) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(std::vector<double> ¶ms, int groupID) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, const std::string &group) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, int groupID) const;
+
+ /** \brief Copy all parameters to a destination address */
+ void copyParams(double *params) const;
+
+ /** \brief Copy all parameters to a destination address */
+ void copyParams(std::vector<double> ¶ms) const;
+
+ /** \brief Copy the parameters describing a given joint */
+ void copyParamsJoint(double *params, const std::string &name) const;
+
+ /** \brief Copy the parameters describing a given joint */
+ void copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const;
+
+ /** \brief Check if all params for a joint were seen */
+ bool seenJoint(const std::string &name) const;
+
+ /** \brief Check if all params in a group were seen */
+ bool seenAllGroup(const std::string &group) const;
+
+ /** \brief Check if all params in a group were seen */
+ bool seenAllGroup(int groupID) const;
+
+ /** \brief Check if all params were seen */
+ bool seenAll(void) const;
+
+ /** \brief Print the data from the state to screen */
+ void print(std::ostream &out = std::cout) const;
+
+ /** \brief Print the missing joint names */
+ void missing(std::ostream &out = std::cout);
+
+ protected:
+
+ KinematicModel *m_owner;
+ msg::Interface m_msg;
+ KinematicModel::ModelInfo &m_mi;
+ double *m_params;
+ std::map<unsigned int, bool> m_seen;
+ };
+
+}
+
+#endif
Property changes on: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -35,6 +35,8 @@
/** \author Ioan Sucan */
#include <planning_models/kinematic.h>
+#include <planning_models/kinematic_state_params.h>
+#include <cassert>
#include <algorithm>
#include <sstream>
#include <cmath>
@@ -323,14 +325,11 @@
return m_built;
}
-planning_models::KinematicModel::StateParams* planning_models::KinematicModel::newStateParams(void)
+planning_models::StateParams* planning_models::KinematicModel::newStateParams(void)
{
- StateParams *sp = new StateParams(this);
- if (m_mi.inRobotFrame)
- sp->setInRobotFrame();
- return sp;
+ return new StateParams(this);
}
-
+
void planning_models::KinematicModel::reduceToRobotFrame(void)
{
if (!m_mi.inRobotFrame)
@@ -625,218 +624,6 @@
globalTrans = attachTrans * parentTrans;
}
-void planning_models::KinematicModel::StateParams::reset(void)
-{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- m_seen[i] = false;
-}
-
-void planning_models::KinematicModel::StateParams::resetGroup(const std::string &group)
-{
- resetGroup(m_owner->getGroupID(group));
-}
-
-void planning_models::KinematicModel::StateParams::resetGroup(int groupID)
-{
- assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
- for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
- {
- unsigned int j = m_mi.groupStateIndexList[groupID][i];
- m_seen[j] = false;
- }
-}
-
-bool planning_models::KinematicModel::StateParams::seenAll(void) const
-{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- {
- std::map<unsigned int, bool>::const_iterator it = m_seen.find(i);
- if (!it->second)
- return false;
- }
- return true;
-}
-
-bool planning_models::KinematicModel::StateParams::seenAllGroup(const std::string &group) const
-{
- return seenAllGroup(m_owner->getGroupID(group));
-}
-
-bool planning_models::KinematicModel::StateParams::seenAllGroup(int groupID) const
-{
- assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
- for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
- {
- unsigned int j = m_mi.groupStateIndexList[groupID][i];
- std::map<unsigned int, bool>::const_iterator it = m_seen.find(j);
- if (!it->second)
- return false;
- }
- return true;
-}
-
-void planning_models::KinematicModel::StateParams::missing(std::ostream &out)
-{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- if (!m_seen[i])
- out << m_mi.parameterName[i] << " ";
-}
-
-const double* planning_models::KinematicModel::StateParams::getParamsJoint(const std::string &name) const
-{
- Joint *joint = m_owner->getJoint(name);
- if (joint)
- {
- std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
- if (it != m_mi.parameterIndex.end())
- return m_params + it->second;
- else
- return NULL;
- }
- else
- return NULL;
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsJoint(const std::vector<double> ¶ms, const std::string &name)
-{
- bool result = false;
- Joint *joint = m_owner->getJoint(name);
- if (joint)
- {
- double *dparams = new double[joint->usedParams];
- result = setParamsJoint(dparams, name);
- delete[] dparams;
- }
- return result;
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsJoint(const double *params, const std::string &name)
-{
- bool result = false;
- Joint *joint = m_owner->getJoint(name);
- if (joint)
- {
- unsigned int pos = m_mi.parameterIndex[name];
- for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- {
- unsigned int pos_i = pos + i;
- if (m_params[pos_i] != params[i] || !m_seen[pos_i])
- {
- m_params[pos_i] = params[i];
- m_seen[pos_i] = true;
- result = true;
- }
- }
- }
- else
- m_msg.error("Unknown joint: '" + name + "'");
- return result;
-}
-
-bool planning_models::KinematicModel::StateParams::setParams(const std::vector<double> ¶ms)
-{
- double *dparams = new double[m_mi.stateDimension];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
- bool result = setParams(dparams);
- delete[] dparams;
- return result;
-}
-
-bool planning_models::KinematicModel::StateParams::setParams(const double *params)
-{
- bool result = false;
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- if (m_params[i] != params[i] || !m_seen[i])
- {
- m_params[i] = params[i];
- m_seen[i] = true;
- result = true;
- }
- return result;
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsGroup(const std::vector<double> ¶ms, const std::string &group)
-{
- return setParamsGroup(params, m_owner->getGroupID(group));
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsGroup(const std::vector<double> ¶ms, int groupID)
-{
- double *dparams = new double[m_owner->getGroupDimension(groupID)];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
- bool result = setParamsGroup(dparams, groupID);
- delete[] dparams;
- return result;
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsGroup(const double *params, const std::string &group)
-{
- return setParamsGroup(params, m_owner->getGroupID(group));
-}
-
-bool planning_models::KinematicModel::StateParams::setParamsGroup(const double *params, int groupID)
-{
- bool result = false;
- for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
- {
- unsigned int j = m_mi.groupStateIndexList[groupID][i];
- if (m_params[j] != params[i] || !m_seen[j])
- {
- m_params[j] = params[i];
- m_seen[j] = true;
- result = true;
- }
- }
- return result;
-}
-
-void planning_models::KinematicModel::StateParams::setAllInGroup(const double value, const std::string &group)
-{
- setAllInGroup(value, m_owner->getGroupID(group));
-}
-
-void planning_models::KinematicModel::StateParams::setAllInGroup(const double value, int groupID)
-{
- assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
- for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
- {
- unsigned int j = m_mi.groupStateIndexList[groupID][i];
- m_params[j] = value;
- m_seen[j] = true;
- }
-}
-
-void planning_models::KinematicModel::StateParams::setAll(const double value)
-{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- {
- m_params[i] = value;
- m_seen[i] = true;
- }
-}
-
-void planning_models::KinematicModel::StateParams::setInRobotFrame(void)
-{
- for (unsigned int j = 0 ; j < m_mi.floatingJoints.size() ; ++j)
- {
- double vals[7] = {0, 0, 0, 0, 0, 0, 1};
- setParamsJoint(vals, m_mi.parameterName[m_mi.floatingJoints[j]]);
- }
-
- for (unsigned int j = 0 ; j < m_mi.planarJoints.size() ; ++j)
- {
- double vals[3] = {0, 0, 0};
- setParamsJoint(vals, m_mi.parameterName[m_mi.planarJoints[j]]);
- }
-}
-
-const double* planning_models::KinematicModel::StateParams::getParams(void) const
-{
- return m_params;
-}
-
int planning_models::KinematicModel::getJointIndex(const std::string &name) const
{
std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
@@ -883,82 +670,6 @@
return -1;
}
-void planning_models::KinematicModel::StateParams::copyParamsJoint(double *params, const std::string &name) const
-{
- Joint *joint = m_owner->getJoint(name);
- if (joint)
- {
- std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
- if (it != m_mi.parameterIndex.end())
- {
- unsigned int pos = it->second;
- for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- params[i] = m_params[pos + i];
- return;
- }
- }
- m_msg.error("Unknown joint: '" + name + "'");
-}
-
-void planning_models::KinematicModel::StateParams::copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const
-{
- Joint *joint = m_owner->getJoint(name);
- if (joint)
- {
- params.resize(joint->usedParams);
- std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
- if (it != m_mi.parameterIndex.end())
- {
- unsigned int pos = it->second;
- for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- params[i] = m_params[pos + i];
- return;
- }
- }
- m_msg.error("Unknown joint: '" + name + "'");
-}
-
-void planning_models::KinematicModel::StateParams::copyParams(double *params) const
-{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- params[i] = m_params[i];
-}
-
-void planning_models::KinematicModel::StateParams::copyParams(std::vector<double> ¶ms) const
-{
- params.resize(m_mi.stateDimension);
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- params[i] = m_params[i];
-}
-
-void planning_models::KinematicModel::StateParams::copyParamsGroup(double *params, const std::string &group) const
-{
- copyParamsGroup(params, m_owner->getGroupID(group));
-}
-
-void planning_models::KinematicModel::StateParams::copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const
-{
- copyParamsGroup(params, m_owner->getGroupID(group));
-}
-
-void planning_models::KinematicModel::StateParams::copyParamsGroup(std::vector<double> ¶ms, int groupID) const
-{
- unsigned int dim = m_owner->getGroupDimension(groupID);
- double *dparams = new double[dim];
- copyParamsGroup(dparams, groupID);
- params.resize(dim);
- for (unsigned int i = 0 ; i < dim ; ++i)
- params[i] = dparams[i];
- delete[] dparams;
-}
-
-void planning_models::KinematicModel::StateParams::copyParamsGroup(double *params, int groupID) const
-{
- assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
- for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
- params[i] = m_params[m_mi.groupStateIndexList[groupID][i]];
-}
-
void planning_models::KinematicModel::printModelInfo(std::ostream &out)
{
out << "Number of robots = " << getRobotCount() << std::endl;
@@ -1037,26 +748,3 @@
out << std::endl;
}
}
-
-void planning_models::KinematicModel::StateParams::print(std::ostream &out) const
-{
- out << std::endl;
- for (std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.begin() ; it != m_mi.parameterIndex.end() ; ++it)
- {
- Joint* joint = m_owner->getJoint(it->first);
- if (joint)
- {
- out << it->first;
- std::map<unsigned int, bool>::const_iterator sit = m_seen.find(it->second);
- if (!sit->second)
- out << "[ *** UNSEEN *** ]";
- out << ": ";
- for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- out << m_params[it->second + i] << std::endl;
- }
- }
- out << std::endl;
- for (unsigned int i = 0; i < m_mi.stateDimension ; ++i)
- out << m_params[i] << " ";
- out << std::endl;
-}
Added: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -0,0 +1,389 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <planning_models/kinematic_state_params.h>
+#include <cassert>
+#include <algorithm>
+#include <sstream>
+#include <cmath>
+
+planning_models::StateParams::StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo())
+{
+ assert(model->isBuilt());
+ m_params = m_mi.stateDimension > 0 ? new double[m_mi.stateDimension] : NULL;
+ setAll(0);
+ reset();
+ if (m_mi.inRobotFrame)
+ setInRobotFrame();
+}
+
+planning_models::StateParams::~StateParams(void)
+{
+ if (m_params)
+ delete[] m_params;
+}
+
+void planning_models::StateParams::reset(void)
+{
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ m_seen[i] = false;
+}
+
+void planning_models::StateParams::resetGroup(const std::string &group)
+{
+ resetGroup(m_owner->getGroupID(group));
+}
+
+void planning_models::StateParams::resetGroup(int groupID)
+{
+ assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
+ for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+ {
+ unsigned int j = m_mi.groupStateIndexList[groupID][i];
+ m_seen[j] = false;
+ }
+}
+
+bool planning_models::StateParams::seenAll(void) const
+{
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ {
+ std::map<unsigned int, bool>::const_iterator it = m_seen.find(i);
+ if (!it->second)
+ return false;
+ }
+ return true;
+}
+
+bool planning_models::StateParams::seenAllGroup(const std::string &group) const
+{
+ return seenAllGroup(m_owner->getGroupID(group));
+}
+
+bool planning_models::StateParams::seenAllGroup(int groupID) const
+{
+ assert(groupID >= 0 && groupID < (int)m_owner->getGroupCount());
+ for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+ {
+ unsigned int j = m_mi.groupStateIndexList[groupID][i];
+ std::map<unsigned int, bool>::const_iterator it = m_seen.find(j);
+ if (!it->second)
+ return false;
+ }
+ return true;
+}
+
+bool planning_models::StateParams::seenJoint(const std::string &name) const
+{
+ KinematicModel::Joint *joint = m_owner->getJoint(name);
+ if (joint)
+ {
+ int idx = m_mi.parameterIndex[name];
+ for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
+ {
+ std::map<unsigned int, bool>::const_iterator it = m_seen.find(idx + i);
+ if (!it->second)
+ return false;
+ }
+ return true;
+ }
+ else
+ {
+ m_msg.error("Unknown joint: '" + name + "'");
+ return false;
+ }
+}
+
+void planning_models::StateParams::missing(std::ostream &out)
+{
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ if (!m_seen[i])
+ out << m_mi.parameterName[i] << " ";
+}
+
+const double* planning_models::StateParams::getParamsJoint(const std::string &name) const
+{
+ KinematicModel::Joint *joint = m_owner->getJoint(name);
+ if (joint)
+ {
+ std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
+ if (it != m_mi.parameterIndex.end())
+ return m_params + it->second;
+ else
+ return NULL;
+ }
+ else
+ return NULL;
+}
+
+bool planning_models::StateParams::setParamsJoint(const std::vector<double> ¶ms, const std::string &name)
+{
+ bool result = false;
+ KinematicModel::Joint *joint = m_owner->getJoint(name);
+ if (joint)
+ {
+ double *dparams = new double[joint->usedParams];
+ result = setParamsJoint(dparams, name);
+ delete[] dparams;
+ }
+ return result;
+}
+
+bool planning_models::StateParams::setParamsJoint(const double *p...
[truncated message content] |
