How to use arms - 10 common examples

def power_on(self):
        pass

    # leaving this unimplemented for now. Advait Nov 14, 2010.
    # something to change and get arm_settings.


if __name__ == '__main__':
    import arms as ar
    import m3.toolbox as m3t
    import hrl_lib.transforms as tr

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    arms = ar.M3HrlRobot(0.16)
    ac = MekaArmClient(arms)

    # print FT sensor readings.
    if False:
        ac.bias_wrist_ft(r_arm)
        while not rospy.is_shutdown():
            f = ac.get_wrist_force(r_arm)
            print 'f:', f.A1
            rospy.sleep(0.05)

    # move the arms.
    if False:
        print 'hit a key to move the arms.'
        k=m3t.get_keystroke()

        rot_mat = tr.rotY(math.radians(-90))

def test_dict(fname):
    dict = ut.load_pickle(fname)
    firenze = ar.M3HrlRobot(connect=False)

    rot = tr.rotY(math.radians(-90))
    p = np.matrix([0.4,-0.42,-0.2]).T

    c = find_good_config(p,dict)
    res = firenze.IK(p,rot,q_guess=c)
    print 'IK soln: ', [math.degrees(qi) for qi in res]

def record_good_configs(use_left_arm):
    import m3.toolbox as m3t
    settings_arm = ar.MekaArmSettings(stiffness_list=[0.,0.,0.,0.,0.],control_mode='torque_gc')

    if use_left_arm:
        firenze = ar.M3HrlRobot(connect=True,left_arm_settings=settings_arm)
        arm = 'left_arm'
    else:
        firenze = ar.M3HrlRobot(connect=True,right_arm_settings=settings_arm)
        arm = 'right_arm'

    print 'hit ENTER to start the recording process'
    k=m3t.get_keystroke()
    firenze.power_on()

    good_configs_list = []

    while k == '\r':
        print 'hit ENTER to record configuration, something else to exit'
        k=m3t.get_keystroke()
        firenze.proxy.step()
        q = firenze.get_joint_angles(arm)

def power_on(self):
        pass

    # leaving this unimplemented for now. Advait Nov 14, 2010.
    # something to change and get arm_settings.


if __name__ == '__main__':
    import arms as ar
    import m3.toolbox as m3t
    import hrl_lib.transforms as tr

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    arms = ar.M3HrlRobot()
    ac = MekaArmClient(arms)

    # print FT sensor readings.
    if False:
        ac.bias_wrist_ft(r_arm)
        while not rospy.is_shutdown():
            f = ac.get_wrist_force(r_arm)
            print 'f:', f.A1
            rospy.sleep(0.05)

    # move the arms.
    if False:
        print 'hit a key to move the arms.'
        k=m3t.get_keystroke()

        rot_mat = tr.rotY(math.radians(-90))

marker.id = marker_id
    marker.pose.orientation.x = 0
    marker.pose.orientation.y = 0
    marker.pose.orientation.z = 0
    marker.pose.orientation.w = 1

    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = 1.
    marker_pub.publish(marker)


if __name__ == '__main__':
    arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():

marker.id = marker_id
    marker.pose.orientation.x = 0
    marker.pose.orientation.y = 0
    marker.pose.orientation.z = 0
    marker.pose.orientation.w = 1

    marker.color.r = color[0]
    marker.color.g = color[1]
    marker.color.b = color[2]
    marker.color.a = 1.
    marker_pub.publish(marker)


if __name__ == '__main__':
    arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():

def create_dict(fname):
    firenze = ar.M3HrlRobot(connect=False)
    good_configs_list = ut.load_pickle(fname)
    cartesian_points_list = []
    for gc in good_configs_list:
        cartesian_points_list.append(firenze.FK('right_arm',gc).A1.tolist())

    m = np.matrix(cartesian_points_list).T
    print 'm.shape:', m.shape
    dict = {'cart_pts_mat':m, 'good_configs_list':good_configs_list}
    ut.save_pickle(dict,ut.formatted_time()+'_goodconf_dict.pkl')

arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
        f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp
        force_r_pub.publish(FloatArray(h, f_r))
        force_l_pub.publish(FloatArray(h, f_l))

        publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
                              time_stamp, 'both_arms', 0)

        for arm in [r_arm, l_arm]:
            q = arm_client.get_joint_angles(arm)
            links = [2, 3, 7]

def publish_cartesian_markers(arm, time_stamp, cep, rot, c1, c2, marker_id):
    marker = Marker()
    marker.header.frame_id = ar.link_tf_name(arm, 0)
    marker.header.stamp = time_stamp
    marker.ns = arm
    marker.type = Marker.ARROW
    marker.action = Marker.ADD
    marker.pose.position.x = cep[0,0]
    marker.pose.position.y = cep[1,0]
    marker.pose.position.z = cep[2,0]
    marker.scale.x = 0.1
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.lifetime = rospy.Duration()

    marker.id = marker_id*100 + 0
    #rot1 = tr.Ry(math.radians(90.)) * rot.T
    rot1 = rot * tr.rotY(math.pi/2)
    quat = tr.matrix_to_quaternion(rot1)

arms = ar.M3HrlRobot()
    arm_client = ac.MekaArmClient(arms)

    force_r_pub = rospy.Publisher('/r_arm/force_base', FloatArray)
    force_l_pub = rospy.Publisher('/l_arm/force_base', FloatArray)
    marker_pub = rospy.Publisher('/cody_arms/viz_marker', Marker)

    rospy.logout('Sleeping ...')
    rospy.sleep(1.0)
    rospy.logout('... begin')

    r_arm = 'right_arm'
    l_arm = 'left_arm'

    transform_bcast = tfb.TransformBroadcaster()
    torso_link_name = ar.link_tf_name(r_arm, 0)
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
        f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
        time_stamp = rospy.Time.now()
        h = Header()
        h.stamp = time_stamp
        force_r_pub.publish(FloatArray(h, f_r))
        force_l_pub.publish(FloatArray(h, f_l))

        publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
                              time_stamp, 'both_arms', 0)

        for arm in [r_arm, l_arm]:
            q = arm_client.get_joint_angles(arm)
            links = [2, 3, 7]