Example #1
0
def vel_test():
    jointCmdPub = rospy.Publisher("/robot/limb/right/joint_command",
                                  JointCommand,
                                  queue_size=100)
    rospy.init_node('vel_test')
    rate = rospy.Rate(1000)
    i = 0
    while not rospy.is_shutdown():
        jointCmd = JointCommand()
        jointCmd.mode = JointCommand.VELOCITY_MODE
        jointCmd.names.append('right_s0')
        jointCmd.names.append('right_s1')
        jointCmd.names.append('right_e0')
        jointCmd.names.append('right_e1')
        jointCmd.names.append('right_w0')
        jointCmd.names.append('right_w1')
        jointCmd.names.append('right_w2')
        if i < 4000:
            vel = i / 1000.0
            jointCmd.command.append(vel)
        else:
            jointCmd.command.append(0.0)
        i = i + 1
        jointCmdPub.publish(jointCmd)
        rospy.loginfo("%s", jointCmd)
        rospy.set_param('time', i)
        rate.sleep()
Example #2
0
def main():

    rospy.init_node('jointPositionPublisherExample', anonymous=True)
    pub_joint_cmd = rospy.Publisher('/robot/limb/left/joint_command',
                                    JointCommand,
                                    queue_size=10)

    # rospy.Subscriber('/robot/limb/' + 'left' +'/gravity_compensation_torques', SEAJointState, force_sensor_callback)

    limb = baxter_interface.Limb("left")
    limb.set_joint_position_speed(0.5)
    limb.set_command_timeout(120.0)

    cmdMsg = JointCommand()
    cmdMsg.mode = JointCommand.POSITION_MODE

    #       q untucked, considered to be the initial q: 'left_w0': 0.669996718047412, 'left_w1': 1.030008750129423, 'left_w2': -0.4999996908801263, 'left_e0': -1.1899720096239292, 'left_e1': 1.940029476507764, 'left_s0': -0.08000000085054282, 'left_s1': -0.9999845808672081

    q = limb.joint_angles()  #joint configuration
    q['left_w2'] = 3
    cmdMsg.command = q.values()
    cmdMsg.names = q.keys()

    print(cmdMsg)
    pub_joint_cmd.publish(cmdMsg)

    #positions = dict(zip(self.limb.joint_names(), theta))

    rate = rospy.Rate(2)

    while not rospy.is_shutdown():
        printCurJointPos(limb)
        pub_joint_cmd.publish(cmdMsg)

        rate.sleep()
def send_to_joint_vals(q, arm='right'):
    pub_joint_cmd = rospy.Publisher('/robot/limb/' + arm + '/joint_command',
                                    JointCommand)
    command_msg = JointCommand()
    command_msg.names = generate_joint_names_for_arm(arm)
    command_msg.command = q
    command_msg.mode = JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions = rospy.wait_for_message("/robot/joint_states", JointState)
    qc = joint_positions.position[9:16]
    qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])

    print('q,qc')
    print(q)
    print(qc)
    while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(
            q, qc)) > 0.07:
        pub_joint_cmd.publish(
            command_msg)  # sends the commanded joint values to Baxter
        control_rate.sleep()
        joint_positions = rospy.wait_for_message("/robot/joint_states",
                                                 JointState)
        qc = (joint_positions.position[9:16])
        qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
        print "joint error = ", numpy.linalg.norm(numpy.subtract(q, qc))
        print((q, qc))
    print("In home pose")
    return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
Example #4
0
    def followTrajectoryFromJointVelocity(self,
                                          trajectory,
                                          stop=True,
                                          limb='left'):
        if limb == 'left':
            joint_names = self._left_limb.joint_names()
        else:
            joint_names = self._right_limb.joint_names()
        topic = 'robot/limb/%s/joint_command' % limb
        pub = rospy.Publisher(topic, JointCommand, queue_size=10)
        rate = rospy.Rate(50.)

        VELOCITY_MODE = 2
        template = JointCommand()
        template.mode = VELOCITY_MODE
        template.names.extend(joint_names)
        for pair in trajectory:
            print pair
            velocities, delta = pair
            msg = copy.deepcopy(template)
            msg.command.extend(velocities)
            pub.publish(msg)
            rospy.sleep(delta)

        if stop:
            velocities = [0.0] * len(joint_names)
            cmd = dict(zip(joint_names, velocities))
            if limb == 'left':
                self._left_limb.set_joint_velocities(cmd)
            else:
                self._right_limb.set_joint_velocities(cmd)
def send_to_joint_vals(q, arm='right'):
    pub_joint_cmd=rospy.Publisher('/robot/limb/' + arm + '/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names = generate_joint_names_for_arm(arm)
    command_msg.command=q
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
    qc = joint_positions.position[9:16]
    qc = ([qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]])

    print('q,qc')
    print(q)
    print(qc)
    while not rospy.is_shutdown() and numpy.linalg.norm(numpy.subtract(q,qc))> 0.07:
        pub_joint_cmd.publish(command_msg)    # sends the commanded joint values to Baxter
        control_rate.sleep()
        joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
        qc = (joint_positions.position[9:16])
        qc = (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
        print "joint error = ", numpy.linalg.norm(numpy.subtract(q,qc))
        print((q,qc))
    print("In home pose")
    return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])
def vel_test(q_f_dot, t_dur, t_spent):
    global CONST_FLAG
    global STOP_FLAG

    jointCmdPub = rospy.Publisher("/robot/limb/right/joint_command", JointCommand, queue_size = 100)
    # rospy.init_node('vel_test')
    rate_val = 100
    rate = rospy.Rate(rate_val)
    i = 0

    print("\n")
    print(t_dur)
    print(t_spent)
    t_dur_sec = float(t_dur.data.secs) + float(t_dur.data.nsecs)/np.power(10,9)
    t = float(t_spent)    
    print("----------------")
    print(t_dur_sec)
    print(t_spent)
    print(t)
    print("\n")

    STOP_FLAG = 0
    while not rospy.is_shutdown() and not STOP_FLAG:
      jointCmd = JointCommand()
      jointCmd.mode = JointCommand.VELOCITY_MODE

      # print("\n\n\n")
      # print(t_dur)
      # print(t_spent)
      # t_dur_sec = float(t_dur.data.secs) + float(t_dur.data.nsecs)/np.power(10,9)
      # t = float((t_dur_sec - t_spent))    
      # print("----------------")
      # print(t_dur_sec)
      # print(t_spent)
      # print(t)
      # print(t + (1/rate_val)
      # print("\n\n\n")
      t = float(t) + (1.0/rate_val)
      # print(t)

      # publish vels until we are past the duration we set
      if t < t_dur_sec:
        for i in range(len(q_f_dot)):
          jointCmd.names.append(TRAJ_COM.names[i])
          jointCmd.command.append(q_f_dot[i])
      else: 
        STOP_FLAG = 1
        CONST_FLAG = 0;

        for i in range(len(q_f_dot)):
          jointCmd.names.append(TRAJ_COM.names[i])
          jointCmd.command.append(0)
        rospy.loginfo("STOP FLAG WAS SET, DURATION WAS UP!")

      # TrajectoryComamnd. = Duration
      jointCmdPub.publish(jointCmd)
      # rospy.loginfo("%s",jointCmd)
      # rospy.loginfo(q_f_dot)
      rate.sleep()
