Beispiel #1
0
 def send_gripper_cmd(self, left, right):
     rospy.loginfo("Send Gripper Command")
     lgmsg = GripperCommandActionGoal()
     lgmsg.header.seq = 0
     lgmsg.goal.command.position = left
     rgmsg = GripperCommandActionGoal()
     rgmsg.header.seq = 0
     rgmsg.goal.command.position = right
     self.leftGripper.publish(lgmsg)
     self.rightGripper.publish(rgmsg)
 def __create_gripper_command(self, position, max_effort):
     """Creating GripperCommand message.
         @position   : [m]
         @max_effort : [N]
     """
     if self.__isSim:
         msg = GripperCommandActionGoal()
         msg.goal.command.position = position
         msg.goal.command.max_effort = max_effort
     else:
         msg = GripperCommand()
         msg.position = position
         msg.max_effort = max_effort
     return msg
Beispiel #3
0
    def set_pos(self, pos):
        """
        Set the gripper position. Function internally maps
        values from API position range to URScript position
        range. After sending position command, update internal
        position variable by sending urscript program to
        controller.

        Args:
            pos (float): Desired gripper position.
        """
        pos = clamp(pos, self.cfgs.EETOOL.OPEN_ANGLE,
                    self.cfgs.EETOOL.CLOSE_ANGLE)
        if not self._gazebo_sim:
            urscript = self._get_new_urscript()

            pos = int(pos * self.cfgs.EETOOL.POSITION_SCALING)

            urscript.set_gripper_position(pos)
            urscript.sleep(0.1)

            gripper_cmd = urscript()
        else:
            gripper_cmd = GripperCommandActionGoal()
            gripper_cmd.goal.command.position = pos

        self._pub_command.publish(gripper_cmd)
        time.sleep(1.0)
        if not self._gazebo_sim:
            self._get_current_pos_urscript()
Beispiel #4
0
 def grasp2(self, width, force):
     ''' width, epsilon_inner, epsilon_outer, speed, force '''
     grasp_goal = GripperCommandGoal()
     grasp_goal.command.position = float(width)
     grasp_goal.command.max_effort = float(force)
     grasp_msg = GripperCommandActionGoal(goal=grasp_goal)
     self.generic_grasp_client.publish(grasp_msg)
Beispiel #5
0
def gripper_closing(gripper_cmd_pub):
    # gripper_cmd_pub = rospy.Publisher(
    #     rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
    #     GripperCommandActionGoal, queue_size=10)
    gripper_cmd = GripperCommandActionGoal()
    gripper_cmd.goal.command.position = gripper_close_value  # sapien  # 0.6 # Gaezebo
    gripper_cmd.goal.command.max_effort = 90.0
    gripper_cmd_pub.publish(gripper_cmd)
    rospy.loginfo("Pub gripper_cmd for closing")
    gripper_block_until_near(gripper_close_value)
    def __init__(self):
        # get launch params
        self.button_img = rospy.get_param('~panel_img')
        self.pressed_button_img = rospy.get_param('~pressed_button_img')
        self.press_offset_x = float(rospy.get_param('~press_offset_x'))
        self.press_offset_y = float(rospy.get_param('~press_offset_y'))
        self.acc_certainty = float(rospy.get_param('~acc_certainty'))
        self.button_size = float(rospy.get_param('~button_size'))
        self.show_image = rospy.get_param('~show_image')
        self.show_status = rospy.get_param('~show_status')

        self.gripper_pub = rospy.Publisher("/gripper_controller/gripper_cmd/goal", GripperCommandActionGoal,
                                           queue_size=10)
        # for handling movement towards and pushing buttons
        self.pb = push_button()
        self.push_ready = 0
        self.counter = 0
        # dispatching for push_button
        self.move_control = [self.move_control0, self.move_control1, self.move_control2,
                             self.move_control3, self.move_control4, self.move_control5, self.move_control6,
                             self.move_control7, self.move_control8, self.move_control9, self.move_control10]
        self.status_str = ["searching button", "searching button", "align robot to button",
                           "move towards button", "align robot to button", "adjust height to button",
                           "pushing button", "lower robot", "check doors", "failed, trying again",
                           "going into elevator"]

        # move arm to 'button' position
        move_arm.target_move(0, 0, 0, "button")
        rospy.sleep(5)

        # close gripper
        gc = GripperCommandActionGoal()
        gc.goal.command.max_effort = 0.3
        self.gripper_pub.publish(gc)
        rospy.sleep(2)

        self.bridge = CvBridge()
        rospy.Subscriber("/kinect2/qhd/image_color_rect/compressed", CompressedImage, self.kinect_callback)
        rospy.Subscriber("/intel_cam/color/image_raw/compressed", CompressedImage, self.intel_callback)
        # args updated by Button_finder
        self.scale = 1
        self.detected = 0
        self.threshold = -1
        self.button_location = (-1, -1)
        self.button_height = -1
        self.button_width = -1

        self.max_scale = 1
        self.min_scale = 0.2
        self.final_align = 1
        self.align_offset = 50

        self.right_door = 0
        self.left_door = 0
