Load a STEP file and create joints

Use pychrono.cascade to load a STEP file and create constraints between the bodies. We provide the .step file in the data/ directory for this example.

Uses PyChrono.

Learn how to:

  • load a STEP file, saved from a 3D CAD.
  • fetch parts and recerences from the STEP document, and create joints between them.
  • assign a ChLinkTrajectory to a part
1 #------------------------------------------------------------------------------
2 # Name: pychrono example
3 # Purpose:
4 #
5 # Author: Alessandro Tasora
6 #
7 # Created: 1/01/2019
8 # Copyright: (c) ProjectChrono 2019
9 #------------------------------------------------------------------------------
10 
11 print ("Example: Load a STEP file, generated by a CAD.");
12 print ("Please wait! this may take a while to load the file...");
13 
14 import pychrono.core as chrono
15 import pychrono.irrlicht as chronoirr
16 import pychrono.cascade as cascade
17 #import pychrono.mkl as mkl
18 try:
19  from OCC.Core import TopoDS
20 except:
21  from OCC import TopoDS
22 # Change this path to asset path, if running from other working dir.
23 # It must point to the data folder, containing GUI assets (textures, fonts, meshes, etc.)
24 chrono.SetChronoDataPath("../../../data/")
25 
26 
27 # ---------------------------------------------------------------------
28 #
29 # Create the simulation system and add items
30 #
31 
32 mysystem = chrono.ChSystemNSC()
33 
34 # Load a STEP file, containing a mechanism. The demo STEP file has been
35 # created using a 3D CAD (in this case, SolidEdge v.18).
36 #
37 
38 # Create the ChCascadeDoc, a container that loads the STEP model
39 # and manages its subassembles
40 mydoc = cascade.ChCascadeDoc()
41 
42 
43 # load the STEP model using this command:
44 load_ok = mydoc.Load_STEP(chrono.GetChronoDataPath() + "cascade/IRB7600_23_500_m2000_rev1_01_decorated.stp"); # or specify abs.path: ("C:\\data\\cascade\\assembly.stp");
45 
46 if not load_ok:
47  raise ValueError("Warning. Desired STEP file could not be opened/parsed \n")
48 
49 
50 
51 CH_C_PI = 3.1456
52 
53 # In most CADs the Y axis is horizontal, but we want it vertical.
54 # So define a root transformation for rotating all the imported objects.
55 rotation1 = chrono.ChQuaternionD()
56 rotation1.Q_from_AngAxis(-CH_C_PI / 2, chrono.ChVectorD(1, 0, 0)); # 1: rotate 90° on X axis
57 rotation2 = chrono.ChQuaternionD()
58 rotation2.Q_from_AngAxis(CH_C_PI, chrono.ChVectorD(0, 1, 0)); # 2: rotate 180° on vertical Y axis
59 tot_rotation = chrono.ChQuaternionD()
60 tot_rotation = rotation2 % rotation1 # rotate on 1 then on 2, using quaternion product
61 root_frame = chrono.ChFrameMovingD(chrono.ChVectorD(0, 0, 0), tot_rotation);
62 
63 
64 # Retrieve some sub shapes from the loaded model, using
65 # the GetNamedShape() function, that can use path/subpath/subsubpath/part
66 # syntax and * or ? wildcards, etc.
67 
68 def make_body_from_name(partname, root_transformation):
69  shape1 = TopoDS.TopoDS_Shape()
70  if (mydoc.GetNamedShape(shape1, partname)):
71  # Make a ChBody representing the TopoDS_Shape part from the CAD:
72  mbody1 = cascade.ChBodyEasyCascade(shape1, # shape
73  1000, # density (center of mass & inertia automatically computed)
74  False, # mesh for collide?
75  True) # mesh for visualization?
76  mysystem.Add(mbody1)
77  # Move the body as for global displacement/rotation (also mbody1 %= root_frame; )
78  mbody1.ConcatenatePreTransformation(root_transformation)
79  return mbody1
80  else:
81  raise ValueError("Warning. Body part name cannot be found in STEP file.\n")
82 
83 
84 mrigidBody_base = make_body_from_name("Assem10/Assem8", root_frame)
85 mrigidBody_turret = make_body_from_name("Assem10/Assem4", root_frame)
86 mrigidBody_bicep = make_body_from_name("Assem10/Assem1", root_frame)
87 mrigidBody_elbow = make_body_from_name("Assem10/Assem5", root_frame)
88 mrigidBody_forearm = make_body_from_name("Assem10/Assem7", root_frame)
89 mrigidBody_wrist = make_body_from_name("Assem10/Assem6", root_frame)
90 mrigidBody_hand = make_body_from_name("Assem10/Assem9", root_frame)
91 mrigidBody_cylinder = make_body_from_name("Assem10/Assem3", root_frame)
92 mrigidBody_rod = make_body_from_name("Assem10/Assem2", root_frame)
93 
94 mrigidBody_base.SetBodyFixed(True)
95 #mrigidBody_hand.SetBodyFixed(True)
96 
97 # Create joints between two parts.
98 # To understand where is the axis of the joint, we can exploit the fact
99 # that in the STEP file that we prepared for this demo, we inserted some
100 # objects called 'marker' and we placed them aligned to the shafts, so now
101 # we can fetch them and get their position/rotation.
102 
103 def make_frame_from_name(partname, root_transformation):
104  shape_marker = TopoDS.TopoDS_Shape()
105  if (mydoc.GetNamedShape(shape_marker, partname)):
106  frame_marker = chrono.ChFrameD()
107  mydoc.FromCascadeToChrono(shape_marker.Location(), frame_marker)
108  frame_marker.ConcatenatePreTransformation(root_transformation)
109  return frame_marker
110  else:
111  raise ValueError("Warning. Marker part name cannot be found in STEP file.\n")
112 
113 frame_marker_base_turret = make_frame_from_name("Assem10/Assem8/marker#1", root_frame)
114 frame_marker_turret_bicep = make_frame_from_name("Assem10/Assem4/marker#2", root_frame)
115 frame_marker_bicep_elbow = make_frame_from_name("Assem10/Assem1/marker#2", root_frame)
116 frame_marker_elbow_forearm = make_frame_from_name("Assem10/Assem5/marker#2", root_frame)
117 frame_marker_forearm_wrist = make_frame_from_name("Assem10/Assem7/marker#2", root_frame)
118 frame_marker_wrist_hand = make_frame_from_name("Assem10/Assem6/marker#2", root_frame)
119 frame_marker_turret_cylinder= make_frame_from_name("Assem10/Assem4/marker#3", root_frame)
120 frame_marker_cylinder_rod = make_frame_from_name("Assem10/Assem3/marker#2", root_frame)
121 frame_marker_rod_bicep = make_frame_from_name("Assem10/Assem2/marker#2", root_frame)
122 
123 
124 # Create joints between the parts.
125 # This can be done by creating link objects between couples of the bodies
126 # created in the section above, where the joint position is one of the
127 # frame_marker_xxxx_zzzz frames created above.
128 
129 my_link1 = chrono.ChLinkLockRevolute()
130 my_link1.Initialize(mrigidBody_base, mrigidBody_turret, frame_marker_base_turret.GetCoord())
131 mysystem.Add(my_link1)
132 
133 my_link2 = chrono.ChLinkLockRevolute()
134 my_link2.Initialize(mrigidBody_turret, mrigidBody_bicep, frame_marker_turret_bicep.GetCoord())
135 mysystem.Add(my_link2)
136 
137 my_link3 = chrono.ChLinkLockRevolute()
138 my_link3.Initialize(mrigidBody_bicep, mrigidBody_elbow, frame_marker_bicep_elbow.GetCoord())
139 mysystem.Add(my_link3)
140 
141 my_link4 = chrono.ChLinkLockRevolute()
142 my_link4.Initialize(mrigidBody_elbow, mrigidBody_forearm, frame_marker_elbow_forearm.GetCoord())
143 mysystem.Add(my_link4)
144 
145 my_link5 = chrono.ChLinkLockRevolute()
146 my_link5.Initialize(mrigidBody_forearm, mrigidBody_wrist, frame_marker_forearm_wrist.GetCoord())
147 mysystem.Add(my_link5)
148 
149 my_link6 = chrono.ChLinkLockRevolute()
150 my_link6.Initialize(mrigidBody_wrist, mrigidBody_hand, frame_marker_wrist_hand.GetCoord())
151 mysystem.Add(my_link6)
152 
153 my_link7 = chrono.ChLinkLockRevolute()
154 my_link7.Initialize(mrigidBody_turret, mrigidBody_cylinder, frame_marker_turret_cylinder.GetCoord())
155 mysystem.Add(my_link7)
156 
157 my_link8 = chrono.ChLinkLockRevolute()
158 my_link8.Initialize(mrigidBody_cylinder, mrigidBody_rod, frame_marker_cylinder_rod.GetCoord())
159 mysystem.Add(my_link8)
160 
161 my_link9 = chrono.ChLinkLockRevolute()
162 my_link9.Initialize(mrigidBody_rod, mrigidBody_bicep, frame_marker_rod_bicep.GetCoord())
163 mysystem.Add(my_link9)
164 
165 
166 # Create a large cube as a floor.
167 
168 mfloor = chrono.ChBodyEasyBox(5, 1, 5, 1000, True, True)
169 mfloor.SetPos(chrono.ChVectorD(0,-0.5,0))
170 mfloor.SetBodyFixed(True)
171 mysystem.Add(mfloor)
172 
173 mcolor = chrono.ChColorAsset(0.3, 0.3, 0.8)
174 mfloor.AddAsset(mcolor)
175 
176 
177 # We want to move the hand of the robot using a trajectory.
178 # Since in this demo all joints are 'off' (i.e just revolute joints),
179 # it follows that if we move the hand all the robot will automatically
180 # move as in inverse kinematics.
181 
182 # Create a ChLinePath geometry, for the hand path, and insert arc/lines sub-paths:
183 mpath = chrono.ChLinePath()
184 ma1 = chrono.ChLineArc(
185  chrono.ChCoordsysD(mrigidBody_hand.GetPos(), # arc center position
186  chrono.Q_ROTATE_X_TO_Z), # arc plane alignment (default: xy plane)
187  0.3, # radius
188  -chrono.CH_C_PI_2, # start arc ngle (counterclockwise, from local x)
189  -chrono.CH_C_PI_2+chrono.CH_C_2PI, # end arc angle
190  True)
191 mpath.AddSubLine(ma1)
192 mpath.SetPathDuration(2)
193 mpath.Set_closed(True)
194 
195 # Create a ChLineShape, a visualization asset for lines.
196 mpathasset = chrono.ChLineShape()
197 mpathasset.SetLineGeometry(mpath)
198 mfloor.AddAsset(mpathasset)
199 
200 # This is the constraint that uses the trajectory
201 mtrajectory = chrono.ChLinkTrajectory()
202 # Define which parts are connected (the trajectory is considered in the 2nd body).
203 mtrajectory.Initialize(mrigidBody_hand, # body1 that follows the trajectory
204  mfloor, # body2 that 'owns' the trajectory
205  chrono.VNULL, # point on body1 that will follow the trajectory, in body1 coords
206  mpath # the trajectory (reuse the one already added to body2 as asset)
207  )
208 mysystem.Add(mtrajectory)
209 # Optionally, set a function that gets the curvilinear
210 # abscyssa s of the line, as a function of time s(t).
211 # By default it was simply s=t.
212 mspacefx = chrono.ChFunction_Ramp(0, 0.5)
213 mtrajectory.Set_space_fx(mspacefx)
214 
215 # Just to constraint the hand rotation:
216 mparallelism = chrono.ChLinkLockParallel()
217 mparallelism.Initialize(mrigidBody_hand, mfloor, frame_marker_wrist_hand.GetCoord())
218 mysystem.Add(mparallelism);
219 
220 # ---------------------------------------------------------------------
221 #
222 # Create an Irrlicht application to visualize the system
223 #
224 
225 myapplication = chronoirr.ChIrrApp(mysystem, 'Import STEP', chronoirr.dimension2du(1024,768))
226 
227 myapplication.AddTypicalSky(chrono.GetChronoDataPath() + 'skybox/')
228 myapplication.AddTypicalLogo(chrono.GetChronoDataPath() + 'logo_pychrono_alpha.png')
229 myapplication.AddTypicalCamera(chronoirr.vector3df(2,2,2),chronoirr.vector3df(0,0.8,0))
230 #myapplication.AddTypicalLights()
231 myapplication.AddLightWithShadow(chronoirr.vector3df(3,6,2), # point
232  chronoirr.vector3df(0,0,0), # aimpoint
233  12, # radius (power)
234  1,11, # near, far
235  55) # angle of FOV
236 
237  # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
238  # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
239  # If you need a finer control on which item really needs a visualization proxy in
240  # Irrlicht, just use application.AssetBind(myitem); on a per-item basis.
241 
242 myapplication.AssetBindAll();
243 
244  # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
245  # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!
246 
247 myapplication.AssetUpdateAll();
248 
249  # If you want to show shadows because you used "AddLightWithShadow()'
250  # you must remember this:
251 myapplication.AddShadowAll();
252 
253 # ---------------------------------------------------------------------
254 #
255 # Run the simulation
256 #
257 
258 # Change the solver form the default SOR to a more precise solver
259 
260 #msolver = mkl.ChSolverMKLcsm()
261 #mysystem.SetSolver(msolver)
262 
263 mysystem.SetSolverType(chrono.ChSolver.Type_BARZILAIBORWEIN);
264 #mysystem.SetSolverType(chrono.ChSolver.Type_MINRES);
265 mysystem.SetMaxItersSolverSpeed(300)
266 
267 myapplication.SetTimestep(0.01)
268 
269 
270 while(myapplication.GetDevice().run()):
271  myapplication.BeginScene()
272  myapplication.DrawAll()
273  myapplication.DoStep()
274  myapplication.EndScene()
275 
276 
277 
278 
279