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