grasp and motion planning with underwater intervention vehicles
TRANSCRIPT
Grasp and Motion Planning with
Underwater Intervention Vehicles running ROS
The experience of
TRIDENT EU project
Mario PratsIROS 2012 Tutorial on
Handling ROS
www.irs.uji.es/trident
Outline
● The TRIDENT FP7 Project● Motivation, goal and challenges● The role of ROS
● UWSim: a ROS-based underwater simulator ● Installation and first steps
● Hands on: Motion planning on underwater vehicles with manipulators
● Hands-on: Laser-stripe 3D reconstruction and grasp planning
The TRIDENT FP7 project
● Main goal:
Improvement of autonomous manipulation capabilities in current underwater robots
The TRIDENT FP7 project
Main goal:
How?● New user interfaces
● More perception
● Free floating
● Etc.
Improvement of autonomous manipulation capabilities in current
underwater robots
Current approach: ROVs
AF 447
2 years4 attempts 45m$
AF 447 Black box recovery
Current approach: ROVs
TRIDENT – Marine Robots and Dexterous Manipulation for Enabling Autonomous Underwater Multipurpose Intervention Missions (2010-2013)
PHASE I (Survey): 1) Launching.2) Survey.3) Recovery.
PHASE II (Intervention): 4) Launching.5) Approaching.6) Intervention.7) Recovery.
Target Selection & InterventionSpecification
Intervention Autonomous Underwater Vehicle (I-AUV)
Challenges: Floating platform
Limited power and sensors
TRIDENT Results
Roses,Girona (Spain) Oct 2011
TRIDENT Results
Soller, Mallorca (Spain) Oct 2012
TRIDENT Results
Use of ROS in TRIDENT
Underwater manipulation
UWSim: a ROS-based underwater simulator
● OpenSceneGraph● osgOcean● Bullet● ROS
UWSim: install the sources and build
$ mkdir ~/iros2012_tutorial$ cd ~/iros2012_tutorial$ rosinstall .https://ujirospkg.googlecode.com/svn/iros2012_tutorial.rosinstall /opt/ros/electric/$ source setup.bash
● underwater_simulation stack includes osgOcean, UWSim and underwater_vehicle_dynamics:
$ rosdep install UWSim$ rosmake UWSim
Install files for rviz:
$ roscd UWSim$ make rvizdata
UWSim
Customizable environment
Multiple robots
Surface Vehicles
Surface Vehicles
Sensor simulation
● Virtual cameras● DVL, IMU, GPS● Joint encoders● Range sensors (sonar)
ROS Interface
● nav_msgs/Odometry● sensor_msgs/JointState● sensor_msgs/Image● sensor_msgs/Range● sensor_msgs/Imu● geometry_msgs/Pose● geometry_msgs/Twist
UWSim – run
$ rosrun UWSim UWSim [disableShaders] [configfile <file.xml>]
UWSim Hands On
Move the vehicle:
$ rosrun UWSim setVehicleTwist /g500/twist 0.2 0 0 0 0 0$ rosrun UWSim setVehiclePose /g500/pose 2 2 0 0 0 0.8
Playing with stereo:
$ rosrun UWSim UWSim –configfile cirs_stereo.xml$ ROS_NAMESPACE=stereo_down rosrun stereo_image_proc stereo_image_proc$ rosrun rviz rviz (add PointCloud2 display)
Use case: vision
Use case: autonomous control
Use case: grasping
Use case: online visualization
Hands on
1) Inverse kinematics on an I-AUV using KDL
2) 3D reconstruction with a laser stripe emitter
3) User-guided grasp planning on a point cloud
Inverse kinematics of an I-AUV
$ roscd auv_ik$ rosmake auv_ik$ roslaunch auv_ik arm5e_ik.launch
With rviz:$ rosrun rviz rviz (load robot_model and set fixed frame to “world”)
With UWSim:$ rosrun UWSim UWSim
$ rosparam set (goalx | goaly | goalz | goalrz ) value
Knowing a goal where to move the hand, compute a suitable vehicle-arm configuration
ARM5Arm class:
mar/mar_robot_arm5e/include /mar_robot_arm5e/ARM5Arm.h
mar/mar_robot_arm5e/src/ARM5Arm.cpp
ARM5Arm::vehicleArmIK(vpHomogeneousMatrix &wMe) method:
//Forward position solver
KDL::ChainFkSolverPos_recursive fksolver(auvarm_chain);
//Custom Inverse velocity solver (grasp redundancy)
KDL::ChainIkSolverVel_pinv_red iksolverv(auvarm_chain);
iksolverv.setBaseJacobian(true);
KDL::ChainIkSolverPos_NR iksolver(auvarm_chain, fksolver,iksolverv,100,1e6);
Inverse kinematics of an I-AUV
Kinematic Solvers:
mar/mar_robot_arm5e/include /mar_robot_arm5e/ARM5Solvers.h
mar/mar_robot_arm5e/src/ARM5Solvers.cpp
KDL::ChainIkSolverVel_pinv_red class:
int ChainIkSolverVel_pinv_red::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
Line 123: qdot=Jriv*vh+(IJriv*Jr)*sv;
Inverse kinematics of an I-AUV
Laser stripe reconstruction and pc_guided_grasp_planning
Laser stripe reconstruction and pc_guided_grasp_planning
Install:$ rosdep install laser_stripe_reconstruction$ rosdep install pc_guided_grasp_planning$ rosmake underwater_grasping- Download laser_scan.bag
Laser stripe reconstruction:$ roslaunch laser_stripe_reconstruction arm5e_laser_reconstruction.launch fixed:=true output_basename:=seafloor$ rosbag play laser_scan.bag Press Ctrl-C when finished$ rosrun pcl pcd_viewer data/seafloor.pcd
Grasp planning:$ roslaunch pc_guided_grasp_planning arm5e_pc_grasp_planning.launch input_basename:=seafloor
Laser stripe reconstruction and pc_guided_grasp_planning
Laser stripe reconstruction and pc_guided_grasp_planning
Conclusions
● Lots of packages ready to use● stereo_image_proc, libviso2, ompl, drivers
● ROS facilitates integration● Great when doing field experiments● Allows focusing on getting results
Questions?
End