def gripper():
    # create an empty control_msgs/GripperCommandActionGoal message
    msg = GripperCommandActionGoal()
    # position range is 0-1 (discreet, not sure why the data type is float)
    # 0 commands the gripper to close
    # 1 commands the gripper to open
    msg.goal.command.position = gripper_closing
    # effort indicates the maximum force which should be
    # exerted to open or close the gripper
    msg.goal.command.max_effort = gripper_effort
    # publish the message
    pub.publish(msg)
 def __init__(self):
     # self.interface = rospy.Publisher('gripper/gripper_cmd/goal', GripperCommandActionGoal, queue_size=10)
     # self.interface = rospy.Publisher('ur10/gripper/grasping', Bool, queue_size=1)
     self.rate1Hz = rospy.Rate(1)
     self.gripperCommandGoal = GripperCommandActionGoal()
     self.gripperCmd = GripperCommand()
     self.goal = GripperCommandGoal()
     self.goal.command = self.gripperCmd
     self.gripperCommandGoal.goal = self.goal
     self.msg = Bool()
     self.msg.data = True
     self.isOpen = True
Beispiel #9
0
    def execute_callback(self, goal):
        rospy.loginfo("Get clean task.")
        print(goal)
        ##### User code example starts #####
        # In the following, an example is provided on how to use the predefined software APIs
        # to get the target configuration, to control the robot, to set the actionlib result.
        # Example: load model.
        rospy.loginfo("Load models")
        for object_name in goal.object_list:
            object_model_dir = self.models_dir + '/' + object_name
            rospy.loginfo("Object model dir: " + object_model_dir)
        rospy.sleep(1.0)

        # Example: control ur5e by topic
        arm_cmd = JointTrajectory()
        arm_cmd.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', \
                               'elbow_joint', 'wrist_1_joint', \
                               'wrist_2_joint', 'wrist_3_joint']
        waypoint = JointTrajectoryPoint()
        waypoint.positions = [-0.68, -0.63, 0.69, -0.88, -0.53, -0.19]
        waypoint.time_from_start.secs = 2.0
        arm_cmd.points.append(waypoint)
        self.arm_cmd_pub.publish(arm_cmd)
        rospy.loginfo("Pub arm_cmd")
        rospy.sleep(1.0)

        # Example: control 2f85 by topic
        gripper_cmd = GripperCommandActionGoal()
        gripper_cmd.goal.command.position = 0.5
        gripper_cmd.goal.command.max_effort = 0.0
        self.gripper_cmd_pub.publish(gripper_cmd)
        rospy.loginfo("Pub gripper_cmd")
        rospy.sleep(1.0)

        # Example: set status "Aborted" and quit.
        if self.action_server.is_preempt_requested():
            self.result.status = "Aborted"
            self.action_server.set_aborted(self.result)
            return

        # Example: send feedback.
        self.feedback.text = "write_feedback_text_here"
        self.action_server.publish_feedback(self.feedback)
        rospy.loginfo("Pub feedback")
        rospy.sleep(1.0)

        # Example: set status "Finished" and quit.
        self.result.status = "Finished"
        rospy.loginfo("Done.")
        self.action_server.set_succeeded(self.result)
    def on_enter(self, userdata):
        msg = GripperCommandActionGoal()
        msg.goal.command.position = self.width
        msg.goal.command.max_effort = self.effort
        self._pub.publish(self._topic, msg)

        if not self._connected:
            (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f)
            if msg_topic == self._topic_f:
                msg_type = self._get_msg_from_path(msg_path)
                self._sub = ProxySubscriberCached({self._topic_f: msg_type})
                self._connected = True
                Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic_f)
            else:
                Logger.logwarn('Topic %s still not available, giving up.' % self._topic_f)
        self._sub.remove_last_msg(self._topic_f)
