resolve side-dependend joint_names and tune grasp-open config

cob_grasp_generation/action/QueryGrasps.action

@@ -3,6 +3,7 @@ string object_name
 
 #gripper used on the robot
 string gripper_type
+string gripper_side
 
 #only return grasp with given id
 #note: cannot be used to return first grasp (id = 0)

cob_grasp_generation/action/ShowGrasps.action

@@ -3,6 +3,7 @@ string object_name
 
 #gripper used on the robot
 string gripper_type
+string gripper_side
 
 int32 grasp_id
 bool sort_by_quality

cob_grasp_generation/scripts/query_grasps_server.py

@@ -40,7 +40,7 @@ def query_cb(self, goal):
     if grasp_query_utils.check_database(goal.object_name, goal.gripper_type):
         rospy.loginfo('GraspTable for object %s exist in the database.', goal.object_name)
         rospy.loginfo('Returning grasp list for selected object.')
-        grasp_list = grasp_query_utils.get_grasps(goal.object_name, goal.gripper_type, goal.grasp_id, goal.num_grasps, goal.threshold)
+        grasp_list = grasp_query_utils.get_grasps(goal.object_name, goal.gripper_type, goal.gripper_side, goal.grasp_id, goal.num_grasps, goal.threshold)
     else:
         rospy.logwarn('GraspTable for object %s does not exist!',goal.object_name)
 

cob_grasp_generation/scripts/show_grasps_rviz_client.py

@@ -26,15 +26,14 @@ def show_grasps_client():
 
     object_name = raw_input("Insert object name: ")
     gripper_type = raw_input("Insert gripper_type: ")
-    #object_name = "yellowsaltcube"
-    #gripper_type = "sdhx"
+    gripper_side = ""
     grasp_id = 0
 
     while not rospy.is_shutdown():
         print grasp_id
 
         # Set the goal here: object_name, grasp_id, sort-by-quality
-        goal = cob_grasp_generation.msg.ShowGraspsGoal(object_name, gripper_type, grasp_id, True)
+        goal = cob_grasp_generation.msg.ShowGraspsGoal(object_name, gripper_type, gripper_side, grasp_id, True)
 
         client.send_goal(goal)
         client.wait_for_result()

cob_grasp_generation/scripts/show_grasps_rviz_server.py

@@ -85,7 +85,7 @@ def show_cb(self, goal):
 
     if grasp_query_utils.check_database(goal.object_name, goal.gripper_type):
       self.marker.mesh_resource = "package://cob_grasp_generation/files/meshes/"+goal.object_name+".stl"
-      grasp_list = grasp_query_utils.get_grasps(goal.object_name, goal.gripper_type, goal.grasp_id, 1)
+      grasp_list = grasp_query_utils.get_grasps(goal.object_name, goal.gripper_type, goal.gripper_side, goal.grasp_id, 1)
       if grasp_list:
         #print grasp_list
         self.js.name = grasp_list[0].pre_grasp_posture.joint_names              #TODO: joint names in urdf are "side-independend"

cob_grasp_generation/src/cob_grasp_generation/grasp_query_utils.py

@@ -57,15 +57,15 @@ def get_grasp_list(object_name, gripper_type, sort_by_quality=False):
 		return sorted(reader, key=lambda d: float(d['id']), reverse=False)
 
 #get the grasps
-def get_grasps(object_name, gripper_type, grasp_id=0, num_grasps=0, threshold=0):
+def get_grasps(object_name, gripper_type, gripper_side="", grasp_id=0, num_grasps=0, threshold=0):
 	#open database
 	grasp_list = get_grasp_list(object_name, gripper_type)
 
 	#check for grasp_id and return
 	if grasp_id > 0:
 		if grasp_id < len(grasp_list):
-			#print _fill_grasp_msg(gripper_type, grasp_list[grasp_id])
-			return [_fill_grasp_msg(gripper_type, grasp_list[grasp_id])]
+			#print _fill_grasp_msg(gripper_type, gripper_side, grasp_list[grasp_id])
+			return [_fill_grasp_msg(gripper_type, gripper_side, grasp_list[grasp_id])]
 		else:
 			print "Grasp not available"
 			return []
@@ -83,24 +83,24 @@ def get_grasps(object_name, gripper_type, grasp_id=0, num_grasps=0, threshold=0)
 	selected_grasp_list = []
 	for i in range(0,max_grasps):
 		if threshold > 0 and float(sorted_list[i]['eps_l1']) >= threshold:
-			selected_grasp_list.append(_fill_grasp_msg(gripper_type, sorted_list[i]))
+			selected_grasp_list.append(_fill_grasp_msg(gripper_type, gripper_side, sorted_list[i]))
 		elif threshold == 0:
-			selected_grasp_list.append(_fill_grasp_msg(gripper_type, sorted_list[i]))
+			selected_grasp_list.append(_fill_grasp_msg(gripper_type, gripper_side, sorted_list[i]))
 		else:
 			pass
 
 	#print len(selected_grasp_list)
 	return selected_grasp_list
 
 #fill the grasp message of ROS
-def _fill_grasp_msg(gripper_type, grasp):
+def _fill_grasp_msg(gripper_type, gripper_side, grasp):
 
 	#grasp posture
 	joint_config = JointTrajectory()
 	#joint_config.header.stamp = rospy.Time.now()
 	#joint_config.header.frame_id = ""
 
-	#ToDo: get rid of this hardcoded-joint names
+	#entries in the grasp table are side-independend
 	if gripper_type == "sdh":
 		joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
 	elif gripper_type == "sdhx":
@@ -109,7 +109,7 @@ def _fill_grasp_msg(gripper_type, grasp):
 		rospy.logerr("Gripper not supported")
 		return Grasp()
 
-	#print "Optimize grasp_configuration"
+	#get grasp joint configuration
 	point = JointTrajectoryPoint()
 	for joint_name in joint_config.joint_names:
 		point.positions.append(float(grasp[joint_name]))
@@ -118,10 +118,27 @@ def _fill_grasp_msg(gripper_type, grasp):
 		point.effort.append(0.0)
 		point.time_from_start = rospy.Duration(3.0)
 	joint_config.points.append(point)
-	## WARNING: in hydro the message format has changed, thus the following does not work anymore
-	##joint_config.position = [float(grasp['sdh_knuckle_joint']), float(grasp['sdh_finger_12_joint']), float(grasp['sdh_finger_13_joint']), float(grasp['sdh_finger_22_joint']), float(grasp['sdh_finger_23_joint']), float(grasp['sdh_thumb_2_joint']), float(grasp['sdh_thumb_3_joint'])]
-	##print joint_config.position
-	#joint_config.position = [float(grasp['sdh_knuckle_joint']), 0.92*float(grasp['sdh_finger_12_joint']), float(grasp['sdh_finger_13_joint']), 0.92*float(grasp['sdh_finger_22_joint']), float(grasp['sdh_finger_23_joint']), 0.92*float(grasp['sdh_thumb_2_joint']), float(grasp['sdh_thumb_3_joint'])]
+
+	#side-specific joint_name correction
+	#ToDo: get rid of this hardcoded-joint names
+	if gripper_type == "sdh":
+		if gripper_side == "left":
+			joint_config.joint_names = ['sdh_left_knuckle_joint', 'sdh_left_finger_12_joint', 'sdh_left_finger_13_joint', 'sdh_left_finger_22_joint', 'sdh_left_finger_23_joint', 'sdh_left_thumb_2_joint', 'sdh_left_thumb_3_joint']
+		elif gripper_side == "right":
+			joint_config.joint_names = ['sdh_right_knuckle_joint', 'sdh_right_finger_12_joint', 'sdh_right_finger_13_joint', 'sdh_right_finger_22_joint', 'sdh_right_finger_23_joint', 'sdh_right_thumb_2_joint', 'sdh_right_thumb_3_joint']
+		else:
+			joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
+	elif gripper_type == "sdhx":
+		if gripper_side == "left":
+			joint_config.joint_names = ['gripper_left_finger_1_joint', 'gripper_left_finger_2_joint']
+		elif gripper_side == "right":
+			joint_config.joint_names = ['gripper_right_finger_1_joint', 'gripper_right_finger_2_joint']
+		else:
+			joint_config.joint_names = ['gripper_finger_1_joint', 'gripper_finger_2_joint']
+	else:
+		rospy.logerr("Gripper not supported")
+		return Grasp()
+
 	#print joint_config
 
 	#pregrasp posture
@@ -131,11 +148,22 @@ def _fill_grasp_msg(gripper_type, grasp):
 
 	#ToDo: get rid of this hardcoded-joint names and open_config
 	if gripper_type == "sdh":