def runExperiment(Kp, Kd, Ki, commandHz):
    bc = BaxterCache()

    print("Resetting robot")
    tucker = baxter_tools.Tuck(False)
    rospy.on_shutdown(tucker.clean_shutdown)
    tucker.supervised_tuck()
    rospy.sleep(1)

    controlTopic = "/robot/limb/right/joint_command"
    pub = rospy.Publisher(controlTopic, JointCommand, queue_size=20)
    messageObj = JointCommand()
    messageObj.mode = JointCommand.TORQUE_MODE
    messageObj.names = joint_names

    rate = rospy.Rate(commandHz)

    print("Waiting for first joint_state message")
    while bc.joints is None:
        rate.sleep()
    print("First joint_state message acquired")

    print("Waiting for first target_loc message")
    while bc.get_desired_joints_and_vel() is None:
        rate.sleep()
    print("First target_loc message acquired")
    time.sleep(1)

    cumulativeJointError = np.zeros(bc.joints.shape)
    fileName = "results/Results%s.csv" % ("Interp"
                                          if bc.trackFuture else "Naive")
    print("logging to %s" % fileName)
    with StateLogger(fileName) as logger:
        it = 0
        maxIt = 2000 * commandHz
        while not rospy.is_shutdown():  # and it < maxIt:
            des = bc.get_desired_joints_and_vel()
            if des is None or des[0] is None:
                continue
            desired_joints = des[0]
            desired_dot = des[1]
            # compute the current joint angles
            err = bc.joints - desired_joints
            # note this needs to be scaled by freq of messages
            cumulativeJointError += err / 1000 * 2
            err_dot = bc.joints_dot - desired_dot
            # log at 40 hz
            if it % int(max(1, math.floor(commandHz / 30))) == 0:
                if it % 100 == 0:
                    print(it)
                logger.log([it, rospy.get_rostime().to_sec()] +
                           bc.end_eff_state + bc.truth)
            it += 1
            torques = compute_torque(err, err_dot, cumulativeJointError, Kp,
                                     Kd, Ki)
            messageObj.command = [float(torque) for torque in torques.tolist()]
            pub.publish(messageObj)
            rate.sleep()
def pubTruePos(pos_f, pub):
    jointCmd = JointCommand()
    jointCmd.mode = JointCommand.POSITION_MODE
    for i in range(len(pos_f)):
        jointCmd.names.append(TRAJ_CMD.names[i])
        jointCmd.command.append(pos_f[i])

    # publish message
    pub.publish(jointCmd)
Example #9
0
    def _send_pos_command(self, pos):
        self._try_enable()

        command = JointCommand()
        command.mode = JointCommand.POSITION_MODE
        command.names = self._limb.joint_names()
        #command.position = pos
        command.command = pos
        self._cmd_publisher.publish(command)
Example #10
0
    def __init__(self):
        # find out the absolute path of model file
        fname = rospy.get_param('~file')

        # create publisher and message instances
        self.leftCommand = JointCommand()
        self.rightCommand = JointCommand()
        self.leftPub = rospy.Publisher('/robot/limb/left/joint_command',
                                       JointCommand, tcp_nodelay=True, queue_size=1)
        self.rightPub = rospy.Publisher('/robot/limb/right/joint_command',
                                       JointCommand, tcp_nodelay=True, queue_size=1)

        # load bgplvm model from pickle file
        self.model = pickle.load(open(fname,'rb'))

        # parameters for doing latent function inference
        self.qDim = self.model.X.mean.shape[1]

        # create a joint_states publisher
        self.statePub = rospy.Publisher('joint_states', JointState, latch=True,
                                        queue_size=3)

        # variables for generating motion
        self.xMove = 0.0
        self.yMove = 0.0
        self.startMotion = False

        # visualize the bgplvm latent space
        self.maxPoints = MAXPOINTS
        self.plotVariance = True
        scales = self.model.kern.input_sensitivity(summarize=False)
        self.plotIndices = np.argsort(scales)[-2:]
        self.title = 'Latent Space Visualization'

        # variables for real-time plotting
        self.testHandle = None
        self.pointerColor = GREEN
        self.pointerHandle = None
        self.testData = np.empty((0,2))

        self.ax = self.plotLatent()
        xmin, xmax = self.ax.get_xlim()
        ymin, ymax = self.ax.get_ylim()


        self.textHandle = plt.text(xmax/2, ymin+0.3, 'Play Mode: OFF',
                                   bbox={'facecolor':'green', 'alpha':0.5, 'pad':10})

        # connect the cursor class
        plt.connect('button_press_event',self.mouseClick)
        plt.connect('motion_notify_event', self.mouseMove)
        plt.show()

        raw_input('Press enter to finish')

        return
    def moveVelocity(self,
                     joint_values_target,
                     speed,
                     thresh,
                     is_printing=False):
        """
    Velocity controller for Baxter arm movements.
    """
        max_iterations = 5000
        step_size = 2.0

        for i in range(0, max_iterations):
            joint_error = numpy.array(self.joint_values)
            joint_velocity_command = joint_error

            # calculate joint error, error magnitude, and command velocity
            joint_error = joint_values_target - joint_error
            joint_velocity_command = joint_error * step_size
            error_magnitude = numpy.linalg.norm(joint_error)
            joint_velocity_command_magnitude = error_magnitude * step_size

            # if command velocity magnitude exceeds speed, scale it back to speed
            if joint_velocity_command_magnitude > speed:
                joint_velocity_command = speed * joint_velocity_command / joint_velocity_command_magnitude

            # send velocity command to joints
            values_out = [
                joint_velocity_command[2], joint_velocity_command[3],
                joint_velocity_command[0], joint_velocity_command[1],
                joint_velocity_command[4], joint_velocity_command[5],
                joint_velocity_command[6]
            ]
            self.pub.publish(
                JointCommand(JointCommand.VELOCITY_MODE, values_out,
                             self.joint_names))

            if is_printing:
                print "iteration:", i, "error_magnitude: ", error_magnitude

            # break loop if close enough to target joint values
            if error_magnitude < thresh:
                break

            rospy.sleep(0.001)

        # need to reset to position mode; o.w. the robot will disable itself
        values_out = [
            joint_values_target[2], joint_values_target[3],
            joint_values_target[0], joint_values_target[1],
            joint_values_target[4], joint_values_target[5],
            joint_values_target[6]
        ]
        self.pub.publish(
            JointCommand(JointCommand.POSITION_MODE, values_out,
                         self.joint_names))
