@@ -716,181 +716,28 @@ def set_xlabel(self, *args, **kwargs):
716716 def set_ylabel (self , * args , ** kwargs ):
717717 self .ax .set_ylabel (* args , ** kwargs )
718718
719- def plotvol2 (dim , ax = None , equal = False ):
720- """
721- Create 2D plot area
722-
723- :param ax: axes of initializer, defaults to new subplot
724- :type ax: AxesSubplot, optional
725- :param equal: set aspect ratio to 1:1, default False
726- :type equal: bool
727- :return: initialized axes
728- :rtype: AxesSubplot
729-
730- Initialize axes with dimensions given by ``dim`` which can be:
731-
732- * A (scalar), -A:A x -A:A
733- * [A,B], A:B x A:B
734- * [A,B,C,D], A:B x C:D
735-
736- :seealso: :func:`plotvol3`, :func:`expand_dims`
737- """
738- dims = expand_dims (dim , 2 )
739- if ax is None :
740- ax = plt .subplot ()
741- ax .axis (dims )
742- ax .set_xlabel ('X' )
743- ax .set_ylabel ('Y' )
744-
745- if equal :
746- ax .set_aspect ('equal' )
747- return ax
748-
749- def plotvol3 (dim , ax = None , equal = False ):
750- """
751- Create 3D plot volume
752-
753- :param ax: axes of initializer, defaults to new subplot
754- :type ax: Axes3DSubplot, optional
755- :param equal: set aspect ratio to 1:1:1, default False
756- :type equal: bool
757- :return: initialized axes
758- :rtype: Axes3DSubplot
759-
760- Initialize axes with dimensions given by ``dim`` which can be:
761719
762- * A (scalar), -A:A x -A:A x -A:A
763- * [A,B], A:B x A:B x A:B
764- * [A,B,C,D,E,F], A:B x C:D x E:F
765720
766- :seealso: :func:`plotvol2`, :func:`expand_dims`
767- """
768- dims = expand_dims (dim , 3 )
769- print (dims )
770- if ax is None :
771- ax = plt .subplot (projection = '3d' )
772- ax .set_xlim3d (dims [0 ], dims [1 ])
773- ax .set_ylim3d (dims [2 ], dims [3 ])
774- ax .set_zlim3d (dims [4 ], dims [5 ])
775- ax .set_xlabel ('X' )
776- ax .set_ylabel ('Y' )
777- ax .set_zlabel ('Z' )
778-
779- if equal :
780- ax .set_aspect ('equal' )
781- return ax
782-
783- def plot_point (pos , marker = 'bs' , text = None , ax = None , color = None , ** kwargs ):
784- """
785- Plot a point using matplotlib
786-
787- :param pos: position of marker
788- :type pos: array_like(2), ndarray(2,n), list of 2-tuples
789- :param marker: matplotlub marker style, defaults to 'bs'
790- :type marker: str or list of str, optional
791- :param text: text label, defaults to None
792- :type text: str, optional
793- :param ax: axes to plot in, defaults to ``gca()````
794- :type ax: Axis, optional
795- :param color: text color, defaults to None
796- :type color: str or array_like(3), optional
797-
798- The color of the marker can be different to the color of the text,
799- the marker color is specified by a single letter in the marker string.
800-
801- A point can multiple markers which will be overlaid, for instance ``["rx",
802- "ro"]`` will give a ⨂ symbol.
803-
804- The optional text label is placed to the right of the marker, and vertically
805- aligned.
806-
807- Multiple points can be marked if ``pos`` is a 2xn array or a list of
808- coordinate pairs. If a label is provided every point will have the same
809- label. However, the text is processed with ``format`` and is provided with a
810- single argument, the point index (starting at zero).
811-
812-
813- """
814-
815- if isinstance (pos , np .ndarray ):
816- if pos .ndim == 1 :
817- x = pos [0 ]
818- y = pos [1 ]
819- elif pos .ndim == 2 and pos .shape [0 ] == 2 :
820- x = pos [0 ,:]
821- y = pos [1 ,:]
822- elif isinstance (pos , (tuple , list )):
823- # [x, y]
824- # [(x,y), (x,y), ...]
825- # [xlist, ylist]
826- # [xarray, yarray]
827- if base .islistof (pos , (tuple , list )):
828- x = [z [0 ] for z in pos ]
829- y = [z [1 ] for z in pos ]
830- elif base .islistof (pos , np .ndarray ):
831- x = pos [0 ]
832- y = pos [1 ]
833- else :
834- x = pos [0 ]
835- y = pos [1 ]
836-
837- if ax is None :
838- ax = plt .gca ()
839- if isinstance (marker , (list , tuple )):
840- for m in marker :
841- plt .plot (x , y , m , ** kwargs )
842- else :
843- plt .plot (x , y , marker )
844- if text :
845- try :
846- for i , xy in enumerate (zip (x , y )):
847- plt .text (xy [0 ], xy [1 ], ' ' + text .format (i ), horizontalalignment = 'left' , verticalalignment = 'center' , color = color , ** kwargs )
848- except :
849- plt .text (x , y , ' ' + text , horizontalalignment = 'left' , verticalalignment = 'center' , color = color , ** kwargs )
850-
851- def expand_dims (dim = None , nd = 2 ):
852- """[summary]
853-
854-
855- :param dim: [description], defaults to None
856- :type dim: [type], optional
857- :param nd: [description], defaults to 2
858- :type nd: int, optional
859- :raises ValueError: bad arguments
860- :return: 2d or 3d dimensions vector
861- :rtype: ndarray(4) or ndarray(6)
862-
863- Compute bounding dimensions for plots from shorthand notation.
864-
865- If ``nd==2``, [xmin, xmax, ymin, ymax]:
866- * A -> [-A, A, -A, A]
867- * [A,B] -> [A, B, A, B]
868- * [A,B,C,D] -> [A, B, C, D]
869-
870- If ``nd==3``, [xmin, xmax, ymin, ymax, zmin, zmax]:
871- * A -> [-A, A, -A, A, -A, A]
872- * [A,B] -> [A, B, A, B, A, B]
873- * [A,B,C,D,E,F] -> [A, B, C, D, E, F]
874- """
875- dim = base .getvector (dim )
876-
877- if nd == 2 :
878- if len (dim ) == 1 :
879- return np .r_ [- dim , dim , - dim , dim ]
880- elif len (dim ) == 2 :
881- return np .r_ [- dim [0 ], dim [0 ], - dim [1 ], dim [1 ]]
882- elif len (dim ) == 4 :
883- return dim
884- else :
885- raise ValueError ('bad dimension specified' )
886- elif nd == 3 :
887- if len (dim ) == 1 :
888- return np .r_ [- dim , dim , - dim , dim , - dim , dim ]
889- elif len (dim ) == 3 :
890- return np .r_ [- dim [0 ], dim [0 ], - dim [1 ], dim [1 ], - dim [2 ], dim [2 ]]
891- elif len (dim ) == 6 :
892- return dim
893- else :
894- raise ValueError ('bad dimension specified' )
895- else :
896- raise ValueError ('nd is 2 or 3' )
721+ if __name__ == "__main__" :
722+ # from spatialmath import UnitQuaternion
723+ # from spatialmath.base import tranimate, r2t
724+
725+ # J = np.array([[2, -1, 0], [-1, 4, 0], [0, 0, 3]])
726+ # dt = 0.05
727+ # def attitude():
728+ # attitude = UnitQuaternion()
729+ # w = 0.2 * np.r_[1, 2, 2].T
730+ # for t in np.arange(0, 3, dt):
731+ # wd = -np.linalg.inv(J) @ (np.cross(w, J @ w))
732+ # w += wd * dt
733+ # attitude.increment(w * dt)
734+ # yield attitude.R
735+ # plt.figure()
736+ # plotvol3(2)
737+ # tranimate(attitude())
738+
739+ # from spatialmath import base
740+
741+ # T = base.rpy2r(0.3, 0.4, 0.5)
742+ # base.tranimate(T, wait=True)
743+ pass
0 commit comments