1+ from __future__ import print_function
12import numpy as np
23import fcl
34
45def print_collision_result (o1_name , o2_name , result ):
5- print 'Collision between {} and {}:' .format (o1_name , o2_name )
6- print '-' * 30
7- print 'Collision?: {}' .format (result .is_collision )
8- print 'Number of contacts: {}' .format (len (result .contacts ))
9- print ''
6+ print ( 'Collision between {} and {}:' .format (o1_name , o2_name ) )
7+ print ( '-' * 30 )
8+ print ( 'Collision?: {}' .format (result .is_collision ) )
9+ print ( 'Number of contacts: {}' .format (len (result .contacts ) ))
10+ print ()
1011
1112def print_continuous_collision_result (o1_name , o2_name , result ):
12- print 'Continuous collision between {} and {}:' .format (o1_name , o2_name )
13- print '-' * 30
14- print 'Collision?: {}' .format (result .is_collide )
15- print 'Time of collision: {}' .format (result .time_of_contact )
16- print ''
13+ print ( 'Continuous collision between {} and {}:' .format (o1_name , o2_name ) )
14+ print ( '-' * 30 )
15+ print ( 'Collision?: {}' .format (result .is_collide ) )
16+ print ( 'Time of collision: {}' .format (result .time_of_contact ) )
17+ print ()
1718
1819def print_distance_result (o1_name , o2_name , result ):
19- print 'Distance between {} and {}:' .format (o1_name , o2_name )
20- print '-' * 30
21- print 'Distance: {}' .format (result .min_distance )
22- print 'Closest Points:'
23- print result .nearest_points [0 ]
24- print result .nearest_points [1 ]
25- print ''
20+ print ( 'Distance between {} and {}:' .format (o1_name , o2_name ) )
21+ print ( '-' * 30 )
22+ print ( 'Distance: {}' .format (result .min_distance ) )
23+ print ( 'Closest Points:' )
24+ print ( result .nearest_points [0 ])
25+ print ( result .nearest_points [1 ])
26+ print ()
2627
2728# Create simple geometries
2829box = fcl .Box (1.0 , 2.0 , 3.0 )
@@ -48,10 +49,10 @@ def print_distance_result(o1_name, o2_name, result):
4849#=====================================================================
4950# Pairwise collision checking
5051#=====================================================================
51- print '=' * 60
52- print 'Testing Pairwise Collision Checking'
53- print '=' * 60
54- print ''
52+ print ( '=' * 60 )
53+ print ( 'Testing Pairwise Collision Checking' )
54+ print ( '=' * 60 )
55+ print ()
5556
5657req = fcl .CollisionRequest (enable_contact = True )
5758res = fcl .CollisionResult ()
@@ -74,10 +75,10 @@ def print_distance_result(o1_name, o2_name, result):
7475#=====================================================================
7576# Pairwise distance checking
7677#=====================================================================
77- print '=' * 60
78- print 'Testing Pairwise Distance Checking'
79- print '=' * 60
80- print ''
78+ print ( '=' * 60 )
79+ print ( 'Testing Pairwise Distance Checking' )
80+ print ( '=' * 60 )
81+ print ()
8182
8283req = fcl .DistanceRequest (enable_nearest_points = True )
8384res = fcl .DistanceResult ()
@@ -100,10 +101,10 @@ def print_distance_result(o1_name, o2_name, result):
100101#=====================================================================
101102# Pairwise continuous collision checking
102103#=====================================================================
103- print '=' * 60
104- print 'Testing Pairwise Continuous Collision Checking'
105- print '=' * 60
106- print ''
104+ print ( '=' * 60 )
105+ print ( 'Testing Pairwise Continuous Collision Checking' )
106+ print ( '=' * 60 )
107+ print ()
107108
108109req = fcl .ContinuousCollisionRequest ()
109110res = fcl .ContinuousCollisionResult ()
@@ -118,10 +119,10 @@ def print_distance_result(o1_name, o2_name, result):
118119#=====================================================================
119120# Managed collision checking
120121#=====================================================================
121- print '=' * 60
122- print 'Testing Managed Collision and Distance Checking'
123- print '=' * 60
124- print ''
122+ print ( '=' * 60 )
123+ print ( 'Testing Managed Collision and Distance Checking' )
124+ print ( '=' * 60 )
125+ print ( '' )
125126objs1 = [fcl .CollisionObject (box , fcl .Transform (np .array ([20 ,0 ,0 ]))), fcl .CollisionObject (sphere )]
126127objs2 = [fcl .CollisionObject (cone ), fcl .CollisionObject (mesh )]
127128objs3 = [fcl .CollisionObject (box ), fcl .CollisionObject (sphere )]
@@ -143,26 +144,26 @@ def print_distance_result(o1_name, o2_name, result):
143144#=====================================================================
144145cdata = fcl .CollisionData ()
145146manager1 .collide (cdata , fcl .defaultCollisionCallback )
146- print 'Collision within manager 1?: {}' .format (cdata .result .is_collision )
147- print ''
147+ print ( 'Collision within manager 1?: {}' .format (cdata .result .is_collision ) )
148+ print ()
148149
149150cdata = fcl .CollisionData ()
150151manager2 .collide (cdata , fcl .defaultCollisionCallback )
151- print 'Collision within manager 2?: {}' .format (cdata .result .is_collision )
152- print ''
152+ print ( 'Collision within manager 2?: {}' .format (cdata .result .is_collision ) )
153+ print ()
153154
154155##=====================================================================
155156## Managed internal (n^2) distance checking
156157##=====================================================================
157158ddata = fcl .DistanceData ()
158159manager1 .distance (ddata , fcl .defaultDistanceCallback )
159- print 'Closest distance within manager 1?: {}' .format (ddata .result .min_distance )
160- print ''
160+ print ( 'Closest distance within manager 1?: {}' .format (ddata .result .min_distance ) )
161+ print ()
161162
162163ddata = fcl .DistanceData ()
163164manager2 .distance (ddata , fcl .defaultDistanceCallback )
164- print 'Closest distance within manager 2?: {}' .format (ddata .result .min_distance )
165- print ''
165+ print ( 'Closest distance within manager 2?: {}' .format (ddata .result .min_distance ) )
166+ print ()
166167
167168#=====================================================================
168169# Managed one to many collision checking
@@ -171,20 +172,19 @@ def print_distance_result(o1_name, o2_name, result):
171172rdata = fcl .CollisionData (request = req )
172173
173174manager1 .collide (fcl .CollisionObject (mesh ), rdata , fcl .defaultCollisionCallback )
174- print 'Collision between manager 1 and Mesh?: {}' .format (rdata .result .is_collision )
175- print 'Contacts:'
175+ print ( 'Collision between manager 1 and Mesh?: {}' .format (rdata .result .is_collision ) )
176+ print ( 'Contacts:' )
176177for c in rdata .result .contacts :
177- print '\t O1: {}, O2: {}' .format (c .o1 , c .o2 )
178- print ''
178+ print ( '\t O1: {}, O2: {}' .format (c .o1 , c .o2 ) )
179+ print ()
179180
180181#=====================================================================
181182# Managed many to many collision checking
182183#=====================================================================
183184rdata = fcl .CollisionData (request = req )
184185manager3 .collide (manager2 , rdata , fcl .defaultCollisionCallback )
185- print 'Collision between manager 2 and manager 3?: {}' .format (rdata .result .is_collision )
186- print 'Contacts:'
186+ print ( 'Collision between manager 2 and manager 3?: {}' .format (rdata .result .is_collision ) )
187+ print ( 'Contacts:' )
187188for c in rdata .result .contacts :
188- print '\t O1: {}, O2: {}' .format (c .o1 , c .o2 )
189- print ''
190-
189+ print ('\t O1: {}, O2: {}' .format (c .o1 , c .o2 ))
190+ print ()
0 commit comments