Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19381] pkg/trunk
|
From: <is...@us...> - 2009-07-22 04:42:22
|
Revision: 19381
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19381&view=rev
Author: isucan
Date: 2009-07-22 04:42:20 +0000 (Wed, 22 Jul 2009)
Log Message:
-----------
simplifying pose constraint message
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-22 04:41:39 UTC (rev 19380)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-22 04:42:20 UTC (rev 19381)
@@ -149,7 +149,8 @@
void setupGoalEEf(const std::string &link, double x, double y, double z, pr2_robot_actions::MoveArmGoal &goal)
{
goal.goal_constraints.pose_constraint.resize(1);
- goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+ goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ + motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
goal.goal_constraints.pose_constraint[0].link_name = link;
goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "/base_link";
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-22 04:41:39 UTC (rev 19380)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-22 04:42:20 UTC (rev 19381)
@@ -544,8 +544,9 @@
// change pose constraints to joint constraints, if possible and so desired
if (req.goal_constraints.joint_constraint.empty() && // we have no joint constraints on the goal,
req.goal_constraints.pose_constraint.size() == 1 && // we have a single pose constraint on the goal
- req.goal_constraints.pose_constraint[0].type == motion_planning_msgs::PoseConstraint::POSITION_XYZ +
- motion_planning_msgs::PoseConstraint::ORIENTATION_RPY) // that is active on all 6 DOFs
+ req.goal_constraints.pose_constraint[0].type ==
+ motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y) // that is active on all 6 DOFs
{
planning_models::KinematicModel::Link *link = planningMonitor_->getKinematicModel()->getLink(req.goal_constraints.pose_constraint[0].link_name);
if (link && link->before && link->before->name == arm_joint_names_.back())
Modified: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-07-22 04:41:39 UTC (rev 19380)
+++ pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-07-22 04:42:20 UTC (rev 19381)
@@ -86,8 +86,8 @@
goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
goalA.goal_constraints.pose_constraint[0].type =
- motion_planning_msgs::PoseConstraint::POSITION_XYZ +
- motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+ motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
std::vector<std::string> names(7);
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-22 04:41:39 UTC (rev 19380)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-22 04:42:20 UTC (rev 19381)
@@ -5,24 +5,16 @@
# Constants that represent possible values for type. A position and an
# orientation constant can be combined (by adding).
-int32 POSITION_XYZ=1 # only x,y,z of position is considered
-int32 POSITION_XY=2 # only x,y of position is considered
-int32 POSITION_XZ=3 # only x,z of position is considered
-int32 POSITION_YZ=4 # only y,z of position is considered
-int32 POSITION_X=5 # only x of position is considered
-int32 POSITION_Y=6 # only y of position is considered
-int32 POSITION_Z=7 # only z of position is considered
+int32 POSITION_X=1 # only x of position is considered
+int32 POSITION_Y=2 # only y of position is considered
+int32 POSITION_Z=4 # only z of position is considered
# the next values can be combined with one of the above, so they are offset by
# 256, so we can use bit operations on them
-int32 ORIENTATION_RPY=256 # only roll, pitch, yaw of orientation is considered
-int32 ORIENTATION_RY=512 # only roll, yaw of orientation is considered
-int32 ORIENTATION_RP=768 # only roll, yaw of orientation is considered
-int32 ORIENTATION_PY=1024 # only roll, yaw of orientation is considered
-int32 ORIENTATION_R=1280 # only roll, yaw of orientation is considered
-int32 ORIENTATION_P=1536 # only roll, yaw of orientation is considered
-int32 ORIENTATION_Y=1792 # only roll, yaw of orientation is considered
+int32 ORIENTATION_R=8 # only roll, yaw of orientation is considered
+int32 ORIENTATION_P=16 # only roll, yaw of orientation is considered
+int32 ORIENTATION_Y=32 # only roll, yaw of orientation is considered
int32 type
Modified: pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp 2009-07-22 04:41:39 UTC (rev 19380)
+++ pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp 2009-07-22 04:42:20 UTC (rev 19381)
@@ -151,156 +151,62 @@
{
if (distPos)
{
- if (m_pc.type & 0xFF)
- {
+ *distPos = 0.0;
+
+ if (m_pc.type & (motion_planning_msgs::PoseConstraint::POSITION_X | motion_planning_msgs::PoseConstraint::POSITION_Y | motion_planning_msgs::PoseConstraint::POSITION_Z))
+ {
const btVector3 &bodyPos = m_link->globalTrans.getOrigin();
- double dx, dy, dz;
- switch (m_pc.type & 0xFF)
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_X)
{
- case motion_planning_msgs::PoseConstraint::POSITION_XYZ:
- dx = bodyPos.getX() - m_x;
- dy = bodyPos.getY() - m_y;
- dz = bodyPos.getZ() - m_z;
- *distPos = 0.0;
+ double dx = bodyPos.getX() - m_x;
if (dx > m_pc.position_tolerance_above.x || -m_pc.position_tolerance_below.x > dx)
*distPos += dx * dx;
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_Y)
+ {
+ double dy = bodyPos.getX() - m_y;
if (dy > m_pc.position_tolerance_above.y || -m_pc.position_tolerance_below.y > dy)
*distPos += dy * dy;
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_Z)
+ {
+ double dz = bodyPos.getZ() - m_z;
if (dz > m_pc.position_tolerance_above.z || -m_pc.position_tolerance_below.z > dz)
*distPos += dz * dz;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_XY:
- dx = bodyPos.getX() - m_x;
- dy = bodyPos.getY() - m_y;
- *distPos = 0.0;
- if (dx > m_pc.position_tolerance_above.x || -m_pc.position_tolerance_below.x > dx)
- *distPos += dx * dx;
- if (dy > m_pc.position_tolerance_above.y || -m_pc.position_tolerance_below.y > dy)
- *distPos += dy * dy;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_XZ:
- dx = bodyPos.getX() - m_x;
- dz = bodyPos.getZ() - m_z;
- *distPos = 0.0;
- if (dx > m_pc.position_tolerance_above.x || -m_pc.position_tolerance_below.x > dx)
- *distPos += dx * dx;
- if (dz > m_pc.position_tolerance_above.z || -m_pc.position_tolerance_below.z > dz)
- *distPos += dz * dz;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_YZ:
- dy = bodyPos.getY() - m_y;
- dz = bodyPos.getZ() - m_z;
- *distPos = 0.0;
- if (dy > m_pc.position_tolerance_above.y || -m_pc.position_tolerance_below.y > dy)
- *distPos += dy * dy;
- if (dz > m_pc.position_tolerance_above.z || -m_pc.position_tolerance_below.z > dz)
- *distPos += dz * dz;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_X:
- dx = bodyPos.getX() - m_x;
- if (dx > m_pc.position_tolerance_above.x || -m_pc.position_tolerance_below.x > dx)
- *distPos = dx * dx;
- else
- *distPos = 0.0;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_Y:
- dy = bodyPos.getY() - m_y;
- if (dy > m_pc.position_tolerance_above.y || -m_pc.position_tolerance_below.y > dy)
- *distPos = dy * dy;
- else
- *distPos = 0.0;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_Z:
- dz = bodyPos.getZ() - m_z;
- if (dz > m_pc.position_tolerance_above.z || -m_pc.position_tolerance_below.z > dz)
- *distPos = dz * dz;
- else
- *distPos = 0.0;
- break;
- default:
- *distPos = 0.0;
}
}
- else
- *distPos = 0.0;
}
+
+
if (distAng)
{
- if (m_pc.type & (~0xFF))
+ *distAng = 0.0;
+
+ if (m_pc.type & (motion_planning_msgs::PoseConstraint::ORIENTATION_R | motion_planning_msgs::PoseConstraint::ORIENTATION_P | motion_planning_msgs::PoseConstraint::ORIENTATION_Y))
{
- double dx, dy, dz;
btScalar yaw, pitch, roll;
- m_link->globalTrans.getBasis().getEulerYPR(yaw, pitch, roll);
- switch (m_pc.type & (~0xFF))
+ m_link->globalTrans.getBasis().getEulerYPR(yaw, pitch, roll);
+
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_R)
{
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RPY:
- dx = angles::shortest_angular_distance(roll, m_roll);
- dy = angles::shortest_angular_distance(pitch, m_pitch);
- dz = angles::shortest_angular_distance(yaw, m_yaw);
- *distAng = 0.0;
+ double dx = angles::shortest_angular_distance(roll, m_roll);
if (dx > m_pc.orientation_tolerance_above.x || -m_pc.orientation_tolerance_below.x > dx)
- *distAng += fabs(dx);
+ *distAng += fabs(dx);
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_P)
+ {
+ double dy = angles::shortest_angular_distance(pitch, m_pitch);
if (dy > m_pc.orientation_tolerance_above.y || -m_pc.orientation_tolerance_below.y > dy)
*distAng += fabs(dy);
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_Y)
+ {
+ double dz = angles::shortest_angular_distance(yaw, m_yaw);
if (dz > m_pc.orientation_tolerance_above.z || -m_pc.orientation_tolerance_below.z > dz)
*distAng += fabs(dz);
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RP:
- dx = angles::shortest_angular_distance(roll, m_roll);
- dy = angles::shortest_angular_distance(pitch, m_pitch);
- *distAng = 0.0;
- if (dx > m_pc.orientation_tolerance_above.x || -m_pc.orientation_tolerance_below.x > dx)
- *distAng += fabs(dx);
- if (dy > m_pc.orientation_tolerance_above.y || -m_pc.orientation_tolerance_below.y > dy)
- *distAng += fabs(dy);
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RY:
- dx = angles::shortest_angular_distance(roll, m_roll);
- dz = angles::shortest_angular_distance(yaw, m_yaw);
- *distAng = 0.0;
- if (dx > m_pc.orientation_tolerance_above.x || -m_pc.orientation_tolerance_below.x > dx)
- *distAng += fabs(dx);
- if (dz > m_pc.orientation_tolerance_above.z || -m_pc.orientation_tolerance_below.z > dz)
- *distAng += fabs(dz);
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_PY:
- dy = angles::shortest_angular_distance(pitch, m_pitch);
- dz = angles::shortest_angular_distance(yaw, m_yaw);
- *distAng = 0.0;
- if (dy > m_pc.orientation_tolerance_above.y || -m_pc.orientation_tolerance_below.y > dy)
- *distAng += fabs(dy);
- if (dz > m_pc.orientation_tolerance_above.z || -m_pc.orientation_tolerance_below.z > dz)
- *distAng += fabs(dz);
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_R:
- dx = angles::shortest_angular_distance(roll, m_roll);
- if (dx > m_pc.orientation_tolerance_above.x || -m_pc.orientation_tolerance_below.x > dx)
- *distAng = fabs(dx);
- else
- *distAng = 0.0;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_P:
- dy = angles::shortest_angular_distance(pitch, m_pitch);
- if (dy > m_pc.orientation_tolerance_above.y || -m_pc.orientation_tolerance_below.y > dy)
- *distAng = fabs(dy);
- else
- *distAng = 0.0;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_Y:
- dz = angles::shortest_angular_distance(yaw, m_yaw);
- if (dz > m_pc.orientation_tolerance_above.z || -m_pc.orientation_tolerance_below.z > dz)
- *distAng = fabs(dz);
- else
- *distAng = 0.0;
- break;
- default:
- *distAng = 0.0;
- break;
- }
+ }
}
- else
- *distAng = 0.0;
}
}
else
@@ -330,127 +236,48 @@
if (m_link)
{
out << "Pose constraint on link '" << m_pc.link_name << "'" << std::endl;
+
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_X)
+ {
+ out << "x = " << m_x << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.x << ", below "
+ << m_pc.position_tolerance_below.x << std::endl;
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_Y)
+ {
+ out << "y = " << m_y << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.y << ", below "
+ << m_pc.position_tolerance_below.y << std::endl;
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::POSITION_Z)
+ {
+ out << "z = " << m_z << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.z << ", below "
+ << m_pc.position_tolerance_below.z << std::endl;
+ }
- if (m_pc.type & 0xFF)
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_R)
{
- out << " Desired position: ";
- switch (m_pc.type & 0xFF)
- {
- case motion_planning_msgs::PoseConstraint::POSITION_XYZ:
- out << "x = " << m_x << " ";
- out << "y = " << m_y << " ";
- out << "z = " << m_z << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.y << ", " << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.y << ", "
- << m_pc.position_tolerance_below.z << "]" << std::endl;
-
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_XY:
- out << "x = " << m_x << " ";
- out << "y = " << m_y << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.y << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.y << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_XZ:
- out << "x = " << m_x << " ";
- out << "z = " << m_z << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_YZ:
- out << "y = " << m_y << " ";
- out << "z = " << m_z << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.y << ", "
- << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.y << ", " << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_X:
- out << "x = " << m_x << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << "], below ["
- << m_pc.position_tolerance_below.x << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_Y:
- out << "y = " << m_y << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.y << "], below ["
- << m_pc.position_tolerance_below.y << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::POSITION_Z:
- out << "z = " << m_z << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- default:
- break;
- }
+ out << "roll = " << m_roll << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.x << ", below "
+ << m_pc.position_tolerance_below.x << std::endl;
}
-
- if (m_pc.type & (~0xFF))
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_P)
{
- out << " Desired orientation: ";
- switch (m_pc.type & (~0xFF))
- {
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RPY:
- out << "roll = " << m_roll << " ";
- out << "pitch = " << m_pitch << " ";
- out << "yaw = " << m_yaw << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.y << ", " << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.y << ", "
- << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RP:
- out << "roll = " << m_roll << " ";
- out << "pitch = " << m_pitch << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.y << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.y << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_RY:
- out << "roll = " << m_roll << " ";
- out << "yaw = " << m_yaw << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << ", "
- << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.x << ", " << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_PY:
- out << "pitch = " << m_pitch << " ";
- out << "yaw = " << m_yaw << " ";
-
- out << " with tolerance: above [" << m_pc.position_tolerance_above.y << ", "
- << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.y << ", " << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_R:
- out << "roll = " << m_roll << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.x << "], below ["
- << m_pc.position_tolerance_below.x << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_P:
- out << "pitch = " << m_pitch << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.y << "], below ["
- << m_pc.position_tolerance_below.y << "]" << std::endl;
- break;
- case motion_planning_msgs::PoseConstraint::ORIENTATION_Y:
- out << "yaw = " << m_yaw << " ";
- out << " with tolerance: above [" << m_pc.position_tolerance_above.z << "], below ["
- << m_pc.position_tolerance_below.z << "]" << std::endl;
- break;
- default:
- break;
- }
+ out << "pitch = " << m_pitch << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.y << ", below "
+ << m_pc.position_tolerance_below.y << std::endl;
+ }
+ if (m_pc.type & motion_planning_msgs::PoseConstraint::ORIENTATION_Y)
+ {
+ out << "yaw = " << m_yaw << " ";
+ out << " with tolerance: above " << m_pc.position_tolerance_above.z << ", below "
+ << m_pc.position_tolerance_below.z << std::endl;
- if (m_pc.type & 0xFF)
- out << "Orientation importance is " << m_pc.orientation_importance;
}
+ if (m_pc.type & (motion_planning_msgs::PoseConstraint::ORIENTATION_R | motion_planning_msgs::PoseConstraint::ORIENTATION_P | motion_planning_msgs::PoseConstraint::ORIENTATION_Y))
+ out << "Orientation importance is " << m_pc.orientation_importance;
}
else
out << "No constraint" << std::endl;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
