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) good_configs_list.append(np.matrix(q).A1.tolist()) firenze.stop() ut.save_pickle(good_configs_list, ut.formatted_time() + '_good_configs_list.pkl')
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 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')
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)
# leaving this unimplemented for now. Advait Nov 14, 2010. 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()