def get_grasps_from_position(self, req):
        x = time.time()
        rospy.loginfo("/get_grasps_from_position service has been called...")

        obj_id = req.object_id
        obj_pose = req.object_pose

        req = GetGraspConfigurationsRequest()
        req.object_id = obj_id
        grasp_configuration = (self.client(req)).grasp_configuration

        # current_joint_configuration
        while self.arm_state.get_num_connections() == 0:
            time.sleep(0.3)
            continue

        rotacion = grasping_functions.rotation_matrix(obj_pose)

        resp = GetGraspsFromPositionResponse()
        resp.feasible_grasp_available = False
        resp.grasp_configuration = []

        for i in range(0, len(grasp_configuration)):
            pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose)
            grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose)

            t = translation_from_matrix(pre_trans)
            q = quaternion_from_matrix(pre_trans)
            tg = translation_from_matrix(grasp_trans)
            qg = quaternion_from_matrix(grasp_trans)

            pre = PoseStamped()
            pre.header.stamp = rospy.Time.now()
            pre.header.frame_id = "/base_link"
            pre.pose.position.x = t[0]
            pre.pose.position.y = t[1]
            pre.pose.position.z = t[2]
            pre.pose.orientation.x = q[0]
            pre.pose.orientation.y = q[1]
            pre.pose.orientation.z = q[2]
            pre.pose.orientation.w = q[3]

            g = PoseStamped()
            g.header.stamp = rospy.Time.now()
            g.header.frame_id = "/base_link"
            g.pose.position.x = tg[0]
            g.pose.position.y = tg[1]
            g.pose.position.z = tg[2]
            g.pose.orientation.x = qg[0]
            g.pose.orientation.y = qg[1]
            g.pose.orientation.z = qg[2]
            g.pose.orientation.w = qg[3]

            sol = False
            for w in range(0, self.ik_loop_reply):
                (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre)
                if error_code.val == error_code.SUCCESS:
                    for k in range(0, self.ik_loop_reply):
                        (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g)
                        if error_code.val == error_code.SUCCESS:

                            new_valid_grasp = GraspSubConfiguration()
                            new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values
                            new_valid_grasp.grasp = g.pose
                            new_valid_grasp.pre_grasp = pre.pose
                            new_valid_grasp.category = grasping_functions.get_grasp_category(
                                pre.pose.position, g.pose.position
                            )
                            sol = grasping_functions.valid_grasp(new_valid_grasp)

                            if sol:
                                resp.feasible_grasp_available = True
                                resp.grasp_configuration.append(new_valid_grasp)
                                break
                    if sol:
                        break
                        # for
                        # for

                        # order grasps
        if len(resp.grasp_configuration) > 0:
            (resp.grasp_configuration).sort()

        rospy.loginfo(str(len(resp.grasp_configuration)) + " valid grasps for this pose.")
        rospy.loginfo("/get_grasps_from_position call has finished.")
        print "Time employed: " + str(time.time() - x)
        print "---------------------------------------"
        return resp
