44class URmsg (object ):
55 """
66 """
7+
78 def __init__ (self , ** kwargs ):
89 for k , v in kwargs .items ():
910 setattr (self , k , v )
@@ -14,39 +15,42 @@ def __str__(self):
1415 if value :
1516 result += "%s, " % value
1617 return result [:- 2 ]
17-
18+
1819 def __repr__ (self ):
1920 return self .__str__
2021
2122
2223class Point (URmsg ):
2324 """
2425 """
26+
2527 def __init__ (self , x , y , z ):
26- self .x = x # [m]
27- self .y = y # [m]
28- self .z = z # [m]
28+ self .x = x # [m]
29+ self .y = y # [m]
30+ self .z = z # [m]
2931
3032 def __str__ (self ):
31- return '%.6f, %.6f, %.6f' % (self .x , self .y , self .z )
33+ return '%.6f, %.6f, %.6f' % (self .x , self .y , self .z )
3234
3335
3436class AxisAngle (URmsg ):
3537 """
3638 """
39+
3740 def __init__ (self , x , y , z ):
3841 self .x = x
3942 self .y = y
4043 self .z = z
41-
44+
4245 def __str__ (self ):
43- return '%.6f, %.6f, %.6f' % (self .x , self .y , self .z )
46+ return '%.6f, %.6f, %.6f' % (self .x , self .y , self .z )
4447
4548
4649class URPose (URmsg ):
4750 """
4851 """
49- def __init__ (self , position = Point (0 ,0 ,0 ), orientation = AxisAngle (0 ,0 ,0 )):
52+
53+ def __init__ (self , position = Point (0 , 0 , 0 ), orientation = AxisAngle (0 , 0 , 0 )):
5054 self .position = position
5155 self .orientation = orientation
5256
@@ -55,21 +59,22 @@ def from_frame(cls, frame):
5559 point = frame .point
5660 ax , ay , az = frame .axis_angle_vector
5761 return cls (Point (* list (point )), AxisAngle (ax , ay , az ))
58-
62+
5963 def __str__ (self ):
6064 return "p[%s, %s]" % (self .position , self .orientation )
6165
6266
6367class URPoseTrajectoryPoint (URmsg ):
6468 """
6569 """
70+
6671 def __init__ (self , pose = URPose (), acceleration = None , velocity = None , time = None , radius = None ):
6772 self .pose = pose
68- self .acceleration = acceleration # [m/s^2]
69- self .velocity = velocity # [m/s]
70- self .time = time # [s]
71- self .radius = radius # [m]
72-
73+ self .acceleration = acceleration # [m/s^2]
74+ self .velocity = velocity # [m/s]
75+ self .time = time # [s]
76+ self .radius = radius # [m]
77+
7378 def __str__ (self ):
7479 result = "%s" % self .pose
7580 if self .acceleration :
@@ -86,6 +91,7 @@ def __str__(self):
8691class URMovej (URmsg ):
8792 """
8893 """
94+
8995 def __init__ (self , pose_trajectory_point = URPoseTrajectoryPoint ()):
9096 self .pose_trajectory_point = pose_trajectory_point
9197
@@ -96,6 +102,7 @@ def __str__(self):
96102class URMovel (URmsg ):
97103 """
98104 """
105+
99106 def __init__ (self , pose_trajectory_point = URPoseTrajectoryPoint ()):
100107 self .pose_trajectory_point = pose_trajectory_point
101108
@@ -106,11 +113,12 @@ def __str__(self):
106113class URGoal (URmsg ):
107114 """
108115 """
116+
109117 def __init__ (self , script_lines = []):
110118 self .script = "def prog():\n \t "
111- self .script += "\n \t " .join ([str (line ) for line in script_lines ])
119+ self .script += "\n \t " .join ([str (line ) for line in script_lines ])
112120 self .script += "\n end\n prog()\n \n "
113-
121+
114122 @property
115123 def msg (self ):
116124 return {"script" : self .script }
@@ -129,11 +137,11 @@ def msg(self):
129137 velocity = 0.17
130138 time = 5.
131139 script_lines = []
132-
133- for frame in frames :
134- ptp = URPoseTrajectoryPoint (URPose .from_frame (frame ), acceleration , velocity , time , None )
140+
141+ for frame in frames :
142+ ptp = URPoseTrajectoryPoint (URPose .from_frame (
143+ frame ), acceleration , velocity , time , None )
135144 move = URMovej (ptp )
136- #move = Movel(ptp)
137145 script_lines .append (move )
138146
139147 urgoal = URGoal (script_lines )
0 commit comments