def pubVels(q_f_dot, pub):

    # set up joint command msg
    jointCmd = JointCommand()
    jointCmd.mode = JointCommand.VELOCITY_MODE
    for i in range(len(q_f_dot)):
        jointCmd.names.append(TRAJ_CMD.names[i])
        jointCmd.command.append(q_f_dot[i])

    # publish message
    pub.publish(jointCmd)
def read_file(file):
    global joint_command_publisher
    global g_limb
    global g_left_gripper
    global g_endpoint_state

    f=open(file,"r")
    lines=f.readlines()
    RATE=0.1
    cmd=JointCommand()
    cmd.mode=1
    g_left_gripper.open()
    names=["left_s0","left_s1","left_e0","left_e1","left_w0","left_w1","left_w2"]
    cmd.names=names
    print cmd
    joint_str=lines[0].rstrip().split(',')
    joints=[float(joint) for joint in joint_str]
    limb_pos=dict(zip(names,joints[0:7]))
    is_grasping=is_grasp(joints[7])
    
    if(is_grasping):
        g_left_gripper.close()
    else:
        g_left_gripper.open()

    g_limb.move_to_joint_positions(limb_pos)
    
    i=1
    
    while i<len(lines) and not rospy.is_shutdown():
        line=lines[i]
        
        joint_str=line.rstrip().split(',')
        joints=[float(joint) for joint in joint_str]
        if is_grasping != is_grasp(joints[7]):
            if is_grasp(joints[7]):
                
                g_left_gripper.close()
                is_grasping=True
            else:
                g_left_gripper.open()

        cmd.command=joints[0:7]
        # print joints[0:7]
        print i
        joint_command_publisher.publish(cmd)
        time.sleep(RATE)
        
        i=i+1
def publishIKdata(joint_vals, limb):
    # available control modes:
    # POSITION_MODE=1
    # VELOCITY_MODE=2
    # TORQUE_MODE=3
    # RAW_POSITION_MODE=4
    msg = JointCommand()
    msg.names = joint_vals.keys()
    msg.command = joint_vals.values()
    msg.mode = JointCommand.POSITION_MODE

    if limb == 'left':
        pub_left.publish(msg)
    if limb == 'right':
        pub_right.publish(msg)
Example #15
0
    def followTrajectory(self, traj, startSpeed=0.75, endSpeed=0.25, startThresh=0.30, \
      endThresh=0.10, forceThresh=float('inf')):
        '''TODO'''

        if not self.isMoving:
            return

        for i in xrange(len(traj)):
            if i == len(traj) - 1:
                self.moveVelocity(traj[i],
                                  endSpeed,
                                  endThresh,
                                  forceThresh=forceThresh)
            if i == len(traj) - 2:
                self.moveVelocity(traj[i],
                                  0.5 * startSpeed + 0.5 * endSpeed,
                                  startThresh,
                                  forceThresh=forceThresh)
            else:
                self.moveVelocity(traj[i],
                                  startSpeed,
                                  startThresh,
                                  forceThresh=forceThresh)

        # send a zero velocity command at the end of the trajectory
        self.pub.publish(
            JointCommand(JointCommand.VELOCITY_MODE, [0] * 7,
                         self.joint_names))
Example #16
0
    def __init__(self, limb):
        """
        Constructor.

        @type limb: str
        @param limb: limb to interface
        """
        self.name = limb
        self._joint_angle = dict()
        self._joint_velocity = dict()
        self._joint_effort = dict()
        self._cartesian_pose = dict()
        self._cartesian_velocity = dict()
        self._cartesian_effort = dict()

        self._joint_names = {
            'left': [
                'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
                'left_w1', 'left_w2'
            ],
            'right': [
                'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
                'right_w1', 'right_w2'
            ]
        }

        ns = '/robot/limb/' + limb + '/'

        self._command_msg = JointCommand()

        self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
                                                Float64,
                                                latch=True)

        self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
                                              JointCommand,
                                              tcp_nodelay=True)

        self._pub_joint_cmd_timeout = rospy.Publisher(ns +
                                                      'joint_command_timeout',
                                                      Float64,
                                                      latch=True)

        _cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
                                                EndpointState,
                                                self._on_endpoint_states,
                                                queue_size=1,
                                                tcp_nodelay=True)

        joint_state_topic = 'robot/joint_states'
        _joint_state_sub = rospy.Subscriber(joint_state_topic,
                                            JointState,
                                            self._on_joint_states,
                                            queue_size=1,
                                            tcp_nodelay=True)

        err_msg = ("%s limb init failed to get current joint_states "
                   "from %s") % (self.name.capitalize(), joint_state_topic)
        baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
                                 timeout_msg=err_msg)
Example #17
0
def left_pub(commands, names):
    global left_command_pub
    """
    This function sets your speed for the publisher. Use this function to implement your ideas.
    :params:
    x:This is the velocity along x-axis in m/sec. Limits: [-0.3,0.3]
    y:This is the velocity along y-axis in m/sec. Limits: [-0.3,0.3]
    wz: This is the angular velocity for turning (yaw) in rad/sec. Limits: [-1,1]
    :return: Returns a speed message
    """

    joint_cmd = JointCommand()
    joint_cmd.mode = 1
    joint_cmd.command = commands
    joint_cmd.names = names

    left_command_pub.publish(joint_cmd)