Example #2
0
    def get_grasps_from_position(self, req):
        x = time.time()
        rospy.loginfo("/get_grasps_from_position service has been called...")

        obj_id = req.object_id
        obj_pose = req.object_pose

        req = GetGraspConfigurationsRequest()
        req.object_id = obj_id
        grasp_configuration = (self.client(req)).grasp_configuration

        #current_joint_configuration
        while self.arm_state.get_num_connections() == 0:
            time.sleep(0.3)
            continue

        rotacion = grasping_functions.rotation_matrix(obj_pose)

        resp = GetGraspsFromPositionResponse()
        resp.feasible_grasp_available = False
        resp.grasp_configuration = []

        for i in range(0, len(grasp_configuration)):
            pre_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].pre_grasp.pose)
            grasp_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].grasp.pose)

            t = translation_from_matrix(pre_trans)
            q = quaternion_from_matrix(pre_trans)
            tg = translation_from_matrix(grasp_trans)
            qg = quaternion_from_matrix(grasp_trans)

            pre = PoseStamped()
            pre.header.stamp = rospy.Time.now()
            pre.header.frame_id = "/base_link"
            pre.pose.position.x = t[0]
            pre.pose.position.y = t[1]
            pre.pose.position.z = t[2]
            pre.pose.orientation.x = q[0]
            pre.pose.orientation.y = q[1]
            pre.pose.orientation.z = q[2]
            pre.pose.orientation.w = q[3]

            g = PoseStamped()
            g.header.stamp = rospy.Time.now()
            g.header.frame_id = "/base_link"
            g.pose.position.x = tg[0]
            g.pose.position.y = tg[1]
            g.pose.position.z = tg[2]
            g.pose.orientation.x = qg[0]
            g.pose.orientation.y = qg[1]
            g.pose.orientation.z = qg[2]
            g.pose.orientation.w = qg[3]

            sol = False
            for w in range(0, self.ik_loop_reply):
                (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
                    current_joint_configuration, pre)
                if (error_code.val == error_code.SUCCESS):
                    for k in range(0, self.ik_loop_reply):
                        (grasp_conf,
                         error_code) = grasping_functions.callIKSolver(
                             pre_grasp_conf, g)
                        if (error_code.val == error_code.SUCCESS):

                            new_valid_grasp = GraspSubConfiguration()
                            new_valid_grasp.sdh_joint_values = grasp_configuration[
                                i].sdh_joint_values
                            new_valid_grasp.grasp = g.pose
                            new_valid_grasp.pre_grasp = pre.pose
                            new_valid_grasp.category = grasping_functions.get_grasp_category(
                                pre.pose.position, g.pose.position)
                            sol = grasping_functions.valid_grasp(
                                new_valid_grasp)

                            if sol:
                                resp.feasible_grasp_available = True
                                resp.grasp_configuration.append(
                                    new_valid_grasp)
                                break
                    if sol:
                        break
            #for
        #for

        #order grasps
        if len(resp.grasp_configuration) > 0:
            (resp.grasp_configuration).sort()

        rospy.loginfo(
            str(len(resp.grasp_configuration)) +
            " valid grasps for this pose.")
        rospy.loginfo("/get_grasps_from_position call has finished.")
        print "Time employed: " + str(time.time() - x)
        print "---------------------------------------"
        return resp
	def get_grasps_from_position(self, req):
		x = time.time();
		rospy.loginfo("/get_grasps_from_position service has been called...");

		obj_id = req.object_id;
		obj_pose = req.object_pose;

		req = GetGraspConfigurationsRequest();
		req.object_id = obj_id;
		grasp_configuration = (self.client(req)).grasp_configuration;

		#current_joint_configuration
		rospy.loginfo("Waiting /arm_controller/state...");
		sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state);
		while sub.get_num_connections() == 0:
			time.sleep(0.3);
			continue;
		rospy.loginfo("/arm_controller/state has finished.");

		rotacion = grasping_functions.rotation_matrix(obj_pose);

		resp = GetGraspsFromPositionResponse();
		resp.feasible_grasp_available = False;
		resp.grasp_configuration = [];

		for i in range(0,len(grasp_configuration)):
			pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose);
			grasp_trans = rotacion *  grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose);

			t = translation_from_matrix(pre_trans);
			q = quaternion_from_matrix(pre_trans);
			tg = translation_from_matrix(grasp_trans);
			qg = quaternion_from_matrix(grasp_trans);

			pre = PoseStamped();
			pre.header.stamp = rospy.Time.now();
			pre.header.frame_id = "/base_link";
			pre.pose.position.x = t[0];
			pre.pose.position.y = t[1];
			pre.pose.position.z = t[2];
			pre.pose.orientation.x = q[0];
			pre.pose.orientation.y = q[1];
			pre.pose.orientation.z = q[2];
			pre.pose.orientation.w = q[3];

			g = PoseStamped();
			g.header.stamp = rospy.Time.now();
			g.header.frame_id = "/base_link";
			g.pose.position.x = tg[0];
			g.pose.position.y = tg[1];
			g.pose.position.z = tg[2];
			g.pose.orientation.x = qg[0];
			g.pose.orientation.y = qg[1];
			g.pose.orientation.z = qg[2];
			g.pose.orientation.w = qg[3];

			offset_x = 0#(g.pose.position.x - pre.pose.position.x)/3
			offset_y = 0#(g.pose.position.y - pre.pose.position.y)/3
			offset_z = 0#(g.pose.position.z - pre.pose.position.z)/3

			pre.pose.position.x += offset_x
			pre.pose.position.y += offset_y
			pre.pose.position.z -= offset_z
			g.pose.position.x += offset_x
			g.pose.position.y += offset_y
			g.pose.position.z -= offset_z
			

		
			sol = False;
			for w in range(0,self.ik_loop_reply):
				(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre);		
				if(error_code.val == error_code.SUCCESS):
					for k in range(0,self.ik_loop_reply):
						(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g);
						if(error_code.val == error_code.SUCCESS):

							new_valid_grasp = GraspSubConfiguration();
							new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values;
							new_valid_grasp.grasp = g.pose;
							new_valid_grasp.pre_grasp = pre.pose;
							new_valid_grasp.category = grasping_functions.get_grasp_category(pre.pose.position, g.pose.position);
							sol = grasping_functions.valid_grasp(new_valid_grasp);

							if sol:
								resp.feasible_grasp_available = True;
								resp.grasp_configuration.append(new_valid_grasp);
								break;
					if sol:
						break;
			#for
		#for

		#order grasps
		if len(resp.grasp_configuration) > 0:
			(resp.grasp_configuration).sort();

		rospy.loginfo(str(len(resp.grasp_configuration))+" valid grasps for this pose.");	
		rospy.loginfo("/get_grasps_from_position call has finished.");
		print "Time employed: " + str(time.time() - x);
		print "---------------------------------------";
		return resp;