Beispiel #11
0
def gripper_closing_and_store_pose(gripper_cmd_pub, model_name,
                                   global_grasp_pose, grasp_id):
    close_value = gripper_close_value  # 0 -> 0.044
    # gripper_cmd_pub = rospy.Publisher(
    #     rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
    #     GripperCommandActionGoal, queue_size=10)

    # publish this so that gazebo can move accordingly
    # Example: control ur5e by topic
    gripper_cmd = GripperCommandActionGoal()
    gripper_cmd.goal.command.position = close_value  # sapien  # 0.6 # Gaezebo
    gripper_cmd.goal.command.max_effort = 0.0
    gripper_cmd_pub.publish(gripper_cmd)
    rospy.loginfo("Pub gripper_cmd for closing")
    rospy.sleep(1.0)
    # publish
    store_pose_based_on_gripper(global_grasp_pose, grasp_id)
Beispiel #12
0
def gripper_openning(gripper_cmd_pub):
    #gripper_cmd_pub = rospy.Publisher(
    #    rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
    #    GripperCommandActionGoal, queue_size=10)

    # real stage moveit does not know the current gripper status
    # # check if the current gripper is open, if not, then open it
    # state = robot.get_current_state().joint_state
    # # go over each name to find the gripper driver
    # for i in range(len(state.name)):
    #     if state.name[i] == gripper_driver_name:
    #         if np.abs(state.position[i] - gripper_open_value) <= 0.001:
    #             return  # consider as open, no need to open
    # # otherwise open

    gripper_cmd = GripperCommandActionGoal()
    gripper_cmd.goal.command.position = gripper_open_value
    gripper_cmd.goal.command.max_effort = 50.0
    gripper_cmd_pub.publish(gripper_cmd)
    rospy.loginfo("Pub gripper_cmd for openning")
    gripper_block_until_near(gripper_open_value)
    def execute(self, userdata):
        #open the arm slightly
        command = GripperCommandActionGoal()
        command.goal.command.position = 0.05
        self.publisher.publish(command)

        joint_values = arm_command.get_current_joint_values()
        joint_values[self.joint_number] = joint_values[
            self.joint_number] + self.wiggle_offset
        try:
            arm_command.set_joint_value_target(joint_values)
        except Exception as e:
            rospy.logerr('unable to set target position: %s' % (str(e)))
            return 'failed'

        error_code = arm_command.go(wait=self.blocking)

        if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
            return 'succeeded'
        else:
            rospy.logerr("Arm movement failed with error code: %d", error_code)
            return 'failed'
Beispiel #14
0
#!/usr/bin/env python

from __future__ import print_function


from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import GripperCommandActionGoal
import tf

import rospy

rospy.init_node('motion_planning_test',
                anonymous=True)
gripper_cmd_pub = rospy.Publisher(
    rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
    GripperCommandActionGoal, queue_size=10)

gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = -0.5
gripper_cmd.goal.command.max_effort = 0.0
gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd for openning")
Beispiel #15
0
 def send_gripper_cmd(self, pos):
     #rospy.loginfo("Send Gripper Command")
     lgmsg = GripperCommandActionGoal()
     lgmsg.header.seq = 0
     lgmsg.goal.command.position = 1 - pos / 10.
     self.pubg.publish(lgmsg)
Beispiel #16
0
print(kdl_kin.chain.getNrOfJoints())
q_targ = solver.DoubleVector(6)

# ik.solvePoseIk(initialPose, q_targ, p_targ, False)

posisions = [0 for i in range(len(joints))]
zeros = [0 for i in range(len(joints))]

posisions[0] = -0.5
posisions[1] = -1
posisions[2] = 1
posisions[3] = -1



gms = GripperCommandActionGoal()
goal = GripperCommand()
g = GripperCommandGoal()
goal.position=0.8
g.command = goal

gms.goal=g

# gp.publish(gms)

msg = JointTrajectory()
subMsg = JointTrajectoryPoint()
# print(msg)
# print(subMsg)

# print(type(zeros))