Python PoseConv.to_homo_mat方法代码示例

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def robot_ellipsoidal_pose(self, lat, lon, height, orient_quat, kinect_frame_mat=None):
     if kinect_frame_mat is None:
         kinect_frame_mat = self.get_ell_frame()
     pos, quat = self.ell_space.ellipsoidal_to_pose(lat, lon, height)
     quat_rotated = trans.quaternion_multiply(quat, orient_quat)
     ell_pose_mat = PoseConv.to_homo_mat(pos, quat_rotated)
     return PoseConv.to_pos_rot(kinect_frame_mat * ell_pose_mat)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
    def init_reg_cb(self, req):
        # TODO REMOVE THIS FACE SIDE MESS
        self.publish_feedback("Performing Head Registration. Please Wait.")
        self.face_side = rospy.get_param("~face_side1", 'r')
        bag_str = self.reg_dir + '/' + '_'.join([self.subject, self.face_side, "head_transform"]) + ".bag"
        rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
        try:
            bag = rosbag.Bag(bag_str, 'r')
            for topic, msg, ts in bag.read_messages():
                head_tf = msg
            assert (head_tf is not None), "Error reading head transform bagfile"
            bag.close()
        except Exception as e:
            self.publish_feedback("Registration failed: Error loading saved registration.")
            rospy.logerr("[%s] Cannot load registration parameters from %s:\r\n%s" %
                         (rospy.get_name(), bag_str, e))
            return (False, PoseStamped())

        if self.face_side == 'r':
            head_registration = self.head_registration_r
        else:
            head_registration = self.head_registration_l
        try:
            rospy.loginfo("[%s] Requesting head registration for %s at pixel (%d, %d)." %(rospy.get_name(),
                                                                                          self.subject,
                                                                                          req.u, req.v))
            self.head_pc_reg = head_registration(req.u, req.v).reg_pose
            if ((self.head_pc_reg.pose.position == Point(0.0, 0.0, 0.0)) and
                (self.head_pc_reg.pose.orientation == Quaternion(0.0, 0.0, 0.0, 1.0))):
               raise rospy.ServiceException("Unable to find a good match.")
               self.head_pc_reg = None
        except rospy.ServiceException as se:
            self.publish_feedback("Registration failed: %s" %se)
            return (False, PoseStamped())

        pc_reg_mat = PoseConv.to_homo_mat(self.head_pc_reg)
        head_tf_mat = PoseConv.to_homo_mat(Transform(head_tf.transform.translation,
                                                     head_tf.transform.rotation))
        self.head_pose = PoseConv.to_pose_stamped_msg(pc_reg_mat**-1 * head_tf_mat)
        self.head_pose.header.frame_id = self.head_pc_reg.header.frame_id
        self.head_pose.header.stamp = self.head_pc_reg.header.stamp

        side = "right" if (self.face_side == 'r') else "left"
        self.publish_feedback("Registered head using %s cheek model, please check and confirm." %side)