def getpose():
    pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names=['right_s0', 'right_s1', 'right_e0', 'right_e1',  'right_w0', 'right_w1', 'right_w2']
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions=rospy.wait_for_message("/robot/joint_states",JointState)
    qc = (joint_positions.position[9:16])
    angles = [qc[2],qc[3],qc[0],qc[1],qc[4],qc[5],qc[6]]
    pub_joint_cmd=rospy.Publisher('/robot/limb/right/joint_command',JointCommand)
    command_msg=JointCommand()
    command_msg.names=['right_e1']
    command_msg.command=[0]
    command_msg.mode=JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()
    exit = 0
    listener2=tf.TransformListener()

    #the transformations
    now=rospy.Time()
    listener2.waitForTransform("/torso","/right_hand",now,rospy.Duration(1.0))
    (trans08,rot08)=listener2.lookupTransform("/torso","/right_hand",now)

    # Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1])
    R08 = transformations.quaternion_matrix(rot08)
    T08 = numpy.vstack((numpy.column_stack((R08[0:3,0:3], numpy.transpose(numpy.array(trans08)))),[0,0,0,1]))
    return (angles, T08)
def callback(comm):
    msg = PoseStamped()
    msg.header.seq = 0
    msg.header.stamp = rospy.get_rostime()
    msg.header.frame_id = 'base'
    msg.pose = comm.pose

    arr = SolvePositionIKRequest()
    arr.pose_stamp.append(msg)
    arr.seed_mode = 0

    print arr

    left_ik_addr = '/ExternalTools/left/PositionKinematicsNode/IKService'
    right_ik_addr = '/ExternalTools/right/PositionKinematicsNode/IKService'

    if rospy.get_param('/robot/unityros/movelock') == True:
        return

    try:
        left_ik = rospy.ServiceProxy(left_ik_addr, SolvePositionIK)
        right_ik = rospy.ServiceProxy(right_ik_addr, SolvePositionIK)

        if comm.limb == 'left':
            ik = left_ik(arr)
        else:
            ik = right_ik(arr)

        right_limb = rospy.Publisher('/robot/limb/right/joint_command',
                                     JointCommand,
                                     queue_size=1)
        left_limb = rospy.Publisher('/robot/limb/left/joint_command',
                                    JointCommand,
                                    queue_size=1)

        message = JointCommand()
        message.mode = 1
        message.command = ik.joints[0].position
        message.names = ik.joints[0].name
        rospy.loginfo(message)
        right_limb.publish(message)
        left_limb.publish(message)

    except rospy.ServiceException, e:
        print 'Service call failed: %s' % e
Example #20
0
def main():
    rospy.init_node('baxter_arm')
    rospy.loginfo('main method called')
    rate = rospy.Rate(10)
    pub = rospy.Publisher('/robot/limb/left/joint_command',
                          JointCommand,
                          queue_size=1)

    while not rospy.is_shutdown():
        msg = JointCommand()
        msg.mode = msg.POSITION_MODE
        msg.names = {
            'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2',
            'right_e0', 'right_e1'
        }
        msg.command = {0.0, 0.0, 0.1, 0.1, 0.0, 0.0, 0.0}
        pub.publish(msg)
        rate.sleep()
def callback(data):
    time_now = rospy.Time().now()
    if (time_now.secs - data.header.stamp.secs >
            1 | time_now.nsecs - data.header.stamp.nsecs > 500000000):
        return
    print "accept command"
    commandPose = data.pose
    limb_name = data.header.frame_id
    limb = baxter_interface.Limb(limb_name)
    currentPose = limb.endpoint_pose()
    while not ("position" in currentPose):
        currentPose = limb.endpoint_pose()
    newPose = dict()
    newPose = {
        'position':
        Point(
            commandPose.position.x,
            commandPose.position.y,
            commandPose.position.z,
        ),
        'orientation':
        Quaternion(
            commandPose.orientation.x,
            commandPose.orientation.y,
            commandPose.orientation.z,
            commandPose.orientation.w,
        ),
    }
    kinematics = baxter_kinematics(limb_name)
    inverseKinematics = kinematics.inverse_kinematics(newPose["position"],
                                                      newPose["orientation"])

    if not (inverseKinematics == None):
        inverseKinematicsSolution = list()
        for num in inverseKinematics.tolist():
            inverseKinematicsSolution.append(num)
        inverseKinematicsSolutionJointCommand = JointCommand()
        inverseKinematicsSolutionJointCommand.mode = 1
        inverseKinematicsSolutionJointCommand.names = limb.joint_names()
        inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
        pub.publish(inverseKinematicsSolutionJointCommand)
Example #22
0
def move_list_smooth_wobble(neutral=False,
                            arm=None,
                            p_list=0,
                            timeout=default_timeout,
                            threshold=default_threshold,
                            speed=default_speed):
    arm = arm.lower()

    if speed > 1.0:
        speed = 1.0
    elif speed <= 0.0:
        speed = 0.1

    if arm == 'l':
        arm = 'left'
    elif arm == 'r':
        arm = 'right'

    if arm == 'left' or arm == 'right':

        limb = baxter_interface.Limb(arm)
        limb.set_joint_position_speed(speed)

        if neutral:
            limb.move_to_neutral()

        for positions in p_list:
            position = dictToList(limb, positions)

            pub_joints = rospy.Publisher('/robot/limb/' + arm +
                                         '/joint_command',
                                         JointCommand,
                                         queue_size=5)

            cmd_msg = JointCommand()
            cmd_msg.mode = JointCommand.POSITION_MODE
            cmd_msg.names = [
                arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0',
                arm + '_e1', arm + '_s0', arm + '_s1'
            ]
            cmd_msg.names = [
                arm + '_w0', arm + '_w1', arm + '_w2', arm + '_e0',
                arm + '_e1', arm + '_s0', arm + '_s1'
            ]

            bo_finish = False

            while not bo_finish and not rospy.is_shutdown():
                cmd_msg.command = position
                pub_joints.publish(cmd_msg)
                time.sleep(0.19)
                bo_finish = not checkThreshold(limb, position, threshold)

        if neutral:
            limb.move_to_neutral()

        limb.set_joint_position_speed(default_speed)

    else:
        print("Error: {} is not a valid limb".format(arm))
Example #23
0
    def move_to_ja(self, waypoints, duration=1.5):
        """
        :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
                                                      Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
        :param duration: Total time trajectory will take before ending
        """
        self._try_enable()

        jointnames = self._limb.joint_names()
        prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
        waypoints = np.array([prev_joint] + waypoints)

        spline = CSpline(waypoints, duration)

        start_time = rospy.get_time()  # in seconds
        finish_time = start_time + duration  # in seconds

        time = rospy.get_time()
        while time < finish_time:
            pos, velocity, acceleration = spline.get(time - start_time)
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            command.command = pos
            #command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
            #command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
            self._cmd_publisher.publish(command)

            self._control_rate.sleep()
            time = rospy.get_time()

        for i in xrange(10):
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            #command.position = waypoints[-1]
            command.command = waypoints[-1]
            self._cmd_publisher.publish(command)

            self._control_rate.sleep()
