From a9a213b278d66e3fc2d320b4a6a6c750ab6d577d Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Fri, 23 Sep 2011 11:08:25 +0200
Subject: [PATCH] First working version.
---
python/walktrans.py | 28 ++--------------------------
1 file changed, 2 insertions(+), 26 deletions(-)
diff --git a/python/walktrans.py b/python/walktrans.py
index 5feabf8..ead8f09 100644
--- a/python/walktrans.py
+++ b/python/walktrans.py
@@ -279,31 +279,6 @@ for i in range(2):
-aMm0 = eye(4); aMm0[0:2,3] = (rf[0][0:2,3]+lf[0][0:2,3])/2
-bMm1 = eye(4); bMm1[0:2,3] = (rf[1][0:2,3]+lf[1][0:2,3])/2
-wMa = inv(aMm0)
-wMm1 = eye(4); wMm1[0,3]=0.2
-wMb = dot(wMm1,inv(bMm1))
-
-wMr0 = dot( wMa, rf[0] )
-wMl0 = dot( wMa, lf[0] )
-wMr1 = dot( wMb, rf[1] )
-wMl1 = dot( wMb, lf[1] )
-
-if abs(wMr0[2,2]-1)>1e-3 or abs(wMl0[2,2]-1)>1e-3 or abs(wMr1[2,2]-1)>1e-3 or abs(wMl1[2,2]-1)>1e-3 :
- print 'Error: the feet '
-
-step1 = ( dot(wMa,lf[0]) )[0:2,3]
-step2 = ( dot( inv( dot(wMa,lf[0]) ), (dot( wMb,rf[1])) ) ) [0:2,3]
-step3 = ( dot( inv(rf[1]), lf[1] ))[0:2,3]
-
-'''
-seqpart = ''
-for s in [step1,step2, step3]:
- seqpart+= str(s[0])+' '+str(s[1])+' '+str(0)+' '
-pg.parseCmd(":stepseq " + seqpart)
-'''
-
rMl0 = dot( inv(rf[0]), lf[0] )
rMl1 = dot( inv(rf[1]), lf[1] )
if abs(rMl1[2,2]-1)>1e-3 or abs(rMl1[2,3])>1e-3:
@@ -333,7 +308,8 @@ step3 = ( dot( inv(rf[1]), lf[1] ))
seqpart = ''
for s in [step1,step2, step3]:
- seqpart+= str(s[0,3])+' '+str(s[1,3])+' '+str(arctan2(s[1,0],s[0,0])*180/pi)+' '
+ si = inv(s)
+ seqpart+= str(-si[0,3])+' '+str(-si[1,3])+' '+str(-arctan2(si[1,0],si[0,0])*180/pi)+' '
pg.parseCmd(":stepseq " + seqpart)
--
GitLab