@@ -896,7 +896,7 @@ def __mul__(self, right): # pylint: disable=no-self-argument
896896 else :
897897 raise ValueError ('bad arguments' )
898898
899- def __rmul__ (self , left ): # pylint: disable=no-self-argument
899+ def __rmul__ (right , left ): # pylint: disable=no-self-argument
900900 """
901901 Line transformation
902902
@@ -912,14 +912,11 @@ def __rmul__(self, left): # pylint: disable=no-self-argument
912912
913913 :seealso: Plucker.__mul__
914914 """
915- right = self
916915 if isinstance (left , SE3 ):
917- A = np .r_ [ np .c_ [left .R , base .skew (- left .t ) @ left .R ],
918- np .c_ [np .zeros ((3 ,3 )), left .R ]
919- ]
920- return self .__class__ ( A @ right .vec ) # premultiply by SE3
916+ A = left .inv ().Ad ()
917+ return right .__class__ ( A @ right .vec ) # premultiply by SE3.Ad
921918 else :
922- raise ValueError ('bad arguments ' )
919+ raise ValueError ('can only premultiply Line3 by SE3 ' )
923920
924921 # ------------------------------------------------------------------------- #
925922 # PLUCKER LINE DISTANCE AND INTERSECTION
@@ -1219,10 +1216,43 @@ def __init__(self, v=None, w=None):
12191216 import pathlib
12201217 import os .path
12211218
1222- a = Plane3 ([0.1 , - 1 , - 1 , 2 ])
1223- base .plotvol3 (5 )
1224- a .plot (color = 'r' , alpha = 0.3 )
1225- plt .show (block = True )
1219+ from spatialmath import Twist3
1220+
1221+ L = Line3 .TwoPoints ((1 ,2 ,0 ), (1 ,2 ,1 ))
1222+ print (L )
1223+ print (L .intersect_plane ([0 , 0 , 1 , 0 ]))
1224+
1225+ z = np .eye (6 ) * L
1226+
1227+ L2 = SE3 (2 , 1 , 10 ) * L
1228+ print (L2 )
1229+ print (L2 .intersect_plane ([0 , 0 , 1 , 0 ]))
1230+
1231+ print ('rx' )
1232+ L2 = SE3 .Rx (np .pi / 4 ) * L
1233+ print (L2 )
1234+ print (L2 .intersect_plane ([0 , 0 , 1 , 0 ]))
1235+
1236+ print ('ry' )
1237+ L2 = SE3 .Ry (np .pi / 4 ) * L
1238+ print (L2 )
1239+ print (L2 .intersect_plane ([0 , 0 , 1 , 0 ]))
1240+
1241+ print ('rz' )
1242+ L2 = SE3 .Rz (np .pi / 4 ) * L
1243+ print (L2 )
1244+ print (L2 .intersect_plane ([0 , 0 , 1 , 0 ]))
1245+
1246+ pass
1247+ # base.plotvol3(10)
1248+ # S = Twist3.UnitRevolute([0, 0, 1], [2, 3, 2], 0.5);
1249+ # L = S.line()
1250+ # L.plot('k:', linewidth=2)
1251+
1252+ # a = Plane3([0.1, -1, -1, 2])
1253+ # base.plotvol3(5)
1254+ # a.plot(color='r', alpha=0.3)
1255+ # plt.show(block=True)
12261256
12271257 # a = SE3.Exp([2,0,0,0,0,0])
12281258
0 commit comments