Beispiel #1
0
def createArmCmd():
    global cmdx, cmdy, cmdn, cmdd, cmds, cmde, cmdl, cmdb, cmdw

    #[  'ShP',  'ShR',  'ShY',  'ElP',  'FoY',  'WrR',  'WrP']
    ZERO_VECTOR = [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0]
    ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
    REACHING_UP = [0.0, 0.0, -1.5, 2.0, 0.0, 0.0, 0.0]
    #REACH_SWITCH = [-1.35, 0.9, 1.5, 0.0, 0.0, 0.0, 0.0]
    REACH_SWITCH = [-1.35, 0.95, 1.5, 0.0, 0.0, 0.0, 0.0]
    RETRACT = [-1.0, 0.0, 1.5, 1.5, 0.0, 0.0, 0.0]
    HOME_VECTOR = [-0.2, 1.2, 0.7, 1.5, 0.0, 0.0, 0.0]
    ARM_STRAIGHT = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    L_ARM_CLOSE = [-0.2, -1.4, 0.2, -1.5, 0.0, 0.0, 0.0]
    L_ARM_OUT = [-0.9, -0.4, 0.2, -1.5, 0.0, 0.0, 0.0]
    R_ARM_CLOSE = [-0.2, 1.4, 0.2, 1.5, 0.0, 0.0, 0.0]
    R_ARM_OUT = [-0.9, 0.4, 0.2, 0.6, 0.0, 0.0, 0.0]

    msgl = ArmTrajectoryRosMessage()
    msgl.robot_side = ArmTrajectoryRosMessage.LEFT
    msgl = appendTrajectoryPoint(msgl, 4.0, L_ARM_OUT)
    msgl = appendTrajectoryPoint(msgl, cmdl + 5.0, L_ARM_OUT)
    msgl = appendTrajectoryPoint(msgl, cmdl + 5.2, L_ARM_CLOSE)
    msgl.unique_id = -1

    msgr = ArmTrajectoryRosMessage()
    msgr.robot_side = ArmTrajectoryRosMessage.RIGHT
    msgr = appendTrajectoryPoint(msgr, 4.0, R_ARM_OUT)
    msgr = appendTrajectoryPoint(msgr, cmdl + 5.0, R_ARM_OUT)
    msgr = appendTrajectoryPoint(msgr, cmdl + 5.2, R_ARM_CLOSE)
    msgr.unique_id = -1

    return msgl, msgr, " arms linger for %6.3fs" % (cmdl)
Beispiel #2
0
 def sendRightArm(self, target):
     msg = ArmTrajectoryRosMessage()
     msg.robot_side = ArmTrajectoryRosMessage.RIGHT
     if target == "home":
         msg = self.appendTrajectoryPoint(msg, 2.0, HOME_POSITION)
     elif target == "button":
         msg = self.appendTrajectoryPoint(msg, 2.0, BUTTON_POSITION)
     elif target == "down":
         msg = self.appendTrajectoryPoint(msg, 2.0, DOWN_POSITION)
     elif target == "extend":
         msg = self.appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
     elif target == "tight":
         msg = self.appendTrajectoryPoint(msg, 2.0, IN_TIGHT)
     elif target == "back":
         msg = self.appendTrajectoryPoint(msg, 2.0, BACK_POSITION)
     else:
         #rospy.loginfo('Failed to send arm to ' + target + ' position')
         #return 1
         msg = self.appendTrajectoryPoint(msg, 2.0, target)
     msg.unique_id = -1
     if isinstance(target, basestring):
         strtarget = target
     else:
         strtarget = str(target)
     rospy.loginfo('sending right arm to ' + strtarget + ' position')
     self.publish(msg)
     return 0
