Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19121] pkg/trunk
|
From: <is...@us...> - 2009-07-17 18:01:43
|
Revision: 19121
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19121&view=rev
Author: isucan
Date: 2009-07-17 18:01:32 +0000 (Fri, 17 Jul 2009)
Log Message:
-----------
update to pose constraints + beginning work on cloning collision environments
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/JointConstraint.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/SpaceInformation.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -127,11 +127,11 @@
goal.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
goal.goal_constraints.joint_constraint[i].joint_name = names[i];
goal.goal_constraints.joint_constraint[i].value.resize(1);
- goal.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
- goal.goal_constraints.joint_constraint[i].toleranceBelow.resize(1);
+ goal.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
+ goal.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
goal.goal_constraints.joint_constraint[i].value[0] = 0.0;
- goal.goal_constraints.joint_constraint[i].toleranceBelow[0] = 0.0;
- goal.goal_constraints.joint_constraint[i].toleranceAbove[0] = 0.0;
+ goal.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
+ goal.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
}
}
@@ -151,9 +151,21 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0.0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
- goal.goal_constraints.pose_constraint[0].position_distance = 0.0001;
- goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_importance = 0.001;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
+
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
+
+ goal.goal_constraints.pose_constraint[0].orientation_importance = 0.01;
}
void setConfig(const planning_models::StateParams *sp, const std::vector<std::string> &names, pr2_robot_actions::MoveArmGoal &goal)
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -492,9 +492,7 @@
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].position_distance < 0.1 && // and the desired position and
- req.goal_constraints.pose_constraint[0].orientation_distance < 0.1) // orientation distances are small
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY) // 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())
@@ -516,8 +514,8 @@
for (unsigned int j = 0 ; j < u ; ++j)
{
jc.value.push_back(solution[n + j]);
- jc.toleranceAbove.push_back(0.0);
- jc.toleranceBelow.push_back(0.0);
+ jc.tolerance_above.push_back(0.0);
+ jc.tolerance_below.push_back(0.0);
}
n += u;
req.goal_constraints.joint_constraint.push_back(jc);
Modified: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -69,8 +69,21 @@
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
- goalA.goal_constraints.pose_constraint[0].position_distance = 0.01;
- goalA.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
+ goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
+
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
+ goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
+
goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
goalA.goal_constraints.pose_constraint[0].type =
motion_planning_msgs::PoseConstraint::POSITION_XYZ +
@@ -93,11 +106,11 @@
goalB.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
goalB.goal_constraints.joint_constraint[i].joint_name = names[i];
goalB.goal_constraints.joint_constraint[i].value.resize(1);
- goalB.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
- goalB.goal_constraints.joint_constraint[i].toleranceBelow.resize(1);
+ goalB.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
+ goalB.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
goalB.goal_constraints.joint_constraint[i].value[0] = 0.0;
- goalB.goal_constraints.joint_constraint[i].toleranceBelow[0] = 0.0;
- goalB.goal_constraints.joint_constraint[i].toleranceAbove[0] = 0.0;
+ goalB.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
+ goalB.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
}
goalB.goal_constraints.joint_constraint[0].value[0] = -2.0;
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg 2009-07-17 18:01:32 UTC (rev 19121)
@@ -3,7 +3,7 @@
# Constrain the position of a joint to be within a certain bound
string joint_name
-# the bound to be achieved is [value - toleranceBelow, value + toleranceAbove]
+# the bound to be achieved is [value - tolerance_below, value + tolerance_above]
float64[] value
-float64[] toleranceAbove
-float64[] toleranceBelow
+float64[] tolerance_above
+float64[] tolerance_below
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-17 18:01:32 UTC (rev 19121)
@@ -32,13 +32,20 @@
# The desired pose of the robot link
robot_msgs/PoseStamped pose
-# the allowed difference (square of euclidian distance) for position
-float64 position_distance
+# The acceptable tolerance
+robot_msgs/Point position_tolerance_above
+robot_msgs/Point position_tolerance_below
-# the allowed difference (shortest angular distance, in radians) for orientation
-float64 orientation_distance
+# The acceptable tolerance (roll pitch yaw)
+robot_msgs/Point orientation_tolerance_above
+robot_msgs/Point orientation_tolerance_below
-# a factor that gets multiplied to the orientation when computing the
-# distance to the goal.
-# Distance_to_goal = position_distance + orientation_importance * orientation_distance
+# The planner may internally compute a distance from current state to goal pose.
+# This is done with a weighted sum such as:
+# Distance_to_goal = position_distance + orientation_importance * orientation_distance;
+# orientation_importance can be used to tell the planner which component is more important
+# (this only makes a difference when approximate solutions are found)
+# If you do not care about this value, simply set it to 1.0
+# Planners should use square of 2-norm for position distance and shortest angular distance
+# for orientations (roll, pitch yaw)
float64 orientation_importance
Modified: pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/motion_planning_srvs/srv/MotionPlan.srv 2009-07-17 18:01:32 UTC (rev 19121)
@@ -28,7 +28,8 @@
# A solution path, if found
motion_planning_msgs/KinematicPath path
-# distance to goal as computed by the heuristic
+# distance to goal as computed by internal planning heuristic
+# this should be 0.0 if the solution was achieved exaclty
float64 distance
# set to 1 if the computed path was approximate
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-17 18:01:32 UTC (rev 19121)
@@ -85,14 +85,7 @@
planning_environment::PoseConstraintEvaluator *pce = new planning_environment::PoseConstraintEvaluator();
pce->use(model_->kmodel, pc[i]);
pce_.push_back(pce);
-
- // if we have position constraints
- if (pc[i].type & 0xFF)
- threshold += pc[i].position_distance;
-
- // 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;
+ threshold = ompl::STATE_EPSILON;
}
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -88,16 +88,16 @@
{
unsigned int usedParams = model->kmodel->getJoint(jc[i].joint_name)->usedParams;
- if (jc[i].toleranceAbove.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != usedParams)
+ if (jc[i].tolerance_above.size() != jc[i].tolerance_below.size() || jc[i].value.size() != jc[i].tolerance_below.size() || jc[i].value.size() != usedParams)
ROS_ERROR("Constraint on joint %s has incorrect number of parameters. Expected %u", jc[i].joint_name.c_str(), usedParams);
else
{
for (unsigned int j = 0 ; j < usedParams ; ++j)
{
- if (bounds_[idx + j].first < jc[i].value[j] - jc[i].toleranceBelow[j])
- bounds_[idx + j].first = jc[i].value[j] - jc[i].toleranceBelow[j];
- if (bounds_[idx + j].second > jc[i].value[j] + jc[i].toleranceAbove[j])
- bounds_[idx + j].second = jc[i].value[j] + jc[i].toleranceAbove[j];
+ if (bounds_[idx + j].first < jc[i].value[j] - jc[i].tolerance_below[j])
+ bounds_[idx + j].first = jc[i].value[j] - jc[i].tolerance_below[j];
+ if (bounds_[idx + j].second > jc[i].value[j] + jc[i].tolerance_above[j])
+ bounds_[idx + j].second = jc[i].value[j] + jc[i].tolerance_above[j];
desiredValueCount[idx + j]++;
desiredValue[idx + j] = jc[i].value[j];
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/SpaceInformation.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/SpaceInformation.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/SpaceInformation.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -166,17 +166,17 @@
{
unsigned int usedParams = kmodel_->getJoint(jc[i].joint_name)->usedParams;
- if (jc[i].toleranceAbove.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != usedParams)
+ if (jc[i].tolerance_above.size() != jc[i].tolerance_below.size() || jc[i].value.size() != jc[i].tolerance_below.size() || jc[i].value.size() != usedParams)
ROS_ERROR("Constraint on joint %s has incorrect number of parameters. Expected %u", jc[i].joint_name.c_str(), usedParams);
else
{
for (unsigned int j = 0 ; j < usedParams ; ++j)
{
- if (m_stateComponent[idx + j].minValue < jc[i].value[j] - jc[i].toleranceBelow[j])
- m_stateComponent[idx + j].minValue = jc[i].value[j] - jc[i].toleranceBelow[j];
- if (m_stateComponent[idx + j].maxValue > jc[i].value[j] + jc[i].toleranceAbove[j])
- m_stateComponent[idx + j].maxValue = jc[i].value[j] + jc[i].toleranceAbove[j];
+ if (m_stateComponent[idx + j].minValue < jc[i].value[j] - jc[i].tolerance_below[j])
+ m_stateComponent[idx + j].minValue = jc[i].value[j] - jc[i].tolerance_below[j];
+ if (m_stateComponent[idx + j].maxValue > jc[i].value[j] + jc[i].tolerance_above[j])
+ m_stateComponent[idx + j].maxValue = jc[i].value[j] + jc[i].tolerance_above[j];
}
}
}
@@ -305,17 +305,17 @@
{
unsigned int usedParams = kmodel_->getJoint(jc[i].joint_name)->usedParams;
- if (jc[i].toleranceAbove.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != jc[i].toleranceBelow.size() || jc[i].value.size() != usedParams)
+ if (jc[i].tolerance_above.size() != jc[i].tolerance_below.size() || jc[i].value.size() != jc[i].tolerance_below.size() || jc[i].value.size() != usedParams)
ROS_ERROR("Constraint on joint %s has incorrect number of parameters. Expected %u", jc[i].joint_name.c_str(), usedParams);
else
{
for (unsigned int j = 0 ; j < usedParams ; ++j)
{
- if (m_stateComponent[idx + j].minValue < jc[i].value[j] - jc[i].toleranceBelow[j])
- m_stateComponent[idx + j].minValue = jc[i].value[j] - jc[i].toleranceBelow[j];
- if (m_stateComponent[idx + j].maxValue > jc[i].value[j] + jc[i].toleranceAbove[j])
- m_stateComponent[idx + j].maxValue = jc[i].value[j] + jc[i].toleranceAbove[j];
+ if (m_stateComponent[idx + j].minValue < jc[i].value[j] - jc[i].tolerance_below[j])
+ m_stateComponent[idx + j].minValue = jc[i].value[j] - jc[i].tolerance_below[j];
+ if (m_stateComponent[idx + j].maxValue > jc[i].value[j] + jc[i].tolerance_above[j])
+ m_stateComponent[idx + j].maxValue = jc[i].value[j] + jc[i].tolerance_above[j];
}
}
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h 2009-07-17 18:01:32 UTC (rev 19121)
@@ -148,12 +148,8 @@
protected:
motion_planning_msgs::PoseConstraint m_pc;
- double m_x;
- double m_y;
- double m_z;
- double m_yaw;
- double m_pitch;
- double m_roll;
+ double m_x, m_y, m_z;
+ double m_roll, m_pitch, m_yaw;
planning_models::KinematicModel::Link *m_link;
};
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-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -64,12 +64,12 @@
bool planning_environment::JointConstraintEvaluator::decide(const double *params, const int groupID) const
{
const double *val = params + (groupID >= 0 ? m_kmodel->getJointIndexInGroup(m_joint->name, groupID) : m_kmodel->getJointIndex(m_joint->name));
- assert(m_jc.value.size() == m_jc.toleranceBelow.size() && m_jc.value.size() == m_jc.toleranceAbove.size());
+ assert(m_jc.value.size() == m_jc.tolerance_below.size() && m_jc.value.size() == m_jc.tolerance_above.size());
for (unsigned int i = 0 ; i < m_jc.value.size() ; ++i)
{
double dif = val[i] - m_jc.value[i];
- if (dif > m_jc.toleranceAbove[i] || dif < - m_jc.toleranceBelow[i])
+ if (dif > m_jc.tolerance_above[i] || dif < - m_jc.tolerance_below[i])
return false;
}
return true;
@@ -95,11 +95,11 @@
for (unsigned int i = 0 ; i < m_jc.value.size() ; ++i)
out << m_jc.value[i] << "; ";
out << " tolerance below = ";
- for (unsigned int i = 0 ; i < m_jc.toleranceBelow.size() ; ++i)
- out << m_jc.toleranceBelow[i] << "; ";
+ for (unsigned int i = 0 ; i < m_jc.tolerance_below.size() ; ++i)
+ out << m_jc.tolerance_below[i] << "; ";
out << " tolerance above = ";
- for (unsigned int i = 0 ; i < m_jc.toleranceAbove.size() ; ++i)
- out << m_jc.toleranceAbove[i] << "; ";
+ for (unsigned int i = 0 ; i < m_jc.tolerance_above.size() ; ++i)
+ out << m_jc.tolerance_above[i] << "; ";
out << std::endl;
}
else
@@ -161,34 +161,61 @@
dx = bodyPos.getX() - m_x;
dy = bodyPos.getY() - m_y;
dz = bodyPos.getZ() - m_z;
- *distPos = dx * dx + dy * dy + dz * dz;
+ *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;
+ 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 = dx * dx + dy * dy;
+ *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 = dx * dx + dz * dz;
+ *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 = dy * dy + dz * dz;
+ *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;
- *distPos = dx * dx;
+ 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;
- *distPos = dy * dy;
+ 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;
- *distPos = dz * dz;
+ 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;
@@ -202,35 +229,70 @@
{
if (m_pc.type & (~0xFF))
{
+ double dx, dy, dz;
btScalar yaw, pitch, roll;
m_link->globalTrans.getBasis().getEulerYPR(yaw, pitch, roll);
switch (m_pc.type & (~0xFF))
{
case motion_planning_msgs::PoseConstraint::ORIENTATION_RPY:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
- fabs(angles::shortest_angular_distance(pitch, m_pitch)) +
- fabs(angles::shortest_angular_distance(yaw, m_yaw));
+ 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;
+ 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);
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
- fabs(angles::shortest_angular_distance(pitch, m_pitch));
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
- fabs(angles::shortest_angular_distance(yaw, m_yaw));
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(pitch, m_pitch)) +
- fabs(angles::shortest_angular_distance(yaw, m_yaw));
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_roll));
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(pitch, m_pitch));
+ 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:
- *distAng = fabs(angles::shortest_angular_distance(yaw, m_yaw));
+ 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;
@@ -252,8 +314,9 @@
bool planning_environment::PoseConstraintEvaluator::decide(double dPos, double dAng) const
{
- bool v1 = (m_pc.type & 0xFF) ? dPos < m_pc.position_distance : true;
- bool v2 = (m_pc.type & (~0xFF)) ? dAng < m_pc.orientation_distance : true;
+ // the values should actually be 0, for this to be true, but we put a small eps
+ bool v1 = (m_pc.type & 0xFF) ? dPos < 1e-12 : true;
+ bool v2 = (m_pc.type & (~0xFF)) ? dAng < 1e-12 : true;
return v1 && v2;
}
@@ -277,33 +340,53 @@
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 << " (within distance: " << m_pc.position_distance << ")" << std::endl;
}
@@ -316,36 +399,57 @@
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 << " (within distance: " << m_pc.orientation_distance;
if (m_pc.type & 0xFF)
- out << " and with importance " << m_pc.orientation_importance;
- out << ")" << std::endl;
+ out << "Orientation importance is " << m_pc.orientation_importance;
}
}
else
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-07-17 18:01:32 UTC (rev 19121)
@@ -451,11 +451,22 @@
delete m_robots[i];
}
+ /** \brief Return a new instance of the same model */
+ KinematicModel* clone(void);
+
+ /** \brief Construct a kinematic model from a string description and a list of planning groups */
void build(const std::string &description, const std::map< std::string, std::vector<std::string> > &groups);
+
+ /** \brief Construct a kinematic model from a parsed description and a list of planning groups */
virtual void build(const robot_desc::URDF &model, const std::map< std::string, std::vector<std::string> > &groups);
+
+ /** \brief Check if the kinematic model has been built */
bool isBuilt(void) const;
+
+ /** \brief Construct a new instance of state parameters */
StateParams* newStateParams(void);
+ /** \brief Turn verbosity on. Useful to see how the model is being constructed */
void setVerbose(bool verbose);
@@ -580,12 +591,24 @@
/* compute the parameter names */
void computeParameterNames(void);
+ /* construct the model info */
+ void constructModelInfo(void);
+
/** \brief Get the index for the parameter of a joint in a given group */
int getJointIndexInGroupSlow(const std::string &name, int groupID) const;
/** \brief Allocate a joint of appropriate type, depending on the loaded link */
Joint* createJoint(const robot_desc::URDF::Link* urdfLink);
+
+ /** \brief Create a new joint instance (a copy) */
+ Joint* copyJoint(const Joint *joint);
+ /** \brief Recursive copy of data after a joint */
+ void cloneAfterJoint(Robot *rb, Joint *dest, const Joint *src);
+
+ /** \brief Recursive copy of data after a link */
+ void cloneAfterLink(Robot *rb, Link *dest, const Link *src);
+
};
}
Modified: 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 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h 2009-07-17 18:01:32 UTC (rev 19121)
@@ -52,9 +52,13 @@
/** \brief A class that can hold the named parameters of this planning model */
class StateParams
{
- public:
+ friend class KinematicModel;
+ protected:
+ // we want users to construct states using newStateParams()
StateParams(KinematicModel *model);
+
+ public:
StateParams(const StateParams &sp);
virtual ~StateParams(void);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -87,6 +87,7 @@
void planning_models::KinematicModel::computeParameterNames(void)
{
m_mi.parameterIndex.clear();
+ m_mi.parameterName.clear();
unsigned int pos = 0;
for (unsigned int i = 0 ; i < m_robots.size(); ++i)
{
@@ -379,6 +380,13 @@
m_robots.push_back(rb);
}
+ constructModelInfo();
+
+ defaultState();
+}
+
+void planning_models::KinematicModel::constructModelInfo(void)
+{
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
/* copy state bounds */
@@ -419,7 +427,6 @@
for (int i = 0 ; i < (int)m_groups.size() ; ++i)
m_jointIndexGroup[it->first][i] = getJointIndexInGroupSlow(it->first, i);
- defaultState();
}
int planning_models::KinematicModel::getGroupID(const std::string &group) const
@@ -749,3 +756,138 @@
out << std::endl;
}
}
+
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::copyJoint(const Joint *joint)
+{
+ Joint *newJoint = NULL;
+
+ if (dynamic_cast<const FixedJoint*>(joint))
+ {
+ newJoint = new FixedJoint();
+ }
+ else
+ if (dynamic_cast<const FloatingJoint*>(joint))
+ {
+ newJoint = new FloatingJoint();
+ }
+ else
+ if (dynamic_cast<const PlanarJoint*>(joint))
+ {
+ newJoint = new PlanarJoint();
+ }
+ else
+ if (dynamic_cast<const PrismaticJoint*>(joint))
+ {
+ PrismaticJoint *pj = new PrismaticJoint();
+ const PrismaticJoint *src = static_cast<const PrismaticJoint*>(joint);
+ pj->axis = src->axis;
+ pj->limit[0] = src->limit[0];
+ pj->limit[1] = src->limit[1];
+ newJoint = pj;
+ }
+ else
+ if (dynamic_cast<const RevoluteJoint*>(joint))
+ {
+ RevoluteJoint *pj = new RevoluteJoint();
+ const RevoluteJoint *src = static_cast<const RevoluteJoint*>(joint);
+ pj->axis = src->axis;
+ pj->anchor = src->anchor;
+ pj->continuous = src->continuous;
+ pj->limit[0] = src->limit[0];
+ pj->limit[1] = src->limit[1];
+ newJoint = pj;
+ }
+ else
+ assert(0);
+
+ if (newJoint)
+ {
+ newJoint->usedParams = joint->usedParams;
+ newJoint->inGroup = joint->inGroup;
+ newJoint->varTrans = joint->varTrans;
+ }
+
+ return newJoint;
+}
+
+void planning_models::KinematicModel::cloneAfterJoint(Robot *rb, Joint *dest, const Joint *src)
+{
+ rb->joints.push_back(dest);
+ dest->owner = rb;
+ if (src->after)
+ {
+ dest->after = new Link();
+ dest->after->before = dest;
+ cloneAfterLink(rb, dest->after, src->after);
+ }
+}
+
+void planning_models::KinematicModel::cloneAfterLink(Robot *rb, Link *dest, const Link *src)
+{
+ rb->links.push_back(dest);
+ dest->owner = rb;
+ dest->name = src->name;
+ dest->constTrans = src->constTrans;
+ dest->constGeomTrans = src->constGeomTrans;
+ dest->globalTransFwd = src->globalTransFwd;
+ dest->globalTrans = src->globalTrans;
+ dest->shape = shapes::clone_shape(src->shape);
+ for (unsigned int i = 0 ; i < src->attachedBodies.size() ; ++i)
+ {
+ AttachedBody *ab = new AttachedBody(dest);
+ ab->attachTrans = src->attachedBodies[i]->attachTrans;
+ ab->shape = shapes::clone_shape(src->attachedBodies[i]->shape);
+ ab->globalTrans = src->attachedBodies[i]->globalTrans;
+ dest->attachedBodies.push_back(ab);
+ }
+ for (unsigned int i = 0 ; i < src->after.size() ; ++i)
+ {
+ Joint *aft = copyJoint(src->after[i]);
+ aft->before = dest;
+ dest->after.push_back(aft);
+ cloneAfterJoint(rb, aft, src->after[i]);
+ }
+}
+
+planning_models::KinematicModel* planning_models::KinematicModel::clone(void)
+{
+ KinematicModel *km = new KinematicModel();
+ km->m_built = m_built;
+ km->m_verbose = m_verbose;
+ km->m_name = m_name;
+ km->m_mi = m_mi;
+
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ {
+ Robot *r = new Robot(km);
+ km->m_robots.push_back(r);
+ r->rootTransform = m_robots[i]->rootTransform;
+ r->chain = copyJoint(m_robots[i]->chain);
+ r->chain->before = NULL;
+ cloneAfterJoint(r, r->chain, m_robots[i]->chain);
+ r->stateDimension = m_robots[i]->stateDimension;
+ r->stateBounds = m_robots[i]->stateBounds;
+ r->floatingJoints = m_robots[i]->floatingJoints;
+ r->planarJoints = m_robots[i]->planarJoints;
+ r->groupStateIndexList = m_robots[i]->groupStateIndexList;
+ r->groupChainStart.resize(m_robots[i]->groupChainStart.size());
+ for (unsigned int j = 0 ; j < r->groupChainStart.size() ; ++j)
+ for (unsigned int k = 0 ; k < m_robots[i]->groupChainStart[j].size() ; ++k)
+ for (unsigned int u = 0 ; u < r->joints.size() ; ++u)
+ if (r->joints[u]->name == m_robots[i]->groupChainStart[j][k]->name)
+ r->groupChainStart[j].push_back(r->joints[u]);
+
+ }
+
+ km->m_groups = m_groups;
+ km->m_groupsMap = m_groupsMap;
+ km->m_groupContent = m_groupContent;
+
+ km->m_mi.inRobotFrame = m_mi.inRobotFrame;
+ km->m_mi.groupStateIndexList.resize(m_groups.size());
+ km->m_mi.groupChainStart.resize(m_groups.size());
+
+ km->constructModelInfo();
+
+ return km;
+}
Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-07-17 17:54:04 UTC (rev 19120)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-07-17 18:01:32 UTC (rev 19121)
@@ -774,8 +774,14 @@
model->reduceToRobotFrame();
EXPECT_EQ((unsigned int)13, model->getModelInfo().stateDimension);
+ // planning_models::KinematicModel *clone = model->clone();
+ // delete model;
+ // model = clone;
+
std::stringstream ss;
model->printModelInfo(ss);
+ printf("%s\n", ss.str().c_str());
+
double param[8] = { -1, -1, 0, 1.57, 0.0, 5, 5, 0 };
model->computeTransformsGroup(param, model->getGroupID("parts"));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