def callback(data):
	#time_now = rospy.Time().now()
	#if (time_now.secs - data.header.stamp.secs > 1):
	#	rospy.loginfo("time stamp too old, ignore...")
	#	return

	commandPose = data.pose
	limb_name = data.header.frame_id;
	limb = baxter_interface.Limb(limb_name)
	newPose = dict()
	newPose = {
		'position': Point(
				commandPose.position.x,
				commandPose.position.y,
				commandPose.position.z,
			),
		'orientation': Quaternion(
				commandPose.orientation.x,
				commandPose.orientation.y,
				commandPose.orientation.z,
				commandPose.orientation.w,
			),
	}
	kinematics = baxter_kinematics(limb_name)
	inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"])

	if not (inverseKinematics == None):
		inverseKinematicsSolution = list()
		for num in inverseKinematics.tolist():
			inverseKinematicsSolution.append(num)
		inverseKinematicsSolutionJointCommand = JointCommand()
		inverseKinematicsSolutionJointCommand.mode = 1
		inverseKinematicsSolutionJointCommand.names = limb.joint_names()
		inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
		commandSolutionPublisher.publish(inverseKinematicsSolutionJointCommand)
	
	# publish to command subscriber to check compute
	commandCheckPublisher.publish(String("done"))
def callback(data):
	time_now = rospy.Time().now()
	if (time_now.secs - data.header.stamp.secs > 1 | time_now.nsecs - data.header.stamp.nsecs > 500000000):
		return
	print "accept command"
	commandPose = data.pose
	limb_name = data.header.frame_id;
	limb = baxter_interface.Limb(limb_name)
	currentPose = limb.endpoint_pose()
	while not ("position" in currentPose):
		currentPose = limb.endpoint_pose()
	newPose = dict()
	newPose = {
		'position': Point(
				commandPose.position.x,
				commandPose.position.y,
				commandPose.position.z,
			),
		'orientation': Quaternion(
				commandPose.orientation.x,
				commandPose.orientation.y,
				commandPose.orientation.z,
				commandPose.orientation.w,
			),
	}
	kinematics = baxter_kinematics(limb_name)
	inverseKinematics = kinematics.inverse_kinematics(newPose["position"], newPose["orientation"])

	if not (inverseKinematics == None):
		inverseKinematicsSolution = list()
		for num in inverseKinematics.tolist():
			inverseKinematicsSolution.append(num)
		inverseKinematicsSolutionJointCommand = JointCommand()
		inverseKinematicsSolutionJointCommand.mode = 1
		inverseKinematicsSolutionJointCommand.names = limb.joint_names()
		inverseKinematicsSolutionJointCommand.command = inverseKinematicsSolution
		pub.publish(inverseKinematicsSolutionJointCommand)
Example #26
0
 def stopMoving(self):
     lcmd = JointCommand()
     rcmd = JointCommand()
     lcmd.names = self.baxter_larm_names
     rcmd.names = self.baxter_rarm_names
     lcmd.mode = rcmd.mode = POSITION_MODE
     lcmd.command = []
     for n in lcmd.names:
         index = self.currentJointStates.name.index(n)
         lcmd.command.append(self.currentJointStates.position[index])
     rcmd.command = []
     for n in rcmd.names:
         index = self.currentJointStates.name.index(n)
         rcmd.command.append(self.currentJointStates.position[index])
     self.pub_larm.publish(lcmd)
     self.pub_rarm.publish(rcmd)
     self.pub_hnod.publish(False)
def getpose():
    pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command',
                                    JointCommand)
    command_msg = JointCommand()
    command_msg.names = [
        'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1',
        'right_w2'
    ]
    command_msg.mode = JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()

    joint_positions = rospy.wait_for_message("/robot/joint_states", JointState)
    qc = (joint_positions.position[9:16])
    angles = [qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6]]
    pub_joint_cmd = rospy.Publisher('/robot/limb/right/joint_command',
                                    JointCommand)
    command_msg = JointCommand()
    command_msg.names = ['right_e1']
    command_msg.command = [0]
    command_msg.mode = JointCommand.POSITION_MODE
    control_rate = rospy.Rate(100)
    start = rospy.get_time()
    exit = 0
    listener2 = tf.TransformListener()

    #the transformations
    now = rospy.Time()
    listener2.waitForTransform("/torso", "/right_hand", now,
                               rospy.Duration(1.0))
    (trans08, rot08) = listener2.lookupTransform("/torso", "/right_hand", now)

    # Get 4*4 rotational matrix from quaternion ( 4th colume is [0][0][0][1])
    R08 = transformations.quaternion_matrix(rot08)
    T08 = numpy.vstack((numpy.column_stack(
        (R08[0:3, 0:3], numpy.transpose(numpy.array(trans08)))), [0, 0, 0, 1]))
    return (angles, T08)
Example #28
0
    def __init__(self, input_topics, output_topic):
        limb = 'left'
        for topic in input_topics:
            rospy.Subscriber(topic,
                             std_msgs.msg.Float64,
                             callback=self.message_receive,
                             callback_args=topic)
        self.names = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
        self.msg = JointCommand()
        self.msg.mode = JointCommand.TORQUE_MODE
        self.msg.names = list()
        for name in self.names:
            self.msg.names.append(limb + '_' + name)
        self.msg.command = [0] * 7

        self.pub = rospy.Publisher(output_topic, JointCommand, queue_size=1)
        self.rate = rospy.Rate(100)

        self.publish_loop()
Example #29
0
 def stopMoving(self):
     lcmd = JointCommand()
     rcmd = JointCommand()
     lcmd.names = self.baxter_larm_names
     rcmd.names = self.baxter_rarm_names
     lcmd.mode = rcmd.mode = POSITION_MODE
     lcmd.command = []
     for n in lcmd.names:
         index = self.currentJointStates.name.index(n)
         lcmd.command.append(self.currentJointStates.position[index])
     rcmd.command = []
     for n in rcmd.names:
         index = self.currentJointStates.name.index(n)
         rcmd.command.append(self.currentJointStates.position[index])
     self.pub_larm.publish(lcmd)
     self.pub_rarm.publish(rcmd)
     self.pub_hnod.publish(False)
