pr2_interactive_manipulation: cartesian_gripper

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "pr2_interactive_manipulation/cartesian_gripper_control.h"
00031 
00032 #include <visualization_msgs/Marker.h>
00033 
00034 #include <rviz/visualization_manager.h>
00035 #include <rviz/frame_manager.h>
00036 #include <rviz/selection/selection_manager.h>
00037 #include <rviz/common.h>
00038 
00039 #include <OGRE/OgreSceneNode.h>
00040 #include <OGRE/OgreSceneManager.h>
00041 #include <OGRE/OgreVector3.h>
00042 #include <OGRE/OgreMaterialManager.h>
00043 #include <OGRE/OgreResourceGroupManager.h>
00044 #include <OGRE/OgreSubEntity.h>
00045 #include <OGRE/OgreMath.h>
00046 
00047 #include "rviz_interaction_tools/unique_string_manager.h"
00048 
00049 #include <math.h>
00050 
00051 namespace pr2_interactive_manipulation
00052 {
00053 
00054 
00055 CartesianGripperControl::CartesianGripperControl( std::string arm_name,
00056     std::string gripper_frame, std::string reference_frame, rviz::VisualizationManager *vis_manager ) :
00057     vis_manager_( vis_manager ),
00058     arm_name_(arm_name),
00059     gripper_frame_( gripper_frame ),
00060     reference_frame_( reference_frame ),
00061     use_reference_frame_(false),
00062     status_level_(rviz::status_levels::Ok),
00063     status_(HIDDEN)
00064 {
00065   main_node_ = vis_manager->getSceneManager()->getRootSceneNode()->createChildSceneNode();
00066   main_node_->setVisible(false);
00067 
00068   controls_node_ = main_node_->createChildSceneNode();
00069   gripper_node_ = main_node_->createChildSceneNode();
00070 
00071   gripper_offset_ = Ogre::Vector3(-0.1,0,0);
00072 
00073   gripper_ = new rviz_interaction_tools::Gripper( vis_manager->getSceneManager(), gripper_node_ );
00074   gripper_->setPosition( gripper_offset_ );
00075 
00076   gripper_->setDepthCheckEnabled(false);
00077   //gripper_->setRenderQueueGroup( Ogre::RENDER_QUEUE_6 );
00078 
00079   addControls(0);
00080   addControls(1);
00081   addControls(2);
00082 
00083   marker_ = new ogre_tools::Shape( ogre_tools::Shape::Sphere, vis_manager->getSceneManager(), main_node_ );
00084   marker_->setScale( Ogre::Vector3( 0.01,0.01,0.01 ) );
00085 }
00086 
00087 CartesianGripperControl::~CartesianGripperControl()
00088 {
00089   vis_manager_->getSceneManager()->destroySceneNode(main_node_);
00090   vis_manager_->getSceneManager()->destroySceneNode(controls_node_);
00091 
00092   for ( unsigned i=0; i<3; i++ )
00093   {
00094     delete drag_boxes_[i][POS];
00095     delete drag_boxes_[i][NEG];
00096   }
00097   delete gripper_;
00098 }
00099 
00100 void CartesianGripperControl::addControls( unsigned axis )
00101 {
00102   std::vector<Ogre::Vector3> axes;
00103   axes.push_back(Ogre::Vector3::UNIT_X);
00104   axes.push_back(Ogre::Vector3::UNIT_Y);
00105   axes.push_back(Ogre::Vector3::UNIT_Z);
00106 
00107   drag_boxes_[axis][POS] = addDragBox( axes[axis] );
00108   drag_boxes_[axis][NEG] = addDragBox( -1 * axes[axis] );
00109 
00110   Ogre::MaterialPtr material = drag_boxes_[axis][POS]->getEntity()->getSubEntity(0)->getMaterial();
00111   material->getBestTechnique()->setCullingMode( Ogre::CULL_NONE );
00112   material->getBestTechnique()->setDepthWriteEnabled( true );
00113   material->getBestTechnique()->setDepthCheckEnabled( true );
00114 
00115   addRing( axes[(axis+1)%3], axes[(axis+2)%3],
00116       material, drag_boxes_[axis][POS]->getEntity()->getRenderQueueGroup() );
00117 }
00118 
00119 ogre_tools::Shape* CartesianGripperControl::addDragBox( Ogre::Vector3 axis )
00120 {
00121   ogre_tools::Shape *shape =
00122       new ogre_tools::Shape( ogre_tools::Shape::Cube, vis_manager_->getSceneManager(), controls_node_ );
00123 
00124   shape->setPosition( axis * (HANDLE_RADIUS_OUTER + HANDLE_SIZE) );
00125   shape->setScale( Ogre::Vector3(HANDLE_SIZE,HANDLE_SIZE,HANDLE_SIZE) );
00126   shape->setColor( fabs(axis.x), fabs(axis.y), fabs(axis.z), HANDLE_ALPHA );
00127 
00128   //put everything into a higher render queue to transparency will work
00129   shape->getEntity()->setRenderQueueGroup( Ogre::RENDER_QUEUE_6 );
00130 
00131   Ogre::MaterialPtr material = shape->getEntity()->getSubEntity(0)->getMaterial();
00132   material->getBestTechnique()->setCullingMode( Ogre::CULL_NONE );
00133   material->getBestTechnique()->setDepthWriteEnabled( true );
00134   material->getBestTechnique()->setDepthCheckEnabled( true );
00135 //  material->getBestTechnique()->setLightingEnabled(false);
00136 
00137   return shape;
00138 }
00139 
00140 void CartesianGripperControl::addRing(
00141     Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00142     Ogre::MaterialPtr material, unsigned render_queue_group)
00143 {
00144   static const float pi = 3.14159265;
00145   for ( float a=0; a<2.0*pi; a+=0.5*pi )
00146   {
00147     addRingSegment( axis1, axis2, material, render_queue_group, a, a+pi*0.5 );
00148   }
00149 }
00150 
00151 void CartesianGripperControl::addRingSegment(
00152     Ogre::Vector3 axis1, Ogre::Vector3 axis2,
00153     Ogre::MaterialPtr material, unsigned render_queue_group,
00154     float a_min, float a_max )
00155 {
00156   rviz_interaction_tools::MeshObject *mesh_object =
00157       new rviz_interaction_tools::MeshObject( vis_manager_->getSceneManager(), controls_node_ );
00158 
00159   Ogre::Vector3 axis0 = Ogre::Vector3(1,1,1) - axis1 - axis2;
00160 
00161   std::vector< rviz_interaction_tools::MeshObject::Point > points;
00162 
00163   int steps = 20;
00164   for ( int i=0; i<=steps; i++ )
00165   {
00166     float a = a_min + (float(i)/float(steps) * (a_max-a_min) );
00167     Ogre::Vector3 p = (sin(a)*axis1 + cos(a)*axis2);
00168     Ogre::Vector3 p1 = p * (HANDLE_RADIUS_INNER);
00169     Ogre::Vector3 p2 = p * (HANDLE_RADIUS_OUTER);
00170     rviz_interaction_tools::MeshObject::Point p_inner = { p1.x, p1.y, p1.z, axis0.x, axis0.y, axis0.z, HANDLE_ALPHA };
00171     rviz_interaction_tools::MeshObject::Point p_outer = { p2.x, p2.y, p2.z, axis0.x, axis0.y, axis0.z, HANDLE_ALPHA };
00172     points.push_back(p_inner);
00173     points.push_back(p_outer);
00174   }
00175 
00176   std::vector< unsigned > triangles;
00177 
00178   for ( unsigned i=0; i<points.size()-2; i++ )
00179   {
00180     triangles.push_back(i);
00181     triangles.push_back(i+1);
00182     triangles.push_back(i+2);
00183   }
00184 
00185   rviz_interaction_tools::UniqueStringManager usm;
00186   mesh_object->loadMesh( usm.unique("cartesian_gripper_control_disc"), points, triangles );
00187 
00188   mesh_object->getEntity()->setMaterial( material );
00189   mesh_object->getEntity()->setRenderQueueGroup( render_queue_group );
00190 }
00191 
00192 void CartesianGripperControl::show()
00193 {
00194   status_ = IDLE;
00195   main_node_->setVisible(true);
00196 }
00197 
00198 void CartesianGripperControl::hide()
00199 {
00200   main_node_->setVisible(false);
00201 
00202   if ( status_ == HIDDEN )
00203   {
00204     return;
00205   }
00206 
00207   status_ = HIDDEN;
00208 
00209   try {
00210     object_manipulator::mechInterface().switchToJoint();
00211   }
00212   catch ( std::runtime_error &e )
00213   {
00214     ROS_ERROR("Caught exception while switching to joint controller: %s", e.what( ));
00215   }
00216 }
00217 
00218 
00219 void CartesianGripperControl::reset()
00220 {
00221   Ogre::Vector3 gripper_position;
00222   Ogre::Quaternion gripper_orientation;
00223 
00224   if (!getTransform(gripper_frame_, gripper_position, gripper_orientation))
00225   {
00226     hide();
00227     return;
00228   }
00229 
00230   //we moved the gripper a bit to the back, so now we need to put everything to the front a bit
00231   Ogre::Vector3 offset = gripper_orientation * gripper_offset_;
00232 
00233   controls_node_->setPosition( gripper_position - offset );
00234   controls_node_->setOrientation( gripper_orientation );
00235   gripper_node_->setPosition( gripper_position - offset );
00236   gripper_node_->setOrientation( gripper_orientation );
00237 
00238   show();
00239 
00240   try {
00241     object_manipulator::mechInterface().switchToCartesian();
00242   }
00243   catch ( std::runtime_error &e )
00244   {
00245     ROS_ERROR("Caught exception while witching to cartesian controller: %s", e.what( ));
00246   }
00247 }
00248 
00249 
00250 bool CartesianGripperControl::getTransform( std::string frame, Ogre::Vector3 &position, Ogre::Quaternion &orientation )
00251 {
00252   if (!vis_manager_->getFrameManager()->getTransform(frame, ros::Time(), position, orientation, false))
00253   {
00254     std::string error;
00255     if (vis_manager_->getFrameManager()->transformHasProblems(frame, ros::Time(), error))
00256     {
00257       setStatus(rviz::status_levels::Error, error);
00258     }
00259     else
00260     {
00261       std::stringstream ss;
00262       ss << "Could not transform from [" << frame << "] to Fixed Frame [" << vis_manager_->getFrameManager()->getFixedFrame() << "] for an unknown reason";
00263       setStatus(rviz::status_levels::Error, ss.str());
00264     }
00265     return false;
00266   }
00267 
00268   setStatus(rviz::status_levels::Ok, "Transform OK");
00269   return true;
00270 }
00271 
00272 
00273 void CartesianGripperControl::setStatus(rviz::status_levels::StatusLevel level, const std::string& text)
00274 {
00275   status_level_ = level;
00276   status_text_ = text;
00277 }
00278 
00279 
00280 void CartesianGripperControl::update()
00281 {
00282   if ( use_reference_frame_ )
00283   {
00284     Ogre::Vector3 reference_position;
00285     Ogre::Quaternion reference_orientation;
00286     if (!getTransform(reference_frame_, reference_position, reference_orientation))
00287     {
00288       hide();
00289       return;
00290     }
00291     controls_node_->setOrientation( reference_orientation );
00292   }
00293   else
00294   {
00295     //TODO:: account for movement of reference frame even if in local coords mode
00296     controls_node_->setOrientation( gripper_node_->getOrientation() );
00297   }
00298 }
00299 
00300 float CartesianGripperControl::getClosestIntersection( Ogre::Ray mouse_ray )
00301 {
00302   Ogre::Vector3 disc_intersection_3d;
00303   Ogre::Vector2 disc_intersection_2d;
00304   float disc_ray_t;
00305   unsigned disc_axis;
00306 
00307   Ogre::Vector3 box_intersection_3d;
00308   float box_intersection_1d;
00309   float box_ray_t;
00310   unsigned box_axis;
00311   SideT box_side;
00312 
00313   if ( !getClosestRing( mouse_ray, disc_intersection_3d, disc_intersection_2d, disc_ray_t, disc_axis )
00314       && !getClosestBox( mouse_ray, box_intersection_3d, box_intersection_1d, box_ray_t, box_axis, box_side ) )
00315     return NO_INTERSECTION;
00316 
00317   //choose what's closer to camera, box or ring
00318   return disc_ray_t < box_ray_t ? disc_ray_t : box_ray_t;
00319 }
00320 
00321 
00322 void CartesianGripperControl::mouseMove( Ogre::Ray mouse_ray )
00323 {
00324   Ogre::Vector3 intersection_3d;
00325   Ogre::Vector2 intersection_2d;
00326   float intersection_1d;
00327   float ray_t;
00328 
00329   switch ( status_ )
00330   {
00331     case HIDDEN:
00332     case IDLE:
00333 //      getClosestRing( mouse_ray, intersection_3d, intersection_2d, ray_t, axis );
00334 //      getClosestBox( mouse_ray, intersection_3d, intersection_1d, ray_t, axis, side );
00335       break;
00336 
00337     case ROTATING:
00338     {
00339       if ( intersectRing( mouse_ray, last_axis_, intersection_3d, intersection_2d, ray_t,
00340           HANDLE_RADIUS_INNER*0.5, HANDLE_RADIUS_OUTER*100 ) )
00341       {
00342         double angle = atan2( intersection_2d.x, intersection_2d.y );
00343         Ogre::Radian delta_angle( (last_angle_-angle) );
00344         Ogre::Vector3 axis_vec = controls_node_->getOrientation() * getAxis( last_axis_ );
00345         Ogre::Quaternion rotation( delta_angle, axis_vec );
00346         rotation = rotation * gripper_node_->getOrientation();
00347 
00348         if ( use_reference_frame_ )
00349         {
00350           last_angle_ = angle;
00351         }
00352         else
00353         {
00354           controls_node_->setOrientation( rotation );
00355         }
00356 
00357         gripper_node_->setOrientation( rotation );
00358 
00359         //ROS_INFO( "delta angle for axis %d = %f (%f - %f)", last_axis_, angle-last_angle_, angle, last_angle_ );
00360 
00361         Ogre::Vector3 diff = intersection_3d - controls_node_->getPosition();
00362         diff.normalise();
00363         diff *= (HANDLE_RADIUS_OUTER+HANDLE_RADIUS_INNER) / 2.0;
00364         marker_->setPosition( controls_node_->getPosition() + diff );
00365         translateRealGripper();
00366       }
00367       else
00368       {
00369         //make sure the angle doesn't jump when we get back to the ring
00370         last_angle_ = atan2( intersection_2d.x, intersection_2d.y );
00371       }
00372       break;
00373     }
00374 
00375     case DRAGGING:
00376     {
00377       getClosestPosition( mouse_ray, last_axis_, intersection_1d );
00378       {
00379         float delta = intersection_1d - last_drag_pos_;
00380         Ogre::Vector3 translate_delta = controls_node_->getOrientation() * getAxis(last_axis_) * delta;
00381         Ogre::Vector3 new_position = controls_node_->getPosition() + translate_delta;
00382 
00383         controls_node_->setPosition( new_position );
00384         gripper_node_->setPosition( new_position );
00385 
00386         marker_->setPosition( controls_node_->getPosition() + controls_node_->getOrientation() * getAxis(last_axis_) * intersection_1d );
00387         translateRealGripper();
00388       }
00389       break;
00390     }
00391   }
00392 }
00393 
00394 void CartesianGripperControl::mouseDown( Ogre::Ray mouse_ray )
00395 {
00396   Ogre::Vector3 disc_intersection_3d;
00397   Ogre::Vector2 disc_intersection_2d;
00398   float disc_ray_t;
00399   unsigned disc_axis;
00400 
00401   Ogre::Vector3 box_intersection_3d;
00402   float box_intersection_1d;
00403   float box_ray_t;
00404   unsigned box_axis;
00405   SideT box_side;
00406 
00407   if ( !getClosestRing( mouse_ray, disc_intersection_3d, disc_intersection_2d, disc_ray_t, disc_axis )
00408       && !getClosestBox( mouse_ray, box_intersection_3d, box_intersection_1d, box_ray_t, box_axis, box_side ) )
00409     return;
00410 
00411   //choose what's closer to camera, box or ring
00412   if ( disc_ray_t < box_ray_t )
00413   {
00414     //choose disc
00415     if ( fabs( (controls_node_->getOrientation()*getAxis(disc_axis)).dotProduct( mouse_ray.getDirection() )) < 0.2 )
00416     {
00417       //if the ray points at the plane at a steep angle, it's too dangerous
00418       return;
00419     }
00420 
00421     ROS_INFO("Rotating around axis %d", disc_axis);
00422     marker_->setPosition( disc_intersection_3d );
00423     status_ = ROTATING;
00424     last_angle_ = atan2( disc_intersection_2d.x, disc_intersection_2d.y );
00425     last_axis_ = disc_axis;
00426   }
00427   else
00428   {
00429     //choose box
00430     if ( fabs( (controls_node_->getOrientation()*getAxis(box_axis)).dotProduct( mouse_ray.getDirection() )) > 0.8 )
00431     {
00432       //if the ray points at the line at a steep angle, that's too dangerous
00433       return;
00434     }
00435 
00436     marker_->setPosition( box_intersection_3d );
00437     status_ = DRAGGING;
00438     last_drag_pos_ = box_intersection_1d;
00439     last_drag_side_ = box_side;
00440     last_axis_ = box_axis;
00441     ROS_INFO("Dragging axis %d t=%f", box_axis, last_drag_pos_);
00442   }
00443 
00444 }
00445 
00446 void CartesianGripperControl::mouseUp( Ogre::Ray mouse_ray )
00447 {
00448   switch ( status_ )
00449   {
00450     case IDLE:
00451     case HIDDEN:
00452 
00453       break;
00454 
00455     case ROTATING:
00456     case DRAGGING:
00457 
00458       translateRealGripper();
00459       status_ = IDLE;
00460       break;
00461   }
00462 }
00463 
00464 
00465 bool CartesianGripperControl::getClosestRing( Ogre::Ray mouse_ray,
00466     Ogre::Vector3 &nearest_intersection_3d, Ogre::Vector2 &nearest_intersection_2d,
00467     float &nearest_t, unsigned &nearest_axis )
00468 {
00469   nearest_t = NO_INTERSECTION;
00470   bool found = false;
00471 
00472   for ( int axis = 0; axis < 3; axis++ )
00473   {
00474     Ogre::Vector3 intersection_3d;
00475     Ogre::Vector2 intersection_2d;
00476     float ray_t;
00477     if ( intersectRing( mouse_ray, axis, intersection_3d, intersection_2d, ray_t ) && ray_t < nearest_t )
00478     {
00479       nearest_axis = axis;
00480       nearest_t = ray_t;
00481       nearest_intersection_3d = intersection_3d;
00482       nearest_intersection_2d = intersection_2d;
00483       found = true;
00484     }
00485   }
00486 
00487 //  marker_->setPosition( nearest_intersection_3d );
00488   return found;
00489 }
00490 
00491 
00492 bool CartesianGripperControl::intersectRing( Ogre::Ray mouse_ray, unsigned axis,
00493     Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t,
00494     float inner_radius, float outer_radius )
00495 {
00496   return ( intersectPlane( mouse_ray, axis, intersection_3d, intersection_2d, ray_t )
00497       && intersection_2d.length() > inner_radius
00498       && intersection_2d.length() < outer_radius );
00499 }
00500 
00501 
00502 bool CartesianGripperControl::intersectPlane( Ogre::Ray mouse_ray, unsigned axis,
00503     Ogre::Vector3 &intersection_3d, Ogre::Vector2 &intersection_2d, float &ray_t )
00504 {
00505   Ogre::Vector3 position = controls_node_->getPosition();
00506 
00507   Ogre::Vector3 normal = controls_node_->getOrientation() * getAxis(axis);
00508   Ogre::Vector3 x_axis = controls_node_->getOrientation() * getAxis(axis+1);
00509   Ogre::Vector3 y_axis = controls_node_->getOrientation() * getAxis(axis+2);
00510 
00511   Ogre::Plane plane( normal, position );
00512 
00513 
00514   Ogre::Vector2 origin_2d( position.dotProduct( x_axis ), position.dotProduct( y_axis ) );
00515 
00516   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( plane );
00517   if ( intersection.first )
00518   {
00519     intersection_3d = mouse_ray.getPoint( intersection.second );
00520     intersection_2d = Ogre::Vector2( intersection_3d.dotProduct(x_axis), intersection_3d.dotProduct(y_axis) );
00521     intersection_2d -= origin_2d;
00522 //    ROS_INFO( "Mouse ray intersects plane %d at %f %f %f (%f %f)", axis,
00523 //        intersection_3d.x, intersection_3d.y, intersection_3d.z, intersection_2d.x, intersection_2d.y );
00524 
00525     ray_t = intersection.second;
00526     return true;
00527   }
00528 
00529   ray_t = 0;
00530   return false;
00531 }
00532 
00533 
00534 bool CartesianGripperControl::getClosestBox( Ogre::Ray mouse_ray,
00535     Ogre::Vector3 &nearest_intersection_3d, float &nearest_intersection_1d,
00536     float &nearest_t, unsigned &nearest_axis, SideT &nearest_side )
00537 {
00538   nearest_t = NO_INTERSECTION;
00539   bool found = false;
00540 
00541   for ( int axis = 0; axis < 3; axis++ )
00542   {
00543     SideT sides[] = {POS,NEG};
00544     for ( unsigned s = 0; s < 2; s++ )
00545     {
00546       SideT side = sides[s];
00547       Ogre::Vector3 intersection_3d;
00548       float ray_t;
00549       float intersection_1d;
00550       if ( intersectBox( mouse_ray, axis, side, intersection_3d, intersection_1d, ray_t ) && ray_t < nearest_t )
00551       {
00552         nearest_intersection_1d = intersection_1d;
00553         nearest_intersection_3d = intersection_3d;
00554         nearest_t = ray_t;
00555         nearest_side = side;
00556         nearest_axis = axis;
00557         found = true;
00558       }
00559     }
00560   }
00561 
00562   return found;
00563 }
00564 
00565 
00566 bool CartesianGripperControl::intersectBox( Ogre::Ray mouse_ray, unsigned axis, SideT side,
00567     Ogre::Vector3 &intersection_3d, float &intersection_1d, float &ray_t )
00568 {
00569   ogre_tools::Shape* shape = drag_boxes_[axis][side];
00570 
00571   Ogre::Vector3 position = shape->getRootNode()->convertLocalToWorldPosition(Ogre::Vector3(0,0,0));
00572   Ogre::Quaternion orientation = shape->getRootNode()->convertLocalToWorldOrientation(Ogre::Quaternion());
00573 
00574   //ROS_INFO_STREAM( "intersectBox axis " << axis << " side " << side << " position " << position << " orientation " << orientation );
00575 
00576   Ogre::PlaneBoundedVolume volume;
00577 
00578   float box_size = HANDLE_SIZE*0.5;
00579 
00580 //  position = Ogre::Vector3(0,0,0);
00581 //  orientation = Ogre::Quaternion();
00582 //  box_size = 1;
00583 
00584   for ( unsigned i = 0; i<3; i++ )
00585   {
00586     Ogre::Vector3 axis_vec = orientation * getAxis(i);
00587     volume.planes.push_back( Ogre::Plane( -1 * axis_vec, position + (axis_vec*box_size) ) );
00588     volume.planes.push_back( Ogre::Plane( axis_vec, position - (axis_vec*box_size) ) );
00589   }
00590 
00591   std::pair<bool, Ogre::Real> intersection = mouse_ray.intersects( volume );
00592   if ( intersection.first )
00593   {
00594     intersection_3d = mouse_ray.getPoint( intersection.second );
00595     ray_t = intersection.second;
00596     getClosestPosition( mouse_ray, axis, intersection_1d );
00597     //ROS_INFO("Bingo");
00598     return true;
00599   }
00600 
00601   ray_t = 0;
00602   return false;
00603 }
00604 
00605 
00606 
00607 Ogre::Vector3 CartesianGripperControl::getAxis( unsigned axis )
00608 {
00609   axis = axis % 3;
00610   switch ( axis )
00611   {
00612     case 0:
00613       return Ogre::Vector3::UNIT_X;
00614     case 1:
00615       return Ogre::Vector3::UNIT_Y;
00616     case 2:
00617       return Ogre::Vector3::UNIT_Z;
00618     default:
00619       break;
00620   }
00621   return Ogre::Vector3();
00622 }
00623 
00624 
00625 bool CartesianGripperControl::getClosestPosition( Ogre::Ray mouse_ray, unsigned axis, float &pos )
00626 {
00627   Ogre::Vector3 axis_vec = controls_node_->getOrientation()*getAxis(axis);
00628 
00629   //axis2 is perpendicular to mouse ray and axis ray
00630   Ogre::Vector3 axis2 = mouse_ray.getDirection().crossProduct( axis_vec );
00631 
00632   //axis3 is perpendicular to axis and axis2, thus the normal of the plane
00633   //that contains the shortest connection line
00634   Ogre::Vector3 normal = axis2.crossProduct( mouse_ray.getDirection() );
00635 
00636   Ogre::Plane mouse_plane( normal, mouse_ray.getOrigin() );
00637   Ogre::Ray axis_ray( controls_node_->getPosition(), axis_vec );
00638 
00639   std::pair<bool,float> result = axis_ray.intersects( mouse_plane );
00640 
00641   //ROS_INFO_STREAM( "pos " << getPosition() << " dir " << axis_vec << " t " << result.second);
00642 
00643   pos = result.second;
00644   return result.first;
00645 }
00646 
00647 
00648 void CartesianGripperControl::translateRealGripper()
00649 {
00650   geometry_msgs::PoseStamped desired_pose;
00651 
00652   desired_pose.header.frame_id = vis_manager_->getFrameManager()->getFixedFrame();
00653   desired_pose.header.stamp = ros::Time::now();
00654 
00655   Ogre::Vector3 gripperPos = gripper_->getRootNode()->convertLocalToWorldPosition(Ogre::Vector3::ZERO);
00656   Ogre::Quaternion gripperOri = gripper_->getRootNode()->_getDerivedOrientation();
00657 
00658   //swap axes
00659   rviz::ogreToRobot( gripperPos );
00660   rviz::ogreToRobot( gripperOri );
00661 
00662   desired_pose.pose.position.x = gripperPos.x;
00663   desired_pose.pose.position.y = gripperPos.y;
00664   desired_pose.pose.position.z = gripperPos.z;
00665 
00666   desired_pose.pose.orientation.w = gripperOri.w;
00667   desired_pose.pose.orientation.x = gripperOri.x;
00668   desired_pose.pose.orientation.y = gripperOri.y;
00669   desired_pose.pose.orientation.z = gripperOri.z;
00670 
00671   try {
00672     object_manipulator::mechInterface().sendCartesianPoseCommand( arm_name_, desired_pose, 0, 0 );
00673   }
00674   catch ( std::runtime_error &e )
00675   {
00676     ROS_ERROR("Caught exception while sending pose command: %s", e.what( ));
00677   }
00678 }
00679 
00680 
00681 
00682 }
cartesian_gripper_control.cpp
