updated readme, added planner hand parameters etc.

README.md

@@ -215,8 +215,8 @@ roscore
 rosparam set use_sim_time 1
 
 # start the simulation environment
-roslaunch iiwa_gazebo iiwa_gazebo_examples.launch model:=iiwa7_kinect_ft world:=iiwa_ex3
-roslaunch trik_controller iiwa.launch
+roslaunch iiwa_gazebo iiwa_gazebo_examples.launch model:=iiwa7_kinect_ft world:=iiwa_fullEx
+roslaunch trik_controller iiwa.launch enabled_on_start:=false vel:=4 tvel:=4 rvel:=4 accel:=3
 rosservice call /disable
 rosrun hybrid_automaton_manager_kuka hybrid_automaton_manager_kuka
 

ec_grasp_planner/data/geometry_graph_example3.yaml

@@ -148,7 +148,7 @@ filter_object_by_size:
   filter_criterion: 1
   max_distance: 0.2
   min_size: 120
-  max_size: 20000
+  max_size: 5000
 filter_closest_object:
   type: ecto::pcl::PclCell<ecto_rbo_pcl::ExtractClosestCluster>
   inputs:
@@ -162,45 +162,16 @@ calculate_centroids:
   inputs:
     input: crop_box/output
     clusters: filter_closest_object/closest_cluster
-tf_to_vector_table_negated:
-  type: ecto_rbo_pcl::TF2Vector
-  inputs:
-    transform: table_fits/transform_biggest
-  column_index: 2
-  negate: true
 tf_to_vector_table:
   type: ecto_rbo_pcl::TF2Vector
   inputs:
     transform: table_fits/transform_biggest
   column_index: 2
   negate: false
-tf_to_vector_wall:
-  type: ecto_rbo_pcl::TF2Vector
-  inputs:
-    transform: wall_fit/transform_biggest
-  column_index: 2
-  negate: false
-wrap_in_vector_table_negated:
-  type: ecto_rbo_pcl::WrapVector3fInVector
-  inputs:
-    input: tf_to_vector_table_negated/vector
 wrap_in_vector_table:
   type: ecto_rbo_pcl::WrapVector3fInVector
   inputs:
     input: tf_to_vector_table/vector
-wrap_in_vector_wall:
-  type: ecto_rbo_pcl::WrapVector3fInVector
-  inputs:
-    input: tf_to_vector_wall/vector
-create_wall_grasps:
-  type: ecto_rbo_grasping::CreateGrasps
-  inputs:
-    header: msg_to_pcl/header
-    positions: calculate_centroids/centroids
-    approach_vectors: wrap_in_vector_table_negated/vector
-    roll_vectors: wrap_in_vector_wall/vector
-  strategy: 11  # APPROACH_THEN_SQUEEZE
-  pregrasp_configuration: 3 # HOOK
 
 #cylinder_fits:
 #  type: ecto::pcl::PclCellWithNormals<ecto_rbo_pcl::CylinderFits>

ec_grasp_planner/src/handarm_parameters.py

@@ -29,42 +29,56 @@ def __init__(self, **kwargs):
         super(RBOHand2WAM, self).__init__()
 
         self['surface_grasp']['initial_goal'] = np.array([0.910306, -0.870773, -2.36991, 2.23058, -0.547684, -0.989835, 0.307618])
-        self['surface_grasp']['pose'] = tra.translation_matrix([0, 0, 0])
         self['surface_grasp']['pregrasp_pose'] = tra.translation_matrix([0, 0, -0.2])