Example #4
0
    def get_pregrasps(self, req):
        x = time.time()
        rospy.loginfo("/get_pregrasps service has been called...")

        obj_id = req.object_id
        obj_pose = req.object_pose
        num_configurations = req.num_configurations

        req = GetGraspConfigurationsRequest()
        req.object_id = obj_id
        grasp_configuration = (self.client(req)).grasp_configuration

        rotacion = grasping_functions.rotation_matrix(obj_pose)

        resp = GetPreGraspResponse()
        resp.side = []
        resp.mside = []
        resp.top = []
        resp.front = []

        for i in range(0, len(grasp_configuration)):
            pre_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].pre_grasp.pose)
            grasp_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].grasp.pose)

            t = translation_from_matrix(pre_trans)
            q = quaternion_from_matrix(pre_trans)
            tg = translation_from_matrix(grasp_trans)
            qg = quaternion_from_matrix(grasp_trans)

            pre = Pose()
            pre.position.x = t[0]
            pre.position.y = t[1]
            pre.position.z = t[2]
            pre.orientation.x = q[0]
            pre.orientation.y = q[1]
            pre.orientation.z = q[2]
            pre.orientation.w = q[3]

            g = Pose()
            g.position.x = tg[0]
            g.position.y = tg[1]
            g.position.z = tg[2]
            g.orientation.x = qg[0]
            g.orientation.y = qg[1]
            g.orientation.z = qg[2]
            g.orientation.w = qg[3]

            aux = GraspConfiguration()
            aux.object_id = obj_id
            aux.hand_type = "SDH"
            aux.sdh_joint_values = grasp_configuration[i].sdh_joint_values
            aux.target_link = "/sdh_palm_link"
            aux.pre_grasp.pose = pre
            aux.grasp.pose = g
            aux.category = grasping_functions.get_grasp_category(
                pre.position, g.position)

            if aux.category == "TOP":
                if len(resp.top) < num_configurations:
                    resp.top.append(aux)
            elif aux.category == "FRONT":
                if len(resp.front) < num_configurations:
                    resp.front.append(aux)
            elif aux.category == "SIDE":
                if len(resp.side) < num_configurations:
                    resp.side.append(aux)
            elif aux.category == "-SIDE":
                if len(resp.mside) < num_configurations:
                    resp.mside.append(aux)
            else:
                continue

            if len(resp.top) == num_configurations and len(
                    resp.side) == num_configurations and len(
                        resp.mside) == num_configurations and len(
                            resp.front) == num_configurations:
                break

        rospy.loginfo("/get_pregrasps call has finished.")
        print "Time employed: " + str(time.time() - x)
        print "---------------------------------------"
        return resp
    def execute_cb(self, server_goal):
        x = time.time()
        rospy.loginfo("/get_grasps_from_position_server has been called...")

        obj_id = server_goal.object_id
        obj = server_goal.object_pose

        goal = GraspCGoal()
        goal.object_id = obj_id
        self.client.send_goal(goal)
        self.client.wait_for_result(rospy.Duration.from_sec(5.0))
        grasp_configuration = (self.client.get_result()).grasp_configuration

        # current_joint_configuration
        rospy.loginfo("Waiting /arm_controller/state...")
        sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
        while sub.get_num_connections() == 0:
            time.sleep(0.3)
            continue
        rospy.loginfo("/arm_controller/state has finished.")

        rotacion = grasping_functions.rotation_matrix(obj)

        server_result = GraspFActionResult().result
        server_result.feasible_grasp_available = False
        server_result.grasp_configuration = []

        for i in range(0, len(grasp_configuration)):
            pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose)
            grasp_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose)

            t = translation_from_matrix(pre_trans)
            q = quaternion_from_matrix(pre_trans)
            tg = translation_from_matrix(grasp_trans)
            qg = quaternion_from_matrix(grasp_trans)

            pre = PoseStamped()
            pre.header.stamp = rospy.Time.now()
            pre.header.frame_id = "/base_link"
            pre.pose.position.x = t[0]
            pre.pose.position.y = t[1]
            pre.pose.position.z = t[2]
            pre.pose.orientation.x = q[0]
            pre.pose.orientation.y = q[1]
            pre.pose.orientation.z = q[2]
            pre.pose.orientation.w = q[3]

            g = PoseStamped()
            g.header.stamp = rospy.Time.now()
            g.header.frame_id = "/base_link"
            g.pose.position.x = tg[0]
            g.pose.position.y = tg[1]
            g.pose.position.z = tg[2]
            g.pose.orientation.x = qg[0]
            g.pose.orientation.y = qg[1]
            g.pose.orientation.z = qg[2]
            g.pose.orientation.w = qg[3]

            offset_x = 0  # (g.pose.position.x - pre.pose.position.x)/3
            offset_y = 0  # (g.pose.position.y - pre.pose.position.y)/3
            offset_z = 0  # (g.pose.position.z - pre.pose.position.z)/3

            pre.pose.position.x += offset_x
            pre.pose.position.y += offset_y
            pre.pose.position.z -= offset_z
            g.pose.position.x += offset_x
            g.pose.position.y += offset_y
            g.pose.position.z -= offset_z

            sol = False
            for w in range(0, self.ik_loop_reply):
                (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre)
                if error_code.val == error_code.SUCCESS:
                    for k in range(0, self.ik_loop_reply):
                        (grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, g)
                        if error_code.val == error_code.SUCCESS:
                            print i
                            new_valid_grasp = GraspSubConfiguration()
                            new_valid_grasp.sdh_joint_values = grasp_configuration[i].sdh_joint_values
                            new_valid_grasp.grasp = g.pose
                            new_valid_grasp.pre_grasp = pre.pose
                            new_valid_grasp.category = grasping_functions.get_grasp_category(
                                pre.pose.position, g.pose.position
                            )

                            server_result.feasible_grasp_available = True
                            server_result.grasp_configuration.append(new_valid_grasp)

                            sol = True
                            break
                    if sol:
                        break
                        # for
                        # while

                        # order grasps
        if len(server_result.grasp_configuration) > 0:
            (server_result.grasp_configuration).sort()

        rospy.loginfo(str(len(server_result.grasp_configuration)) + " valid grasps for this pose.")
        rospy.loginfo("/get_grasps_from_position call has finished.")
        print "Time employed: " + str(time.time() - x)
        print "---------------------------------------"
        self.get_grasps_from_position_server.set_succeeded(server_result)