-		pre_joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
+		if gripper_side == "left":
+			pre_joint_config.joint_names = ['sdh_left_knuckle_joint', 'sdh_left_finger_12_joint', 'sdh_left_finger_13_joint', 'sdh_left_finger_22_joint', 'sdh_left_finger_23_joint', 'sdh_left_thumb_2_joint', 'sdh_left_thumb_3_joint']
+		elif gripper_side == "right":
+			pre_joint_config.joint_names = ['sdh_right_knuckle_joint', 'sdh_right_finger_12_joint', 'sdh_right_finger_13_joint', 'sdh_right_finger_22_joint', 'sdh_right_finger_23_joint', 'sdh_right_thumb_2_joint', 'sdh_right_thumb_3_joint']
+		else:
+			pre_joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
 		open_config = [0.0, -0.9854, 0.9472, -0.9854, 0.9472, -0.9854, 0.9472]
 	elif gripper_type == "sdhx":
-		pre_joint_config.joint_names = ['gripper_finger_1_joint', 'gripper_finger_2_joint']
-		open_config = [0.85, -1.4]
+		if gripper_side == "left":
+			pre_joint_config.joint_names = ['gripper_left_finger_1_joint', 'gripper_left_finger_2_joint']
+		elif gripper_side == "right":
+			pre_joint_config.joint_names = ['gripper_right_finger_1_joint', 'gripper_right_finger_2_joint']
+		else:
+			pre_joint_config.joint_names = ['gripper_finger_1_joint', 'gripper_finger_2_joint']
+		open_config = [-0.85, -0.5]
+
 	else:
 		rospy.logerr("Gripper not supported")
 		return Grasp()

cob_pick_place_action/action/CobPick.action

@@ -7,6 +7,7 @@ string object_name
 geometry_msgs/PoseStamped object_pose
 
 string gripper_type
+string gripper_side
 
 string grasp_database
 uint32 grasp_id

cob_pick_place_action/ros/include/cob_pick_place_action/cob_pick_place_action.h