Example #30
0
    def __init__(self, model_file, max_points, timer_freq, dim1, dim2,
                 resolution, manual):
        # create joint angle commands and corresponding publishers
        self.left_command = JointCommand(names=LEFT_NAMES,
                                         mode=JointCommand.POSITION_MODE)
        self.right_command = JointCommand(names=RIGHT_NAMES,
                                          mode=JointCommand.POSITION_MODE)

        self.left_pub = rospy.Publisher('/robot/limb/left/joint_command',
                                        JointCommand,
                                        tcp_nodelay=True,
                                        queue_size=1)
        self.right_pub = rospy.Publisher('/robot/limb/right/joint_command',
                                         JointCommand,
                                         tcp_nodelay=True,
                                         queue_size=1)

        # load mrd model from pickle file
        self.mrd_model = GPy.load(model_file)

        mrd_X = self.mrd_model.X.mean
        self.mrd_point_count = mrd_X.shape[0]
        if self.mrd_point_count > max_points:
            rospy.logwarn('Mean contains more samples. Shape: (%d, %d)' %
                          mrd_X.shape)
            downsample_indices = np.random.choice(self.mrd_point_count,
                                                  size=max_points,
                                                  replace=False)
            mrd_X = mrd_X[downsample_indices]

        # parameters for doing latent function inference
        self.q_dim = mrd_X.shape[1]
        self.latent_X = np.zeros((1, self.q_dim))

        self.dim1 = dim1
        self.dim2 = dim2
        self.resolution = resolution

        self.mrd_X = mrd_X[:, [self.dim1, self.dim2]]

        title = 'Baxter Whill Movement using MRD'
        fig, (self.ax1, self.ax2) = plt.subplots(nrows=1,
                                                 ncols=2,
                                                 figsize=(10, 4))
        self.plot_latent_space()
        self.plot_whill_movement()
        fig.canvas.set_window_title(title)
        self.text_handle = self.ax1.text(0.8,
                                         0.1,
                                         'Play Mode: OFF',
                                         horizontalalignment='center',
                                         verticalalignment='center',
                                         transform=self.ax1.transAxes,
                                         bbox={
                                             'facecolor': 'green',
                                             'alpha': 0.5,
                                             'pad': 6
                                         })

        self.counter = 0
        self.whill_move_handle = None
        self.latent_cursor_handle = None

        if manual:
            fig.suptitle(
                'Predicted Whill movements are not sent to the Whill controller',
                fontstyle='italic',
                color='red')
            # variables for mouse cursor based motion
            self.mouse_xy = np.zeros((1, 2))
            self.start_motion = False
            self.cursor_color = 'red'

            # connect the cursor class
            fig.canvas.mpl_connect('button_press_event', self.mouse_click)
            fig.canvas.mpl_connect('motion_notify_event', self.mouse_move)
            fig.subplots_adjust(top=0.80)
        else:
            self.whill_move = None
            self.init_ros_whillpy()
            self.cursor_color = 'green'
            self.text_handle.set_text('Automatic Mode: ON')

            # create a timer to follow the mean trajectory
            self.ros_timer = rospy.Timer(rospy.Duration(1 / timer_freq),
                                         self.timer_callback)

        # adjust the space at the bottom
        fig.subplots_adjust(bottom=0.15)
Example #31
0
    def process(self):
        if self.currentAssemblyState==None:
            #not enabled, return
            print "Waiting for /robot/state..."
            time.sleep(1.0)
            return
        if self.currentAssemblyState.stopped:
            print "Robot is stopped"
            time.sleep(1.0)
            return
        if self.currentJointStates==None:
            print "Waiting for joint states to be published..."
            time.sleep(1.0)
            return
        if self.lastUpdateTime == None:
            self.lastUpdateTime = self.currentJointStates.header.stamp
            return
        self.currentTime = self.currentJointStates.header.stamp

        #convert to Klamp't message
        klamptInput = dict()
        klamptInput['t'] = self.currentTime.to_sec()
        klamptInput['dt'] = self.currentTime.to_sec() - self.lastUpdateTime.to_sec()
        if klamptInput['dt'] == 0.0:
            #no new clock messages read
            return
        klamptInput['q'] = [0.0]*self.robot_model.numLinks()
        klamptInput['dq'] = [0.0]*self.robot_model.numLinks()
        klamptInput['torque'] = [0.0]*self.robot_model.numDrivers()
        for i,n in enumerate(self.currentJointStates.name):
            if n not in self.nameToLinkIndex:
                if n != 'torso_t0':
                    print "Ignoring state of link",n
                continue
            klamptIndex = self.nameToLinkIndex[n]
            klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
            if len(self.currentJointStates.velocity) > 0:
                klamptInput['dq'][klamptIndex] = self.currentJointStates.velocity[i]
            if len(self.currentJointStates.effort) > 0:
                driverIndex = self.nameToDriverIndex[n]
                klamptInput['torque'][driverIndex] = self.currentJointStates.effort[i]
        #if self.currentHeadState != None:
        #    klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan

        #output is defined in SerialController
        klamptReply = self.output(**klamptInput)
        if klamptReply == None:
            return False

        #now conver the Klamp't reply to a Baxter command
        lcmd = JointCommand()
        rcmd = JointCommand()
        lcmd.names = self.baxter_larm_names
        rcmd.names = self.baxter_rarm_names
        hpcmd = HeadPanCommand()
        hncmd = Bool()
        if 'qcmd' in klamptReply:
            #use position mode
            lcmd.mode = rcmd.mode = POSITION_MODE
            q = klamptReply['qcmd']
            lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
            rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
            hpcmd.target = q[self.head_pan_link_index]
            hpcmd.speed = q[self.head_pan_link_index]
        elif 'dqcmd' in klamptReply:
            lcmd.mode = rcmd.mode = VELOCITY_MODE
            dq = klamptReply['dqcmd']
            lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
            rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]

            hpcmd = None
        elif 'torquecmd' in klamptReply:
            lcmd.mode = rcmd.mode = TORQUE_MODE
            torque = klamptReply['torquecmd']
            lcmd.command = [torque[self.nameToDriverIndex[n]] for n in lcmd.names]
            rcmd.command = [torque[self.nameToDriverIndex[n]] for n in rcmd.names]

            hpcmd = None
        else:
            lcmd = rcmd = None
            hpcmd = None
        hncmd = None

        if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
            print "Estop is on..."
            time.sleep(1.0)
        elif not self.currentAssemblyState.enabled:
            print "Waiting for robot to turn on..."
            time.sleep(1.0)
        else:
            #publish the messages
            if lcmd:
                self.pub_larm.publish(lcmd)
            if rcmd:
                self.pub_rarm.publish(rcmd)
            if hpcmd:
                self.pub_hpan.publish(hpcmd) 
            if hncmd:
                self.pub_hnod.publish(hncmd)
        self.lastUpdateTime = self.currentTime
        return True