Beispiel #3
0
    def pushButtonHardCode(self, in_msg):
        msg = ArmTrajectoryRosMessage()

        msg.robot_side = ArmTrajectoryRosMessage.RIGHT

        # ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
        # to understand this, generate a frame list; these joints are in that order
        # the first is shoulder roll; last is wrist roll
        # if invalid values are used, the message silently fails.

        #setup_vector1 = [+numpy.pi/6, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]
        setup_vector2 = [+numpy.pi / 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        ##button_push = [-1.151, .930, 1.110, 0.624, 0.0, 0.0, 0.0]
        button_push = [-1.151, .75, 0.7, 0.624, 0.0, 0.0, 0.0]
        rest_state = [0.0, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]

        # The numbers represent time by which this motion should be completed
        # speeding up the part before button_push vector has an advantage, the others
        # not (because we are waiting for the door to open anyway
        #msg = self.appendArmTrajectoryPoint(msg, 0.5, setup_vector1)
        # msg = self.appendArmTrajectoryPoint(msg, 1.0, setup_vector2)
        # msg = self.appendArmTrajectoryPoint(msg, 1.5, button_push)
        # msg = self.appendArmTrajectoryPoint(msg, 2.0, setup_vector2)
        # msg = self.appendArmTrajectoryPoint(msg, 2.5, rest_state)

        msg = self.appendArmTrajectoryPoint(msg, 0.5, setup_vector2)
        msg = self.appendArmTrajectoryPoint(msg, 1.0, button_push)
        msg = self.appendArmTrajectoryPoint(msg, 1.5, rest_state)

        msg.execution_mode = ArmTrajectoryRosMessage.OVERRIDE
        msg.unique_id = rospy.Time.now().nsecs

        rospy.loginfo('publishing right trajectory')
        self.armTrajectoryPublisher.publish(msg)
Beispiel #4
0
 def sendLeftArm(self, target):
     msg = ArmTrajectoryRosMessage()
     msg.robot_side = ArmTrajectoryRosMessage.LEFT
     if target == "home":
         msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(HOME_POSITION))
     elif target == "button":
         msg = self.appendTrajectoryPoint(msg, 2.0,
                                          makeLeft(BUTTON_POSITION))
     elif target == "down":
         msg = self.appendTrajectoryPoint(
             msg, 2.0, makeLeft(DOWN_POSITION)
         )  # [0.0, -1.3, 0.0, 0.0, 0.0, 0.0, 0.0]) # DOWN_POSITION)
     elif target == "extend":
         msg = self.appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
     elif target == "tight":
         msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(IN_TIGHT))
     else:
         #rospy.loginfo('Failed to send arm to ' + target + ' position')
         #return 1
         msg = self.appendTrajectoryPoint(msg, 2.0, makeLeft(target))
     msg.unique_id = -1
     if isinstance(target, basestring):
         strtarget = target
     else:
         strtarget = str(target)
     rospy.loginfo('sending left arm to ' + strtarget + ' position')
     self.publish(msg)
     return 0
Beispiel #5
0
def ArmMsgMaker(time, side, POSITION):
    msg = ArmTrajectoryRosMessage()
    msg.execution_mode = 0
    msg.robot_side = side
    msg.unique_id = -1
    msg = appendTrajectoryPoint(msg, time, POSITION)
    armTrajectoryPublisher.publish(msg)
Beispiel #6
0
def createArmCmd():
    global cmdx, cmdy, cmdn, cmdd, cmds, cmde, cmdl

    #[  'ShP',  'ShR',  'ShY',  'ElP',  'FoY',  'WrR',  'WrP']
    ZERO_VECTOR = [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0]
    ELBOW_BENT_UP = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0]
    REACHING_UP = [0.0, 0.0, -1.5, 2.0, 0.0, 0.0, 0.0]
    #REACH_SWITCH = [-1.35, 0.9, 1.5, 0.0, 0.0, 0.0, 0.0]
    REACH_SWITCH = [-1.35, 0.95, 1.5, 0.0, 0.0, 0.0, 0.0]
    RETRACT = [-1.0, 0.0, 1.5, 1.5, 0.0, 0.0, 0.0]
    HOME_VECTOR = [-0.2, 1.2, 0.7, 1.5, 0.0, 0.0, 0.0]
    ARM_STRAIGHT = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    msg = appendTrajectoryPoint(msg, 2.0, ARM_STRAIGHT)
    msg = appendTrajectoryPoint(msg, 3.0, REACHING_UP)
    msg = appendTrajectoryPoint(msg, 4.0, REACH_SWITCH)
    msg = appendTrajectoryPoint(msg, cmdl + 5.0, REACH_SWITCH)
    msg = appendTrajectoryPoint(msg, cmdl + 5.5, RETRACT)
    msg = appendTrajectoryPoint(msg, cmdl + 6.0, HOME_VECTOR)

    msg.unique_id = -1
    return msg, " arm linger for %6.3fs" % (cmdl)
