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()