Example #32
0
 def setJoints(self, angles, mode=1):
     self.mode = mode
     self.goalc = Point(*self.bk.forward_position_kinematics_arg2p(angles))
     names = [name for name in iter(angles)]
     angles = [angles[name] for name in iter(angles)]
     self.jointCmd = JointCommand(mode, angles, names)
def main():
    rospy.init_node('trayectoria_circular')

    #Ponemos RATE de mensaje
    rate = rospy.Rate(20)

    #Creamos objeto PUBLICADOR
    pub = rospy.Publisher('/robot/limb/left/joint_command',JointCommand,queue_size=10)


    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument(
        "-l", "--limb", dest="limb", default="left",
        choices=['left', 'right'],
        help="joint trajectory limb"
    )

    parser.add_argument(
        "-R", "--radius", dest="radius", default=0.2,
        type=float,
        help="trajectory radius"
    )

    parser.add_argument(
        "-v", "--velocity", dest="velocity", default=5.0,
        type=float, help="trajectory velocity"
    )

    parser.add_argument(
        "-r", "--revolution", dest="revolution", default=0.9,
        type=float, help="Revolutions"
    )

    args = parser.parse_args(rospy.myargv()[1:])

    #print(sys.argv[1]);

    kin = baxter_kinematics(args.limb)
    limb = baxter_interface.Limb(args.limb)
    timeout = args.velocity/10.0
    radio = args.radius

    decremento = 0.001

    posiciones = [];

    centro_y = 0.35
    centro_z = 0.1

    if args.limb == 'right':
     centro_y = centro_y * (-1)

    # FK Position
    limb.move_to_neutral()
    kp = kin.forward_position_kinematics()

    #Array donde vamos acumulando configuraciones q1,q2,...,qn para seguir la trayectoria
    #Util para control por Torques con PID
    posiciones.append(kp);

    #Centro de la Circunferencia
    pos = [kp[0],kp[1]-centro_y,kp[2]+centro_z]
    rot = [kp[3],kp[4],kp[5],kp[6]]

    #Calculamos configuracion Qcentro
    l = kin.inverse_kinematics(pos, rot)
    m = {args.limb +'_w0': l[4], args.limb + '_w1': l[5], args.limb +'_w2': l[6], args.limb +'_e0': l[2], args.limb +'_e1': l[3], args.limb +'_s0': l[0], args.limb +'_s1': l[1]}
    #Movemos al centro de la circunferencia
    limb.move_to_joint_positions(m)

    posiciones.append(l);

    puntos_circunferencia = []
    puntos_circunferencia2 = []
    
    #Calculamos los valores que tomara Z y que nos servira para calcular Y posteriormente
    valoresTomar = []
    i = 1
    while(i >= -0.5):
     valoresTomar.append(i)
     i = i - decremento

    #Calculamos las posiciones para describir circunferencia en plano ZY
    for z in valoresTomar:
     c = 2*pos[1]
     b = (-1)*(-z*z + 2*z*pos[2] + radio*radio - pos[2]*pos[2] - pos[1]*pos[1])
     if c*c - 4*b > 0:
      y = (c + sqrt(c*c - 4*b)) / 2
      y2 = (c - sqrt(c*c - 4*b)) / 2
      puntos_circunferencia.append([y,z])
      puntos_circunferencia2.append([y2,z])
    
    Posiciones = []
    Rotaciones = []
    ErroresPos = []
    ErroresRot = []

    #Vamos PUBLICANDO las posiciones que deseamos alcanzar para seguir la trayectoria

    #Creamos MENSAJE
    msg = JointCommand()
    #Modo - 1 (CONTROL POR POSICION)
    msg.mode = 1
    msg.names = [args.limb +'_w0',args.limb + '_w1',args.limb +'_w2',args.limb +'_e0',args.limb +'_e1',args.limb +'_s0',args.limb +'_s1']

    for p in puntos_circunferencia2:
     kp = kin.forward_position_kinematics()
     #Calculamos (x,y,z)
     pos = [kp[0],p[0],p[1]]
     #Calculamos tupla Orientacion (i,j,w,k)
     rot =  [kp[3],kp[4],kp[5],kp[6]]
     #Calculamos (theta1,...) para (x,y,z)
     l = kin.inverse_kinematics(pos, rot)
     #Posicion Valida
     if l != None:
      Posiciones.append(pos)
      Rotaciones.append(rot)

      msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
      pub.publish(msg)
      rate.sleep()

      ErroresRot.append(rot)
      posiciones.append(l);
     else:
      print(p);
      ErroresPos.append(pos)


    puntos_circunferencia = puntos_circunferencia[::-1]
    for p in puntos_circunferencia:
     kp = kin.forward_position_kinematics()
     pos = [kp[0],p[0],p[1]]
     rot =  [kp[3],kp[4],kp[5],kp[6]]
     l = kin.inverse_kinematics(pos, rot)
     if l != None:
      Posiciones.append(pos)
      Rotaciones.append(rot)
      msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
      pub.publish(msg)
      rate.sleep()
      posiciones.append(l);
     else:
      ErroresPos.append(pos)
      ErroresRot.append(rot)

    #Caso de que queramos realizar la trayectoria en mas de una ocasion
    n_vueltas = args.revolution
    k = 1
    while k < n_vueltas:
     for i in range(len(Posiciones)):
      pos = Posiciones[i]
      rot = Rotaciones[i]
      l = kin.inverse_kinematics(pos, rot)
      msg.command = [l[4],l[5],l[6],l[2],l[3],l[0],l[1]]
      pub.publish(msg)
      rate.sleep()
     k = k+1

    np.save('trayectoriaC',np.array(posiciones));