Beispiel #7
0
def sendLeftArmDown():
    global uid
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.LEFT

    msg = appendTrajectoryPoint(msg, 0.5, LOWER_ARM_LF)

    msg.unique_id = time.time()

    rospy.loginfo('publishing left dn trajectory')
    armTrajectoryPublisher.publish(msg)
def sendTrajectory(joint_waypoints):
    msgs = {}
    msgs["left"] = copy.deepcopy(ArmTrajectoryRosMessage())
    msgs["right"] = copy.deepcopy(ArmTrajectoryRosMessage())
    msgs["left"].robot_side = ArmTrajectoryRosMessage.LEFT
    msgs["right"].robot_side = ArmTrajectoryRosMessage.RIGHT
    # for each set of joint states
    for y in joint_waypoints:
        # first value is time duration
        time = float(y[0])
        side = str(y[1]).lower()
        # subsequent values are desired joint positions
        commandPosition = array([float(x) for x in y[2].split()])
        if side in msgs.keys():
            msgs[side] = appendTrajectoryPoint(msgs[side], time,
                                               commandPosition)
            msgs[side].unique_id = -1
    for side in msgs.keys():
        if msgs[side].unique_id == -1:
            rospy.loginfo('publishing %s trajectory' % side)
            armTrajectoryPublisher.publish(msgs[side])
Beispiel #9
0
def sendLeftArmTrajectory():
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.LEFT

    msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
    msg = appendTrajectoryPoint(msg, 4.0, LEFT_HOME)

    msg.unique_id = -1

    rospy.loginfo('publishing left trajectory')
    armTrajectoryPublisher.publish(msg)
Beispiel #10
0
def sendRightArmDown():
    global uid
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    msg = appendTrajectoryPoint(msg, 0.5, LOWER_ARM)

    uid +=1
    msg.unique_id = time.time()

    rospy.loginfo('publishing right dn trajectory')
    armTrajectoryPublisher.publish(msg)
Beispiel #11
0
def sendRightArmTrajectory():
    msg = ArmTrajectoryRosMessage()
    
    msg.robot_side = ArmTrajectoryRosMessage.RIGHT
    
    msg = appendTrajectoryPoint(msg, 3.0, ZERO_VECTOR)
    msg = appendTrajectoryPoint(msg, 4.0, ELBOW_BENT_UP)
    msg = appendTrajectoryPoint(msg, 5.0, ZERO_VECTOR)

    msg.unique_id = -1

    rospy.loginfo('publishing right trajectory')
    armTrajectoryPublisher.publish(msg)
Beispiel #12
0
def make_arm_trajectory(side, mode, time, joints, previous_msg = None):
    trajectories = []
    for x in joints:
        trajectories.append(makeJoint(time, x))

    msg = ArmTrajectoryRosMessage()
    msg.robot_side = side
    msg.joint_trajectory_messages = trajectories
    msg.execution_mode = mode
    msg.unique_id = uid()
    if previous_msg is not None:
        msg.previous_message_id = previous_msg

    return msg
Beispiel #13
0
    def returnHome(self, in_msg):
        msg = ArmTrajectoryRosMessage()

        msg.robot_side = in_msg.data

        if self.LEFT == in_msg.data:
            rest_state = [0.0, -1.5, 0.0, -2.0, 0.0, 0.0, 0.0]
        else:
            rest_state = [0.0, 1.5, 0.0, 2.0, 0.0, 0.0, 0.0]

        msg = self.appendArmTrajectoryPoint(msg, 1.0, rest_state)

        msg.execution_mode = ArmTrajectoryRosMessage.OVERRIDE
        msg.unique_id = rospy.Time.now().nsecs

        rospy.loginfo('publishing right trajectory')
        self.armTrajectoryPublisher.publish(msg)
Beispiel #14
0
    def process_arm_command(self, binding, ch):
        msg = ArmTrajectoryRosMessage()
        msg.unique_id = -1

        side = binding['side']
        if side == 'left':
            msg.robot_side = ArmTrajectoryRosMessage.LEFT
        elif side == 'right':
            msg.robot_side = ArmTrajectoryRosMessage.RIGHT
        else:
            assert False, "Unknown arm side '%s'" % side

        self._update_joint_values('%s arm' % side, binding, ch)
        self._append_trajectory_point_1d(msg, 1.0,
                                         self.joint_values['%s arm' % side])

        self.arm_publisher.publish(msg)
