Example #1
0
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]
Example #2
0
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]
Example #4
0
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)
Example #5
0
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)
Example #6
0
    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
Example #8
0
    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
Example #9
0
    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