@@ -675,20 +675,20 @@ <h1>SO(2) matrix<a class="headerlink" href="#so-2-matrix" title="Link to this he
675675< p > Example:</ p >
676676< div class ="highlight-pycon notranslate "> < div class ="highlight "> < pre > < span > </ span > < span class ="gp "> >>> </ span > < span class ="kn "> from</ span > < span class ="w "> </ span > < span class ="nn "> spatialmath</ span > < span class ="w "> </ span > < span class ="kn "> import</ span > < span class ="n "> SE3</ span >
677677< span class ="gp "> >>> </ span > < span class ="n "> SE3</ span > < span class ="o "> .</ span > < span class ="n "> Rx</ span > < span class ="p "> (</ span > < span class ="mf "> 0.1</ span > < span class ="p "> )</ span > < span class ="o "> **</ span > < span class ="mi "> 2</ span >
678- < span class ="go "> SE3(array([[ 1. , 0. , 0. , 0. ],</ span >
679- < span class ="go "> [ 0. , 0.98006658 , -0.19866933 , 0. ],</ span >
680- < span class ="go "> [ 0. , 0.19866933 , 0.98006658 , 0. ],</ span >
681- < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
678+ < span class ="go "> SE3(array([[ 1. , 0. , 0. , 0. ],</ span >
679+ < span class ="go "> [ 0. , 0.9801 , -0.1987 , 0. ],</ span >
680+ < span class ="go "> [ 0. , 0.1987 , 0.9801 , 0. ],</ span >
681+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
682682< span class ="gp "> >>> </ span > < span class ="n "> SE3</ span > < span class ="o "> .</ span > < span class ="n "> Rx</ span > < span class ="p "> ([</ span > < span class ="mi "> 0</ span > < span class ="p "> ,</ span > < span class ="mf "> 0.1</ span > < span class ="p "> ])</ span > < span class ="o "> **</ span > < span class ="mi "> 2</ span >
683683< span class ="go "> SE3([</ span >
684684< span class ="go "> array([[1., 0., 0., 0.],</ span >
685685< span class ="go "> [0., 1., 0., 0.],</ span >
686686< span class ="go "> [0., 0., 1., 0.],</ span >
687687< span class ="go "> [0., 0., 0., 1.]]),</ span >
688- < span class ="go "> array([[ 1. , 0. , 0. , 0. ],</ span >
689- < span class ="go "> [ 0. , 0.98006658 , -0.19866933 , 0. ],</ span >
690- < span class ="go "> [ 0. , 0.19866933 , 0.98006658 , 0. ],</ span >
691- < span class ="go "> [ 0. , 0. , 0. , 1. ]]) ])</ span >
688+ < span class ="go "> array([[ 1. , 0. , 0. , 0. ],</ span >
689+ < span class ="go "> [ 0. , 0.9801 , -0.1987 , 0. ],</ span >
690+ < span class ="go "> [ 0. , 0.1987 , 0.9801 , 0. ],</ span >
691+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]) ])</ span >
692692</ pre > </ div >
693693</ div >
694694</ dd > </ dl >
@@ -1118,11 +1118,11 @@ <h1>SO(2) matrix<a class="headerlink" href="#so-2-matrix" title="Link to this he
11181118< span class ="gp "> >>> </ span > < span class ="n "> R</ span > < span class ="o "> =</ span > < span class ="n "> SO2</ span > < span class ="p "> (</ span > < span class ="mf "> 0.5</ span > < span class ="p "> )</ span >
11191119< span class ="gp "> >>> </ span > < span class ="n "> A</ span > < span class ="o "> =</ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> array</ span > < span class ="p "> ([[</ span > < span class ="mi "> 10</ span > < span class ="p "> ,</ span > < span class ="mi "> 0</ span > < span class ="p "> ],</ span > < span class ="p "> [</ span > < span class ="mi "> 0</ span > < span class ="p "> ,</ span > < span class ="mi "> 1</ span > < span class ="p "> ]])</ span >
11201120< span class ="gp "> >>> </ span > < span class ="nb "> print</ span > < span class ="p "> (</ span > < span class ="n "> R</ span > < span class ="o "> *</ span > < span class ="n "> A</ span > < span class ="o "> *</ span > < span class ="n "> R</ span > < span class ="o "> .</ span > < span class ="n "> inv</ span > < span class ="p "> ())</ span >
1121- < span class ="go "> [[7.93136038 3.78661943 ]</ span >
1122- < span class ="go "> [3.78661943 3.06863962 ]]</ span >
1121+ < span class ="go "> [[7.9314 3.7866 ]</ span >
1122+ < span class ="go "> [3.7866 3.0686 ]]</ span >
11231123< span class ="gp "> >>> </ span > < span class ="nb "> print</ span > < span class ="p "> (</ span > < span class ="n "> R</ span > < span class ="o "> .</ span > < span class ="n "> conjugation</ span > < span class ="p "> (</ span > < span class ="n "> A</ span > < span class ="p "> ))</ span >
1124- < span class ="go "> [[7.93136038 3.78661943 ]</ span >
1125- < span class ="go "> [3.78661943 3.06863962 ]]</ span >
1124+ < span class ="go "> [[7.9314 3.7866 ]</ span >
1125+ < span class ="go "> [3.7866 3.0686 ]]</ span >
11261126</ pre > </ div >
11271127</ div >
11281128</ dd > </ dl >
@@ -1251,33 +1251,36 @@ <h1>SO(2) matrix<a class="headerlink" href="#so-2-matrix" title="Link to this he
12511251< li > < p > < code class ="docutils literal notranslate "> < span class ="pre "> X.interp(Y,</ span > < span class ="pre "> N)</ span > </ code > interpolates pose between X and Y in < code class ="docutils literal notranslate "> < span class ="pre "> N</ span > </ code > steps.</ p > </ li >
12521252</ ul >
12531253< p > Example:</ p >
1254- < div class ="highlight-pycon notranslate "> < div class ="highlight "> < pre > < span > </ span > < span class ="gt "> Traceback (most recent call last):</ span >
1255- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1256- < span class ="gr "> NameError</ span > : < span class ="n "> name 'SE3' is not defined</ span >
1257- < span class ="gt "> Traceback (most recent call last):</ span >
1258- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1259- < span class ="gr "> NameError</ span > : < span class ="n "> name 'SE3' is not defined</ span >
1260- < span class ="gt "> Traceback (most recent call last):</ span >
1261- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1262- < span class ="gr "> NameError</ span > : < span class ="n "> name 'x' is not defined</ span >
1263- < span class ="gt "> Traceback (most recent call last):</ span >
1264- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1265- < span class ="gr "> NameError</ span > : < span class ="n "> name 'x' is not defined</ span >
1266- < span class ="gt "> Traceback (most recent call last):</ span >
1267- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1268- < span class ="gr "> NameError</ span > : < span class ="n "> name 'x' is not defined</ span >
1269- < span class ="gt "> Traceback (most recent call last):</ span >
1270- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1271- < span class ="gr "> NameError</ span > : < span class ="n "> name 'x' is not defined</ span >
1272- < span class ="gt "> Traceback (most recent call last):</ span >
1273- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1274- < span class ="gr "> NameError</ span > : < span class ="n "> name 'z' is not defined</ span >
1275- < span class ="gt "> Traceback (most recent call last):</ span >
1276- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1277- < span class ="gr "> NameError</ span > : < span class ="n "> name 'z' is not defined</ span >
1278- < span class ="gt "> Traceback (most recent call last):</ span >
1279- File < span class ="nb "> "<input>"</ span > , line < span class ="m "> 1</ span > , in < span class ="n "> <module></ span >
1280- < span class ="gr "> NameError</ span > : < span class ="n "> name 'z' is not defined</ span >
1254+ < div class ="highlight-pycon notranslate "> < div class ="highlight "> < pre > < span > </ span > < span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> =</ span > < span class ="n "> SE3</ span > < span class ="p "> (</ span > < span class ="o "> -</ span > < span class ="mi "> 1</ span > < span class ="p "> ,</ span > < span class ="o "> -</ span > < span class ="mi "> 2</ span > < span class ="p "> ,</ span > < span class ="mi "> 0</ span > < span class ="p "> )</ span > < span class ="o "> *</ span > < span class ="n "> SE3</ span > < span class ="o "> .</ span > < span class ="n "> Rx</ span > < span class ="p "> (</ span > < span class ="o "> -</ span > < span class ="mf "> 0.3</ span > < span class ="p "> )</ span >
1255+ < span class ="gp "> >>> </ span > < span class ="n "> y</ span > < span class ="o "> =</ span > < span class ="n "> SE3</ span > < span class ="p "> (</ span > < span class ="mi "> 1</ span > < span class ="p "> ,</ span > < span class ="mi "> 2</ span > < span class ="p "> ,</ span > < span class ="mi "> 0</ span > < span class ="p "> )</ span > < span class ="o "> *</ span > < span class ="n "> SE3</ span > < span class ="o "> .</ span > < span class ="n "> Rx</ span > < span class ="p "> (</ span > < span class ="mf "> 0.3</ span > < span class ="p "> )</ span >
1256+ < span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> .</ span > < span class ="n "> interp</ span > < span class ="p "> (</ span > < span class ="n "> y</ span > < span class ="p "> ,</ span > < span class ="mi "> 0</ span > < span class ="p "> )</ span > < span class ="c1 "> # this is x</ span >
1257+ < span class ="go "> SE3(array([[ 1. , 0. , 0. , -1. ],</ span >
1258+ < span class ="go "> [ 0. , 0.9553, 0.2955, -2. ],</ span >
1259+ < span class ="go "> [ 0. , -0.2955, 0.9553, 0. ],</ span >
1260+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
1261+ < span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> .</ span > < span class ="n "> interp</ span > < span class ="p "> (</ span > < span class ="n "> y</ span > < span class ="p "> ,</ span > < span class ="mi "> 1</ span > < span class ="p "> )</ span > < span class ="c1 "> # this is y</ span >
1262+ < span class ="go "> SE3(array([[ 1. , 0. , 0. , 1. ],</ span >
1263+ < span class ="go "> [ 0. , 0.9553, -0.2955, 2. ],</ span >
1264+ < span class ="go "> [ 0. , 0.2955, 0.9553, 0. ],</ span >
1265+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
1266+ < span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> .</ span > < span class ="n "> interp</ span > < span class ="p "> (</ span > < span class ="n "> y</ span > < span class ="p "> ,</ span > < span class ="mf "> 0.5</ span > < span class ="p "> )</ span > < span class ="c1 "> # this is in between</ span >
1267+ < span class ="go "> SE3(array([[1., 0., 0., 0.],</ span >
1268+ < span class ="go "> [0., 1., 0., 0.],</ span >
1269+ < span class ="go "> [0., 0., 1., 0.],</ span >
1270+ < span class ="go "> [0., 0., 0., 1.]]))</ span >
1271+ < span class ="gp "> >>> </ span > < span class ="n "> z</ span > < span class ="o "> =</ span > < span class ="n "> x</ span > < span class ="o "> .</ span > < span class ="n "> interp</ span > < span class ="p "> (</ span > < span class ="n "> y</ span > < span class ="p "> ,</ span > < span class ="mi "> 11</ span > < span class ="p "> )</ span > < span class ="c1 "> # in 11 steps</ span >
1272+ < span class ="gp "> >>> </ span > < span class ="nb "> len</ span > < span class ="p "> (</ span > < span class ="n "> z</ span > < span class ="p "> )</ span >
1273+ < span class ="go "> 11</ span >
1274+ < span class ="gp "> >>> </ span > < span class ="n "> z</ span > < span class ="p "> [</ span > < span class ="mi "> 0</ span > < span class ="p "> ]</ span > < span class ="c1 "> # this is x</ span >
1275+ < span class ="go "> SE3(array([[ 1. , 0. , 0. , -1. ],</ span >
1276+ < span class ="go "> [ 0. , 0.9553, 0.2955, -2. ],</ span >
1277+ < span class ="go "> [ 0. , -0.2955, 0.9553, 0. ],</ span >
1278+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
1279+ < span class ="gp "> >>> </ span > < span class ="n "> z</ span > < span class ="p "> [</ span > < span class ="mi "> 5</ span > < span class ="p "> ]</ span > < span class ="c1 "> # this is in between</ span >
1280+ < span class ="go "> SE3(array([[1., 0., 0., 0.],</ span >
1281+ < span class ="go "> [0., 1., 0., 0.],</ span >
1282+ < span class ="go "> [0., 0., 1., 0.],</ span >
1283+ < span class ="go "> [0., 0., 0., 1.]]))</ span >
12811284</ pre > </ div >
12821285</ div >
12831286< div class ="admonition note ">
@@ -1703,15 +1706,13 @@ <h1>SO(2) matrix<a class="headerlink" href="#so-2-matrix" title="Link to this he
17031706< span class ="go "> 1 0 0 0 </ span >
17041707< span class ="go "> 0 1 0 0 </ span >
17051708< span class ="go "> 0 0 1 0 </ span >
1706- < span class ="go "> 0 0 0 1 </ span >
1707-
1709+ < span class ="go "> 0 0 0 1</ span >
17081710< span class ="gp "> >>> </ span > < span class ="n "> SE3</ span > < span class ="p "> ()</ span > < span class ="o "> .</ span > < span class ="n "> print</ span > < span class ="p "> (</ span > < span class ="s2 "> "pose is:"</ span > < span class ="p "> )</ span >
17091711< span class ="go "> pose is:</ span >
17101712< span class ="go "> 1 0 0 0 </ span >
17111713< span class ="go "> 0 1 0 0 </ span >
17121714< span class ="go "> 0 0 1 0 </ span >
1713- < span class ="go "> 0 0 0 1 </ span >
1714-
1715+ < span class ="go "> 0 0 0 1</ span >
17151716</ pre > </ div >
17161717</ div >
17171718< dl class ="field-list simple ">
@@ -1830,10 +1831,10 @@ <h1>SO(2) matrix<a class="headerlink" href="#so-2-matrix" title="Link to this he
18301831< div class ="highlight-pycon notranslate "> < div class ="highlight "> < pre > < span > </ span > < span class ="gp "> >>> </ span > < span class ="kn "> from</ span > < span class ="w "> </ span > < span class ="nn "> spatialmath</ span > < span class ="w "> </ span > < span class ="kn "> import</ span > < span class ="n "> SE3</ span >
18311832< span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> =</ span > < span class ="n "> SE3</ span > < span class ="o "> .</ span > < span class ="n "> Rx</ span > < span class ="p "> ([</ span > < span class ="mi "> 0</ span > < span class ="p "> ,</ span > < span class ="mf "> 0.1</ span > < span class ="p "> ,</ span > < span class ="mf "> 0.2</ span > < span class ="p "> ,</ span > < span class ="mf "> 0.3</ span > < span class ="p "> ])</ span >
18321833< span class ="gp "> >>> </ span > < span class ="n "> x</ span > < span class ="o "> .</ span > < span class ="n "> prod</ span > < span class ="p "> ()</ span >
1833- < span class ="go "> SE3(array([[ 1. , 0. , 0. , 0. ],</ span >
1834- < span class ="go "> [ 0. , 0.82533561 , -0.56464247 , 0. ],</ span >
1835- < span class ="go "> [ 0. , 0.56464247 , 0.82533561 , 0. ],</ span >
1836- < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
1834+ < span class ="go "> SE3(array([[ 1. , 0. , 0. , 0. ],</ span >
1835+ < span class ="go "> [ 0. , 0.8253 , -0.5646 , 0. ],</ span >
1836+ < span class ="go "> [ 0. , 0.5646 , 0.8253 , 0. ],</ span >
1837+ < span class ="go "> [ 0. , 0. , 0. , 1. ]]))</ span >
18371838</ pre > </ div >
18381839</ div >
18391840< div class ="admonition note ">
0 commit comments