planning_environment: planning_environment: kinematic_state_constraint

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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<btVector3> 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       btVector3 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   btTransform 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= btVector3(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   btQuaternion q;
00195   tf::quaternionMsgToTF(m_oc.orientation,q);
00196   m_rotation_matrix = btMatrix3x3(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   /*  btScalar yaw, pitch, roll;
00248       m_link->global_collision_body_transform.getBasis().getEulerYPR(yaw, pitch, roll);
00249       btVector3 orientation(roll,pitch,yaw);
00250   */
00251   btScalar yaw, pitch, roll;
00252   if(m_oc.type == m_oc.HEADER_FRAME)
00253   {
00254     btMatrix3x3 result = link_state->getGlobalLinkTransform().getBasis() *  m_rotation_matrix.inverse();
00255     result.getRPY(roll, pitch, yaw);
00256     //    result.getEulerYPR(yaw, pitch, roll);
00257   }
00258   else
00259   {
00260     btMatrix3x3 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     btQuaternion 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   btScalar yaw, pitch, roll;
00321   if(m_oc.type == m_oc.HEADER_FRAME)
00322   {
00323     btMatrix3x3 result = m_rotation_matrix.inverse() * link_state->getGlobalLinkTransform().getBasis();
00324     result.getEulerYPR(yaw, pitch, roll);
00325   }
00326   else
00327   {
00328     btMatrix3x3 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   btTransform 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   btVector3 x_axis(1,0,0);
00523   btVector3 target_vector(dx,dy,dz);
00524   btVector3 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
kinematic_state_constraint_evaluator.cpp