Example #6
0
	def get_pregrasps(self, req):
		x = time.time();
		rospy.loginfo("/get_pregrasps service has been called...");

		obj_id = req.object_id;
		obj_pose = req.object_pose;
		num_configurations = req.num_configurations;

		req = GetGraspConfigurationsRequest();
		req.object_id = obj_id;
		grasp_configuration = (self.client(req)).grasp_configuration;


		rotacion = grasping_functions.rotation_matrix(obj_pose);

		resp = GetPreGraspResponse();
		resp.side = []
		resp.mside = []
		resp.top = []
		resp.front = []

		for i in range(0,len(grasp_configuration)):
			pre_trans = rotacion * grasping_functions.matrix_from_pose(grasp_configuration[i].pre_grasp.pose);
			grasp_trans = rotacion *  grasping_functions.matrix_from_pose(grasp_configuration[i].grasp.pose);

			t = translation_from_matrix(pre_trans);
			q = quaternion_from_matrix(pre_trans);
			tg = translation_from_matrix(grasp_trans);
			qg = quaternion_from_matrix(grasp_trans);

			pre = Pose();
			pre.position.x = t[0];
			pre.position.y = t[1];
			pre.position.z = t[2];
			pre.orientation.x = q[0];
			pre.orientation.y = q[1];
			pre.orientation.z = q[2];
			pre.orientation.w = q[3];

			g = Pose();
			g.position.x = tg[0];
			g.position.y = tg[1];
			g.position.z = tg[2];
			g.orientation.x = qg[0];
			g.orientation.y = qg[1];
			g.orientation.z = qg[2];
			g.orientation.w = qg[3];



			aux = GraspConfiguration();
			aux.object_id = obj_id;
			aux.hand_type = "SDH";
			aux.sdh_joint_values = grasp_configuration[i].sdh_joint_values;
			aux.target_link = "/sdh_palm_link";
			aux.pre_grasp.pose = pre;
			aux.grasp.pose = g;
			aux.category = grasping_functions.get_grasp_category(pre.position, g.position);

			if aux.category == "TOP":
				if len(resp.top) < num_configurations:
					resp.top.append(aux);
			elif aux.category == "FRONT":
				if len(resp.front) < num_configurations:
					resp.front.append(aux);
			elif aux.category == "SIDE":
				if len(resp.side) < num_configurations:
					resp.side.append(aux);
			elif aux.category == "-SIDE":
				if len(resp.mside) < num_configurations:
					resp.mside.append(aux);
			else:
				continue

			if len(resp.top)==num_configurations and len(resp.side)==num_configurations and len(resp.mside)==num_configurations and len(resp.front)==num_configurations:
				break;

		rospy.loginfo("/get_pregrasps call has finished.");
		print "Time employed: " + str(time.time() - x);
		print "---------------------------------------";
		return resp;
    def execute_cb(self, server_goal):
        x = time.time()
        rospy.loginfo("/get_grasps_from_position_server has been called...")

        obj_id = server_goal.object_id
        obj = server_goal.object_pose

        goal = GraspCGoal()
        goal.object_id = obj_id
        self.client.send_goal(goal)
        self.client.wait_for_result(rospy.Duration.from_sec(5.0))
        grasp_configuration = (self.client.get_result()).grasp_configuration

        #current_joint_configuration
        rospy.loginfo("Waiting /arm_controller/state...")
        sub = rospy.Subscriber("/arm_controller/state",
                               JointTrajectoryControllerState,
                               self.get_joint_state)
        while sub.get_num_connections() == 0:
            time.sleep(0.3)
            continue
        rospy.loginfo("/arm_controller/state has finished.")

        rotacion = grasping_functions.rotation_matrix(obj)

        server_result = GraspFActionResult().result
        server_result.feasible_grasp_available = False
        server_result.grasp_configuration = []

        for i in range(0, len(grasp_configuration)):
            pre_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].pre_grasp.pose)
            grasp_trans = rotacion * grasping_functions.matrix_from_pose(
                grasp_configuration[i].grasp.pose)

            t = translation_from_matrix(pre_trans)
            q = quaternion_from_matrix(pre_trans)
            tg = translation_from_matrix(grasp_trans)
            qg = quaternion_from_matrix(grasp_trans)

            pre = PoseStamped()
            pre.header.stamp = rospy.Time.now()
            pre.header.frame_id = "/base_link"
            pre.pose.position.x = t[0]
            pre.pose.position.y = t[1]
            pre.pose.position.z = t[2]
            pre.pose.orientation.x = q[0]
            pre.pose.orientation.y = q[1]
            pre.pose.orientation.z = q[2]
            pre.pose.orientation.w = q[3]

            g = PoseStamped()
            g.header.stamp = rospy.Time.now()
            g.header.frame_id = "/base_link"
            g.pose.position.x = tg[0]
            g.pose.position.y = tg[1]
            g.pose.position.z = tg[2]
            g.pose.orientation.x = qg[0]
            g.pose.orientation.y = qg[1]
            g.pose.orientation.z = qg[2]
            g.pose.orientation.w = qg[3]

            offset_x = 0  #(g.pose.position.x - pre.pose.position.x)/3
            offset_y = 0  #(g.pose.position.y - pre.pose.position.y)/3
            offset_z = 0  #(g.pose.position.z - pre.pose.position.z)/3

            pre.pose.position.x += offset_x
            pre.pose.position.y += offset_y
            pre.pose.position.z -= offset_z
            g.pose.position.x += offset_x
            g.pose.position.y += offset_y
            g.pose.position.z -= offset_z

            sol = False
            for w in range(0, self.ik_loop_reply):
                (pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
                    current_joint_configuration, pre)
                if (error_code.val == error_code.SUCCESS):
                    for k in range(0, self.ik_loop_reply):
                        (grasp_conf,
                         error_code) = grasping_functions.callIKSolver(
                             pre_grasp_conf, g)
                        if (error_code.val == error_code.SUCCESS):
                            new_valid_grasp = GraspSubConfiguration()
                            new_valid_grasp.sdh_joint_values = grasp_configuration[
                                i].sdh_joint_values
                            new_valid_grasp.grasp = g.pose
                            new_valid_grasp.pre_grasp = pre.pose
                            new_valid_grasp.category = grasping_functions.get_grasp_category(
                                pre.pose.position, g.pose.position)

                            sol = grasping_functions.valid_grasp(
                                new_valid_grasp)

                            if sol:
                                server_result.feasible_grasp_available = True
                                server_result.grasp_configuration.append(
                                    new_valid_grasp)
                                break
                    if sol:
                        break
            #for
        #while

        #order grasps
        if len(server_result.grasp_configuration) > 0:
            (server_result.grasp_configuration).sort()

        rospy.loginfo(
            str(len(server_result.grasp_configuration)) +
            " valid grasps for this pose.")
        rospy.loginfo("/get_grasps_from_position call has finished.")
        print "Time employed: " + str(time.time() - x)
        print "---------------------------------------"
        self.get_grasps_from_position_server.set_succeeded(server_result)