From 506b2ebe9559bced38e7a7b4202c8991078606dd Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 23 Sep 2011 09:21:27 +0200
Subject: [PATCH] Added the rpy-to-matrix.
---
python/matrix_util.py | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/python/matrix_util.py b/python/matrix_util.py
index bcad56d..2aaf588 100644
--- a/python/matrix_util.py
+++ b/python/matrix_util.py
@@ -80,6 +80,14 @@ def matrixToRPY( M ):
rot = tr2rpy(M)
return [ M[0][3], M[1][3], M[2][3], rot[2],rot[1],rot[0]]
+def RPYToMatrix( pr ):
+ '''
+ Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
+ '''
+ M=array(rpy2tr(pr[3],pr[4],pr[5]))
+ M[0:3,3] = pr[0:3]
+ return M
+
# Transformation Matrix corresponding to a rotation about x,y or z
def rotate(axis,ang):
''' eg. T=rot('x',pi/4): rotate pi/4 rad about x axis
--
GitLab