+        self['surface_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(90.), [0, 0, 1]))
         self['surface_grasp']['downward_force'] = 7.
         self['surface_grasp']['valve_pattern'] = (np.array([[ 0. ,  4.1], [ 0. ,  0.1], [ 0. ,  5. ], [ 0. ,  5.], [ 0. ,  2.], [ 0. ,  3.5]]), np.array([[1,0]]*6))
 
+        self['wall_grasp']['initial_goal'] = np.array([0.910306, -0.870773, -2.36991, 2.23058, -0.547684, -0.989835, 0.307618])
+        self['wall_grasp']['pregrasp_pose'] = tra.translation_matrix([-0.2, 0, 0])
+        self['wall_grasp']['hand_object_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.1]), tra.rotation_matrix(math.radians(-69.0), [1, 0, 0]), tra.euler_matrix(0, math.pi / 2., math.pi / 2.))
+        self['wall_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.]), tra.rotation_matrix(math.radians(-69.0), [1, 0, 0]), tra.euler_matrix(0, math.pi / 2., math.pi / 2.))
+        self['wall_grasp']['postgrasp_pose'] = tra.translation_matrix([-0.1, 0, -0.1])
         self['wall_grasp']['table_force'] = 3.
         self['wall_grasp']['sliding_speed'] = 0.04
         self['wall_grasp']['wall_force'] = -11.0
-        self['wall_grasp']['angle_of_attack'] = math.radians(69.0)
+        self['wall_grasp']['valve_pattern'] = (np.array([[1,0]]*6), np.array([[0, 2.5]]*6))
 
-        self['edge_grasp']['edge_distance_factor'] = -0.007
-        self['edge_grasp']['distance'] = 0.
+        self['edge_grasp']['initial_goal'] = np.array([0.910306, -0.870773, -2.36991, 2.23058, -0.547684, -0.989835, 0.307618])
+        self['edge_grasp']['pregrasp_pose'] = tra.translation_matrix([0, 0, -0.3])
+        self['edge_grasp']['hand_object_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.05]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.))
+        self['edge_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, -0.05, 0]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.))
+        self['edge_grasp']['postgrasp_pose'] = tra.translation_matrix([0, 0, -0.1])
         self['edge_grasp']['downward_force'] = 4.0
         self['edge_grasp']['sliding_speed'] = 0.04
-        self['edge_grasp']['angle_of_sliding'] = math.radians(-10.)
+        self['edge_grasp']['valve_pattern'] = (np.array([[0,0],[0,0],[1,0],[1,0],[1,0],[1,0]]), np.array([[0, 3.0]]*6))
 
 class RBOHand2Kuka(RBOHand2):
     def __init__(self, **kwargs):
         super(RBOHand2Kuka, self).__init__()
 
         self['surface_grasp']['initial_goal'] = np.array([-0.05864322834179703, 0.4118988657714642, -0.05864200146127985, -1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0])
-        #[-0.05864319407846175, 0.5766581592731841, -0.41049970582905093, -0.7002267282972259, 0.3518577791947308, 0.6178477409765168, 0])
-        self['surface_grasp']['pose'] = tra.translation_matrix([0, 0, 0])
         self['surface_grasp']['pregrasp_pose'] = tra.translation_matrix([0, 0, -0.2])
+        self['surface_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0]), tra.rotation_matrix(math.radians(90.), [0, 0, 1]))
         self['surface_grasp']['downward_force'] = 7.
         self['surface_grasp']['valve_pattern'] = (np.array([[ 0. ,  4.1], [ 0. ,  0.1], [ 0. ,  5. ], [ 0. ,  5.], [ 0. ,  2.], [ 0. ,  3.5]]), np.array([[1,0]]*6))
 
+        self['wall_grasp']['initial_goal'] = np.array([-0.006732293343088003, 0.44862315666582386, -0.82419548039534, -1.1710313279336022, 1.0578411443466278, 1.647228541451713, 1.3889329077205828])
+        self['wall_grasp']['pregrasp_pose'] = tra.translation_matrix([-0.1, 0, 0])
+        self['wall_grasp']['hand_object_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.1]), tra.rotation_matrix(math.radians(-69.0), [1, 0, 0]), tra.euler_matrix(0, math.pi / 2., math.pi / 2.))
+        self['wall_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.]), tra.rotation_matrix(math.radians(-69.0), [1, 0, 0]), tra.euler_matrix(0, math.pi / 2., math.pi / 2.))
+        self['wall_grasp']['postgrasp_pose'] = tra.translation_matrix([-0.1, 0, -0.1])
         self['wall_grasp']['table_force'] = 3.
         self['wall_grasp']['sliding_speed'] = 0.04
         self['wall_grasp']['wall_force'] = -11.0
-        self['wall_grasp']['angle_of_attack'] = math.radians(69.0)
+        self['wall_grasp']['valve_pattern'] = (np.array([[1,0]]*6), np.array([[0, 2.5]]*6))
 
-        self['edge_grasp']['edge_distance_factor'] = -0.007
-        self['edge_grasp']['distance'] = 0.
+        self['edge_grasp']['initial_goal'] = np.array([-0.05864322834179703, 0.4118988657714642, -0.05864200146127985, -1.6887810963180838, -0.11728653060066829, -0.8237944986945402, 0])
+        self['edge_grasp']['pregrasp_pose'] = tra.translation_matrix([0, 0, -0.3])
+        self['edge_grasp']['hand_object_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, 0, 0.05]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.))
+        self['edge_grasp']['grasp_pose'] = tra.concatenate_matrices(tra.translation_matrix([0, -0.05, 0]), tra.rotation_matrix(math.radians(10.), [1, 0, 0]), tra.euler_matrix(0, 0, -math.pi / 2.))
+        self['edge_grasp']['postgrasp_pose'] = tra.translation_matrix([0, 0, -0.1])
         self['edge_grasp']['downward_force'] = 4.0
         self['edge_grasp']['sliding_speed'] = 0.04
-        self['edge_grasp']['angle_of_sliding'] = math.radians(-10.)
-
+        self['edge_grasp']['valve_pattern'] = (np.array([[0,0],[0,0],[1,0],[1,0],[1,0],[1,0]]), np.array([[0, 3.0]]*6))