Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10398] pkg/trunk
|
From: <is...@us...> - 2009-01-30 05:25:06
|
Revision: 10398
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10398&view=rev
Author: isucan
Date: 2009-01-30 05:25:03 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
more complex pose constraints
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-30 05:25:03 UTC (rev 10398)
@@ -58,21 +58,14 @@
PoseConstraintEvaluator *pce = new PoseConstraintEvaluator();
pce->use(m_model->kmodel, pc[i]);
m_pce.push_back(pce);
-
- switch (pc[i].type)
- {
- case robot_msgs::PoseConstraint::ONLY_POSITION:
+
+ // if we have position constraints
+ if (pc[i].type & 0xFF)
threshold += pc[i].position_distance;
- break;
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- threshold += pc[i].orientation_distance;
- break;
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- threshold += pc[i].position_distance + pc[i].orientation_distance * pc[i].orientation_importance;
- break;
- default:
- break;
- }
+
+ // if we have orientation constraints
+ if (pc[i].type & (~0xFF))
+ threshold += ((pc[i].type & 0xFF) ? pc[i].orientation_importance : 1.0) * pc[i].orientation_distance;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-30 05:25:03 UTC (rev 10398)
@@ -39,6 +39,7 @@
#include <planning_models/kinematic.h>
#include <robot_msgs/KinematicConstraints.h>
+#include <angles/angles.h>
#include <iostream>
#include <vector>
@@ -110,13 +111,48 @@
{
if (distPos)
{
- if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_POSITION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ if (m_pc.type & 0xFF)
{
- btVector3 bodyPos = m_link->globalTrans.getOrigin();
- double dx = bodyPos.getX() - m_pc.pose.position.x;
- double dy = bodyPos.getY() - m_pc.pose.position.y;
- double dz = bodyPos.getZ() - m_pc.pose.position.z;
- *distPos = dx * dx + dy * dy + dz * dz;
+ const btVector3 &bodyPos = m_link->globalTrans.getOrigin();
+ double dx, dy, dz;
+ switch (m_pc.type & 0xFF)
+ {
+ case robot_msgs::PoseConstraint::POSITION_XYZ:
+ dx = bodyPos.getX() - m_pc.x;
+ dy = bodyPos.getY() - m_pc.y;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dx * dx + dy * dy + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XY:
+ dx = bodyPos.getX() - m_pc.x;
+ dy = bodyPos.getY() - m_pc.y;
+ *distPos = dx * dx + dy * dy;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XZ:
+ dx = bodyPos.getX() - m_pc.x;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dx * dx + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_YZ:
+ dy = bodyPos.getY() - m_pc.y;
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dy * dy + dz * dz;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_X:
+ dx = bodyPos.getX() - m_pc.x;
+ *distPos = dx * dx;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Y:
+ dy = bodyPos.getY() - m_pc.y;
+ *distPos = dy * dy;
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Z:
+ dz = bodyPos.getZ() - m_pc.z;
+ *distPos = dz * dz;
+ break;
+ default:
+ *distPos = 0.0;
+ }
}
else
*distPos = 0.0;
@@ -124,10 +160,42 @@
if (distAng)
{
- if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_ORIENTATION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ if (m_pc.type & (~0xFF))
{
- btQuaternion quat(m_pc.pose.orientation.x, m_pc.pose.orientation.y, m_pc.pose.orientation.z, m_pc.pose.orientation.w);
- *distAng = quat.angle(m_link->globalTrans.getRotation());
+ btScalar yaw, pitch, roll;
+ m_link->globalTrans.getBasis().getEulerYPR(yaw, pitch, roll);
+ switch (m_pc.type & (~0xFF))
+ {
+ case robot_msgs::PoseConstraint::ORIENTATION_RPY:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(pitch, m_pc.pitch) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RP:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(pitch, m_pc.pitch);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RY:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_PY:
+ *distAng = angles::shortest_angular_distance(pitch, m_pc.pitch) +
+ angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_R:
+ *distAng = angles::shortest_angular_distance(roll, m_pc.roll);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_P:
+ *distAng = angles::shortest_angular_distance(pitch, m_pc.pitch);
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_Y:
+ *distAng = angles::shortest_angular_distance(yaw, m_pc.yaw);
+ break;
+ default:
+ *distAng = 0.0;
+ break;
+ }
}
else
*distAng = 0.0;
@@ -144,25 +212,9 @@
bool decide(double dPos, double dAng)
{
- bool result = true;
- switch (m_pc.type)
- {
- case robot_msgs::PoseConstraint::ONLY_POSITION:
- result = dPos < m_pc.position_distance;
- break;
-
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- result = dAng < m_pc.orientation_distance;
- break;
-
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- result = (dPos < m_pc.position_distance) && (dAng < m_pc.orientation_distance);
- break;
-
- default:
- break;
- }
- return result;
+ bool v1 = (m_pc.type & 0xFF) ? dPos < m_pc.position_distance : true;
+ bool v2 = (m_pc.type & (~0xFF)) ? dAng < m_pc.orientation_distance : true;
+ return v1 && v2;
}
const robot_msgs::PoseConstraint& getConstraintMessage(void) const
@@ -175,12 +227,86 @@
if (m_link)
{
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;
+
+ if (m_pc.type & 0xFF)
+ {
+ out << " Desired position: ";
+ switch (m_pc.type & 0xFF)
+ {
+ case robot_msgs::PoseConstraint::POSITION_XYZ:
+ out << "x = " << m_pc.x << " ";
+ out << "y = " << m_pc.y << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XY:
+ out << "x = " << m_pc.x << " ";
+ out << "y = " << m_pc.y << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_XZ:
+ out << "x = " << m_pc.x << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_YZ:
+ out << "y = " << m_pc.y << " ";
+ out << "z = " << m_pc.z << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_X:
+ out << "x = " << m_pc.x << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Y:
+ out << "y = " << m_pc.y << " ";
+ break;
+ case robot_msgs::PoseConstraint::POSITION_Z:
+ out << "z = " << m_pc.z << " ";
+ break;
+ default:
+ break;
+ }
+
+ out << " (within distance: " << m_pc.position_distance << ")" << std::endl;
+ }
+
+
+ if (m_pc.type & (~0xFF))
+ {
+ out << " Desired orientation: ";
+ switch (m_pc.type & (~0xFF))
+ {
+ case robot_msgs::PoseConstraint::ORIENTATION_RPY:
+ out << "roll = " << m_pc.roll << " ";
+ out << "pitch = " << m_pc.pitch << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RP:
+ out << "roll = " << m_pc.roll << " ";
+ out << "pitch = " << m_pc.pitch << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_RY:
+ out << "roll = " << m_pc.roll << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_PY:
+ out << "pitch = " << m_pc.pitch << " ";
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_R:
+ out << "roll = " << m_pc.roll << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_P:
+ out << "pitch = " << m_pc.pitch << " ";
+ break;
+ case robot_msgs::PoseConstraint::ORIENTATION_Y:
+ out << "yaw = " << m_pc.yaw << " ";
+ break;
+ default:
+ break;
+ }
+
+ out << " (within distance: " << m_pc.orientation_distance;
+ if (m_pc.type & 0xFF)
+ out << " and with importance " << m_pc.orientation_importance;
+ out << ")" << std::endl;
+ }
}
else
out << "No constraint" << std::endl;
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-01-30 05:25:03 UTC (rev 10398)
@@ -6,6 +6,7 @@
<depend package="roscpp" />
<depend package="rosconsole" />
<depend package="tf" />
+ <depend package="angles" />
<depend package="std_msgs" />
<depend package="std_srvs" />
Modified: pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -123,11 +123,11 @@
// 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::PoseConstraint::ONLY_POSITION;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ;
req.goal_constraints[0].robot_link = "wrist_flex_right";
- req.goal_constraints[0].pose.position.x = 0.0;
- req.goal_constraints[0].pose.position.y = 0.0;
- req.goal_constraints[0].pose.position.z = -100.0;
+ req.goal_constraints[0].x = 0.0;
+ req.goal_constraints[0].y = 0.0;
+ req.goal_constraints[0].z = -100.0;
req.goal_constraints[0].position_distance = 0.01;
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 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -127,17 +127,15 @@
// set the goal constraints
req.set_goal_constraints_size(1);
- req.goal_constraints[0].type = robot_msgs::PoseConstraint::COMPLETE_POSE;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RY;
req.goal_constraints[0].robot_link = "r_gripper_palm_link";
- req.goal_constraints[0].pose.position.x = 0.75025;
- req.goal_constraints[0].pose.position.y = -0.188;
- req.goal_constraints[0].pose.position.z = 0.829675;
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
- req.goal_constraints[0].pose.orientation.x = 0;
- req.goal_constraints[0].pose.orientation.y = 0;
- req.goal_constraints[0].pose.orientation.z = 0;
- req.goal_constraints[0].pose.orientation.w = 1;
-
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
req.goal_constraints[0].position_distance = 0.001;
req.goal_constraints[0].orientation_distance = 0.03;
req.goal_constraints[0].orientation_importance = 0.01;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -80,18 +80,16 @@
// place some constraints on the goal
req.set_goal_constraints_size(1);
- // other options are ONLY_POSITION, ONLY_ORIENTATION
// see the constraints message definition
- req.goal_constraints[0].type = robot_msgs::PoseConstraint::COMPLETE_POSE;
- req.goal_constraints[0].robot_link = "r_gripper_palm_link";
- req.goal_constraints[0].pose.position.x = 0.75025;
- req.goal_constraints[0].pose.position.y = -0.188;
- req.goal_constraints[0].pose.position.z = 0.829675;
- req.goal_constraints[0].pose.orientation.x = 0;
- req.goal_constraints[0].pose.orientation.y = 0;
- req.goal_constraints[0].pose.orientation.z = 0;
- req.goal_constraints[0].pose.orientation.w = 1;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RY;
+ req.goal_constraints[0].robot_link = "r_gripper_palm_link";
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
// the threshold for solution is position_distance + orientation_distance * orientation_importance
// but the distance requirements must be satisfied by
req.goal_constraints[0].position_distance = 0.005; // in L2square norm
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-01-30 05:25:03 UTC (rev 10398)
@@ -119,11 +119,11 @@
// are set large so that we always find a solution
// all these values have to be in the base_link frame
req.constraints.set_pose_size(1);
- req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
+ req.constraints.pose[0].type = robot_msgs::PoseConstraint::POSITION_XYZ;
req.constraints.pose[0].robot_link = "r_gripper_palm_link";
- req.constraints.pose[0].pose.position.x = 1;
- req.constraints.pose[0].pose.position.y = 1;
- req.constraints.pose[0].pose.position.z = 1;
+ req.constraints.pose[0].x = 1;
+ req.constraints.pose[0].y = 1;
+ req.constraints.pose[0].z = 1;
req.constraints.pose[0].position_distance = 10.0; // L2Square
// allow 1 second computation time
Modified: pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-30 03:04:05 UTC (rev 10397)
+++ pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-30 05:25:03 UTC (rev 10398)
@@ -2,22 +2,43 @@
# 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
+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
+# 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 type
+
# The robot link this constraint refers to
string robot_link
-# The desired pose of the robot link
-std_msgs/Pose pose
+# The desired pose of the robot link (in the robot frame)
+float64 x
+float64 y
+float64 z
+float64 roll # around Z axis
+float64 pitch # around X axis
+float64 yaw # around Y axis
# the allowed difference (square of euclidian distance) for position
float64 position_distance
-# the allowed difference (shortest angular distance) for orientation
+# the allowed difference (shortest angular distance, in radians) for orientation
float64 orientation_distance
# a factor that gets multiplied to the orientation when computing the
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
