コード例 #1
0
ファイル: ros_utils.py プロジェクト: cameronlee/python
def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
    rospy.logdebug("joint: %s"%jointname)
    U = get_pr2_urdf()
        
    joint = U.joints[jointname]
    joint.origin = joint.origin or urdf.Pose()
    if not joint.origin.position: joint.origin.position = [0,0,0]
    if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
   
    parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))

    childFromRotated = np.eye(4)
    if valuedict and jointname in valuedict:        
        
        if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
            childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
        elif joint.joint_type == 'prismatic':
            childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) 
            
    parentFromRotated = np.dot(parentFromChild, childFromRotated)            
    originFromParent = conversions.pose_to_hmat(ps.pose)
    originFromRotated = np.dot(originFromParent, parentFromRotated)
    
    newps = gm.PoseStamped()
    newps.header = ps.header
    newps.pose = conversions.hmat_to_pose(originFromRotated)
    
    return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)
コード例 #2
0
ファイル: ros_utils.py プロジェクト: warriorarmentaix/lfd
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
    rospy.logdebug("joint: %s" % jointname)
    U = get_pr2_urdf()

    joint = U.joints[jointname]
    joint.origin = joint.origin or urdf.Pose()
    if not joint.origin.position: joint.origin.position = [0, 0, 0]
    if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]

    parentFromChild = conversions.trans_rot_to_hmat(
        joint.origin.position,
        transformations.quaternion_from_euler(*joint.origin.rotation))

    childFromRotated = np.eye(4)
    if valuedict and jointname in valuedict:

        if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
            childFromRotated = transformations.rotation_matrix(
                valuedict[jointname], joint.axis)
        elif joint.joint_type == 'prismatic':
            childFromRotated = transformations.translation_matrix(
                np.array(joint.axis) * valuedict[jointname])

    parentFromRotated = np.dot(parentFromChild, childFromRotated)
    originFromParent = conversions.pose_to_hmat(ps.pose)
    originFromRotated = np.dot(originFromParent, parentFromRotated)

    newps = gm.PoseStamped()
    newps.header = ps.header
    newps.pose = conversions.hmat_to_pose(originFromRotated)

    return make_kin_tree_from_link(newps,
                                   joint.child,
                                   ns=ns,
                                   valuedict=valuedict)
コード例 #3
0
ファイル: ros_utils.py プロジェクト: cameronlee/python
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
    markers = []
    U = get_pr2_urdf()
    link = U.links[linkname]
    
    if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
        rospy.logdebug("%s is a mesh. drawing it.", linkname)
        marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
        marker.ns = ns
        marker.header = ps.header        
        
        linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
        origFromLink = conversions.pose_to_hmat(ps.pose)
        origFromGeom = np.dot(origFromLink, linkFromGeom)
        
        marker.pose = conversions.hmat_to_pose(origFromGeom)           
        marker.scale = gm.Vector3(1,1,1)
        marker.color = stdm.ColorRGBA(1,1,0,.5)
        marker.id = 0
        marker.lifetime = rospy.Duration()
        marker.mesh_resource = str(link.visual.geometry.filename)
        marker.mesh_use_embedded_materials = False
        markers.append(marker)
    else:
        rospy.logdebug("%s is not a mesh", linkname)
        
    if U.child_map.has_key(linkname):
        for (cjoint,clink) in U.child_map[linkname]:
            markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
            
    return markers    
コード例 #4
0
ファイル: conversions.py プロジェクト: warriorarmentaix/lfd
def hmat_to_trans_rot(hmat):
    ''' 
    Converts a 4x4 homogenous rigid transformation matrix to a translation and a 
    quaternion rotation. 
    '''
    scale, shear, angles, trans, persp = transformations.decompose_matrix(hmat)
    rot = transformations.quaternion_from_euler(*angles)
    return trans, rot
コード例 #5
0
ファイル: conversions.py プロジェクト: hojonathanho/python
def hmat_to_trans_rot(hmat): 
    ''' 
    Converts a 4x4 homogenous rigid transformation matrix to a translation and a 
    quaternion rotation. 
    ''' 
    scale, shear, angles, trans, persp = transformations.decompose_matrix(hmat) 
    rot = transformations.quaternion_from_euler(*angles) 
    return trans, rot 
コード例 #6
0
 def run(self):
     while not rospy.is_shutdown() and not self.wants_exit:
         rot = quaternion_from_euler(*self.transform.R)
         trans = self.transform.T
         self.br.sendTransform(trans, rot, rospy.Time.now(), "/camera_link", PARENT_FRAME)
         ps = gm.PoseStamped()
         ps.pose = trans_rot_to_pose(trans, rot)
         ps.header.frame_id = "/camera_link"
         ps.header.stamp = rospy.Time.now()
         self.pose_pub.publish(ps)
         rospy.sleep(0.01)
コード例 #7
0
 def run(self):
     while not rospy.is_shutdown() and not self.wants_exit:
         rot = quaternion_from_euler(*self.transform.R)
         trans = self.transform.T
         self.br.sendTransform(trans, rot, rospy.Time.now(),
                               "/camera2_link", PARENT_FRAME)
         ps = gm.PoseStamped()
         ps.pose = trans_rot_to_pose(trans, rot)
         ps.header.frame_id = PARENT_FRAME
         ps.header.stamp = rospy.Time.now()
         self.pose_pub.publish(ps)
         rospy.sleep(.01)
コード例 #8
0
ファイル: ros_utils.py プロジェクト: warriorarmentaix/lfd
def make_kin_tree_from_link(ps, linkname, ns='default_ns', valuedict=None):
    markers = []
    U = get_pr2_urdf()
    link = U.links[linkname]

    if link.visual and link.visual.geometry and isinstance(
            link.visual.geometry, urdf.Mesh):
        rospy.logdebug("%s is a mesh. drawing it.", linkname)
        marker = Marker(type=Marker.MESH_RESOURCE, action=Marker.ADD)
        marker.ns = ns
        marker.header = ps.header

        linkFromGeom = conversions.trans_rot_to_hmat(
            link.visual.origin.position,
            transformations.quaternion_from_euler(
                *link.visual.origin.rotation))
        origFromLink = conversions.pose_to_hmat(ps.pose)
        origFromGeom = np.dot(origFromLink, linkFromGeom)

        marker.pose = conversions.hmat_to_pose(origFromGeom)
        marker.scale = gm.Vector3(1, 1, 1)
        marker.color = stdm.ColorRGBA(1, 1, 0, .5)
        marker.id = 0
        marker.lifetime = rospy.Duration()
        marker.mesh_resource = str(link.visual.geometry.filename)
        marker.mesh_use_embedded_materials = False
        markers.append(marker)
    else:
        rospy.logdebug("%s is not a mesh", linkname)

    if U.child_map.has_key(linkname):
        for (cjoint, clink) in U.child_map[linkname]:
            markers.extend(
                make_kin_tree_from_joint(ps,
                                         cjoint,
                                         ns=ns,
                                         valuedict=valuedict))

    return markers
コード例 #9
0
ファイル: conversions.py プロジェクト: warriorarmentaix/lfd
def yaw_to_quat(yaw):
    return transformations.quaternion_from_euler(0, 0, yaw)
コード例 #10
0
ファイル: conversions.py プロジェクト: hojonathanho/python
def yaw_to_quat(yaw):
    return transformations.quaternion_from_euler(0, 0, yaw)