@@ -84,7 +84,7 @@ class CobPickPlaceActionServer
 	void fillSingleGraspKIT(unsigned int objectClassId, std::string gripper_type, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
 	void convertGraspKIT(Grasp* current_grasp, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
 
-	void fillGraspsOR(unsigned int objectClassId, std::string gripper_type, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
+	void fillGraspsOR(unsigned int objectClassId, std::string gripper_type, std::string gripper_side, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps);
 
 	trajectory_msgs::JointTrajectory MapHandConfiguration(sensor_msgs::JointState table_config);
 	tf::Transform transformPose(tf::Transform transform_O_from_SDH, tf::Transform transform_HEADER_from_OBJECT, std::string object_frame_id);

cob_pick_place_action/ros/src/cob_pick_place_action.cpp

@@ -126,14 +126,14 @@ void CobPickPlaceActionServer::pick_goal_cb(const cob_pick_place_action::CobPick
 	else if(goal->grasp_database=="OpenRAVE")
 	{
 		ROS_INFO("Using OpenRAVE grasp table");
-		fillGraspsOR(goal->object_class, goal->gripper_type, goal->grasp_id, goal->object_pose, grasps);
+		fillGraspsOR(goal->object_class, goal->gripper_type, goal->gripper_side, goal->grasp_id, goal->object_pose, grasps);
 	}
 	else if(goal->grasp_database=="ALL")
 	{
 		ROS_INFO("Using all available databases");
 		std::vector<moveit_msgs::Grasp> grasps_OR, grasps_KIT;
 		fillAllGraspsKIT(goal->object_class, goal->gripper_type, goal->object_pose, grasps_KIT);
-		fillGraspsOR(goal->object_class, goal->gripper_type, goal->grasp_id, goal->object_pose, grasps_OR);
+		fillGraspsOR(goal->object_class, goal->gripper_type, goal->gripper_side, goal->grasp_id, goal->object_pose, grasps_OR);
 
 		grasps = grasps_KIT;
 		std::vector<moveit_msgs::Grasp>::iterator it = grasps.end();
@@ -155,7 +155,7 @@ void CobPickPlaceActionServer::pick_goal_cb(const cob_pick_place_action::CobPick
 		ROS_INFO("PickGoalCB: Found %lu grasps for this object", grasps.size());
 		for(unsigned int i=0; i<grasps.size(); i++)
 		{
-			ROS_INFO_STREAM("Grasp "<< i << ": " << grasps[i]);
+			ROS_DEBUG_STREAM("Grasp "<< i << ": " << grasps[i]);
 		}
 	}
 	else
@@ -543,7 +543,7 @@ void CobPickPlaceActionServer::convertGraspKIT(Grasp* current_grasp, geometry_ms
 
 
 
-void CobPickPlaceActionServer::fillGraspsOR(unsigned int objectClassId, std::string gripper_type, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
+void CobPickPlaceActionServer::fillGraspsOR(unsigned int objectClassId, std::string gripper_type, std::string gripper_side, unsigned int grasp_id, geometry_msgs::PoseStamped object_pose, std::vector<moveit_msgs::Grasp> &grasps)
 {
 	bool finished_before_timeout;
 	grasps.clear();
@@ -559,6 +559,7 @@ void CobPickPlaceActionServer::fillGraspsOR(unsigned int objectClassId, std::str
 	cob_grasp_generation::QueryGraspsGoal goal_query_grasps;
 	goal_query_grasps.object_name = map_classid_to_classname.find(objectClassId)->second;
 	goal_query_grasps.gripper_type = gripper_type;
+	goal_query_grasps.gripper_side = gripper_side;
 	goal_query_grasps.grasp_id = grasp_id;
 	goal_query_grasps.num_grasps = 0;
 	goal_query_grasps.threshold = 0;//0.012;

cob_pick_place_action/scripts/cob_multi_pick_place_client.py

@@ -73,6 +73,8 @@ def cob_pick_action_client(object_class, object_name, object_pose):
 	goal.object_pose = object_pose
 
 	goal.gripper_type = "sdh"
+	#goal.gripper_side = ""
+	goal.gripper_side = "left"
 
 	#goal.grasp_id = 21
 	#goal.grasp_database = "KIT"

cob_pick_place_action/scripts/cob_pick_action_client.py

@@ -62,31 +62,34 @@ def cob_pick_action_client():
 
 	# Creates a goal to send to the action server.
 	goal = cob_pick_place_action.msg.CobPickGoal()
-	goal.object_class = 18
-	goal.object_name = "yellowsaltcube"
+	#goal.object_class = 18
+	#goal.object_name = "yellowsaltcube"
 	#goal.object_class = 50
 	#goal.object_name = "instantsoup"
 	#goal.object_class = 103
 	#goal.object_name = "instanttomatosoup"
-	#goal.object_class = 5001
-	#goal.object_name = "pringles"
-
+	goal.object_class = 5001
+	goal.object_name = "pringles"
 	goal.object_pose.header.stamp = rospy.Time.now()
 	goal.object_pose.header.frame_id = "base_footprint"
+
 	### cob3
 	#goal.object_pose.pose.position.x = random.uniform(-0.8, -0.6)
 	#goal.object_pose.pose.position.y = random.uniform(-0.3,  0.3)
 	#goal.object_pose.pose.position.z = random.uniform( 0.8,  1.1)
 	#goal.object_pose.pose.orientation.x, goal.object_pose.pose.orientation.y, goal.object_pose.pose.orientation.z, goal.object_pose.pose.orientation.w = quaternion_from_euler(random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2))
 	#goal.gripper_type = "sdh"
-	### cob4
+	#goal.gripper_side = ""
 
+	### cob4
 	goal.object_pose.pose.position.x = 0.617 + random.uniform(-0.1, 0.1)
 	goal.object_pose.pose.position.y = 0.589 + random.uniform(-0.1, 0.1)
 	goal.object_pose.pose.position.z = 0.979 + random.uniform(-0.1, 0.1)
 	goal.object_pose.pose.orientation.x, goal.object_pose.pose.orientation.y, goal.object_pose.pose.orientation.z, goal.object_pose.pose.orientation.w = quaternion_from_euler(random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2),random.uniform(-pi/2, pi/2))
 	#goal.object_pose.pose.orientation.x, goal.object_pose.pose.orientation.y, goal.object_pose.pose.orientation.z, goal.object_pose.pose.orientation.w = quaternion_from_euler(-1.571, -0.000, -1.400)
 	goal.gripper_type = "sdhx"
+	#goal.gripper_side = ""
+	goal.gripper_side = "left"
 
 	#goal.grasp_database = "KIT"
 	goal.grasp_database = "OpenRAVE"