U Z-dl@sddlZddlZddlZddlZejddkr>ddlm Z n ddl m Z ddl m Z ddZ dd Zd d Zdd dZdddZddZdddZdS)NUSERmike)quaternion_from_euler)URDFcCsttjd}tj|ddtjd}t||}||krJtdS|t d|9}t ||}tjd|d|d|d |d |d |d d f|d |d d|d|d|d|dd f|d |d |d|dd|d|dd fdftjdS)z_Return homogeneous rotation matrix from quaternion. Copied from 2006, Christoph Gohlke g@Ndtypeg@?)r )r )rr r )rr r r )rr)r r rr )rrrr ) npfinfofloatepsarrayfloat64copydotidentitymathsqrtouter)Z quaternionZ_EPSZq_Znqqr;/home/daniela/catkin_ws/src/hpe/scripts/utils/transforms.pyquaternionMatrixs   222r cCs8t|}|d|d<|d|d<|d|d<d|d<|S)Nrrr rr r )r r )r )transquatmatrixrrr translationQuaternionToTransform"s    r$c Cst|}i}|jD]~}|j}|j}|jj}|jj}t||}i||<|||d<|||d<|||d<t t |d|d|ddd||d <q|S) Nchildparentr!rr r Zsxyz)axesr") rZ from_xml_filejointsr%r&originxyzrpy generateKeylistr) fileZ xml_robotdictjointr%r&r*r+keyrrrget_transform_tree_dict+s      *r2cCs|d||S)N-r)r&r%suffixrrrr,>sr,Fc Csg}t}|D]\}}||d|dq|dkr\tj|ddddlm}|t|||} t dt | dD]0} | | } | | d} | | | t | | dq||S) a* Gets a chain of transforms given two reference frames and a se of transformations. Computes a graph from the set of transforms, and then finds a path in the graph between the two given links. @param from_frame: initial frame @param to_frame: final frame @param transform_pool: a dictionary containing several transforms @return: a chain of transforms The standard we have is to use a list of dictionaries, each containing # information about the transform: [{'parent': parent, 'child': child, 'key': 'parent-child'}, {...}] r&r%T)Z with_labelsrNr )r&r%r1) nxZGraphitemsZadd_edgedrawmatplotlib.pyplotpyplotshow shortest_pathrangelenappendr,) from_frameto_frameZtransform_poolr8chaingraphZ transform_key transformpltpathidxr&r%rrrgetChainAs   rHc Cstjdtjd}|D]}t|d|d}t|d|d}||krl||d}||d}t||}nX||kr||d}||d}tjt||}n td|dd|dd t ||}q|S) z Multiplies local transforms in a chain to get the global transform of the chain @param chain: a list of transforms @param transforms: a pool of transformations @return: the global transformation (4x4 homogeneous) rrr&r%r!r"zTransform from z to z does not exist.) reyerr,keysr$linalginv ValueErrorr) rB transformsrDlinkr1Z inverse_keyr!r"Zparent_T_childrrrgetAggregateTransform`s        rPcCst||||}t||S)z Gets a transformation between any two frames @param from_frame: Starting frame @param to_frame: Ending frame @param transforms: dictionary of several transforms @return: the global transformation (4x4 homogeneous) )rHrP)r@rArNr8rBrrr getTransformsrQ)r3)F)F)rosnumpyrZnetworkxr6environgetZtf_transformationsrZtf.transformationsZurdf_parser_py.urdfrr r$r2r,rHrPrQrrrrs