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 test_dict(fname):
dic = ut.load_pickle(fname)
firenze = cak.CodyArmKinematics('r')
rot = tr.rotY(math.radians(-90))
p = np.matrix([0.4,-0.42,-0.2]).T
c = find_good_config(p, dic)
res = firenze.IK(p, rot, q_guess=c)
print 'IK soln: ', [math.degrees(qi) for qi in res]
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 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)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id * 100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi / 2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi / 2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
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)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c1[0]
marker.color.g = c1[1]
marker.color.b = c1[2]
marker.color.a = 1.
marker_pub.publish(marker)
marker.id = marker_id*100 + 1
if arm == 'left_arm':
#rot2 = tr.Rz(math.radians(90.)) * rot.T
rot2 = rot * tr.rotZ(-math.pi/2)
else:
#rot2 = tr.Rz(math.radians(-90.)) * rot.T
rot2 = rot * tr.rotZ(math.pi/2)
quat = tr.matrix_to_quaternion(rot2)
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.color.r = c2[0]
marker.color.g = c2[1]
marker.color.b = c2[2]
marker.color.a = 1.
marker_pub.publish(marker)
def detect_artag(self):
try:
rospy.logout("Finding the AR tag..")
self.pub_rate.sleep()
(self.ar_pos, rot) = self.tf_listener.lookupTransform("/torso_lift_link",
self.marker_frame, rospy.Time(0))
self.pub_rate.sleep()
gripper_rot = hrl_tr.rotY(math.pi/2) #gripper facing -z direction
self.ar_rot = hrl_tr.quaternion_to_matrix(rot)*gripper_rot
rospy.logout("Found AR tag!\nPosition: "+pplist(self.ar_pos)+"\nQuaterion: "+pplist(rot))
self.ar_ep = []
self.ar_ep.append(np.matrix(self.ar_pos).T)
self.ar_ep.append(self.ar_rot)
self.found_tag = True
except:
rospy.logout('AARtagDetect: Transform failed for '+self.tool)
return False
def detect_artag(self):
try:
rospy.logout("Finding the AR tag..")
self.pub_rate.sleep()
(self.ar_pos, rot) = self.tf_listener.lookupTransform("/torso_lift_link",
self.marker_frame, rospy.Time(0))
self.pub_rate.sleep()
gripper_rot = hrl_tr.rotY(math.pi/2) #gripper facing -z direction
self.ar_rot = hrl_tr.quaternion_to_matrix(rot)*gripper_rot
rospy.logout("Found AR tag!\nPosition: "+pplist(self.ar_pos)+"\nQuaterion: "+pplist(rot))
self.ar_ep = []
self.ar_ep.append(np.matrix(self.ar_pos).T)
self.ar_ep.append(self.ar_rot)
self.found_tag = True
except:
rospy.logout('AARtagDetect: Transform failed for '+self.tool)
return False
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))
p = np.matrix([0.3, -0.24, -0.3]).T
# q = arms.IK(l_arm, p, rot_mat)
# ac.go_jep(l_arm, q)
# ac.go_cep(l_arm, p, rot_mat)
ac.go_cep(r_arm, p, rot_mat)
# jep = ac.get_jep(r_arm)
# ac.go_jep(r_arm, jep)
rospy.sleep(0.5)
raw_input('Hit ENTER to print ee position')
q = ac.get_joint_angles(r_arm)
ee = ac.arms.FK(r_arm, q)
print 'ee:', ee.A1
print 'desired ee:', p.A1
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))
p = np.matrix([0.3, -0.24, -0.3]).T
# q = arms.IK(l_arm, p, rot_mat)
# ac.go_jep(l_arm, q)
# ac.go_cep(l_arm, p, rot_mat)
ac.go_cep(r_arm, p, rot_mat)
# jep = ac.get_jep(r_arm)
# ac.go_jep(r_arm, jep)
rospy.sleep(0.5)
raw_input('Hit ENTER to print ee position')
q = ac.get_joint_angles(r_arm)
ee = ac.arms.FK(r_arm, q)
print 'ee:', ee.A1
print 'desired ee:', p.A1
