00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "planning_environment/util/kinematic_state_constraint_evaluator.h"
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <tf/transform_datatypes.h>
00040 #include <angles/angles.h>
00041 #include <cassert>
00042
00043 bool planning_environment::JointConstraintEvaluator::use(const arm_navigation_msgs::JointConstraint &jc)
00044 {
00045 m_jc = jc;
00046 return true;
00047 }
00048
00049 bool planning_environment::JointConstraintEvaluator::decide(const planning_models::KinematicState* state,
00050 bool verbose) const
00051 {
00052 std::vector<double> cur_joint_values;
00053 const planning_models::KinematicState::JointState* joint = state->getJointState(m_jc.joint_name);
00054 if(!joint) {
00055 ROS_WARN_STREAM("No joint in state with name " << m_jc.joint_name);
00056 }
00057 cur_joint_values = joint->getJointStateValues();
00058
00059 if(cur_joint_values.size() == 0) {
00060 ROS_WARN_STREAM("Trying to decide joint with no value " << joint->getName());
00061 }
00062 if(cur_joint_values.size() > 1) {
00063 ROS_WARN_STREAM("Trying to decide joint value with more than one value " << joint->getName());
00064 }
00065
00066 double current_joint_position = cur_joint_values[0];
00067 double dif;
00068
00069 const planning_models::KinematicModel::RevoluteJointModel *revolute_joint = dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(joint->getJointModel());
00070
00071 if(revolute_joint && revolute_joint->continuous_)
00072 dif = angles::shortest_angular_distance(m_jc.position,current_joint_position);
00073 else
00074 dif = current_joint_position - m_jc.position;
00075
00076 if(verbose)
00077 ROS_DEBUG("Joint name:%s, value: %f, Constraint: %f, tolerance_above: %f, tolerance_below: %f",joint->getName().c_str(),current_joint_position,m_jc.position,m_jc.tolerance_above,m_jc.tolerance_below);
00078 if (dif > m_jc.tolerance_above || dif < - m_jc.tolerance_below)
00079 {
00080 if(verbose) {
00081 ROS_INFO("Constraint violated:: Joint name:%s, value: %f, Constraint: %f, tolerance_above: %f, tolerance_below: %f",joint->getName().c_str(),current_joint_position,m_jc.position,m_jc.tolerance_above,m_jc.tolerance_below);
00082 }
00083 return false;
00084 }
00085 return true;
00086 }
00087
00088 void planning_environment::JointConstraintEvaluator::clear(void)
00089 {
00090 }
00091
00092 const arm_navigation_msgs::JointConstraint& planning_environment::JointConstraintEvaluator::getConstraintMessage(void) const
00093 {
00094 return m_jc;
00095 }
00096
00097 void planning_environment::JointConstraintEvaluator::print(std::ostream &out) const
00098 {
00099 if (m_joint)
00100 {
00101 out << "Joint constraint for joint " << m_jc.joint_name << ": " << std::endl;
00102 out << " value = ";
00103 out << m_jc.position << "; ";
00104 out << " tolerance below = ";
00105 out << m_jc.tolerance_below << "; ";
00106 out << " tolerance above = ";
00107 out << m_jc.tolerance_above << "; ";
00108 out << std::endl;
00109 }
00110 else
00111 out << "No constraint" << std::endl;
00112 }
00113
00114 bool planning_environment::createConstraintRegionFromMsg(const arm_navigation_msgs::Shape &constraint_region_shape,
00115 const geometry_msgs::Pose &constraint_region_pose,
00116 boost::scoped_ptr<bodies::Body> &body)
00117 {
00118 if(constraint_region_shape.type == arm_navigation_msgs::Shape::SPHERE)
00119 {
00120 if(constraint_region_shape.dimensions.empty())
00121 return false;
00122 shapes::Sphere shape(constraint_region_shape.dimensions[0]);
00123 body.reset(new bodies::Sphere(&shape));
00124 }
00125 else if(constraint_region_shape.type == arm_navigation_msgs::Shape::BOX)
00126 {
00127 if((int) constraint_region_shape.dimensions.size() < 3)
00128 return false;
00129 shapes::Box shape(constraint_region_shape.dimensions[0],constraint_region_shape.dimensions[1],constraint_region_shape.dimensions[2]);
00130 body.reset(new bodies::Box(&shape));
00131 }
00132 else if(constraint_region_shape.type == arm_navigation_msgs::Shape::CYLINDER)
00133 {
00134 if((int) constraint_region_shape.dimensions.size() < 2)
00135 return false;
00136 shapes::Cylinder shape(constraint_region_shape.dimensions[0],constraint_region_shape.dimensions[1]);
00137 body.reset(new bodies::Cylinder(&shape));
00138 }
00139 else if(constraint_region_shape.type == arm_navigation_msgs::Shape::MESH)
00140 {
00141 std::vector<tf::Vector3> vertices;
00142 std::vector<unsigned int> triangles;
00143 for(unsigned int i=0; i < constraint_region_shape.triangles.size(); i++)
00144 {
00145 triangles.push_back((unsigned int) constraint_region_shape.triangles[i]);
00146 }
00147 for(unsigned int i=0; i < constraint_region_shape.triangles.size(); i++)
00148 {
00149 tf::Vector3 tmp;
00150 tf::pointMsgToTF(constraint_region_shape.vertices[i],tmp);
00151 vertices.push_back(tmp);
00152 }
00153 shapes::Mesh *shape = shapes::createMeshFromVertices(vertices,triangles);
00154 body.reset(new bodies::ConvexMesh(shape));
00155 }
00156 else
00157 {
00158 ROS_ERROR("Could not recognize constraint type");
00159 return false;
00160 }
00161 tf::Transform pose_tf;
00162 tf::poseMsgToTF(constraint_region_pose,pose_tf);
00163 body->setPose(pose_tf);
00164 return true;
00165 }
00166
00167 bool planning_environment::PositionConstraintEvaluator::use(const arm_navigation_msgs::PositionConstraint &pc)
00168 {
00169 m_pc = pc;
00170
00171 m_x = m_pc.position.x;
00172 m_y = m_pc.position.y;
00173 m_z = m_pc.position.z;
00174 m_offset= tf::Vector3(m_pc.target_point_offset.x,m_pc.target_point_offset.y,m_pc.target_point_offset.z);
00175
00176 geometry_msgs::Pose constraint_region_pose;
00177 constraint_region_pose.orientation = pc.constraint_region_orientation;
00178 constraint_region_pose.position = m_pc.position;
00179 createConstraintRegionFromMsg(pc.constraint_region_shape,constraint_region_pose,m_constraint_region);
00180 ROS_DEBUG("Position Constraint: frame_id: %s",pc.header.frame_id.c_str());
00181 ROS_DEBUG("Position Constraint Desired position: (%f,%f,%f)",m_pc.position.x,m_pc.position.y,m_pc.position.z);
00182 ROS_DEBUG("Position Constraint Region: orientation: (%f,%f,%f,%f)",
00183 m_pc.constraint_region_orientation.x,
00184 m_pc.constraint_region_orientation.y,
00185 m_pc.constraint_region_orientation.z,
00186 m_pc.constraint_region_orientation.w);
00187 ROS_DEBUG("Offset (%f,%f,%f)", m_pc.target_point_offset.x,m_pc.target_point_offset.y,m_pc.target_point_offset.z);
00188 return true;
00189 }
00190
00191 bool planning_environment::OrientationConstraintEvaluator::use(const arm_navigation_msgs::OrientationConstraint &oc)
00192 {
00193 m_oc = oc;
00194 tf::Quaternion q;
00195 tf::quaternionMsgToTF(m_oc.orientation,q);
00196 m_rotation_matrix = tf::Matrix3x3(q);
00197 geometry_msgs::Pose id;
00198 id.orientation.w = 1.0;
00199 ROS_DEBUG("Orientation constraint: %f %f %f %f",m_oc.orientation.x,m_oc.orientation.y,m_oc.orientation.z,m_oc.orientation.w);
00200 return true;
00201 }
00202
00203 void planning_environment::PositionConstraintEvaluator::clear(void)
00204 {
00205 }
00206
00207 void planning_environment::OrientationConstraintEvaluator::clear(void)
00208 {
00209 }
00210
00211 bool planning_environment::PositionConstraintEvaluator::decide(const planning_models::KinematicState *state,
00212 bool verbose) const
00213 {
00214 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(m_pc.link_name);
00215
00216 if(!link_state)
00217 {
00218 ROS_WARN_STREAM("No link in state with name " << m_pc.link_name);
00219 return false;
00220 }
00221
00222 bool result = m_constraint_region->containsPoint(link_state->getGlobalLinkTransform()(m_offset),false);
00223 if(!result)
00224 {
00225 ROS_DEBUG("Position constraint violated : desired:: %f, %f, %f, current:: %f, %f, %f, tolerance: %f, %f, %f",m_x,m_y,m_z,
00226 link_state->getGlobalLinkTransform().getOrigin().x(),link_state->getGlobalLinkTransform().getOrigin().y(),link_state->getGlobalLinkTransform().getOrigin().z(),
00227 m_pc.constraint_region_shape.dimensions[0],m_pc.constraint_region_shape.dimensions[1],m_pc.constraint_region_shape.dimensions[2]);
00228 if(verbose) {
00229 ROS_INFO("Link name %s Position constraint satisfied: desired:: %f, %f, %f, current:: %f, %f, %f, tolerance: %f, %f, %f",link_state->getName().c_str(), m_x,m_y,m_z,
00230 link_state->getGlobalLinkTransform().getOrigin().x(),link_state->getGlobalLinkTransform().getOrigin().y(),link_state->getGlobalLinkTransform().getOrigin().z(),
00231 m_pc.constraint_region_shape.dimensions[0],m_pc.constraint_region_shape.dimensions[1],m_pc.constraint_region_shape.dimensions[2]);
00232 }
00233 }
00234 return result;
00235 }
00236
00237 bool planning_environment::OrientationConstraintEvaluator::decide(const planning_models::KinematicState *state,
00238 bool verbose) const
00239 {
00240 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(m_oc.link_name);
00241
00242 if(!link_state)
00243 {
00244 ROS_WARN_STREAM("No link in state with name " << m_oc.link_name);
00245 return false;
00246 }
00247
00248
00249
00250
00251 tfScalar yaw, pitch, roll;
00252 if(m_oc.type == m_oc.HEADER_FRAME)
00253 {
00254 tf::Matrix3x3 result = link_state->getGlobalLinkTransform().getBasis() * m_rotation_matrix.inverse();
00255 result.getRPY(roll, pitch, yaw);
00256
00257 }
00258 else
00259 {
00260 tf::Matrix3x3 result = m_rotation_matrix.inverse() * link_state->getGlobalLinkTransform().getBasis();
00261 result.getRPY(roll, pitch, yaw);
00262 }
00263 if(fabs(roll) < m_oc.absolute_roll_tolerance &&
00264 fabs(pitch) < m_oc.absolute_pitch_tolerance &&
00265 fabs(yaw) < m_oc.absolute_yaw_tolerance)
00266 {
00267 return true;
00268 }
00269 else
00270 {
00271 tf::Quaternion quat;
00272 link_state->getGlobalLinkTransform().getBasis().getRotation(quat);
00273 geometry_msgs::Quaternion quat_msg;
00274 tf::quaternionTFToMsg(quat,quat_msg);
00275 if(!verbose) {
00276 ROS_DEBUG("Orientation constraint: violated");
00277 ROS_DEBUG("Orientation Constraint: Quaternion desired: %f %f %f %f",m_oc.orientation.x,m_oc.orientation.y,m_oc.orientation.z,m_oc.orientation.w);
00278 ROS_DEBUG("Orientation Constraint: Quaternion actual: %f %f %f %f",quat_msg.x,quat_msg.y,quat_msg.z,quat_msg.w);
00279 ROS_DEBUG("Orientation Constraint: Error: Roll: %f, Pitch: %f, Yaw: %f",roll,pitch,yaw);
00280 ROS_DEBUG("Orientation Constraint: Tolerance: Roll: %f, Pitch: %f, Yaw: %f",m_oc.absolute_roll_tolerance,m_oc.absolute_pitch_tolerance,m_oc.absolute_yaw_tolerance);
00281 } else {
00282 ROS_INFO("Orientation constraint: violated");
00283 ROS_INFO("Orientation Constraint: Quaternion desired: %f %f %f %f",m_oc.orientation.x,m_oc.orientation.y,m_oc.orientation.z,m_oc.orientation.w);
00284 ROS_INFO("Orientation Constraint: Quaternion actual: %f %f %f %f",quat_msg.x,quat_msg.y,quat_msg.z,quat_msg.w);
00285 ROS_INFO("Orientation Constraint: Error: Roll: %f, Pitch: %f, Yaw: %f",roll,pitch,yaw);
00286 ROS_INFO("Orientation Constraint: Tolerance: Roll: %f, Pitch: %f, Yaw: %f",m_oc.absolute_roll_tolerance,m_oc.absolute_pitch_tolerance,m_oc.absolute_yaw_tolerance);
00287 }
00288 return false;
00289 }
00290 }
00291
00292 void planning_environment::PositionConstraintEvaluator::evaluate(const planning_models::KinematicState* state, double& distPos, bool verbose) const
00293 {
00294 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(m_pc.link_name);
00295
00296 if(!link_state)
00297 {
00298 ROS_WARN_STREAM("No link in state with name " << m_pc.link_name);
00299 distPos = DBL_MAX;
00300 }
00301
00302 double dx = link_state->getGlobalLinkTransform().getOrigin().x() - m_pc.position.x;
00303 double dy = link_state->getGlobalLinkTransform().getOrigin().y() - m_pc.position.y;
00304 double dz = link_state->getGlobalLinkTransform().getOrigin().z() - m_pc.position.z;
00305
00306 distPos = sqrt(dx*dx+dy*dy+dz*dz);
00307 }
00308
00309 void planning_environment::OrientationConstraintEvaluator::evaluate(const planning_models::KinematicState* state, double& distAng, bool verbose) const
00310 {
00311 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(m_oc.link_name);
00312
00313 if(!link_state)
00314 {
00315 ROS_WARN_STREAM("No link in state with name " << m_oc.link_name);
00316 distAng = DBL_MAX;
00317 }
00318
00319 distAng = 0.0;
00320 tfScalar yaw, pitch, roll;
00321 if(m_oc.type == m_oc.HEADER_FRAME)
00322 {
00323 tf::Matrix3x3 result = m_rotation_matrix.inverse() * link_state->getGlobalLinkTransform().getBasis();
00324 result.getEulerYPR(yaw, pitch, roll);
00325 }
00326 else
00327 {
00328 tf::Matrix3x3 result = link_state->getGlobalLinkTransform().getBasis() * m_rotation_matrix.inverse();
00329 result.getRPY(roll, pitch, yaw);
00330 }
00331 distAng += fabs(yaw);
00332 distAng += fabs(pitch);
00333 distAng += fabs(roll);
00334 if(verbose) {
00335 std::cout << "Dist angle is " << distAng << std::endl;
00336 }
00337 }
00338
00339 bool planning_environment::PositionConstraintEvaluator::decide(double dPos, bool verbose) const
00340 {
00341 return dPos < 1e-12;
00342 }
00343
00344 bool planning_environment::OrientationConstraintEvaluator::decide(double dAng, bool verbose) const
00345 {
00346 return dAng < 1e-12;
00347 }
00348
00349 const arm_navigation_msgs::PositionConstraint& planning_environment::PositionConstraintEvaluator::getConstraintMessage(void) const
00350 {
00351 return m_pc;
00352 }
00353
00354 const arm_navigation_msgs::OrientationConstraint& planning_environment::OrientationConstraintEvaluator::getConstraintMessage(void) const
00355 {
00356 return m_oc;
00357 }
00358
00359 void planning_environment::PositionConstraintEvaluator::print(std::ostream &out) const
00360 {
00361 out << "Position constraint on link '" << m_pc.link_name << "'" << std::endl;
00362 if (m_pc.constraint_region_shape.type == arm_navigation_msgs::Shape::SPHERE)
00363 {
00364 if(m_pc.constraint_region_shape.dimensions.empty())
00365 {
00366 out << "No radius specified for spherical constraint region.";
00367 }
00368 else
00369 {
00370 out << "Spherical constraint region with radius " << m_pc.constraint_region_shape.dimensions[0] << std::endl;
00371 }
00372 }
00373 else if (m_pc.constraint_region_shape.type == arm_navigation_msgs::Shape::BOX)
00374 {
00375 if((int) m_pc.constraint_region_shape.dimensions.size() < 3)
00376 {
00377 out << "Length, width, height must be specified for box constraint region.";
00378 }
00379 else
00380 {
00381 out << "Box constraint region with dimensions " << m_pc.constraint_region_shape.dimensions[0] << " x "
00382 << m_pc.constraint_region_shape.dimensions[1] << " x " << m_pc.constraint_region_shape.dimensions[2] << std::endl;
00383 }
00384 }
00385 else if (m_pc.constraint_region_shape.type == arm_navigation_msgs::Shape::CYLINDER)
00386 {
00387 if((int) m_pc.constraint_region_shape.dimensions.size() < 2)
00388 {
00389 out << "Radius and height must be specified for cylinder constraint region.";
00390 }
00391 else
00392 {
00393 out << "Cylinder constraint region with radius " << m_pc.constraint_region_shape.dimensions[0] << " and length "
00394 << m_pc.constraint_region_shape.dimensions[1] << std::endl;
00395 }
00396 }
00397 else if(m_pc.constraint_region_shape.type == arm_navigation_msgs::Shape::MESH)
00398 {
00399 out << "Mesh type constraint region.";
00400 }
00401 }
00402
00403 void planning_environment::OrientationConstraintEvaluator::print(std::ostream &out) const
00404 {
00405 out << "Orientation constraint on link '" << m_oc.link_name << "'" << std::endl;
00406 out << "Desired orientation:" << m_oc.orientation.x << "," << m_oc.orientation.y << "," << m_oc.orientation.z << "," << m_oc.orientation.w << std::endl;
00407 }
00408
00409 void planning_environment::KinematicConstraintEvaluatorSet::clear(void)
00410 {
00411 for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
00412 delete m_kce[i];
00413 m_kce.clear();
00414 m_jc.clear();
00415 m_pc.clear();
00416 m_oc.clear();
00417 }
00418
00419 bool planning_environment::KinematicConstraintEvaluatorSet::add(const std::vector<arm_navigation_msgs::JointConstraint> &jc)
00420 {
00421 bool result = true;
00422 for (unsigned int i = 0 ; i < jc.size() ; ++i)
00423 {
00424 JointConstraintEvaluator *ev = new JointConstraintEvaluator();
00425 result = result && ev->use(jc[i]);
00426 m_kce.push_back(ev);
00427 m_jc.push_back(jc[i]);
00428 }
00429 return result;
00430 }
00431
00432 bool planning_environment::KinematicConstraintEvaluatorSet::add(const std::vector<arm_navigation_msgs::PositionConstraint> &pc)
00433 {
00434 bool result = true;
00435 for (unsigned int i = 0 ; i < pc.size() ; ++i)
00436 {
00437 PositionConstraintEvaluator *ev = new PositionConstraintEvaluator();
00438 result = result && ev->use(pc[i]);
00439 m_kce.push_back(ev);
00440 m_pc.push_back(pc[i]);
00441 }
00442 return result;
00443 }
00444
00445 bool planning_environment::KinematicConstraintEvaluatorSet::add(const std::vector<arm_navigation_msgs::OrientationConstraint> &oc)
00446 {
00447 bool result = true;
00448 for (unsigned int i = 0 ; i < oc.size() ; ++i)
00449 {
00450 OrientationConstraintEvaluator *ev = new OrientationConstraintEvaluator();
00451 result = result && ev->use(oc[i]);
00452 m_kce.push_back(ev);
00453 m_oc.push_back(oc[i]);
00454 }
00455 return result;
00456 }
00457
00458
00459 bool planning_environment::KinematicConstraintEvaluatorSet::add(const std::vector<arm_navigation_msgs::VisibilityConstraint> &vc)
00460 {
00461 bool result = true;
00462 for (unsigned int i = 0 ; i < vc.size() ; ++i)
00463 {
00464 VisibilityConstraintEvaluator *ev = new VisibilityConstraintEvaluator();
00465 result = result && ev->use(vc[i]);
00466 m_kce.push_back(ev);
00467 m_vc.push_back(vc[i]);
00468 }
00469 return result;
00470 }
00471
00472 bool planning_environment::KinematicConstraintEvaluatorSet::decide(const planning_models::KinematicState* state,
00473 bool verbose) const
00474 {
00475 for (unsigned int i = 0 ; i < m_kce.size() ; ++i) {
00476 if (!m_kce[i]->decide(state, verbose)) {
00477 return false;
00478 }
00479 }
00480 return true;
00481 }
00482
00483 void planning_environment::KinematicConstraintEvaluatorSet::print(std::ostream &out) const
00484 {
00485 out << m_kce.size() << " kinematic constraints" << std::endl;
00486 for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
00487 m_kce[i]->print(out);
00488 }
00489
00490 void planning_environment::VisibilityConstraintEvaluator::print(std::ostream &out) const
00491 {
00492 out << "Visibility constraint for sensor on link '" << m_vc.sensor_pose.header.frame_id << "'" << std::endl;
00493 }
00494 const arm_navigation_msgs::VisibilityConstraint& planning_environment::VisibilityConstraintEvaluator::getConstraintMessage(void) const
00495 {
00496 return m_vc;
00497 }
00498 void planning_environment::VisibilityConstraintEvaluator::clear(void)
00499 {
00500 }
00501 bool planning_environment::VisibilityConstraintEvaluator::use(const arm_navigation_msgs::VisibilityConstraint &vc)
00502 {
00503 m_vc = vc;
00504 tf::poseMsgToTF(m_vc.sensor_pose.pose,m_sensor_offset_pose);
00505 return true;
00506 }
00507 bool planning_environment::VisibilityConstraintEvaluator::decide(const planning_models::KinematicState* state,
00508 bool verbose) const
00509 {
00510 std::string link_name = m_vc.sensor_pose.header.frame_id;
00511 const planning_models::KinematicState::LinkState* link_state = state->getLinkState(link_name);
00512 if(!link_state) {
00513 ROS_WARN_STREAM("No link state for link " << link_name);
00514 return false;
00515 }
00516
00517 tf::Transform sensor_pose = link_state->getGlobalLinkTransform() * m_sensor_offset_pose;
00518 double dx = m_vc.target.point.x - sensor_pose.getOrigin().x();
00519 double dy = m_vc.target.point.y - sensor_pose.getOrigin().y();
00520 double dz = m_vc.target.point.z - sensor_pose.getOrigin().z();
00521
00522 tf::Vector3 x_axis(1,0,0);
00523 tf::Vector3 target_vector(dx,dy,dz);
00524 tf::Vector3 sensor_x_axis = sensor_pose.getBasis()*x_axis;
00525
00526 double angle = fabs(target_vector.angle(sensor_x_axis));
00527 if(angle < m_vc.absolute_tolerance)
00528 return true;
00529 else
00530 return false;
00531 }
00532