Beispiel #15
0
def sendRightArmTrajectory():
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
    #    msg = appendTrajectoryPoint(msg, 3.0, ELBOW_BENT_UP)
    #    msg = appendTrajectoryPoint(msg, 1.0, ZERO_VECTOR)
    #    msg = appendTrajectoryPoint(msg, 4.0, ZERO_VECTOR)

    #    msg = appendTrajectoryPoint(msg, 2.0, BUTTON_PRESS)
    msg.unique_id = -1

    rospy.loginfo('publishing right trajectory')
    armTrajectoryPublisher.publish(msg)
    time.sleep(0.5)
    msg.robot_side = ArmTrajectoryRosMessage.LEFT
    armTrajectoryPublisher.publish(msg)
Beispiel #16
0
    def process_arm_command(self, binding, data):
        msg = ArmTrajectoryRosMessage()
        msg.unique_id = -1

        side = binding['side']
        if side == 'left':
            msg.robot_side = ArmTrajectoryRosMessage.LEFT
        elif side == 'right':
            msg.robot_side = ArmTrajectoryRosMessage.RIGHT
        else:
            assert False, "Unknown arm side '%s'" % side

        self._update_joint_values_d('%s arm' % side, binding, data)
        if binding['joint_index'] == 'reset': time = 1.0
        else: time = data.get_sibling("tim").get_rosval()
        self._append_trajectory_point_1d(msg, time,
                                         self.joint_values['%s arm' % side])

        self.arm_publisher.publish(msg)
Beispiel #17
0
def sendRightArmTrajectory():
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    msg = appendTrajectoryPoint(msg, 2.0, ARM_STRAIGHT)
    #msg = appendTrajectoryPoint(msg, 4.0, ELBOW_BENT_UP)
    #msg = appendTrajectoryPoint(msg, 8.0, REACHING_UP)
    #msg = appendTrajectoryPoint(msg, 1.0, RETRACT)
    #msg = appendTrajectoryPoint(msg, 4.5, RETRACT)
    #msg = appendTrajectoryPoint(msg, 2.0, REACH_SWITCH)
    msg = appendTrajectoryPoint(msg, 3.0, REACHING_UP)
    msg = appendTrajectoryPoint(msg, 4.0, REACH_SWITCH)
    msg = appendTrajectoryPoint(msg, dwell + 3.0, REACH_SWITCH)
    msg = appendTrajectoryPoint(msg, dwell + 3.5, RETRACT)
    msg = appendTrajectoryPoint(msg, dwell + 4.0, HOME_VECTOR)

    msg.unique_id = -1

    rospy.loginfo('publishing right trajectory')
    armTrajectoryPublisher.publish(msg)
Beispiel #18
0
def sendRightArmTrajectory():
    global uid
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    #msg = appendTrajectoryPoint(msg, 2.0, ZERO_VECTOR)
    #msg = appendTrajectoryPoint(msg, 2.0, ELBOW_BENT_UP)
    msg = appendTrajectoryPoint(msg, 0.5, ARM_UP0)
    time.sleep(2)
    msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
    time.sleep(2)
    #msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
    #time.sleep(2)
    #msg = appendTrajectoryPoint(msg, 1.0, PUNCH)

    uid -= 1
    msg.unique_id = uid
    print uid
    rospy.loginfo('publishing right trajectory')
    armTrajectoryPublisher.publish(msg)
Beispiel #19
0
def sendRightArmTrajectory():
    msg = ArmTrajectoryRosMessage()

    msg.robot_side = ArmTrajectoryRosMessage.RIGHT

    test = inverseKinematics(chi)
    test1, test2, test3, test4 = test
    test1 = test1[0]
    test2 = test2[0]
    test3 = test3[0]
    test4 = test4[0]
    test = [test1, test2, test3, test4]

    print test

    ZERO_VECTOR = [test[0], test[1], test[2], test[3], 0, 0, 0]

    msg = appendTrajectoryPoint(msg, 4.0, ZERO_VECTOR)

    msg.unique_id = -1

    rospy.loginfo('publishing right trajectory')
    armTrajectoryPublisher.publish(msg)