#        rospy.loginfo('[%s] Head frame registered at:\r\n %s' %(rospy.get_name(), self.head_pose))
        self.test_pose.publish(self.head_pose)
        return (True, self.head_pose)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def load_params(self, params):
     kinect_B_head = PoseConv.to_homo_mat(params.e_frame)
     base_B_kinect = self.kin_head.forward(base_link="base_link",
                                           end_link=self.kinect_frame)
     base_B_head = base_B_kinect * kinect_B_head
     self.head_center = PoseConv.to_pose_stamped_msg("/base_link",base_B_head)
     self.ell_space = EllipsoidSpace()
     self.ell_space.load_ell_params(params.E, params.is_oblate, params.height)
     rospy.loginfo("Loaded ellispoidal parameters.")

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def point_towards_midpoint(self, clean_joints, psm1_pos, psm2_pos, key_hole,ecm_pose):
     mid_point = (psm1_pos + psm2_pos)/2
 #     mid_point = ecm_pose[0:3,3] - numpy.array([0,0,.01]).reshape(3,1)
     self.add_marker(PoseConv.to_homo_mat([mid_point, [0,0,0]]), '/marker_subscriber',color=[1,0,0], scale=[0.047/5,0.047/5,0.047/5])
     self.add_marker(PoseConv.to_homo_mat([key_hole,[0,0,0]]), '/keyhole_subscriber',[0,0,1])
     self.add_marker(ecm_pose, '/current_ecm_pose', [1,0,0], Marker.ARROW, scale=[.1,.005,.005])
     temp = clean_joints['ecm'].position
     b,_ = self.ecm_kin.FK([temp[0],temp[1],.14,temp[3]])
     # find the equation of the line that goes through the key_hole and the 
     # mid_point
     ab_vector = (mid_point-key_hole)
     ecm_current_direction = b-key_hole 
     self.add_marker(ecm_pose, '/midpoint_to_keyhole', [0,1,1], type=Marker.LINE_LIST, scale = [0.005, 0.005, 0.005], points=[b, key_hole])
     
     self.add_marker(PoseConv.to_homo_mat([ab_vector,[0,0,0]]), '/ab_vector',[0,1,0], type=Marker.ARROW)
     r = self.find_rotation_matrix_between_two_vectors(ecm_current_direction, ab_vector)
     m = math.sqrt(ab_vector[0]**2 + ab_vector[1]**2 + ab_vector[2]**2) # ab_vector's length
     
     # insertion joint length
     l = math.sqrt( (ecm_pose[0,3]-key_hole[0])**2 + (ecm_pose[1,3]-key_hole[1])**2 + (ecm_pose[2,3]-key_hole[2])**2)
     
     # Equation of the line that passes through the midpoint of the tools and the key hole
     x = lambda t: key_hole[0] + ab_vector[0] * t
     y = lambda t: key_hole[1] + ab_vector[1] * t
     z = lambda t: key_hole[2] + ab_vector[2] * t
     
     t = l/m
     
     new_ecm_position = numpy.array([x(t), y(t), z(t)]).reshape(3,1)
     
     ecm_pose[0:3,0:3] =  r* ecm_pose[0:3,0:3]  
     ecm_pose[0:3,3] = new_ecm_position
     self.add_marker(ecm_pose, '/target_ecm_pose', [0,0,1], Marker.ARROW, scale=[.1,.005,.005])
     output_msg = clean_joints['ecm']
     
     
     try:
         p = self.ecm_kin.inverse(ecm_pose)
     except Exception as e:
         rospy.logerr('error')
     if p != None:  
         p[3] = 0
         output_msg.position = p
     return output_msg

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
    def register_ellipse(self, mode, side):
        reg_e_params = EllipsoidParams()
        try:
            now = rospy.Time.now()
            self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
            pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
            self.head_pose = PoseStamped()
            self.head_pose.header.stamp = now
            self.head_pose.header.frame_id = "/base_link"
            self.head_pose.pose.position = Point(*pos)
            self.head_pose.pose.orientation = Quaternion(*quat)
        except:
            rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
            return (False, reg_e_params)
        reg_prefix = rospy.get_param("~registration_prefix", "")
        registration_files = rospy.get_param("~registration_files", None)
        if mode not in registration_files:
            rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
            return (False, reg_e_params)
        try:
            bag_str = reg_prefix + registration_files[mode][side]
            rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
            bag = rosbag.Bag(bag_str, 'r')
            e_params = None
            for topic, msg, ts in bag.read_messages():
                e_params = msg
            assert e_params is not None
            bag.close()
        except:
            rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
            return (False, reg_e_params)

        head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
        ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
                                                      e_params.e_frame.transform.rotation))
        reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
        reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
        reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
        reg_e_params.height = e_params.height
        reg_e_params.E = e_params.E
        self.ell_params_pub.publish(reg_e_params)
        self.ell_frame = reg_e_params.e_frame
        return (True, reg_e_params)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def create_tool_arrow():
     arrows = MarkerArray()
     color = ColorRGBA(0., 0., 1., 1.)
     for i, param in enumerate(params):
         ell_pos, ell_rot = params[param]
         _, ell_rot_mat = PoseConv.to_pos_rot([0]*3, ell_rot)
         cart_pose = PoseConv.to_homo_mat(ell_space.ellipsoidal_to_pose(*ell_pos))
         cart_pose[:3,:3] = cart_pose[:3,:3] * ell_rot_mat
         arrow = create_arrow_marker(cart_pose, i, color)
         arrow.header.stamp = rospy.Time.now()
         arrows.markers.append(arrow)
     return arrows

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def ar_sub(self, msg):
     if self.kin_arm == None:
         self.kin_arm = create_joint_kin(base_link="base_link", 
                                         end_link=msg.header.frame_id)
     base_B_camera = self.kin_arm.forward()
     camera_B_tag = PoseConv.to_homo_mat(msg.markers[0].pose.pose) #changed to use Alvar Markers
          
     cur_ar_pose = base_B_camera * camera_B_tag
     # check to see if the tag is in front of the robot
     if cur_ar_pose[0,3] < 0.:
         rospy.logwarn("Tag behind robot: Strange AR toolkit bug!")
         return
     self.cur_ar_pose = cur_ar_pose
     self.ar_pose_updated = True

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
def main():
    rospy.init_node("joint_kinematics")
    import sys
    def usage():
        print("Tests for kdl_parser:\n")
        print("kdl_parser <urdf file>")
        print("\tLoad the URDF from file.")
        print("kdl_parser")
        print("\tLoad the URDF from the parameter server.")
        sys.exit(1)

    if len(sys.argv) > 2:
        usage()
    if len(sys.argv) == 2 and (sys.argv[1] == "-h" or sys.argv[1] == "--help"):
        usage()
    if (len(sys.argv) == 1):
        robot = URDF.load_from_parameter_server(verbose=False)
    else:
        robot = URDF.load_xml_file(sys.argv[1], verbose=False)

    if True:
        import random
        base_link = robot.get_root()
        end_link = robot.links.keys()[random.randint(0, len(robot.links)-1)]
        print "Root link: %s; Random end link: %s" % (base_link, end_link)
        js_kin = JointKinematics(robot, base_link, end_link)
        print "Joint angles:", js_kin.get_joint_angles()
        print "Joint angles (wrapped):", js_kin.get_joint_angles(True)
        print "Joint velocities:", js_kin.get_joint_velocities()
        print "Joint efforts:", js_kin.get_joint_efforts()
        print "Jacobian:", js_kin.jacobian()
        kdl_pose = js_kin.forward()
        print "FK:", kdl_pose
        print "End effector force:", js_kin.end_effector_force()

        if True:
            import tf 
            from hrl_geom.pose_converter import PoseConv
            tf_list = tf.TransformListener()
            rospy.sleep(1)
            t = tf_list.getLatestCommonTime(base_link, end_link)
            tf_pose = PoseConv.to_homo_mat(tf_list.lookupTransform(base_link, end_link, t))
            print "Error with TF:", np.linalg.norm(kdl_pose - tf_pose)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def _ctrl_state_cb(self, ctrl_state):
     self._save_ep(PoseConv.to_homo_mat(ctrl_state.x_desi_filtered))
     with self.ctrl_state_lock:
         self.ctrl_state_dict["frame"] = ctrl_state.header.frame_id
         self.ctrl_state_dict["x_desi"] = PoseConv.to_pos_rot(ctrl_state.x_desi)
         self.ctrl_state_dict["xd_desi"] = extract_twist(ctrl_state.xd_desi)
         self.ctrl_state_dict["x_act"] = PoseConv.to_pos_rot(ctrl_state.x)
         self.ctrl_state_dict["xd_act"] = extract_twist(ctrl_state.xd)
         self.ctrl_state_dict["x_desi_filt"] = PoseConv.to_pos_rot(
                                                             ctrl_state.x_desi_filtered)
         self.ctrl_state_dict["x_err"] = extract_twist(ctrl_state.x_err)
         self.ctrl_state_dict["tau_pose"] = np.array(ctrl_state.tau_pose)
         self.ctrl_state_dict["tau_posture"] = np.array(ctrl_state.tau_posture)
         self.ctrl_state_dict["tau"] = np.array(ctrl_state.tau)
         self.ctrl_state_dict["F"] = np.array([ctrl_state.F.force.x, 
                                               ctrl_state.F.force.y,
                                               ctrl_state.F.force.z,
                                               ctrl_state.F.torque.x,
                                               ctrl_state.F.torque.y,
                                               ctrl_state.F.torque.z])

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
    def command_absolute_cb(self, msg):
        if self.arm is None:
            rospy.logwarn("[cartesian_manager] Cartesian controller not enabled.")
            return
        self.cart_ctrl.stop_moving(wait=True)
        if msg.header.frame_id == "":
            msg.header.frame_id = "torso_lift_link"
        if self.kin is None or msg.header.frame_id not in self.kin.get_link_names():
            self.kin = create_joint_kin("torso_lift_link", msg.header.frame_id)
        torso_pos_ep, torso_rot_ep = self.arm.get_ep()
        torso_B_ref = self.kin.forward(base_link="torso_lift_link", 
                                       end_link=msg.header.frame_id)
        ref_B_goal = PoseConv.to_homo_mat(msg)
        torso_B_goal = torso_B_ref * ref_B_goal

        change_pos, change_rot = PoseConv.to_pos_rot(torso_B_goal)
        change_pos_xyz = change_pos.T.A[0]
        rospy.loginfo("[cartesian_manager] Command absolute movement." + 
                      str((change_pos_xyz, change_rot)))
        self.cart_ctrl.execute_cart_move((change_pos_xyz, change_rot), ((1, 1, 1), 1), 
                                         velocity=self.velocity, blocking=True)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
    def find_zoom_level(self, msg, cam_info, clean_joints):
        if cam_info != None:
            T1W = self.psm1_kin.forward(clean_joints['psm1'].position)
            T2W = self.psm2_kin.forward(clean_joints['psm2'].position)
            TEW = self.ecm_kin.forward(clean_joints['ecm'].position)
            TEW_inv = numpy.linalg.inv(TEW)
            T1W_inv = numpy.linalg.inv(T1W)
            T2W_inv = numpy.linalg.inv(T2W)
            
            mid_point = (T1W[0:4,3] + T2W[0:4,3])/2
            p1 = T1W[0:4,3]
            p2 = T2W[0:4,3]
            
            T2E = TEW_inv * T2W
    
    #         ig = image_geometry.PinholeCameraModel()
            ig = image_geometry.StereoCameraModel()
            
            ig.fromCameraInfo(cam_info['right'], cam_info['left'])
            
            # Format in fakecam.launch:  x y z  yaw pitch roll [fixed-axis rotations: x(roll),y(pitch),z(yaw)]
            # Format for PoseConv.to_homo_mat:  (x,y,z)  (roll, pitch, yaw) [fixed-axis rotations: x(roll),y(pitch),z(yaw)]
            r = PoseConv.to_homo_mat( [ (0.0, 0.0, 0.0), (0.0, 0.0, 1.57079632679) ])
            r_inv = numpy.linalg.inv(r);
            