Example #34
0
def main():
    rospy.init_node('trayectoria_rombo')

    #Definimos el RATE de publicacion de mensajes en ROS
    rate = rospy.Rate(10)

    #Creamos objeto PUBLICADOR
    pub = rospy.Publisher('/robot/limb/left/joint_command',
                          JointCommand,
                          queue_size=10)

    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
    parser.add_argument("-l",
                        "--limb",
                        dest="limb",
                        default="left",
                        choices=['left', 'right'],
                        help="joint trajectory limb")

    parser.add_argument("-v",
                        "--velocity",
                        dest="velocity",
                        default=5.0,
                        type=float,
                        help="trajectory velocity")

    #Leemos los argumentos dados como entrada
    args = parser.parse_args(rospy.myargv()[1:])

    kin = baxter_kinematics(args.limb)
    limb = baxter_interface.Limb(args.limb)
    timeout = args.velocity / 10.0

    n_mov = 32
    # Movemos a posicion neutral
    limb.move_to_neutral()
    # Consultamos la posicion actual en formato : (x,y,z) y (i,j,w,k)
    kp = kin.forward_position_kinematics()

    #Limites del rombo a describir
    limite_inferior_y = -1.5
    limite_inferior_z = -0.8

    proporcionY = limite_inferior_y / n_mov
    proporcionZ = limite_inferior_z / n_mov

    if args.limb == 'right':
        proporcionY = proporcionY * (-1)

    #Creamos MENSAJE
    msg = JointCommand()
    #Modo - 1 (CONTROL POR POSICION)
    msg.mode = 1
    msg.names = [
        args.limb + '_w0', args.limb + '_w1', args.limb + '_w2',
        args.limb + '_e0', args.limb + '_e1', args.limb + '_s0',
        args.limb + '_s1'
    ]

    #Vamos describiendo la trayectoria de Rombo
    for i in range(n_mov):
        kp = kin.forward_position_kinematics()
        if i < n_mov // 2:
            pos = [kp[0], kp[1] + proporcionY, kp[2] - proporcionZ]
        else:
            pos = [kp[0], kp[1] + proporcionY, kp[2] + proporcionZ]

        rot = [kp[3], kp[4], kp[5], kp[6]]
        l = kin.inverse_kinematics(pos, rot)

        if l != None:
            msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]]
            pub.publish(msg)
            rate.sleep()

    for i in range(n_mov):
        kp = kin.forward_position_kinematics()
        if i < n_mov // 2:
            pos = [kp[0], kp[1] - proporcionY, kp[2] + proporcionZ]
        else:
            pos = [kp[0], kp[1] - proporcionY, kp[2] - proporcionZ]

        rot = [kp[3], kp[4], kp[5], kp[6]]
        theta_i = kin.inverse_kinematics(pos, rot)

        if theta_i != None:
            m = {
                args.limb + '_w0': theta_i[4],
                args.limb + '_w1': theta_i[5],
                args.limb + '_w2': theta_i[6],
                args.limb + '_e0': theta_i[2],
                args.limb + '_e1': theta_i[3],
                args.limb + '_s0': theta_i[0],
                args.limb + '_s1': theta_i[1]
            }
            limb.move_to_joint_positions(m, timeout)
Example #35
0
def execute_path(data_path):

    left_limb_commander = rospy.Publisher('/robot/limb/left/joint_command',
                                          JointCommand,
                                          queue_size=10)
    right_limb_commander = rospy.Publisher('/robot/limb/right/joint_command',
                                           JointCommand,
                                           queue_size=10)
    head_pan_commander = rospy.Publisher('/robot/head/command_head_pan',
                                         HeadPanCommand,
                                         queue_size=10)
    head_nod_commander = rospy.Publisher('/robot/head/command_head_nod',
                                         Bool,
                                         queue_size=10)

    rospy.init_node('joint_commander', anonymous=True)
    traj = get_trajectory(data_path)

    for joint_state in traj:

        left_limb_joint_names = []
        left_limb_joint_values = []

        right_limb_joint_names = []
        right_limb_joint_values = []

        head_pan_target = None
        head_nod = False

        joint_state = literal_eval(joint_state)
        for joint_name, state in joint_state.items():

            if joint_name.startswith('left'):
                left_limb_joint_names.append(joint_name)
                left_limb_joint_values.append(state)

            elif joint_name.startswith('right'):
                right_limb_joint_names.append(joint_name)
                right_limb_joint_values.append(state)

            elif joint_name == "head_pan":
                head_pan_target = state

            elif joint_name == "head_nod":
                if abs(state) > 0:
                    head_nod = True
                else:
                    head_nod = False

        left_command = JointCommand()
        left_command.mode = 1
        left_command.names = left_limb_joint_names
        left_command.command = left_limb_joint_values

        right_command = JointCommand()
        right_command.mode = 1
        right_command.names = right_limb_joint_names
        right_command.command = right_limb_joint_values

        head_pan_command = HeadPanCommand()
        head_pan_command.target = head_pan_target
        head_pan_command.speed = 0.1

        head_nod_command = Bool()
        head_nod_command.data = head_nod

        left_limb_commander.publish(left_command)
        right_limb_commander.publish(right_command)

        head_pan_commander.publish(head_pan_command)

        head_nod_commander.publish(head_nod_command)

        rospy.loginfo("State reached...")
Example #36
0
def calcularFitness(individual, qObjs):

    fitness = 0

    KP = np.array(individual[0:7])
    KD = np.array(individual[7:14])
    KI = np.array(individual[14:21])

    m = {
        'left_w0': 0.0,
        'left_w1': 0.0,
        'left_w2': 0.0,
        'left_e0': 0.0,
        'left_e1': 0.0,
        'left_s0': -1.0,
        'left_s1': 0.0
    }
    limb.move_to_joint_positions(m)

    #Creamos MENSAJE
    msg = JointCommand()
    #Modo - 1 (CONTROL POR POSICION)
    msg.mode = 3
    msg.names = [
        'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
        'left_w2'
    ]

    errores = []
    errores2 = []
    dt = []

    t = 0
    i = 0

    q_sensores = []

    kinematics = baxter_kinematics('left')

    while i < len(qObjs):

        dt.append(time.clock())

        qDes = qObjs[i]

        angles = limb.joint_angles()
        q = np.array([
            angles['left_s0'], angles['left_s1'], angles['left_e0'],
            angles['left_e1'], angles['left_w0'], angles['left_w1'],
            angles['left_w2']
        ])

        q_sensores.append(q)

        error = np.array(qDes - q)
        errores.append(error)

        if i != 0:
            der = (errores[len(errores) - 1] - errores[len(errores) - 2]) / (
                dt[len(dt) - 2] - dt[len(dt) - 1])
            torque = KP * error + KD * np.array(der)
        else:
            der = errores[len(errores) - 1]
            torque = KP * error + KD * np.array(der)

        m = {
            'left_w0': torque[4],
            'left_w1': torque[5],
            'left_w2': torque[6],
            'left_e0': torque[2],
            'left_e1': torque[3],
            'left_s0': torque[0],
            'left_s1': torque[1]
        }
        msg.command = [
            torque[0], torque[1], torque[2], torque[3], torque[4], torque[5],
            torque[6]
        ]
        pub.publish(msg)  #Publicamos el mensaje
        rate.sleep()  #Esperamos para conseguir el rate deseado
        #limb.set_joint_torques(m)

        for l in range(len(error)):
            fitness = fitness + error[l] * error[l]

        i = i + 10

    print(fitness)
    return fitness