def sendRightArmTrajectory():

    with open("tracefile_l.txt") as infile:
        msg = ArmTrajectoryRosMessage()

        msg.robot_side = ArmTrajectoryRosMessage.LEFT
        msg.unique_id = -1
        msg.execution_mode = 0
        for line in infile:
            time = 2.0
            # if i == 1:

            #     #msg = appendTrajectoryPoint(msg, time, [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0])

            #     time = time + 1.0
            # else:
            #     msg = ArmTrajectoryRosMessage()

            #     msg.robot_side = ArmTrajectoryRosMessage.LEFT
            #     msg.unique_id = -1
            #     msg.execution_mode = 0
            #     msg.previous_message_id = 0

            elems = line.split(' ')
            traj = []
            traj.append(float(elems[1]))
            traj.append(-float(elems[2]))
            traj.append(float(elems[3]))
            traj.append(float(elems[4]))
            traj.append(float(elems[5]))
            traj.append(float(elems[6]))
            traj.append(float(elems[7]))
            #print(traj)
            """
            left elbow pitch issues
            """
            if traj[3] < -2.174:
                traj[3] = -2.174
            if traj[3] > 0.12:
                traj[3] = 0.12
            """
            left shoulder roll issues
            """
            if traj[1] < -1.519:
                traj[1] = -1.519
            if traj[1] > 1.266:
                traj[1] = 1.266
            """
            left shoudler yaw issues:
            """
            if traj[2] < -3.1:
                traj[2] = -3.1
            if traj[2] > 2.18:
                traj[2] = 2.18
            """
            left shoudler pitch issues:
            """

            if traj[0] < -2.85:
                traj[0] = -2.85
            if traj[0] > 2.0:
                traj[0] = 2.0
            """
            left forearm yaw issues
            """
            if traj[4] < -2.019:
                traj[4] = -2.019
            if traj[4] > 3.14:
                traj[4] = 3.14

            msg = appendTrajectoryPoint(msg, time, traj)
            time = time + 500.0
        attempts = 0
        while attempts < 5:
            try:
                armTrajectoryPublisher.publish(msg)
                time.sleep(1)
                print('publised left message')
                i = i + 1
                attempts = 6
            except:
                print("msg left failed, ignoring")
                attempts = attempts + 1

    with open("tracefile_r.txt") as infile:
        msg = ArmTrajectoryRosMessage()

        msg.robot_side = ArmTrajectoryRosMessage.RIGHT
        msg.unique_id = -1
        msg.execution_mode = 0
        for line in infile:
            time = 2.0
            # if i == 1:

            #     #msg = appendTrajectoryPoint(msg, time, [0.0, -1.0, 2.0, 1.0, 0.0, 0.0, 0.0])

            #     time = time + 1.0
            # else:
            #     msg = ArmTrajectoryRosMessage()

            #     msg.robot_side = ArmTrajectoryRosMessage.LEFT
            #     msg.unique_id = -1
            #     msg.execution_mode = 0
            #     msg.previous_message_id = 0

            elems = line.split(' ')
            traj = []
            traj.append(float(elems[1]))
            traj.append(float(elems[2]))
            traj.append(float(elems[3]))
            traj.append(float(elems[4]))
            traj.append(float(elems[5]))
            traj.append(float(elems[6]))
            traj.append(float(elems[7]))
            #print(traj)
            """
            right elbow pitch issues
            """
            if traj[3] < -0.12:
                traj[3] = -0.12
            if traj[3] > 2.174:
                traj[3] = 2.174
            """
            right shoulder roll issues
            """
            if traj[1] < -1.226:
                traj[1] = -1.226
            if traj[1] > 1.519:
                traj[1] = 1.519
            """
            right shoudler yaw issues:
            """
            if traj[2] < -3.1:
                traj[2] = -3.1
            if traj[2] > 2.18:
                traj[2] = 2.18
            """
            right forearm yaw issues
            """
            if traj[4] < -2.019:
                traj[4] = -2.019
            if traj[4] > 3.14:
                traj[4] = 3.14
            """
            right sh pitch issues
            """
            if traj[0] < -2.85:
                traj[0] = -2.85
            if traj[0] > 2.0:
                traj[0] = 2.0

            msg = appendTrajectoryPoint(msg, time, traj)
            time = time + 500.0
        attempts = 0
        while attempts < 3:
            try:
                armTrajectoryPublisher.publish(msg)
                time.sleep(1)
                print('publised right message')
                i = i + 1
                attempts = 6
            except:
                print("msg right failed, ignoring")
                attempts = attempts + 1