grasp and motion planning with underwater intervention vehicles

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://uji­ros­pkg.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 rviz­data

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,1e­6);

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+(I­Jriv*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