Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3549] pkg/trunk
|
From: <is...@us...> - 2008-08-25 17:18:31
|
Revision: 3549
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3549&view=rev
Author: isucan
Date: 2008-08-25 17:18:32 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
more moving things around. the interface should now be stable.. .only internals of kinematic_planning will change
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h
pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/KinematicConstraints.msg
pkg/trunk/robot_msgs/msg/PoseConstraint.msg
Removed Paths:
-------------
pkg/trunk/robot_msgs/msg/KinematicConstraint.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -38,45 +38,82 @@
#define KINEMATIC_PLANNING_KINEMATIC_CONSTRAINT_EVALUATOR_
#include <planning_models/kinematic.h>
-#include <robot_msgs/KinematicConstraint.h>
+#include <robot_msgs/KinematicConstraints.h>
#include <iostream>
#include <vector>
+
class KinematicConstraintEvaluator
{
public:
KinematicConstraintEvaluator(void)
{
+ }
+
+ virtual ~KinematicConstraintEvaluator(void)
+ {
+ }
+
+ virtual void clear(void) = 0;
+ virtual void use(planning_models::KinematicModel *kmodel, const ros::msg *kc) = 0;
+ virtual bool decide(void) = 0;
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ }
+};
+
+
+class PoseConstraintEvaluator : public KinematicConstraintEvaluator
+{
+ public:
+
+ PoseConstraintEvaluator(void) : KinematicConstraintEvaluator()
+ {
m_link = NULL;
}
- void use(planning_models::KinematicModel *kmodel, const robot_msgs::KinematicConstraint &kc)
+ virtual void use(planning_models::KinematicModel *kmodel, const ros::msg *kc)
{
- m_link = kmodel->getLink(kc.robot_link);
- m_kc = kc;
+ const robot_msgs::PoseConstraint *pc = dynamic_cast<const robot_msgs::PoseConstraint*>(kc);
+ if (pc)
+ use(kmodel, *pc);
}
- void clear(void)
+ void use(planning_models::KinematicModel *kmodel, const robot_msgs::PoseConstraint &pc)
{
+ m_link = kmodel->getLink(pc.robot_link);
+ m_pc = pc;
+ }
+
+ virtual void clear(void)
+ {
m_link = NULL;
}
+ virtual bool decide(void)
+ {
+ double dPos, dAng;
+ evaluate(&dPos, &dAng);
+
+ return decide(dPos, dAng);
+ }
+
void evaluate(double *distPos, double *distAng)
{
if (m_link)
{
- switch (m_kc.type)
+ switch (m_pc.type)
{
- case robot_msgs::KinematicConstraint::ONLY_POSITION:
+ case robot_msgs::PoseConstraint::ONLY_POSITION:
if (distPos)
{
libTF::Pose3D::Position bodyPos;
m_link->globalTrans.getPosition(bodyPos);
- double dx = bodyPos.x - m_kc.pose.position.x;
- double dy = bodyPos.y - m_kc.pose.position.y;
- double dz = bodyPos.z - m_kc.pose.position.z;
+ double dx = bodyPos.x - m_pc.pose.position.x;
+ double dy = bodyPos.y - m_pc.pose.position.y;
+ double dz = bodyPos.z - m_pc.pose.position.z;
*distPos = dx * dx + dy * dy + dz * dz;
}
@@ -84,8 +121,8 @@
*distAng = 0.0;
break;
- case robot_msgs::KinematicConstraint::ONLY_ORIENTATION:
- case robot_msgs::KinematicConstraint::COMPLETE_POSE:
+ case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
+ case robot_msgs::PoseConstraint::COMPLETE_POSE:
default:
if (distPos)
*distPos = 0.0;
@@ -103,29 +140,21 @@
}
}
- bool decide(void)
- {
- double dPos, dAng;
- evaluate(&dPos, &dAng);
-
- return decide(dPos, dAng);
- }
-
bool decide(double dPos, double dAng)
{
bool result = true;
- switch (m_kc.type)
+ switch (m_pc.type)
{
- case robot_msgs::KinematicConstraint::ONLY_POSITION:
- result = dPos < m_kc.position_distance;
+ case robot_msgs::PoseConstraint::ONLY_POSITION:
+ result = dPos < m_pc.position_distance;
break;
- case robot_msgs::KinematicConstraint::ONLY_ORIENTATION:
- result = dAng < m_kc.orientation_distance;
+ case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
+ result = dAng < m_pc.orientation_distance;
break;
- case robot_msgs::KinematicConstraint::COMPLETE_POSE:
- result = (dPos < m_kc.position_distance) && (dAng < m_kc.orientation_distance);
+ case robot_msgs::PoseConstraint::COMPLETE_POSE:
+ result = (dPos < m_pc.position_distance) && (dAng < m_pc.orientation_distance);
break;
default:
@@ -134,17 +163,17 @@
return result;
}
- void print(std::ostream &out = std::cout) const
+ virtual void print(std::ostream &out = std::cout) const
{
if (m_link)
{
- out << "Constraint on link '" << m_kc.robot_link << "'" << std::endl;
- if (m_kc.type != robot_msgs::KinematicConstraint::ONLY_ORIENTATION)
- out << " Desired position: " << m_kc.pose.position.x << ", " << m_kc.pose.position.y << ", " << m_kc.pose.position.z
- << " (within distance: " << m_kc.position_distance << ")" << std::endl;
- if (m_kc.type != robot_msgs::KinematicConstraint::ONLY_POSITION)
- out << " Desired orientation: " << m_kc.pose.orientation.x << ", " << m_kc.pose.orientation.y << ", " << m_kc.pose.orientation.z << ", " << m_kc.pose.orientation.w
- << " (within distance: " << m_kc.orientation_distance << ")" << std::endl;
+ out << "Constraint on link '" << m_pc.robot_link << "'" << std::endl;
+ if (m_pc.type != robot_msgs::PoseConstraint::ONLY_ORIENTATION)
+ out << " Desired position: " << m_pc.pose.position.x << ", " << m_pc.pose.position.y << ", " << m_pc.pose.position.z
+ << " (within distance: " << m_pc.position_distance << ")" << std::endl;
+ if (m_pc.type != robot_msgs::PoseConstraint::ONLY_POSITION)
+ out << " Desired orientation: " << m_pc.pose.orientation.x << ", " << m_pc.pose.orientation.y << ", " << m_pc.pose.orientation.z << ", " << m_pc.pose.orientation.w
+ << " (within distance: " << m_pc.orientation_distance << ")" << std::endl;
}
else
out << "No constraint" << std::endl;
@@ -152,7 +181,7 @@
protected:
- robot_msgs::KinematicConstraint m_kc;
+ robot_msgs::PoseConstraint m_pc;
planning_models::KinematicModel::Link *m_link;
};
@@ -178,12 +207,12 @@
m_kce.clear();
}
- void use(planning_models::KinematicModel *kmodel, const std::vector<robot_msgs::KinematicConstraint> &kc)
+ void use(planning_models::KinematicModel *kmodel, const std::vector<robot_msgs::PoseConstraint> &kc)
{
clear();
for (unsigned int i = 0 ; i < kc.size() ; ++i)
{
- KinematicConstraintEvaluator *ev = new KinematicConstraintEvaluator();
+ PoseConstraintEvaluator *ev = new PoseConstraintEvaluator();
ev->use(kmodel, kc[i]);
m_kce.push_back(ev);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -129,9 +129,9 @@
}
/** Set the kinematic constraints to follow */
- void setupConstraints(KinematicPlannerSetup &psetup, const std::vector<robot_msgs::KinematicConstraint> &cstrs)
+ void setupPoseConstraints(KinematicPlannerSetup &psetup, const std::vector<robot_msgs::PoseConstraint> &cstrs)
{
- static_cast<StateValidityPredicate*>(psetup.si->getStateValidityChecker())->setConstraints(cstrs);
+ static_cast<StateValidityPredicate*>(psetup.si->getStateValidityChecker())->setPoseConstraints(cstrs);
}
/** Compute the actual motion plan */
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -44,21 +44,21 @@
{
public:
- GoalToPosition(ompl::SpaceInformation_t si, XMLModel *model, const std::vector<robot_msgs::KinematicConstraint> &kc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
+ GoalToPosition(ompl::SpaceInformation_t si, XMLModel *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
{
m_model = model;
- for (unsigned int i = 0 ; i < kc.size() ; ++i)
+ for (unsigned int i = 0 ; i < pc.size() ; ++i)
{
- KinematicConstraintEvaluator *kce = new KinematicConstraintEvaluator();
- kce->use(m_model->kmodel, kc[i]);
- m_kce.push_back(kce);
+ PoseConstraintEvaluator *pce = new PoseConstraintEvaluator();
+ pce->use(m_model->kmodel, pc[i]);
+ m_pce.push_back(pce);
}
}
virtual ~GoalToPosition(void)
{
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
- delete m_kce[i];
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
+ delete m_pce[i];
}
virtual double distanceGoal(ompl::SpaceInformationKinematic::StateKinematic_t state)
@@ -79,42 +79,42 @@
return true;
}
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ ompl::SpaceInformationKinematic::GoalRegionKinematic::print(out);
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
+ m_pce[i]->print(out);
+ }
+
+ protected:
+
double evaluateGoalAux(ompl::SpaceInformationKinematic::StateKinematic_t state, std::vector<bool> *decision)
{
update(state);
if (decision)
- decision->resize(m_kce.size());
+ decision->resize(m_pce.size());
double distance = 0.0;
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
{
double dPos, dAng;
- m_kce[i]->evaluate(&dPos, &dAng);
+ m_pce[i]->evaluate(&dPos, &dAng);
if (decision)
- (*decision)[i] = m_kce[i]->decide(dPos, dAng);
+ (*decision)[i] = m_pce[i]->decide(dPos, dAng);
distance += dPos + dAng;
}
return distance;
}
- virtual void print(std::ostream &out = std::cout) const
- {
- ompl::SpaceInformationKinematic::GoalRegionKinematic::print(out);
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
- m_kce[i]->print(out);
- }
-
- protected:
-
void update(ompl::SpaceInformationKinematic::StateKinematic_t state)
{
m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
- XMLModel *m_model;
- std::vector<KinematicConstraintEvaluator*> m_kce;
+ XMLModel *m_model;
+ std::vector<PoseConstraintEvaluator*> m_pce;
};
@@ -143,10 +143,10 @@
void setupGoalState(XMLModel *model, KinematicPlannerSetup &psetup, robot_srvs::KinematicPlanLinkPosition::request &req)
{
/* set the goal */
- std::vector<robot_msgs::KinematicConstraint> kc;
- req.get_goal_constraints_vec(kc);
+ std::vector<robot_msgs::PoseConstraint> pc;
+ req.get_goal_constraints_vec(pc);
- GoalToPosition *goal = new GoalToPosition(psetup.si, model, kc);
+ GoalToPosition *goal = new GoalToPosition(psetup.si, model, pc);
psetup.si->setGoal(goal);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -61,7 +61,7 @@
return valid;
}
- void setConstraints(const std::vector<robot_msgs::KinematicConstraint> &kc)
+ void setPoseConstraints(const std::vector<robot_msgs::PoseConstraint> &kc)
{
m_kce.use(m_model->kmodel, kc);
}
@@ -72,8 +72,8 @@
}
protected:
- XMLModel *m_model;
- KinematicConstraintEvaluatorSet m_kce;
+ XMLModel *m_model;
+ KinematicConstraintEvaluatorSet m_kce;
};
#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 17:18:32 UTC (rev 3549)
@@ -136,9 +136,9 @@
/* configure state space and starting state */
setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::KinematicConstraint> cstrs;
- req.get_constraints_vec(cstrs);
- setupConstraints(psetup, cstrs);
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ req.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(psetup, cstrs);
/* add goal state */
RequestPlanState::setupGoalState(m, psetup, req);
@@ -179,10 +179,10 @@
/* configure state space and starting state */
setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::KinematicConstraint> cstrs;
- req.get_constraints_vec(cstrs);
- setupConstraints(psetup, cstrs);
-
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ req.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(psetup, cstrs);
+
/* add goal state */
RequestPlanLinkPosition::setupGoalState(m, psetup, req);
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-25 17:18:32 UTC (rev 3549)
@@ -186,7 +186,7 @@
// the goal region is basically the position of a set of bodies
req.set_goal_constraints_size(1);
- req.goal_constraints[0].type = robot_msgs::KinematicConstraint::ONLY_POSITION;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
req.goal_constraints[0].robot_link = "wrist_flex_left";
req.goal_constraints[0].pose.position.x = 0.5;
req.goal_constraints[0].pose.position.y = 0.3;
@@ -194,13 +194,13 @@
req.goal_constraints[0].position_distance = 0.01;
// an example of constraints: do not move the elbow too much
- req.set_constraints_size(1);
- req.constraints[0].type = robot_msgs::KinematicConstraint::ONLY_POSITION;
- req.constraints[0].robot_link = "elbow_flex_left";
- req.constraints[0].pose.position.x = 0.45;
- req.constraints[0].pose.position.y = 0.188;
- req.constraints[0].pose.position.z = 0.74;
- req.constraints[0].position_distance = 0.01;
+ req.constraints.set_pose_size(1);
+ req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
+ req.constraints.pose[0].robot_link = "elbow_flex_left";
+ req.constraints.pose[0].pose.position.x = 0.45;
+ req.constraints.pose[0].pose.position.y = 0.188;
+ req.constraints.pose[0].pose.position.z = 0.74;
+ req.constraints.pose[0].position_distance = 0.01;
req.allowed_time = 3.0;
Deleted: pkg/trunk/robot_msgs/msg/KinematicConstraint.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicConstraint.msg 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_msgs/msg/KinematicConstraint.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -1,21 +0,0 @@
-# This message contains the definition of a motion planning constraint.
-# Since there are multiple types of constraints, the 'type' member is used
-# to identify the different constraints
-
-byte COMPLETE_POSE=0 # 0 = complete pose is considered
-byte ONLY_POSITION=1 # 1 = only pose position is considered
-byte ONLY_ORIENTATION=2 # 2 = only pose orientation is considered
-
-byte type
-
-# The robot link this constraint refers to
-string robot_link
-
-# The desired pose of the robot link
-std_msgs/Pose3DFloat64 pose
-
-# the allowed difference (square of euclidian distance) for position
-float64 position_distance
-
-# the allowed difference (shortest angular distance) for orientation
-float64 orientation_distance
Added: pkg/trunk/robot_msgs/msg/KinematicConstraints.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicConstraints.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/KinematicConstraints.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -0,0 +1,3 @@
+# This message contains a list of motion planning constraints.
+
+PoseConstraint[] pose
Added: pkg/trunk/robot_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/PoseConstraint.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/PoseConstraint.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -0,0 +1,21 @@
+# This message contains the definition of a motion planning constraint.
+# Since there are multiple types of constraints, the 'type' member is used
+# to identify the different constraints
+
+byte COMPLETE_POSE=0 # 0 = complete pose is considered
+byte ONLY_POSITION=1 # 1 = only pose position is considered
+byte ONLY_ORIENTATION=2 # 2 = only pose orientation is considered
+
+byte type
+
+# The robot link this constraint refers to
+string robot_link
+
+# The desired pose of the robot link
+std_msgs/Pose3DFloat64 pose
+
+# the allowed difference (square of euclidian distance) for position
+float64 position_distance
+
+# the allowed difference (shortest angular distance) for orientation
+float64 orientation_distance
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-25 17:18:32 UTC (rev 3549)
@@ -17,11 +17,11 @@
# The goal state for the model to plan for. The state is not explicit.
# The goal is considered achieved if all the constraints are satisfied.
-robot_msgs/KinematicConstraint[] goal_constraints
+robot_msgs/PoseConstraint[] goal_constraints
# No state in the produced motion plan will violate these constraints
-robot_msgs/KinematicConstraint[] constraints
+robot_msgs/KinematicConstraints constraints
# The number of times this plan is to be computed. Shortest solution
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-25 17:18:32 UTC (rev 3549)
@@ -21,7 +21,7 @@
robot_msgs/KinematicState goal_state
# No state in the produced motion plan will violate these constraints
-robot_msgs/KinematicConstraint[] constraints
+robot_msgs/KinematicConstraints constraints
# The number of times this plan is to be computed. Shortest solution
# will be reported.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