#             r = numpy.linalg.inv(r)
            self.logerror( r.__str__())
            
#             rotate_vector = lambda x: (r * numpy.array([ [x[0]], [x[1]], [x[2]], [1] ]) )[0:3,3]
             
            self.add_marker(T2W, '/left_arm', scale=[.002,.002,.002])
            
            
            l1, r1 = ig.project3dToPixel( ( r_inv * TEW_inv * T1W )[0:3,3]) # tool1 left and right pixel positions
            l2, r2 = ig.project3dToPixel( ( r_inv * TEW_inv * T2W )[0:3,3]) # tool2 left and right pixel positions
            lm, rm = ig.project3dToPixel( ( r_inv * TEW_inv * mid_point)[0:3,0]) # midpoint left and right pixel positions
    #         add_100 = lambda x : (x[0] *.5 + cam_info.width/2, x[1])
    #         l1 = add_100(l1)
    #         l2 = add_100(l2)
    #         lm = add_100(lm)
    
            
            self.zoom_level_positions = {'l1':l1, 'r1':r1, 'l2':l2, 'r2':r2, 'lm':lm, 'rm':rm}    
            
            test1_l, test1_r = ig.project3dToPixel( [1,0,0])
            test2_l, test2_r = ig.project3dToPixel( [0,0,1])
            self.logerror('\ntest1_l = ' + test1_l.__str__() + '\ntest2_l = ' + test2_l.__str__() )
    #         logerror('xm=%f,'%lm[0] +  'ym=%f'%lm[1])
            
            msg.position[3] = 0
    #         zoom_percentage = zoom_fitness(cam_info=cam_info, mid_point=[xm, ym], inner_margin=.20,
    #                                         deadzone_margin= .70, tool_point= [x1,y1])
            
            zoom_percentage = self.zoom_fitness2(cam_info['left'], mid_point=lm, tool_point=l1, 
                                            tool_point2=l2, radius=.1, deadzone_radius=.01)
            msg.position[2] =  msg.position[2] + zoom_percentage 
            if msg.position[2] < 0 : # minimum 0
                msg.position[2] = 0.00
            elif msg.position[2] > .21: # maximum .23
                msg.position[2] = .21
        return msg   

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def get_ell_frame(self, frame="/torso_lift_link"):
     # find the current ellipsoid frame location in this frame
     base_B_head = PoseConv.to_homo_mat(self.head_center)
     target_B_base = self.kin_head.forward(end_link=frame)
     return target_B_base**-1 * base_B_head

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
 def get_ell_pose(self, pose):
     torso_B_kinect = self.kin_head.forward(base_link="/torso_lift_link")
     torso_B_ee = PoseConv.to_homo_mat(pose)
     kinect_B_ee = torso_B_kinect**-1 * torso_B_ee
     ell_B_pose = self.get_ell_frame(self.kinect_frame)**-1 * kinect_B_ee
     return self.ell_space.pose_to_ellipsoidal(ell_B_pose)

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv

if __name__ == "__main__":
    rospy.init_node("test_poseconv")
    homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
    pose_msg = PoseConv.to_pose_msg(homo_mat)
    pos, quat = PoseConv.to_pos_quat(pose_msg)
    pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
    pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
    tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
    p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
    for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", 
                 "pose_stamped_msg2", "tf_stamped", "p", "R"]:
        print "%s: \n" % name, locals()[name]

# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_homo_mat [as 别名]
def main():
    if True:
        #q = [ 4.07545758,  5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389]
        d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922]
        a = [0, a2, a3, 0, 0, 0]
        d = [d1, 0, 0, d4, d5, d6]
        l = [pi/2, 0, 0, pi/2, -pi/2, 0]
        kin = RAVEKinematics()
        rospy.init_node("test_ur_ik")
        start_time = rospy.get_time()
        n = 0
        while not rospy.is_shutdown():
            q = (np.random.rand(6)-.5)*4*pi
            x1 = kin.forward(q)
            pos, euler = PoseConv.to_pos_euler(x1)
            m = np.random.randint(-4,5)
            euler = [euler[0], m*np.pi/2 + 0., euler[2]]
            #euler = [euler[0], 0.*np.pi/2 + m*np.pi, euler[2]]
            T = PoseConv.to_homo_mat(pos, euler)
            #q[4] = 0.
            T = kin.forward(q)
            sols = inverse_kin(T,a,d,l)
            print m, len(sols)
            if False and len(sols) == 0:
                print 'wuh', T
                sols = inverse_kin(T,a,d,l,True)
                
            if True:
                for qsol in sols:
                    #Tsol, _ = forward_kin(qsol, a, d, l)
                    Tsol = kin.forward(qsol)
                    #print qsol
                    #print q
                    #print Tsol
                    #print T
                    diff = Tsol**-1 * T
                    ang, _, _ = mat_to_ang_axis_point(diff)
                    dist = np.linalg.norm(diff[:3,3])
                    #print ang, dist
                    if abs(dist) > 1e-5 or abs(ang) > 1e-5:
                        print 'BAD'
                    else:
                        pass
                        #print 'GOOD'
            n += 1
        time_diff = rospy.get_time() - start_time
        print time_diff, n, n/time_diff

    if False:
        #q = [ 4.07545758,  5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389]
        d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922]
        a = [0, a2, a3, 0, 0, 0]
        d = [d1, 0, 0, d4, d5, d6]
        l = [pi/2, 0, 0, pi/2, -pi/2, 0]
        kin = RAVEKinematics()
        rospy.init_node("test_ur_ik")
        start_time = rospy.get_time()
        n = 0
        while not rospy.is_shutdown():
            q = (np.random.rand(6)-.5)*4*pi
            T = kin.forward(q)
            sols = inverse_kin(T,a,d,l)
            #print len(sols)
            if False:
                print len(sols)
                for qsol in sols:
                    #Tsol, _ = forward_kin(qsol, a, d, l)
                    Tsol = kin.forward(qsol)
                    diff = Tsol**-1 * T
                    ang, _, _ = mat_to_ang_axis_point(diff)
                    dist = np.linalg.norm(diff[:3,3])
                    #print ang, dist
                    if abs(dist) > 1e-8 or abs(ang) > 1e-8:
                        print 'BAD'
            n += 1
        time_diff = rospy.get_time() - start_time
        print time_diff, n, n/time_diff
        
    if False:
        #q = [ 4.07545758,  5.71643082, -4.57552159, -2.79061482, -3.17069678, 1.42865389]
        q = (np.random.rand(6)-.5)*4*pi
        d1, a2, a3, d4, d5, d6 = [0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922]
        a = [0, a2, a3, 0, 0, 0]
        d = [d1, 0, 0, d4, d5, d6]
        l = [pi/2, 0, 0, pi/2, -pi/2, 0]
        kin = RAVEKinematics()
        T = kin.forward(q)
        print T
        print forward_kin(q,a,d,l)
        print q
        sols = inverse_kin(T,a,d,l)
        for qsol in sols:
            Tsol, _ = forward_kin(qsol, a, d, l)
            diff = Tsol**-1 * T
            ang, _, _ = mat_to_ang_axis_point(diff)
            dist = np.linalg.norm(diff[:3,3])
            if False:
                if abs(dist) > 1e-6:
                    print '-'*80
                else:
#.........这里部分代码省略.........